* 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()
diff --git a/Eigen/Geometry b/Eigen/Geometry
new file mode 100644
index 0000000..42e18a8
--- /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 1b974a0..613bc94 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 cffdb1c..5e31870 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 @@
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 @@
}
}
+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 0000000..0dc0e92
--- /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 0000000..debe43c
--- /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 0000000..4df490e
--- /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 0140de1..01b31e7 100644
--- a/Eigen/src/QR/SelfAdjointEigenSolver.h
+++ b/Eigen/src/QR/SelfAdjointEigenSolver.h
@@ -260,7 +260,7 @@
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];
