Added MatrixBase::Unit*() static function to easily create unit/basis vectors. Removed EulerAngles, addes typdefs for Quaternion and AngleAxis, and added automatic conversions from Quaternion/AngleAxis to Matrix3 such that: Matrix3f m = AngleAxisf(0.2,Vector3f::UnitX) * AngleAxisf(0.2,Vector3f::UnitY); just works.
diff --git a/Eigen/Geometry b/Eigen/Geometry index 2dd44fb..ae3012b 100644 --- a/Eigen/Geometry +++ b/Eigen/Geometry
@@ -8,7 +8,6 @@ #include "src/Geometry/Cross.h" #include "src/Geometry/Quaternion.h" #include "src/Geometry/AngleAxis.h" -#include "src/Geometry/EulerAngles.h" #include "src/Geometry/Rotation.h" #include "src/Geometry/Transform.h"
diff --git a/Eigen/src/Core/CwiseNullaryOp.h b/Eigen/src/Core/CwiseNullaryOp.h index 167993e..3cf9070 100644 --- a/Eigen/src/Core/CwiseNullaryOp.h +++ b/Eigen/src/Core/CwiseNullaryOp.h
@@ -43,13 +43,13 @@ template<typename NullaryOp, typename MatrixType> struct ei_traits<CwiseNullaryOp<NullaryOp, MatrixType> > { - typedef typename MatrixType::Scalar Scalar; + typedef typename ei_traits<MatrixType>::Scalar Scalar; enum { - RowsAtCompileTime = MatrixType::RowsAtCompileTime, - ColsAtCompileTime = MatrixType::ColsAtCompileTime, - MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime, - MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime, - Flags = (MatrixType::Flags + RowsAtCompileTime = ei_traits<MatrixType>::RowsAtCompileTime, + ColsAtCompileTime = ei_traits<MatrixType>::ColsAtCompileTime, + MaxRowsAtCompileTime = ei_traits<MatrixType>::MaxRowsAtCompileTime, + MaxColsAtCompileTime = ei_traits<MatrixType>::MaxColsAtCompileTime, + Flags = (ei_traits<MatrixType>::Flags & ( HereditaryBits | (ei_functor_has_linear_access<NullaryOp>::ret ? LinearAccessBit : 0) | (ei_functor_traits<NullaryOp>::PacketAccess ? PacketAccessBit : 0))) @@ -453,7 +453,7 @@ * \sa identity(), setIdentity(), isIdentity() */ template<typename Derived> -inline const CwiseNullaryOp<ei_scalar_identity_op<typename ei_traits<Derived>::Scalar>, Derived> +inline const typename MatrixBase<Derived>::IdentityReturnType MatrixBase<Derived>::identity(int rows, int cols) { return NullaryExpr(rows, cols, ei_scalar_identity_op<Scalar>()); @@ -470,7 +470,7 @@ * \sa identity(int,int), setIdentity(), isIdentity() */ template<typename Derived> -inline const CwiseNullaryOp<ei_scalar_identity_op<typename ei_traits<Derived>::Scalar>, Derived> +inline const typename MatrixBase<Derived>::IdentityReturnType MatrixBase<Derived>::identity() { EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived) @@ -522,4 +522,72 @@ return *this = identity(rows(), cols()); } +/** \returns an expression of the i-th unit (basis) vector. + * + * \only_for_vectors + * + * \sa MatrixBase::Unit(int), MatrixBase::UnitX(), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW() + */ +template<typename Derived> +const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::Unit(int size, int i) +{ + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived); + return BasisReturnType(SquareMatrixType::identity(size,size), i); +} + +/** \returns an expression of the i-th unit (basis) vector. + * + * \only_for_vectors + * + * This variant is for fixed-size vector only. + * + * \sa MatrixBase::Unit(int,int), MatrixBase::UnitX(), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW() + */ +template<typename Derived> +const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::Unit(int i) +{ + EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived); + return BasisReturnType(SquareMatrixType::identity(),i); +} + +/** \returns an expression of the X axis unit vector (1{,0}^*) + * + * \only_for_vectors + * + * \sa MatrixBase::Unit(int,int), MatrixBase::Unit(int), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW() + */ +template<typename Derived> +const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitX() +{ return Derived::Unit(0); } + +/** \returns an expression of the Y axis unit vector (0,1{,0}^*) + * + * \only_for_vectors + * + * \sa MatrixBase::Unit(int,int), MatrixBase::Unit(int), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW() + */ +template<typename Derived> +const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitY() +{ return Derived::Unit(1); } + +/** \returns an expression of the Z axis unit vector (0,0,1{,0}^*) + * + * \only_for_vectors + * + * \sa MatrixBase::Unit(int,int), MatrixBase::Unit(int), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW() + */ +template<typename Derived> +const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitZ() +{ return Derived::Unit(2); } + +/** \returns an expression of the W axis unit vector (0,0,0,1) + * + * \only_for_vectors + * + * \sa MatrixBase::Unit(int,int), MatrixBase::Unit(int), MatrixBase::UnitY(), MatrixBase::UnitZ(), MatrixBase::UnitW() + */ +template<typename Derived> +const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitW() +{ return Derived::Unit(3); } + #endif // EIGEN_CWISE_NULLARY_OP_H
diff --git a/Eigen/src/Core/MatrixBase.h b/Eigen/src/Core/MatrixBase.h index cf6a937..c6ea5f1 100644 --- a/Eigen/src/Core/MatrixBase.h +++ b/Eigen/src/Core/MatrixBase.h
@@ -148,6 +148,10 @@ */ typedef typename NumTraits<Scalar>::Real RealScalar; + /** type of the equivalent square matrix */ + typedef Matrix<Scalar,EIGEN_ENUM_MAX(RowsAtCompileTime,ColsAtCompileTime), + EIGEN_ENUM_MAX(RowsAtCompileTime,ColsAtCompileTime)> SquareMatrixType; + /** \returns the number of rows. \sa cols(), RowsAtCompileTime */ inline int rows() const { return derived().rows(); } /** \returns the number of columns. \sa row(), ColsAtCompileTime*/ @@ -193,7 +197,14 @@ /** the return type of MatrixBase::adjoint() */ typedef Transpose<NestByValue<typename ei_unref<ConjugateReturnType>::type> > AdjointReturnType; + /** the return type of MatrixBase::eigenvalues() */ typedef Matrix<typename NumTraits<typename ei_traits<Derived>::Scalar>::Real, ei_traits<Derived>::ColsAtCompileTime, 1> EigenvaluesReturnType; + /** the return type of identity */ + typedef CwiseNullaryOp<ei_scalar_identity_op<Scalar>,Derived> IdentityReturnType; + /** the return type of unit vectors */ + typedef Block<CwiseNullaryOp<ei_scalar_identity_op<Scalar>, SquareMatrixType>, + ei_traits<Derived>::RowsAtCompileTime, + ei_traits<Derived>::ColsAtCompileTime> BasisReturnType; /** Copies \a other into *this. \returns a reference to *this. */ @@ -391,8 +402,14 @@ static const ConstantReturnType ones(int rows, int cols); static const ConstantReturnType ones(int size); static const ConstantReturnType ones(); - static const CwiseNullaryOp<ei_scalar_identity_op<Scalar>,Derived> identity(); - static const CwiseNullaryOp<ei_scalar_identity_op<Scalar>,Derived> identity(int rows, int cols); + static const IdentityReturnType identity(); + static const IdentityReturnType identity(int rows, int cols); + static const BasisReturnType Unit(int size, int i); + static const BasisReturnType Unit(int i); + static const BasisReturnType UnitX(); + static const BasisReturnType UnitY(); + static const BasisReturnType UnitZ(); + static const BasisReturnType UnitW(); const DiagonalMatrix<Derived> asDiagonal() const;
diff --git a/Eigen/src/Core/util/ForwardDeclarations.h b/Eigen/src/Core/util/ForwardDeclarations.h index beda186..0246b43 100644 --- a/Eigen/src/Core/util/ForwardDeclarations.h +++ b/Eigen/src/Core/util/ForwardDeclarations.h
@@ -102,7 +102,6 @@ template<typename Scalar> class Quaternion; template<typename Scalar> class Rotation2D; template<typename Scalar> class AngleAxis; -template<typename Scalar> class EulerAngles; template<typename Scalar,int Dim> class Transform; #endif // EIGEN_FORWARDDECLARATIONS_H
diff --git a/Eigen/src/Core/util/Macros.h b/Eigen/src/Core/util/Macros.h index 9c013f6..eaa5df2 100644 --- a/Eigen/src/Core/util/Macros.h +++ b/Eigen/src/Core/util/Macros.h
@@ -151,5 +151,6 @@ friend class Eigen::MatrixBase<Derived>; #define EIGEN_ENUM_MIN(a,b) (((int)a <= (int)b) ? (int)a : (int)b) +#define EIGEN_ENUM_MAX(a,b) (((int)a >= (int)b) ? (int)a : (int)b) #endif // EIGEN_MACROS_H
diff --git a/Eigen/src/Geometry/AngleAxis.h b/Eigen/src/Geometry/AngleAxis.h index c77264e..639e75a 100644 --- a/Eigen/src/Geometry/AngleAxis.h +++ b/Eigen/src/Geometry/AngleAxis.h
@@ -31,6 +31,10 @@ * * \param _Scalar the scalar type, i.e., the type of the coefficients. * + * The following two typedefs are provided for convenience: + * \li \c AngleAxisf for \c float + * \li \c AngleAxisd for \c double + * * \sa class Quaternion, class EulerAngles, class Transform */ template<typename _Scalar> @@ -43,7 +47,6 @@ typedef Matrix<Scalar,3,3> Matrix3; typedef Matrix<Scalar,3,1> Vector3; typedef Quaternion<Scalar> QuaternionType; - typedef EulerAngles<Scalar> EulerAnglesType; protected: @@ -56,7 +59,6 @@ template<typename Derived> inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {} inline AngleAxis(const QuaternionType& q) { *this = q; } - inline AngleAxis(const EulerAnglesType& ea) { *this = ea; } template<typename Derived> inline AngleAxis(const MatrixBase<Derived>& m) { *this = m; } @@ -66,8 +68,26 @@ const Vector3& axis() const { return m_axis; } Vector3& axis() { return m_axis; } + operator Matrix3 () const { return toRotationMatrix(); } + + inline QuaternionType operator* (const AngleAxis& other) const + { return QuaternionType(*this) * QuaternionType(other); } + + inline QuaternionType operator* (const QuaternionType& other) const + { return QuaternionType(*this) * other; } + + friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b) + { return a * QuaternionType(b); } + + inline typename ProductReturnType<Matrix3,Matrix3>::Type + operator* (const Matrix3& other) const + { return toRotationMatrix() * other; } + + inline friend typename ProductReturnType<Matrix3,Matrix3>::Type + operator* (const Matrix3& a, const AngleAxis& b) + { return a * b.toRotationMatrix(); } + AngleAxis& operator=(const QuaternionType& q); - AngleAxis& operator=(const EulerAnglesType& ea); template<typename Derived> AngleAxis& operator=(const MatrixBase<Derived>& m); @@ -76,6 +96,9 @@ Matrix3 toRotationMatrix(void) const; }; +typedef AngleAxis<float> AngleAxisf; +typedef AngleAxis<double> AngleAxisd; + /** Set \c *this from a quaternion. * The axis is normalized. */ @@ -96,14 +119,6 @@ return *this; } -/** Set \c *this from Euler angles \a ea. - */ -template<typename Scalar> -AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const EulerAnglesType& ea) -{ - return *this = QuaternionType(ea); -} - /** Set \c *this from a 3x3 rotation matrix \a mat. */ template<typename Scalar>
diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h index d8ccf49..fcc7e21 100644 --- a/Eigen/src/Geometry/Quaternion.h +++ b/Eigen/src/Geometry/Quaternion.h
@@ -40,9 +40,13 @@ * orientations and rotations of objects in three dimensions. Compared to other * representations like Euler angles or 3x3 matrices, quatertions offer the * following advantages: - * - compact storage (4 scalars) - * - efficient to compose (28 flops), - * - stable spherical interpolation + * \li \c compact storage (4 scalars) + * \li \c efficient to compose (28 flops), + * \li \c stable spherical interpolation + * + * The following two typedefs are provided for convenience: + * \li \c Quaternionf for \c float + * \li \c Quaterniond for \c double * * \sa class AngleAxis, class EulerAngles, class Transform */ @@ -60,7 +64,6 @@ typedef Matrix<Scalar,3,1> Vector3; typedef Matrix<Scalar,3,3> Matrix3; typedef AngleAxis<Scalar> AngleAxisType; - typedef EulerAngles<Scalar> EulerAnglesType; inline Scalar x() const { return m_coeffs.coeff(0); } inline Scalar y() const { return m_coeffs.coeff(1); } @@ -97,16 +100,16 @@ inline Quaternion(const Quaternion& other) { m_coeffs = other.m_coeffs; } explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; } - explicit inline Quaternion(const EulerAnglesType& ea) { *this = ea; } template<typename Derived> explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; } Quaternion& operator=(const Quaternion& other); Quaternion& operator=(const AngleAxisType& aa); - Quaternion& operator=(EulerAnglesType ea); template<typename Derived> Quaternion& operator=(const MatrixBase<Derived>& m); + operator Matrix3 () const { return toRotationMatrix(); } + /** \returns a quaternion representing an identity rotation * \sa MatrixBase::identity() */ @@ -144,6 +147,9 @@ }; +typedef Quaternion<float> Quaternionf; +typedef Quaternion<double> Quaterniond; + /** \returns the concatenation of two rotations as a quaternion-quaternion product */ template <typename Scalar> inline Quaternion<Scalar> Quaternion<Scalar>::operator* (const Quaternion& other) const @@ -204,30 +210,6 @@ return *this; } -/** Set \c *this from the rotation defined by the Euler angles \a ea, - * and returns a reference to \c *this - */ -template<typename Scalar> -inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(EulerAnglesType ea) -{ - ea.coeffs() *= 0.5; - - Vector3 cosines = ea.coeffs().cwise().cos(); - Vector3 sines = ea.coeffs().cwise().sin(); - - Scalar cYcZ = cosines.y() * cosines.z(); - Scalar sYsZ = sines.y() * sines.z(); - Scalar sYcZ = sines.y() * cosines.z(); - Scalar cYsZ = cosines.y() * sines.z(); - - this->w() = cosines.x() * cYcZ + sines.x() * sYsZ; - this->x() = sines.x() * cYcZ - cosines.x() * sYsZ; - this->y() = cosines.x() * sYcZ + sines.x() * cYsZ; - this->z() = cosines.x() * cYsZ - sines.x() * sYcZ; - - return *this; -} - /** Set \c *this from the expression \a xpr: * - if \a xpr is a 4x1 vector, then \a xpr is assumed to be a quaternion * - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix
diff --git a/Eigen/src/Geometry/Rotation.h b/Eigen/src/Geometry/Rotation.h index ba533b9..fb41797 100644 --- a/Eigen/src/Geometry/Rotation.h +++ b/Eigen/src/Geometry/Rotation.h
@@ -89,14 +89,6 @@ { return aa.toRotationMatrix(); } }; -// euler angles to rotation matrix -template<typename Scalar, typename OtherScalarType> -struct ToRotationMatrix<Scalar, 3, EulerAngles<OtherScalarType> > -{ - inline static Matrix<Scalar,3,3> convert(const EulerAngles<OtherScalarType>& ea) - { return ea.toRotationMatrix(); } -}; - // matrix xpr to matrix xpr template<typename Scalar, int Dim, typename OtherDerived> struct ToRotationMatrix<Scalar, Dim, MatrixBase<OtherDerived> >
diff --git a/Eigen/src/Geometry/EulerAngles.h b/disabled/EulerAngles.h similarity index 98% rename from Eigen/src/Geometry/EulerAngles.h rename to disabled/EulerAngles.h index 6377447..65489f2 100644 --- a/Eigen/src/Geometry/EulerAngles.h +++ b/disabled/EulerAngles.h
@@ -30,6 +30,13 @@ int OtherCols=Other::ColsAtCompileTime> struct ei_eulerangles_assign_impl; +// enum { +// XYZ, +// XYX, +// +// +// }; + /** \class EulerAngles * * \brief Represents a rotation in a 3 dimensional space as three Euler angles
diff --git a/test/array.cpp b/test/array.cpp index 6233aae..3acb4ab 100644 --- a/test/array.cpp +++ b/test/array.cpp
@@ -53,6 +53,11 @@ m3 = m1; m3.cwise() -= s1; VERIFY_IS_APPROX(m3, m1.cwise() - s1); + + VERIFY_IS_APPROX(m1.colwise().sum().sum(), m1.sum()); + VERIFY_IS_APPROX(m1.rowwise().sum().sum(), m1.sum()); + VERIFY_IS_NOT_APPROX((m1.rowwise().sum()*2).sum(), m1.sum()); + VERIFY_IS_APPROX(m1.colwise().sum(), m1.colwise().redux(ei_scalar_sum_op<Scalar>())); } template<typename MatrixType> void comparisons(const MatrixType& m)
diff --git a/test/geometry.cpp b/test/geometry.cpp index 64b096a..395d1c2 100644 --- a/test/geometry.cpp +++ b/test/geometry.cpp
@@ -40,12 +40,12 @@ typedef Matrix<Scalar,4,1> Vector4; typedef Quaternion<Scalar> Quaternion; typedef AngleAxis<Scalar> AngleAxis; - typedef EulerAngles<Scalar> EulerAngles; Quaternion q1, q2; Vector3 v0 = Vector3::random(), v1 = Vector3::random(), v2 = Vector3::random(); + Matrix3 matrot1; Scalar a = ei_random<Scalar>(-M_PI, M_PI); @@ -61,11 +61,13 @@ q2 = q1.toRotationMatrix(); VERIFY_IS_APPROX(q1*v1,q2*v1); - // Euler angle conversion - VERIFY_IS_APPROX(Quaternion(EulerAngles(q1)) * v1, q1 * v1); - EulerAngles ea = q2; - VERIFY_IS_APPROX(EulerAngles(Quaternion(ea)).coeffs(), ea.coeffs()); - VERIFY_IS_NOT_APPROX(EulerAngles(Quaternion(EulerAngles(v2.cwise() * Vector3(0.2,-0.2,1)))).coeffs(), v2); + matrot1 = AngleAxis(0.1, Vector3::UnitX()) + * AngleAxis(0.2, Vector3::UnitY()) + * AngleAxis(0.3, Vector3::UnitZ()); + VERIFY_IS_APPROX(matrot1 * v1, + AngleAxis(0.1, Vector3(1,0,0)).toRotationMatrix() + * (AngleAxis(0.2, Vector3(0,1,0)).toRotationMatrix() + * (AngleAxis(0.3, Vector3(0,0,1)).toRotationMatrix() * v1))); // angle-axis conversion AngleAxis aa = q1;