* start of the Geometry module with a cross product and quaternion expressions

  (haven't tried them yet)
* applied the meta selector rule to MatrixBase::swap()

7 files changed, 480 insertions, 26 deletions

diff --git a/Eigen/Geometry b/Eigen/Geometry
new file mode 100644
index 000000000..42e18a8f8
--- /dev/null
+++ b/Eigen/Geometry
@@ -0,0 +1,13 @@
+#ifndef EIGEN_GEOMETRY_MODULE_H
+#define EIGEN_GEOMETRY_MODULE_H
+
+#include "Core"
+
+namespace Eigen {
+
+#include "src/Geometry/Cross.h"
+#include "src/Geometry/Quaternion.h"
+
+} // namespace Eigen
+
+#endif // EIGEN_GEOMETRY_MODULE_H
diff --git a/Eigen/src/Array/CMakeLists.txt b/Eigen/src/Array/CMakeLists.txt
index 1b974a069..613bc94b7 100644
--- a/Eigen/src/Array/CMakeLists.txt
+++ b/Eigen/src/Array/CMakeLists.txt
@@ -1,6 +1,6 @@
-FILE(GLOB Eigen_ARRAY_SRCS "*.h")
+FILE(GLOB Eigen_Array_SRCS "*.h")
 
 INSTALL(FILES
-  ${Eigen_ARRAY_SRCS}
-  DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/ARRAY
+  ${Eigen_Array_SRCS}
+  DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Array
   )
diff --git a/Eigen/src/Core/Swap.h b/Eigen/src/Core/Swap.h
index cffdb1cc6..5e3187071 100644
--- a/Eigen/src/Core/Swap.h
+++ b/Eigen/src/Core/Swap.h
@@ -5,12 +5,12 @@
 //
 // 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 
+// 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 
+// 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
@@ -18,13 +18,16 @@
 // 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 
+// 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/>.
 
 #ifndef EIGEN_SWAP_H
 #define EIGEN_SWAP_H
 
+template <typename Derived, typename OtherDerived, bool IsVector = Derived::IsVectorAtCompileTime>
+struct ei_swap_selector;
+
 /** swaps *this with the expression \a other.
   *
   * \note \a other is only marked const because I couln't find another way
@@ -41,25 +44,7 @@ void MatrixBase<Derived>::swap(const MatrixBase<OtherDerived>& other)
   MatrixBase<OtherDerived> *_other = const_cast<MatrixBase<OtherDerived>*>(&other);
   if(SizeAtCompileTime == Dynamic)
   {
-    Scalar tmp;
-    if(IsVectorAtCompileTime)
-    {
-      ei_assert(OtherDerived::IsVectorAtCompileTime && size() == _other->size());
-      for(int i = 0; i < size(); i++)
-      {
-        tmp = coeff(i);
-        coeffRef(i) = _other->coeff(i);
-        _other->coeffRef(i) = tmp;
-      }
-    }
-    else
-      for(int j = 0; j < cols(); j++)
-        for(int i = 0; i < rows(); i++)
-        {
-          tmp = coeff(i, j);
-          coeffRef(i, j) = _other->coeff(i, j);
-          _other->coeffRef(i, j) = tmp;
-        }
+    ei_swap_selector<Derived,OtherDerived>::run(derived(),other.const_cast_derived());
   }
   else // SizeAtCompileTime != Dynamic
   {
@@ -69,4 +54,36 @@ void MatrixBase<Derived>::swap(const MatrixBase<OtherDerived>& other)
   }
 }
 
+template<typename Derived, typename OtherDerived>
+struct ei_swap_selector<Derived,OtherDerived,true>
+{
+  inline static void run(Derived& src, OtherDerived& other)
+  {
+    typename Derived::Scalar tmp;
+    ei_assert(OtherDerived::IsVectorAtCompileTime && src.size() == other.size());
+    for(int i = 0; i < src.size(); i++)
+    {
+      tmp = src.coeff(i);
+      src.coeffRef(i) = other.coeff(i);
+      other.coeffRef(i) = tmp;
+    }
+  }
+};
+
+template<typename Derived, typename OtherDerived>
+struct ei_swap_selector<Derived,OtherDerived,false>
+{
+  inline void run(Derived& src, OtherDerived& other)
+  {
+    typename Derived::Scalar tmp;
+    for(int j = 0; j < src.cols(); j++)
+      for(int i = 0; i < src.rows(); i++)
+      {
+        tmp = src.coeff(i, j);
+        src.coeffRef(i, j) = other.coeff(i, j);
+        other.coeffRef(i, j) = tmp;
+      }
+  }
+};
+
 #endif // EIGEN_SWAP_H
diff --git a/Eigen/src/Geometry/CMakeLists.txt b/Eigen/src/Geometry/CMakeLists.txt
new file mode 100644
index 000000000..0dc0e927c
--- /dev/null
+++ b/Eigen/src/Geometry/CMakeLists.txt
@@ -0,0 +1,6 @@
+FILE(GLOB Eigen_Geometry_SRCS "*.h")
+
+INSTALL(FILES
+  ${Eigen_Geometry_SRCS}
+  DESTINATION ${INCLUDE_INSTALL_DIR}/Eigen/src/Geometry
+  )
diff --git a/Eigen/src/Geometry/Cross.h b/Eigen/src/Geometry/Cross.h
new file mode 100644
index 000000000..debe43ca4
--- /dev/null
+++ b/Eigen/src/Geometry/Cross.h
@@ -0,0 +1,102 @@
+// 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/>.
+
+#ifndef EIGEN_CROSS_H
+#define EIGEN_CROSS_H
+
+/** \class Cross
+  *
+  * \brief Expression of the cross product of two vectors
+  *
+  * \param Lhs the type of the left-hand side
+  * \param Rhs the type of the right-hand side
+  *
+  * This class represents an expression of the cross product of two 3D vectors.
+  * It is the return type of MatrixBase::cross(), and most
+  * of the time this is the only way it is used.
+  */
+template<typename Lhs, typename Rhs>
+struct ei_traits<Cross<Lhs, Rhs> >
+{
+  typedef typename Lhs::Scalar Scalar;
+  typedef typename ei_nested<Lhs,2>::type LhsNested;
+  typedef typename ei_nested<Rhs,2>::type RhsNested;
+  typedef typename ei_unref<LhsNested>::type _LhsNested;
+  typedef typename ei_unref<RhsNested>::type _RhsNested;
+  enum {
+    RowsAtCompileTime = 3,
+    ColsAtCompileTime = 1,
+    MaxRowsAtCompileTime = 3,
+    MaxColsAtCompileTime = 1,
+    Flags = ((_RhsNested::Flags | _LhsNested::Flags) & HereditaryBits)
+          | EvalBeforeAssigningBit,
+    CoeffReadCost = NumTraits<Scalar>::AddCost + 2 * NumTraits<Scalar>::MulCost
+  };
+};
+
+template<typename Lhs, typename Rhs> class Cross : ei_no_assignment_operator,
+    public MatrixBase<Cross<Lhs, Rhs> >
+{
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(Cross)
+    typedef typename ei_traits<Cross>::LhsNested LhsNested;
+    typedef typename ei_traits<Cross>::RhsNested RhsNested;
+
+    Cross(const Lhs& lhs, const Rhs& rhs)
+      : m_lhs(lhs), m_rhs(rhs)
+    {
+      assert(lhs.isVector());
+      assert(rhs.isVector());
+      assert(lhs.size() == 3 && rhs.size() == 3);
+    }
+
+  private:
+
+    int _rows() const { return 3; }
+    int _cols() const { return 1; }
+
+    Scalar _coeff(int i, int) const
+    {
+      return m_lhs[(i+1)%3]*m_rhs[(i+2)%3] - m_lhs[(i+2)%3]*m_rhs[(i+1)%3];
+    }
+
+  protected:
+    const LhsNested m_lhs;
+    const RhsNested m_rhs;
+};
+
+/** \returns an expression of the cross product of \c *this and \a other
+  *
+  * \sa class Cross
+  */
+template<typename Derived>
+template<typename OtherDerived>
+const Cross<Derived,OtherDerived>
+MatrixBase<Derived>::cross(const MatrixBase<OtherDerived>& other) const
+{
+    return Cross<Derived,OtherDerived>(derived(),other.derived());
+}
+
+#endif // EIGEN_CROSS_H
diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h
new file mode 100644
index 000000000..4df490ea3
--- /dev/null
+++ b/Eigen/src/Geometry/Quaternion.h
@@ -0,0 +1,316 @@
+// 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/>.
+
+#ifndef EIGEN_QUATERNION_H
+#define EIGEN_QUATERNION_H
+
+template<typename _Scalar>
+struct ei_traits<Quaternion<_Scalar> >
+{
+  typedef _Scalar Scalar;
+  enum {
+    RowsAtCompileTime = 4,
+    ColsAtCompileTime = 1,
+    MaxRowsAtCompileTime = 4,
+    MaxColsAtCompileTime = 1,
+    Flags = ei_corrected_matrix_flags<_Scalar, 4, 0>::ret,
+    CoeffReadCost = NumTraits<Scalar>::ReadCost
+  };
+};
+
+template<typename _Scalar>
+class Quaternion : public MatrixBase<Quaternion<_Scalar> >
+{
+public:
+
+  public:
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(Quaternion)
+
+  private:
+
+    EIGEN_ALIGN_128 Scalar m_data[4];
+
+    inline int _rows() const { return 4; }
+    inline int _cols() const { return 1; }
+
+    inline const Scalar& _coeff(int i, int) const { return m_data[i]; }
+
+    inline Scalar& _coeffRef(int i, int) { return m_data[i]; }
+
+    template<int LoadMode>
+    inline PacketScalar _packetCoeff(int row, int) const
+    {
+      ei_internal_assert(Flags & VectorizableBit);
+      if (LoadMode==Eigen::Aligned)
+        return ei_pload(&m_data[row]);
+      else
+        return ei_ploadu(&m_data[row]);
+    }
+
+    template<int StoreMode>
+    inline void _writePacketCoeff(int row, int , const PacketScalar& x)
+    {
+      ei_internal_assert(Flags & VectorizableBit);
+      if (StoreMode==Eigen::Aligned)
+        ei_pstore(&m_data[row], x);
+      else
+        ei_pstoreu(&m_data[row], x);
+    }
+
+    inline int _stride(void) const { return _rows(); }
+
+  public:
+
+    typedef Matrix<Scalar,3,1> Vector3;
+    typedef Matrix<Scalar,3,3> Matrix3;
+
+    // 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;
+    }
+
+    /** Constructor copying the value of the expression \a other */
+    template<typename OtherDerived>
+    inline Quaternion(const Eigen::MatrixBase<OtherDerived>& other)
+    {
+      *this = other;
+    }
+    /** Copy constructor */
+    inline Quaternion(const Quaternion& other)
+    {
+      *this = other;
+    }
+
+    /** Copies the value of the expression \a other into \c *this.
+      */
+    template<typename OtherDerived>
+    inline Quaternion& operator=(const MatrixBase<OtherDerived>& other)
+    {
+      return Base::operator=(other.derived());
+    }
+
+    /** This is a special case of the templated operator=. Its purpose is to
+      * prevent a default operator= from hiding the templated operator=.
+      */
+    inline Quaternion& operator=(const Quaternion& other)
+    {
+      return operator=<Quaternion>(other);
+    }
+
+    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);
+    template<typename Derived1, typename Derived2>
+    Quaternion& fromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
+
+    inline Quaternion operator* (const Quaternion& q) const;
+    inline Quaternion& operator*= (const Quaternion& q);
+
+    Quaternion inverse(void) const;
+    Quaternion unitInverse(void) const;
+
+    /** Rotation of a vector by a quaternion.
+        \remarks If the quaternion is used to rotate several points (>3)
+        then it is much more efficient to first convert it to a 3x3 Matrix.
+        Comparison of the operation cost for n transformations:
+            * Quaternion:  30n
+            * Via Matrix3: 24 + 15n
+        \todo write a small benchmark.
+    */
+    template<typename Derived>
+    Vector3 operator* (const MatrixBase<Derived>& vec) const;
+
+private:
+    // TODO discard here unreliable members.
+
+};
+
+template <typename Scalar>
+inline Quaternion<Scalar> Quaternion<Scalar>::operator* (const Quaternion& other) const
+{
+  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()
+  );
+}
+
+template <typename Scalar>
+inline Quaternion<Scalar>& Quaternion<Scalar>::operator*= (const Quaternion& other)
+{
+  return (*this = *this * other);
+}
+
+template <typename Scalar>
+inline typename Quaternion<Scalar>::Vector3
+Quaternion<Scalar>::operator* (const Vector3& v) const
+{
+    // Note that this algorithm comes from the optimization by hand
+    // of the conversion to a Matrix followed by a Matrix/Vector product.
+    // It appears to be much faster than the common algorithm found
+    // 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);
+}
+
+template<typename Scalar>
+typename Quaternion<Scalar>::Matrix3
+Quaternion<Scalar>::toRotationMatrix(void) const
+{
+  Matrix3 res;
+
+  Scalar tx  = 2*this->x();
+  Scalar ty  = 2*this->y();
+  Scalar tz  = 2*this->z();
+  Scalar twx = tx*this->w();
+  Scalar twy = ty*this->w();
+  Scalar twz = tz*this->w();
+  Scalar txx = tx*this->x();
+  Scalar txy = ty*this->x();
+  Scalar txz = tz*this->x();
+  Scalar tyy = ty*this->y();
+  Scalar tyz = tz*this->y();
+  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(1,1) = 1-(txx+tzz);
+  res(1,2) = tyz-twx;
+  res(2,0) = txz-twy;
+  res(2,1) = tyz+twx;
+  res(2,2) = 1-(txx+tyy);
+
+  return res;
+}
+
+template<typename Scalar>
+template<typename Derived>
+void Quaternion<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& m)
+{
+  assert(Derived::RowsAtCompileTime==3 && Derived::ColsAtCompileTime==3);
+  // This algorithm comes from  "Quaternion Calculus and Fast Animation",
+  // Ken Shoemake, 1987 SIGGRAPH course notes
+  Scalar t = m.trace();
+  if (t > 0)
+  {
+    t = ei_sqrt(t + 1.0);
+    this->w() = 0.5*t;
+    t = 0.5/t;
+    this->x() = (m.coeff(2,1) - m.coeff(1,2)) * t;
+    this->y() = (m.coeff(0,2) - m.coeff(2,0)) * t;
+    this->z() = (m.coeff(1,0) - m.coeff(0,1)) * t;
+  }
+  else
+  {
+    int i = 0;
+    if (m(1,1) > m(0,0))
+      i = 1;
+    if (m(2,2) > m(i,i))
+      i = 2;
+    int j = (i+1)%3;
+    int k = (j+1)%3;
+
+    t = ei_sqrt(m.coeff(i,i)-m.coeff(j,j)-m.coeff(k,k) + 1.0);
+    this->coeffRef(i) = 0.5 * t;
+    t = 0.5/t;
+    this->w() = (m.coeff(k,j)-m.coeff(j,k))*t;
+    this->coeffRef(j) = (m.coeff(j,i)+m.coeff(i,j))*t;
+    this->coeffRef(k) = (m.coeff(k,i)+m.coeff(i,k))*t;
+  }
+}
+
+template<typename Scalar>
+template<typename Derived>
+inline void 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;
+}
+
+/** Makes a quaternion representing the rotation between two vectors \a a and \a b.
+  *  \returns a reference to the actual quaternion
+  * Note that the two input vectors are \b not assumed to be normalized.
+  */
+template<typename Scalar>
+template<typename Derived1, typename Derived2>
+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);
+
+  // if dot == 1, vectors are the same
+  if (ei_isApprox(d,1))
+  {
+    // set to identity
+    this->w() = 1; this->start<3>().setZero();
+  }
+  Scalar s = ei_sqrt((1+d)*2);
+  Scalar invs = 1./s;
+
+  this->start<3>() = c * invs;
+  this->w() = s * 0.5;
+
+  return *this;
+}
+
+template <typename Scalar>
+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
+    return this->zero();
+  }
+}
+
+template <typename Scalar>
+inline Quaternion<Scalar> Quaternion<Scalar>::unitInverse() const
+{
+  return Quaternion(this->w(),-this->x(),-this->y(),-this->z());
+}
+
+#endif // EIGEN_QUATERNION_H
diff --git a/Eigen/src/QR/SelfAdjointEigenSolver.h b/Eigen/src/QR/SelfAdjointEigenSolver.h
index 0140de118..01b31e704 100644
--- a/Eigen/src/QR/SelfAdjointEigenSolver.h
+++ b/Eigen/src/QR/SelfAdjointEigenSolver.h
@@ -260,7 +260,7 @@ static void ei_tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, int st
       int kn1 = (k+1)*n;
       #endif
       // let's do the product manually to avoid the need of temporaries...
-      for (uint i=0; i<n; ++i)
+      for (int i=0; i<n; ++i)
       {
         #ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
         Scalar matrixQ_i_k = matrixQ[i*n+k];