add a geometry unit test and fix a couple of typo in Quaternion.h

Eigen/src/Core/MatrixBase.h

@@ -555,10 +555,15 @@ template<typename Derived> class MatrixBase : public ArrayBase<Derived>
 /////////// QR module ///////////
 
     const QR<typename ei_eval<Derived>::type> qr() const;
-    
+
     EigenvaluesReturnType eigenvalues() const;
     RealScalar matrixNorm() const;
 
+/////////// Geometry module ///////////
+
+    template<typename OtherDerived>
+    const Cross<Derived,OtherDerived> cross(const MatrixBase<OtherDerived>& other) const;
+
 };
 
 #endif // EIGEN_MATRIXBASE_H

Eigen/src/Core/util/ForwardDeclarations.h

@@ -51,6 +51,8 @@ template<int Direction, typename UnaryOp, typename MatrixType> class PartialRedu
 template<typename MatrixType, unsigned int Mode> class Part;
 template<typename MatrixType, unsigned int Mode> class Extract;
 template<typename Derived, bool HasArrayFlag = int(ei_traits<Derived>::Flags) & ArrayBit> class ArrayBase {};
+template<typename Lhs, typename Rhs> class Cross;
+template<typename Scalar> class Quaternion;
 
 
 template<typename Scalar> struct ei_scalar_sum_op;

Eigen/src/Geometry/Quaternion.h

@@ -89,10 +89,10 @@ public:
     // FIXME what is the prefered order: w x,y,z or x,y,z,w ?
     inline Quaternion(Scalar w = 1.0, Scalar x = 0.0, Scalar y = 0.0, Scalar z = 0.0)
     {
-      m_data[0] = _x;
-      m_data[1] = _y;
-      m_data[2] = _z;
-      m_data[3] = _w;
+      m_data[0] = x;
+      m_data[1] = y;
+      m_data[2] = z;
+      m_data[3] = w;
     }
 
     /** Constructor copying the value of the expression \a other */
@@ -126,8 +126,10 @@ public:
     Matrix3 toRotationMatrix(void) const;
     template<typename Derived>
     void fromRotationMatrix(const MatrixBase<Derived>& m);
+
     template<typename Derived>
-    void fromAngleAxis (const Scalar& angle, const MatrixBase<Derived>& axis);
+    Quaternion& fromAngleAxis (const Scalar& angle, const MatrixBase<Derived>& axis);
+
     template<typename Derived1, typename Derived2>
     Quaternion& fromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
 
@@ -158,10 +160,10 @@ inline Quaternion<Scalar> Quaternion<Scalar>::operator* (const Quaternion& other
 {
   return Quaternion
   (
-    this->w() * other.w() - this->x() * other.x() - this->y() * other.y() - this->z() * rkQ.z(),
-    this->w() * other.x() + this->x() * other.w() + this->y() * other.z() - this->z() * rkQ.y(),
-    this->w() * other.y() + this->y() * other.w() + this->z() * other.x() - this->x() * rkQ.z(),
-    this->w() * other.z() + this->z() * other.w() + this->x() * other.y() - this->y() * rkQ.x()
+    this->w() * other.w() - this->x() * other.x() - this->y() * other.y() - this->z() * other.z(),
+    this->w() * other.x() + this->x() * other.w() + this->y() * other.z() - this->z() * other.y(),
+    this->w() * other.y() + this->y() * other.w() + this->z() * other.x() - this->x() * other.z(),
+    this->w() * other.z() + this->z() * other.w() + this->x() * other.y() - this->y() * other.x()
   );
 }
 
@@ -172,8 +174,9 @@ inline Quaternion<Scalar>& Quaternion<Scalar>::operator*= (const Quaternion& oth
 }
 
 template <typename Scalar>
+template<typename Derived>
 inline typename Quaternion<Scalar>::Vector3
-Quaternion<Scalar>::operator* (const Vector3& v) const
+Quaternion<Scalar>::operator* (const MatrixBase<Derived>& v) const
 {
     // Note that this algorithm comes from the optimization by hand
     // of the conversion to a Matrix followed by a Matrix/Vector product.
@@ -181,8 +184,8 @@ Quaternion<Scalar>::operator* (const Vector3& v) const
     // in the litterature (30 versus 39 flops). On the other hand it
     // requires two Vector3 as temporaries.
     Vector3 uv;
-    uv = 2 * start<3>().cross(v);
-    return v + this->w() * uv + start<3>().cross(uv);
+    uv = 2 * this->template start<3>().cross(v);
+    return v + this->w() * uv + this->template start<3>().cross(uv);
 }
 
 template<typename Scalar>
@@ -205,9 +208,9 @@ Quaternion<Scalar>::toRotationMatrix(void) const
   Scalar tzz = tz*this->z();
 
   res(0,0) = 1-(tyy+tzz);
-  res(0,1) = fTxy-twz;
-  res(0,2) = fTxz+twy;
-  res(1,0) = fTxy+twz;
+  res(0,1) = txy-twz;
+  res(0,2) = txz+twy;
+  res(1,0) = txy+twz;
   res(1,1) = 1-(txx+tzz);
   res(1,2) = tyz-twx;
   res(2,0) = txz-twy;
@@ -255,11 +258,13 @@ void Quaternion<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& m)
 
 template<typename Scalar>
 template<typename Derived>
-inline void Quaternion<Scalar>::fromAngleAxis (const Scalar& angle, const MatrixBase<Derived>& axis)
+inline Quaternion<Scalar>& Quaternion<Scalar>
+::fromAngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis)
 {
   Scalar ha = 0.5*angle;
   this->w() = ei_cos(ha);
-  this->start<3>() = ei_sin(ha) * axis;
+  this->template start<3>() = ei_sin(ha) * axis;
+  return *this;
 }
 
 /** Makes a quaternion representing the rotation between two vectors \a a and \a b.
@@ -268,26 +273,22 @@ inline void Quaternion<Scalar>::fromAngleAxis (const Scalar& angle, const Matrix
   */
 template<typename Scalar>
 template<typename Derived1, typename Derived2>
-Quaternion<Scalar>& Quaternion<Scalar>::fromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
+inline Quaternion<Scalar>& Quaternion<Scalar>::fromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
 {
   Vector3 v0 = a.normalized();
-  Vector3 v1 = a.normalized();
-  Vector3 c = v0.cross(v1);
-
-  // if the magnitude of the cross product approaches zero,
-  // we get unstable because ANY axis will do when a == +/- b
-  Scalar d = v0.dot(v1);
+  Vector3 v1 = b.normalized();
+  Vector3 axis = v0.cross(v1);
+  Scalar c = v0.dot(v1);
 
   // if dot == 1, vectors are the same
-  if (ei_isApprox(d,1))
+  if (ei_isApprox(c,Scalar(1)))
   {
     // set to identity
-    this->w() = 1; this->start<3>().setZero();
+    this->w() = 1; this->template start<3>().setZero();
   }
-  Scalar s = ei_sqrt((1+d)*2);
+  Scalar s = ei_sqrt((1+c)*2);
   Scalar invs = 1./s;
-
-  this->start<3>() = c * invs;
+  this->template start<3>() = axis * invs;
   this->w() = s * 0.5;
 
   return *this;
@@ -299,7 +300,6 @@ inline Quaternion<Scalar> Quaternion<Scalar>::inverse() const
   Scalar n2 = this->norm2();
   if (n2 > 0)
     return (*this) / norm;
-  }
   else
   {
     // return an invalid result to flag the error

test/CMakeLists.txt

@@ -85,5 +85,6 @@ EI_ADD_TEST(cholesky)
 EI_ADD_TEST(inverse)
 EI_ADD_TEST(qr)
 EI_ADD_TEST(eigensolver)
+EI_ADD_TEST(geometry)
 
 ENDIF(BUILD_TESTS)

test/geometry.cpp

@@ -0,0 +1,74 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra. Eigen itself is part of the KDE project.
+//
+// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
+//
+// Eigen is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation; either
+// version 3 of the License, or (at your option) any later version.
+//
+// Alternatively, you can redistribute it and/or
+// modify it under the terms of the GNU General Public License as
+// published by the Free Software Foundation; either version 2 of
+// the License, or (at your option) any later version.
+//
+// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public
+// License and a copy of the GNU General Public License along with
+// Eigen. If not, see <http://www.gnu.org/licenses/>.
+
+#include "main.h"
+#include <Eigen/Geometry>
+
+template<typename Scalar> void geometry(void)
+{
+  /* this test covers the following files:
+     Cross.h Quaternion.h
+  */
+
+  typedef Matrix<Scalar,3,3> Matrix3;
+  typedef Matrix<Scalar,4,4> Matrix4;
+  typedef Matrix<Scalar,3,1> Vector3;
+  typedef Matrix<Scalar,4,1> Vector4;
+  typedef Quaternion<Scalar> Quaternion;
+
+  Quaternion q1, q2, q3;
+  Vector3 v0 = Vector3::random(),
+    v1 = Vector3::random(),
+    v2 = Vector3::random();
+
+  q1.fromAngleAxis(ei_random<Scalar>(-M_PI, M_PI), v0.normalized());
+  q2.fromAngleAxis(ei_random<Scalar>(-M_PI, M_PI), v1.normalized());
+
+  VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2);
+  VERIFY_IS_APPROX(q1 * q2 * v2,
+    q1.toRotationMatrix() * q2.toRotationMatrix() * v2);
+  VERIFY_IS_NOT_APPROX(q2 * q1 * v2,
+    q1.toRotationMatrix() * q2.toRotationMatrix() * v2);
+
+  q2.fromRotationMatrix(q1.toRotationMatrix());
+  VERIFY_IS_APPROX(q1*v1,q2*v1);
+
+  VERIFY_IS_APPROX(v2.normalized(),(q2.fromTwoVectors(v1,v2)*v1).normalized());
+
+  VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).dot(v1), Scalar(1));
+
+  Matrix3 m;
+  m << v0.normalized(),
+      (v0.cross(v1)).normalized(),
+      (v0.cross(v1).cross(v0)).normalized();
+  VERIFY(m.isOrtho());
+}
+
+void test_geometry()
+{
+  for(int i = 0; i < g_repeat; i++) {
+//     CALL_SUBTEST( geometry<float>() );
+    CALL_SUBTEST( geometry<double>() );
+  }
+}

test/main.h

@@ -165,7 +165,7 @@ namespace Eigen {
 
 template<typename T> inline typename NumTraits<T>::Real test_precision();
 template<> inline int test_precision<int>() { return 0; }
-template<> inline float test_precision<float>() { return 1e-2f; }
+template<> inline float test_precision<float>() { return 1e-3f; }
 template<> inline double test_precision<double>() { return 1e-5; }
 template<> inline float test_precision<std::complex<float> >() { return test_precision<float>(); }
 template<> inline double test_precision<std::complex<double> >() { return test_precision<double>(); }