* Quaternion: added dot product and angularDistance functions. The latter is

based on the former.
* opengl_demo: makes IcoSphere better (vertices are instanciated only once) and
               removed the generation of a big geometry for the fancy spheres...

Eigen/src/Geometry/Quaternion.h

@@ -163,6 +163,15 @@ public:
     */
   inline Scalar norm() const { return m_coeffs.norm(); }
 
+  /** \returns the dot product of \c *this and \a other
+    * Geometrically speaking, the dot product of two unit quaternions
+    * corresponds to the cosine of half the angle between the two rotations.
+    * \sa angularDistance()
+    */
+  inline Scalar dot(const Quaternion& other) const { return m_coeffs.dot(other.m_coeffs); }
+
+  inline Scalar angularDistance(const Quaternion& other) const;
+
   Matrix3 toRotationMatrix(void) const;
 
   template<typename Derived1, typename Derived2>
@@ -357,22 +366,28 @@ inline Quaternion<Scalar> Quaternion<Scalar>::conjugate() const
   return Quaternion(this->w(),-this->x(),-this->y(),-this->z());
 }
 
+/** \returns the angle (in radian) between two rotations
+  * \sa dot()
+  */
+template <typename Scalar>
+inline Scalar Quaternion<Scalar>::angularDistance(const Quaternion& other) const
+{
+  double d = ei_abs(this->dot(other));
+  if (d>=1.0)
+    return 0;
+  return 2.0 * std::acos(d);
+}
+
 /** \returns the spherical linear interpolation between the two quaternions
   * \c *this and \a other at the parameter \a t
   */
 template <typename Scalar>
 Quaternion<Scalar> Quaternion<Scalar>::slerp(Scalar t, const Quaternion& other) const
 {
-  // FIXME options for this function would be:
-  // 1 - Quaternion& fromSlerp(Scalar t, const Quaternion& q0, const Quaternion& q1);
-  //     which set *this from the s-lerp and returns *this
-  // 2 - Quaternion slerp(Scalar t, const Quaternion& other) const
-  //     which returns the s-lerp between this and other
-  // ??
   static const Scalar one = Scalar(1) - precision<Scalar>();
-  Scalar d = m_coeffs.dot(other.m_coeffs);
+  Scalar d = this->dot(other);
   Scalar absD = ei_abs(d);
-  if (d>=one)
+  if (absD>=one)
     return *this;
 
   // theta is the angle between the 2 quaternions

demos/opengl/icosphere.cpp

@@ -25,6 +25,7 @@
 #include "icosphere.h"
 
 #include <GL/gl.h>
+#include <map>
 
 using namespace Eigen;
 
@@ -74,24 +75,39 @@ const std::vector<int>& IcoSphere::indices(int level) const
 
 void IcoSphere::_subdivide(void)
 {
+  typedef unsigned long long Key;
+  std::map<Key,int> edgeMap;
   const std::vector<int>& indices = *mIndices.back();
   mIndices.push_back(new std::vector<int>);
   std::vector<int>& refinedIndices = *mIndices.back();
   int end = indices.size();
   for (int i=0; i<end; i+=3)
   {
-    int i0, i1, i2;
+    int ids0[3],  // indices of outer vertices
-    Vector3f v0 = mVertices[i0=indices[i+0]];
+        ids1[3];  // indices of edge vertices
-    Vector3f v1 = mVertices[i1=indices[i+1]];
+    for (int k=0; k<3; ++k)
-    Vector3f v2 = mVertices[i2=indices[i+2]];
+    {
-    int start = mVertices.size();
+      int k1 = (k+1)%3;
-    mVertices.push_back( (v0+v1).normalized() );
+      int e0 = indices[i+k];
-    mVertices.push_back( (v1+v2).normalized() );
+      int e1 = indices[i+k1];
-    mVertices.push_back( (v2+v0).normalized() );
+      ids0[k] = e0;
-    refinedIndices.push_back(i0); refinedIndices.push_back(start+0); refinedIndices.push_back(start+2);
+      if (e1>e0)
-    refinedIndices.push_back(i1); refinedIndices.push_back(start+1); refinedIndices.push_back(start+0);
+        std::swap(e0,e1);
-    refinedIndices.push_back(i2); refinedIndices.push_back(start+2); refinedIndices.push_back(start+1);
+      Key edgeKey = Key(e0) | (Key(e1)<<32);
-    refinedIndices.push_back(start+0); refinedIndices.push_back(start+1); refinedIndices.push_back(start+2);
+      std::map<Key,int>::iterator it = edgeMap.find(edgeKey);
+      if (it==edgeMap.end())
+      {
+        ids1[k] = mVertices.size();
+        edgeMap[edgeKey] = ids1[k];
+        mVertices.push_back( (mVertices[e0]+mVertices[e1]).normalized() );
+      }
+      else
+        ids1[k] = it->second;
+    }
+    refinedIndices.push_back(ids0[0]); refinedIndices.push_back(ids1[0]); refinedIndices.push_back(ids1[2]);
+    refinedIndices.push_back(ids0[1]); refinedIndices.push_back(ids1[1]); refinedIndices.push_back(ids1[0]);
+    refinedIndices.push_back(ids0[2]); refinedIndices.push_back(ids1[2]); refinedIndices.push_back(ids1[1]);
+    refinedIndices.push_back(ids1[0]); refinedIndices.push_back(ids1[1]); refinedIndices.push_back(ids1[2]);
   }
   mListIds.push_back(0);
 }

demos/opengl/quaternion_demo.cpp

@@ -41,6 +41,89 @@
 
 using namespace Eigen;
 
+class FancySpheres
+{
+  public:
+    FancySpheres()
+    {
+      const int levels = 4;
+      const float scale = 0.33;
+      float radius = 100;
+      std::vector<int> parents;
+
+      // leval 0
+      mCenters.push_back(Vector3f::Zero());
+      parents.push_back(-1);
+      mRadii.push_back(radius);
+      
+      // generate level 1 using icosphere vertices
+      radius *= 0.45;
+      {
+        float dist = mRadii[0]*0.9;
+        for (int i=0; i<12; ++i)
+        {
+          mCenters.push_back(mIcoSphere.vertices()[i] * dist);
+          mRadii.push_back(radius);
+          parents.push_back(0);
+        }
+      }
+
+      static const float angles [10] = {
+        0, 0,
+        M_PI, 0.*M_PI,
+        M_PI, 0.5*M_PI,
+        M_PI, 1.*M_PI,
+        M_PI, 1.5*M_PI
+      };
+
+      // generate other levels
+      int start = 1;
+      for (int l=1; l<levels; l++)
+      {
+        radius *= scale;
+        int end = mCenters.size();
+        for (int i=start; i<end; ++i)
+        {
+          Vector3f c = mCenters[i];
+          Vector3f ax0 = (c - mCenters[parents[i]]).normalized();
+          Vector3f ax1 = ax0.unitOrthogonal();
+          Quaternionf q;
+          q.setFromTwoVectors(Vector3f::UnitZ(), ax0);
+          Transform3f t = Translation3f(c) * q * Scaling3f(mRadii[i]+radius);
+          for (int j=0; j<5; ++j)
+          {
+            Vector3f newC = c + ( (AngleAxisf(angles[j*2+1], ax0)
+                                * AngleAxisf(angles[j*2+0] * (l==1 ? 0.35 : 0.5), ax1)) * ax0)
+                                * (mRadii[i] + radius*0.8);
+            mCenters.push_back(newC);
+            mRadii.push_back(radius);
+            parents.push_back(i);
+          }
+        }
+        start = end;
+      }
+    }
+    
+    void draw()
+    {
+      int end = mCenters.size();
+      glEnable(GL_NORMALIZE);
+      for (int i=0; i<end; ++i)
+      {
+        Transform3f t = Translation3f(mCenters[i]) * Scaling3f(mRadii[i]);
+        gpu.pushMatrix(GL_MODELVIEW);
+        gpu.multMatrix(t.matrix(),GL_MODELVIEW);
+        mIcoSphere.draw(2);
+        gpu.popMatrix(GL_MODELVIEW);
+      }
+      glDisable(GL_NORMALIZE);
+    }
+  protected:
+    std::vector<Vector3f> mCenters;
+    std::vector<float> mRadii;
+    IcoSphere mIcoSphere;
+};
+
 
 // generic linear interpolation method
 template<typename T> T lerp(float t, const T& a, const T& b)
@@ -156,7 +239,8 @@ void RenderingWidget::grabFrame(void)
 
 void RenderingWidget::drawScene()
 {
-  float length = 50;
+  static FancySpheres sFancySpheres;
+  float length = 200;
   gpu.drawVector(Vector3f::Zero(), length*Vector3f::UnitX(), Color(1,0,0,1));
   gpu.drawVector(Vector3f::Zero(), length*Vector3f::UnitY(), Color(0,1,0,1));
   gpu.drawVector(Vector3f::Zero(), length*Vector3f::UnitZ(), Color(0,0,1,1));
@@ -183,13 +267,14 @@ void RenderingWidget::drawScene()
   glEnable(GL_LIGHT1);
   
   glColor3f(0.4, 0.7, 0.4);
-  glVertexPointer(3, GL_FLOAT, 0, mVertices[0].data());
+  sFancySpheres.draw();
-  glNormalPointer(GL_FLOAT, 0, mNormals[0].data());
+//   glVertexPointer(3, GL_FLOAT, 0, mVertices[0].data());
-  glEnableClientState(GL_VERTEX_ARRAY);
+//   glNormalPointer(GL_FLOAT, 0, mNormals[0].data());
-  glEnableClientState(GL_NORMAL_ARRAY);
+//   glEnableClientState(GL_VERTEX_ARRAY);
-  glDrawArrays(GL_TRIANGLES, 0, mVertices.size());
+//   glEnableClientState(GL_NORMAL_ARRAY);
-  glDisableClientState(GL_VERTEX_ARRAY);
+//   glDrawArrays(GL_TRIANGLES, 0, mVertices.size());
-  glDisableClientState(GL_NORMAL_ARRAY);
+//   glDisableClientState(GL_VERTEX_ARRAY);
+//   glDisableClientState(GL_NORMAL_ARRAY);
   
   glDisable(GL_LIGHTING);
 }
@@ -413,93 +498,6 @@ void RenderingWidget::initializeGL()
   mCamera.setTarget(Vector3f(0, 0, 0));
   mInitFrame.orientation = mCamera.orientation().inverse();
   mInitFrame.position = mCamera.viewMatrix().translation();
-
-  // create a kind of fractal sphere
-  {
-    IcoSphere pattern;
-    
-    int levels = 3;
-    float scale = 0.45;
-    float radius = 100;
-    std::vector<Vector3f> centers;
-    std::vector<int> parents;
-    std::vector<float> radii;
-    centers.push_back(Vector3f::Zero());
-    parents.push_back(-1);
-    radii.push_back(radius);
-    radius *= scale;
-
-    // generate level 1 using icosphere vertices
-    {
-      float dist = radii[0]*0.9;
-      for (int i=0; i<12; ++i)
-      {
-        centers.push_back(pattern.vertices()[i] * dist);
-        radii.push_back(radius);
-        parents.push_back(0);
-      }
-    }
-
-    scale = 0.33;
-    static const float angles [10] = {
-      0, 0,
-      M_PI, 0.*M_PI,
-      M_PI, 0.5*M_PI,
-      M_PI, 1.*M_PI,
-      M_PI, 1.5*M_PI};
-    
-    // generate other levels
-    int start = 1;
-    float maxAngle = M_PI/2;
-    for (int l=1; l<levels; l++)
-    {
-      radius *= scale;
-      int end = centers.size();
-      for (int i=start; i<end; ++i)
-      {
-        Vector3f c = centers[i];
-        Vector3f ax0, ax1;
-        if (parents[i]==-1)
-          ax0 = Vector3f::UnitZ();
-        else
-          ax0 = (c - centers[parents[i]]).normalized();
-        ax1 = ax0.unitOrthogonal();
-        Quaternionf q;
-        q.setFromTwoVectors(Vector3f::UnitZ(), ax0);
-        Transform3f t = Translation3f(c) * q * Scaling3f(radii[i]+radius);
-        for (int j=0; j<5; ++j)
-        {
-          Vector3f newC = c + ( (AngleAxisf(angles[j*2+1], ax0)
-                               * AngleAxisf(angles[j*2+0] * (l==1 ? 0.35 : 0.5), ax1)) * ax0)*(radii[i] + radius*0.8);
-          centers.push_back(newC);
-          radii.push_back(radius);
-          parents.push_back(i);
-        }
-      }
-      start = end;
-      maxAngle = M_PI/2;
-    }
-    parents.clear();
-    // instanciate the geometry
-    {
-      const std::vector<int>& sphereIndices = pattern.indices(2);
-      std::cout << "instanciate geometry...  (" << sphereIndices.size() * centers.size() << " vertices)\n";
-      mVertices.reserve(sphereIndices.size() * centers.size());
-      mNormals.reserve(sphereIndices.size() * centers.size());
-      int end = centers.size();
-      for (int i=0; i<end; ++i)
-      {
-        Transform3f t = Translation3f(centers[i]) * Scaling3f(radii[i]);
-        // copy vertices
-        for (unsigned int j=0; j<sphereIndices.size(); ++j)
-        {
-          Vector3f v = pattern.vertices()[sphereIndices[j]];
-          mVertices.push_back(t * v);
-          mNormals.push_back(v);
-        }
-      }
-    }
-  }
 }
 
 void RenderingWidget::resizeGL(int width, int height)
@@ -539,10 +537,7 @@ void RenderingWidget::resetCamera()
   Frame aux1 = mCamera.frame();
   aux1.orientation = aux1.orientation.inverse();
   aux1.position = mCamera.viewMatrix().translation();
-  float rangle = AngleAxisf(aux0.orientation.inverse() * aux1.orientation).angle();
+  float duration = aux0.orientation.angularDistance(aux1.orientation) * 0.9;
-  if (rangle>M_PI)
-    rangle = 2.*M_PI - rangle;
-  float duration = rangle * 0.9;
   if (duration<0.1) duration = 0.1;
 
   // put the camera at that time step:
@@ -660,3 +655,4 @@ int main(int argc, char *argv[])
 }
 
 #include "quaternion_demo.moc"
+

test/geometry.cpp

@@ -48,6 +48,10 @@ template<typename Scalar> void geometry(void)
   typedef Translation<Scalar,2> Translation2;
   typedef Translation<Scalar,3> Translation3;
 
+  Scalar largeEps = test_precision<Scalar>();
+  if (ei_is_same_type<Scalar,float>::ret)
+    largeEps = 1e-3f;
+
   Quaternion q1, q2;
   Vector3 v0 = Vector3::Random(),
     v1 = Vector3::Random(),
@@ -82,6 +86,12 @@ template<typename Scalar> void geometry(void)
   q1 = AngleAxis(a, v0.normalized());
   q2 = AngleAxis(a, v1.normalized());
 
+  // angular distance
+  Scalar refangle = ei_abs(AngleAxis(q1.inverse()*q2).angle());
+  if (refangle>M_PI)
+    refangle = 2.*M_PI - refangle;
+  VERIFY(ei_isApprox(q1.angularDistance(q2), refangle, largeEps));
+
   // rotation matrix conversion
   VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2);
   VERIFY_IS_APPROX(q1 * q2 * v2,