diff options
author | Gael Guennebaud <g.gael@free.fr> | 2008-06-21 15:01:49 +0000 |
---|---|---|
committer | Gael Guennebaud <g.gael@free.fr> | 2008-06-21 15:01:49 +0000 |
commit | 32c5ea388ef678c1aa24bf1c2164455a319c92f8 (patch) | |
tree | 1111b4909a2ebf45ae4c7df62160c6424818a564 | |
parent | 574416b8428756c9fc52e79fc00a2402d1a4e59c (diff) |
work on rotations in the Geometry module:
- convertions are done trough constructors and operator=
- added a EulerAngles class
-rw-r--r-- | Eigen/Geometry | 2 | ||||
-rw-r--r-- | Eigen/src/Core/util/ForwardDeclarations.h | 13 | ||||
-rw-r--r-- | Eigen/src/Geometry/AngleAxis.h | 147 | ||||
-rw-r--r-- | Eigen/src/Geometry/EulerAngles.h | 158 | ||||
-rw-r--r-- | Eigen/src/Geometry/Quaternion.h | 296 | ||||
-rw-r--r-- | Eigen/src/Geometry/Rotation.h | 98 | ||||
-rw-r--r-- | Eigen/src/Geometry/Transform.h | 20 | ||||
-rw-r--r-- | test/geometry.cpp | 41 |
8 files changed, 489 insertions, 286 deletions
diff --git a/Eigen/Geometry b/Eigen/Geometry index 1452e9752..2dd44fb0b 100644 --- a/Eigen/Geometry +++ b/Eigen/Geometry @@ -7,6 +7,8 @@ namespace Eigen { #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/util/ForwardDeclarations.h b/Eigen/src/Core/util/ForwardDeclarations.h index 96949a12a..ea6478a04 100644 --- a/Eigen/src/Core/util/ForwardDeclarations.h +++ b/Eigen/src/Core/util/ForwardDeclarations.h @@ -56,11 +56,6 @@ 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 MatrixType> class Array; -template<typename Lhs, typename Rhs> class Cross; -template<typename Scalar> class Quaternion; -template<typename Scalar> class Rotation2D; -template<typename Scalar> class AngleAxis; -template<typename Scalar,int Dim> class Transform; template<typename Lhs, typename Rhs> struct ei_product_mode; template<typename Lhs, typename Rhs, int ProductMode = ei_product_mode<Lhs,Rhs>::value> struct ProductReturnType; @@ -98,4 +93,12 @@ void ei_cache_friendly_product( template<typename ExpressionType, bool CheckExistence = true> class Inverse; template<typename MatrixType> class QR; +// Geometry module: +template<typename Lhs, typename Rhs> class Cross; +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/Geometry/AngleAxis.h b/Eigen/src/Geometry/AngleAxis.h new file mode 100644 index 000000000..2b0ee6196 --- /dev/null +++ b/Eigen/src/Geometry/AngleAxis.h @@ -0,0 +1,147 @@ +// 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_ANGLEAXIS_H +#define EIGEN_ANGLEAXIS_H + +/** \class AngleAxis + * + * \brief Represents a rotation in a 3 dimensional space as a rotation angle around a 3D axis + * + * \param _Scalar the scalar type, i.e., the type of the coefficients. + * + * \sa class Quaternion, class EulerAngles, class Transform + */ +template<typename _Scalar> +class AngleAxis +{ +public: + enum { Dim = 3 }; + /** the scalar type of the coefficients */ + typedef _Scalar Scalar; + typedef Matrix<Scalar,3,3> Matrix3; + typedef Matrix<Scalar,3,1> Vector3; + typedef Quaternion<Scalar> QuaternionType; + typedef EulerAngles<Scalar> EulerAnglesType; + +protected: + + Vector3 m_axis; + Scalar m_angle; + +public: + + AngleAxis() {} + 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; } + + Scalar angle() const { return m_angle; } + Scalar& angle() { return m_angle; } + + const Vector3& axis() const { return m_axis; } + Vector3& axis() { return m_axis; } + + AngleAxis& operator=(const QuaternionType& q); + AngleAxis& operator=(const EulerAnglesType& ea); + template<typename Derived> + AngleAxis& operator=(const MatrixBase<Derived>& m); + + template<typename Derived> + AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m); + Matrix3 toRotationMatrix(void) const; +}; + +/** Set \c *this from a quaternion. + * The axis is normalized. + */ +template<typename Scalar> +AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionType& q) +{ + Scalar n2 = q.vec().norm2(); + if (ei_isMuchSmallerThan(n2,Scalar(1))) + { + m_angle = 0; + m_axis << 1, 0, 0; + } + else + { + m_angle = 2*std::acos(q.w()); + m_axis = q.vec() / ei_sqrt(n2); + } + 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> +template<typename Derived> +AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat) +{ + // Since a direct conversion would not be really faster, + // let's use the robust Quaternion implementation: + return *this = QuaternionType(mat); +} + +/** Constructs and \returns an equivalent 3x3 rotation matrix. + */ +template<typename Scalar> +typename AngleAxis<Scalar>::Matrix3 +AngleAxis<Scalar>::toRotationMatrix(void) const +{ + Matrix3 res; + Vector3 sin_axis = ei_sin(m_angle) * m_axis; + Scalar c = ei_cos(m_angle); + Vector3 cos1_axis = (Scalar(1)-c) * m_axis; + + Scalar tmp; + tmp = cos1_axis.x() * m_axis.y(); + res.coeffRef(0,1) = tmp - sin_axis.z(); + res.coeffRef(1,0) = tmp + sin_axis.z(); + + tmp = cos1_axis.x() * m_axis.z(); + res.coeffRef(0,2) = tmp + sin_axis.y(); + res.coeffRef(2,0) = tmp - sin_axis.y(); + + tmp = cos1_axis.y() * m_axis.z(); + res.coeffRef(1,2) = tmp - sin_axis.x(); + res.coeffRef(2,1) = tmp + sin_axis.x(); + + res.diagonal() = Vector3::constant(c) + cos1_axis.cwiseProduct(m_axis); + + return res; +} + +#endif // EIGEN_ANGLEAXIS_H diff --git a/Eigen/src/Geometry/EulerAngles.h b/Eigen/src/Geometry/EulerAngles.h new file mode 100644 index 000000000..1e12e50cc --- /dev/null +++ b/Eigen/src/Geometry/EulerAngles.h @@ -0,0 +1,158 @@ +// 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_EULERANGLES_H +#define EIGEN_EULERANGLES_H + +template<typename Other, + int OtherRows=Other::RowsAtCompileTime, + int OtherCols=Other::ColsAtCompileTime> +struct ei_eulerangles_assign_impl; + +/** \class EulerAngles + * + * \brief Represents a rotation in a 3 dimensional space as three Euler angles + * + * \param _Scalar the scalar type, i.e., the type of the angles. + * + * \sa class Quaternion, class AngleAxis, class Transform + */ +template<typename _Scalar> +class EulerAngles +{ +public: + enum { Dim = 3 }; + /** the scalar type of the coefficients */ + typedef _Scalar Scalar; + typedef Matrix<Scalar,3,3> Matrix3; + typedef Matrix<Scalar,3,1> Vector3; + typedef Quaternion<Scalar> QuaternionType; + typedef AngleAxis<Scalar> AngleAxisType; + +protected: + + Vector3 m_angles; + +public: + + EulerAngles() {} + template<typename Derived> + inline EulerAngles(Scalar a0, Scalar a1, Scalar a2) : m_angles(a0, a1, a2) {} + inline EulerAngles(const QuaternionType& q) { *this = q; } + inline EulerAngles(const AngleAxisType& aa) { *this = aa; } + template<typename Derived> + inline EulerAngles(const MatrixBase<Derived>& m) { *this = m; } + + Scalar angle(int i) const { return m_angles.coeff(i); } + Scalar& angle(int i) { return m_angles.coeffRef(i); } + + const Vector3& coeffs() const { return m_angles; } + Vector3& coeffs() { return m_angles; } + + EulerAngles& operator=(const QuaternionType& q); + EulerAngles& operator=(const AngleAxisType& ea); + template<typename Derived> + EulerAngles& operator=(const MatrixBase<Derived>& m); + + template<typename Derived> + EulerAngles& fromRotationMatrix(const MatrixBase<Derived>& m); + Matrix3 toRotationMatrix(void) const; +}; + +/** Set \c *this from a quaternion. + * The axis is normalized. + */ +template<typename Scalar> +EulerAngles<Scalar>& EulerAngles<Scalar>::operator=(const QuaternionType& q) +{ + Scalar y2 = q.y() * q.y(); + m_angles.coeffRef(0) = std::atan2(2*(q.w()*q.x() + q.y()*q.z()), (1 - 2*(q.x()*q.x() + y2))); + m_angles.coeffRef(1) = std::asin( 2*(q.w()*q.y() - q.z()*q.x())); + m_angles.coeffRef(2) = std::atan2(2*(q.w()*q.z() + q.x()*q.y()), (1 - 2*(y2 + q.z()*q.z()))); + return *this; +} + +/** Set \c *this from Euler angles \a ea. + */ +template<typename Scalar> +EulerAngles<Scalar>& EulerAngles<Scalar>::operator=(const AngleAxisType& aa) +{ + return *this = QuaternionType(aa); +} + +/** Set \c *this from the expression \a xpr: + * - if \a xpr is a 3x1 vector, then \a xpr is assumed to be a vector of angles + * - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix + * and \a xpr is converted to Euler angles + */ +template<typename Scalar> +template<typename Derived> +EulerAngles<Scalar>& EulerAngles<Scalar>::operator=(const MatrixBase<Derived>& other) +{ + ei_eulerangles_assign_impl<Derived>::run(*this,other.derived()); + return *this; +} + +/** Constructs and \returns an equivalent 3x3 rotation matrix. + */ +template<typename Scalar> +typename EulerAngles<Scalar>::Matrix3 +EulerAngles<Scalar>::toRotationMatrix(void) const +{ + Vector3 c = m_angles.cwiseCos(); + Vector3 s = m_angles.cwiseSin(); + return Matrix3() << + c.y()*c.z(), -c.y()*s.z(), s.y(), + c.z()*s.x()*s.y()+c.x()*s.z(), c.x()*c.z()-s.x()*s.y()*s.z(), -c.y()*s.x(), + -c.x()*c.z()*s.y()+s.x()*s.z(), c.z()*s.x()+c.x()*s.y()*s.z(), c.x()*c.y(); +} + +// set from a rotation matrix +template<typename Other> +struct ei_eulerangles_assign_impl<Other,3,3> +{ + typedef typename Other::Scalar Scalar; + inline static void run(EulerAngles<Scalar>& ea, const Other& mat) + { + // mat = cy*cz -cy*sz sy + // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx + // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy + ea.angle(1) = std::asin(mat.coeff(0,2)); + ea.angle(0) = std::atan2(-mat.coeff(1,2),mat.coeff(2,2)); + ea.angle(2) = std::atan2(-mat.coeff(0,1),mat.coeff(0,0)); + } +}; + +// set from a vector of angles +template<typename Other> +struct ei_eulerangles_assign_impl<Other,3,1> +{ + typedef typename Other::Scalar Scalar; + inline static void run(EulerAngles<Scalar>& ea, const Other& vec) + { + ea.coeffs() = vec; + } +}; + +#endif // EIGEN_EULERANGLES_H diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h index 354d23e89..f08f82151 100644 --- a/Eigen/src/Geometry/Quaternion.h +++ b/Eigen/src/Geometry/Quaternion.h @@ -25,6 +25,11 @@ #ifndef EIGEN_QUATERNION_H #define EIGEN_QUATERNION_H +template<typename Other, + int OtherRows=Other::RowsAtCompileTime, + int OtherCols=Other::ColsAtCompileTime> +struct ei_quaternion_assign_impl; + /** \class Quaternion * * \brief The quaternion class used to represent 3D orientations and rotations @@ -39,6 +44,7 @@ * - efficient to compose (28 flops), * - stable spherical interpolation * + * \sa class AngleAxis, class EulerAngles, class Transform */ template<typename _Scalar> class Quaternion @@ -53,6 +59,8 @@ public: 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); } @@ -71,10 +79,10 @@ public: inline Block<Coefficients,3,1> vec() { return m_coeffs.template start<3>(); } /** \returns a read-only vector expression of the coefficients */ - inline const Coefficients& _coeffs() const { return m_coeffs; } + inline const Coefficients& coeffs() const { return m_coeffs; } /** \returns a vector expression of the coefficients */ - inline Coefficients& _coeffs() { return m_coeffs; } + inline Coefficients& coeffs() { return m_coeffs; } // 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) @@ -88,14 +96,16 @@ public: /** Copy constructor */ inline Quaternion(const Quaternion& other) { m_coeffs = other.m_coeffs; } - /** 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) - { - m_coeffs = other.m_coeffs; - return *this; - } + 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); /** \returns a quaternion representing an identity rotation * \sa MatrixBase::identity() @@ -116,20 +126,10 @@ public: */ inline Scalar norm() const { return m_coeffs.norm(); } - template<typename Derived> - Quaternion& fromRotationMatrix(const MatrixBase<Derived>& m); Matrix3 toRotationMatrix(void) const; - template<typename Derived> - Quaternion& fromAngleAxis (const Scalar& angle, const MatrixBase<Derived>& axis); - void toAngleAxis(Scalar& angle, Vector3& axis) const; - - Quaternion& fromEulerAngles(Vector3 eulerAngles); - - Vector3 toEulerAngles(void) const; - template<typename Derived1, typename Derived2> - Quaternion& fromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); + Quaternion& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); inline Quaternion operator* (const Quaternion& q) const; inline Quaternion& operator*= (const Quaternion& q); @@ -142,24 +142,6 @@ public: template<typename Derived> Vector3 operator* (const MatrixBase<Derived>& vec) const; -protected: - - /** Constructor copying the value of the expression \a other */ - template<typename OtherDerived> - inline Quaternion(const Eigen::MatrixBase<OtherDerived>& other) - { - m_coeffs = other; - } - - /** Copies the value of the expression \a other into \c *this. - */ - template<typename OtherDerived> - inline Quaternion& operator=(const MatrixBase<OtherDerived>& other) - { - m_coeffs = other.derived(); - return *this; - } - }; /** \returns the concatenation of two rotations as a quaternion-quaternion product */ @@ -203,137 +185,35 @@ Quaternion<Scalar>::operator* (const MatrixBase<Derived>& v) const return v + this->w() * uv + this->vec().cross(uv); } -/** Convert the quaternion to a 3x3 rotation matrix */ -template<typename Scalar> -inline typename Quaternion<Scalar>::Matrix3 -Quaternion<Scalar>::toRotationMatrix(void) const -{ - // FIXME another option would be to declare toRotationMatrix like that: - // OtherDerived& toRotationMatrix(MatrixBase<OtherDerived>& m) - // it would fill m and returns a ref to m. - // the advantages is that way we can accept 4x4 and 3x4 matrices filling the rest of the - // matrix with I... ?? - 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.coeffRef(0,0) = 1-(tyy+tzz); - res.coeffRef(0,1) = txy-twz; - res.coeffRef(0,2) = txz+twy; - res.coeffRef(1,0) = txy+twz; - res.coeffRef(1,1) = 1-(txx+tzz); - res.coeffRef(1,2) = tyz-twx; - res.coeffRef(2,0) = txz-twy; - res.coeffRef(2,1) = tyz+twx; - res.coeffRef(2,2) = 1-(txx+tyy); - - return res; -} - -/** updates \c *this from the rotation matrix \a m and returns a reference to \c *this - * \warning the size of the input matrix expression \a m must be 3x3 at compile time - */ template<typename Scalar> -template<typename Derived> -Quaternion<Scalar>& Quaternion<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) +inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const Quaternion& other) { - // FIXME maybe this function could accept 4x4 and 3x4 matrices as well ? (simply update the assert) - // FIXME this function could also be static and returns a temporary ? - EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==3 && Derived::ColsAtCompileTime==3,you_did_a_programming_error); - // This algorithm comes from "Quaternion Calculus and Fast Animation", - // Ken Shoemake, 1987 SIGGRAPH course notes - Scalar t = mat.trace(); - if (t > 0) - { - t = ei_sqrt(t + 1.0); - this->w() = 0.5*t; - t = 0.5/t; - this->x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t; - this->y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t; - this->z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t; - } - else - { - int i = 0; - if (mat.coeff(1,1) > mat.coeff(0,0)) - i = 1; - if (mat.coeff(2,2) > mat.coeff(i,i)) - i = 2; - int j = (i+1)%3; - int k = (j+1)%3; - - t = ei_sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + 1.0); - m_coeffs.coeffRef(i) = 0.5 * t; - t = 0.5/t; - this->w() = (mat.coeff(k,j)-mat.coeff(j,k))*t; - m_coeffs.coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t; - m_coeffs.coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t; - } - + m_coeffs = other.m_coeffs; return *this; } -/** updates \c *this from the rotation defined by axis \a axis and angle \a angle +/** Set \c *this from an angle-axis \a aa * and returns a reference to \c *this - * \warning the size of the input vector expression \a axis must be 3 at compile time */ template<typename Scalar> -template<typename Derived> -inline Quaternion<Scalar>& Quaternion<Scalar> -::fromAngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis) +inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const AngleAxisType& aa) { - ei_assert(Derived::SizeAtCompileTime==3); - Scalar ha = 0.5*angle; + Scalar ha = 0.5*aa.angle(); this->w() = ei_cos(ha); - this->vec() = ei_sin(ha) * axis; + this->vec() = ei_sin(ha) * aa.axis(); return *this; } -/** Computes and returns the angle and axis of the rotation represented by the quaternion. - * The values are returned in the arguments \a angle and \a axis respectively. - * The returned axis is normalized. - */ -template <typename Scalar> -void Quaternion<Scalar>::toAngleAxis(Scalar& angle, Vector3& axis) const -{ - // FIXME should we split this function to an "angle" and an "axis" functions ? - // the drawbacks is that this approach would require to compute twice the norm of (x,y,z)... - // or we returns a Vector4, or a small AngleAxis object... ??? - Scalar n2 = this->vec().norm2(); - if (ei_isMuchSmallerThan(n2,Scalar(1))) - { - angle = 0; - axis << 1, 0, 0; - } - else - { - angle = 2*std::acos(this->w()); - axis = this->vec() / ei_sqrt(n2); - } -} - -/** updates \c *this from the rotation defined by the Euler angles \a eulerAngles, +/** Set \c *this from the rotation defined by the Euler angles \a ea, * and returns a reference to \c *this */ -template <typename Scalar> -Quaternion<Scalar>& Quaternion<Scalar>::fromEulerAngles(Vector3 eulerAngles) +template<typename Scalar> +inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(EulerAnglesType ea) { - // FIXME should the arguments be 3 scalars or a single Vector3 ? - eulerAngles *= 0.5; + ea.coeffs() *= 0.5; - Vector3 cosines = eulerAngles.cwiseCos(); - Vector3 sines = eulerAngles.cwiseSin(); + Vector3 cosines = ea.coeffs().cwiseCos(); + Vector3 sines = ea.coeffs().cwiseSin(); Scalar cYcZ = cosines.y() * cosines.z(); Scalar sYsZ = sines.y() * sines.z(); @@ -348,16 +228,54 @@ Quaternion<Scalar>& Quaternion<Scalar>::fromEulerAngles(Vector3 eulerAngles) return *this; } -/** Computes and returns the Euler angles corresponding to the quaternion \c *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 + * and \a xpr is converted to a quaternion */ -template <typename Scalar> -typename Quaternion<Scalar>::Vector3 Quaternion<Scalar>::toEulerAngles(void) const +template<typename Scalar> +template<typename Derived> +inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const MatrixBase<Derived>& xpr) { - Scalar y2 = this->y() * this->y(); - return Vector3( - std::atan2(2*(this->w()*this->x() + this->y()*this->z()), (1 - 2*(this->x()*this->x() + y2))), - std::asin( 2*(this->w()*this->y() - this->z()*this->x())), - std::atan2(2*(this->w()*this->z() + this->x()*this->y()), (1 - 2*(y2 + this->z()*this->z())))); + ei_quaternion_assign_impl<Derived>::run(*this, xpr.derived()); + return *this; +} + +/** Convert the quaternion to a 3x3 rotation matrix */ +template<typename Scalar> +inline typename Quaternion<Scalar>::Matrix3 +Quaternion<Scalar>::toRotationMatrix(void) const +{ + // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!) + // if not inlined then the cost of the return by value is huge ~ +35%, + // however, not inlining this function is an order of magnitude slower, so + // it has to be inlined, and so the return by value is not an issue + 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.coeffRef(0,0) = 1-(tyy+tzz); + res.coeffRef(0,1) = txy-twz; + res.coeffRef(0,2) = txz+twy; + res.coeffRef(1,0) = txy+twz; + res.coeffRef(1,1) = 1-(txx+tzz); + res.coeffRef(1,2) = tyz-twx; + res.coeffRef(2,0) = txz-twy; + res.coeffRef(2,1) = tyz+twx; + res.coeffRef(2,2) = 1-(txx+tyy); + + return res; } /** Makes a quaternion representing the rotation between two vectors \a a and \a b. @@ -366,7 +284,7 @@ typename Quaternion<Scalar>::Vector3 Quaternion<Scalar>::toEulerAngles(void) con */ template<typename Scalar> template<typename Derived1, typename Derived2> -inline Quaternion<Scalar>& Quaternion<Scalar>::fromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) +inline Quaternion<Scalar>& Quaternion<Scalar>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) { Vector3 v0 = a.normalized(); Vector3 v1 = b.normalized(); @@ -399,11 +317,11 @@ inline Quaternion<Scalar> Quaternion<Scalar>::inverse() const // FIXME should this funtion be called multiplicativeInverse and conjugate() be called inverse() or opposite() ?? Scalar n2 = this->norm2(); if (n2 > 0) - return conjugate()._coeffs() / n2; + return Quaternion(conjugate().coeffs() / n2); else { // return an invalid result to flag the error - return Coefficients::zero(); + return Quaternion(Coefficients::zero()); } } @@ -448,4 +366,54 @@ Quaternion<Scalar> Quaternion<Scalar>::slerp(Scalar t, const Quaternion& other) return scale0 * (*this) + scale1 * other; } +// set from a rotation matrix +template<typename Other> +struct ei_quaternion_assign_impl<Other,3,3> +{ + typedef typename Other::Scalar Scalar; + inline static void run(Quaternion<Scalar>& q, const Other& mat) + { + // This algorithm comes from "Quaternion Calculus and Fast Animation", + // Ken Shoemake, 1987 SIGGRAPH course notes + Scalar t = mat.trace(); + if (t > 0) + { + t = ei_sqrt(t + 1.0); + q.w() = 0.5*t; + t = 0.5/t; + q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t; + q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t; + q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t; + } + else + { + int i = 0; + if (mat.coeff(1,1) > mat.coeff(0,0)) + i = 1; + if (mat.coeff(2,2) > mat.coeff(i,i)) + i = 2; + int j = (i+1)%3; + int k = (j+1)%3; + + t = ei_sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + 1.0); + q.coeffs().coeffRef(i) = 0.5 * t; + t = 0.5/t; + q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t; + q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t; + q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t; + } + } +}; + +// set from a vector of coefficients assumed to be a quaternion +template<typename Other> +struct ei_quaternion_assign_impl<Other,4,1> +{ + typedef typename Other::Scalar Scalar; + inline static void run(Quaternion<Scalar>& q, const Other& vec) + { + q.coeffs() = vec; + } +}; + #endif // EIGEN_QUATERNION_H diff --git a/Eigen/src/Geometry/Rotation.h b/Eigen/src/Geometry/Rotation.h index 7e07b48f5..ba533b9fc 100644 --- a/Eigen/src/Geometry/Rotation.h +++ b/Eigen/src/Geometry/Rotation.h @@ -65,7 +65,7 @@ struct ToRotationMatrix<Scalar, 2, OtherScalarType> { return Rotation2D<Scalar>(r).toRotationMatrix(); } }; -// 2D rotation to matrix +// 2D rotation to rotation matrix template<typename Scalar, typename OtherScalarType> struct ToRotationMatrix<Scalar, 2, Rotation2D<OtherScalarType> > { @@ -73,7 +73,7 @@ struct ToRotationMatrix<Scalar, 2, Rotation2D<OtherScalarType> > { return Rotation2D<Scalar>(r).toRotationMatrix(); } }; -// quaternion to matrix +// quaternion to rotation matrix template<typename Scalar, typename OtherScalarType> struct ToRotationMatrix<Scalar, 3, Quaternion<OtherScalarType> > { @@ -81,7 +81,7 @@ struct ToRotationMatrix<Scalar, 3, Quaternion<OtherScalarType> > { return q.toRotationMatrix(); } }; -// angle axis to matrix +// angle axis to rotation matrix template<typename Scalar, typename OtherScalarType> struct ToRotationMatrix<Scalar, 3, AngleAxis<OtherScalarType> > { @@ -89,6 +89,14 @@ struct ToRotationMatrix<Scalar, 3, AngleAxis<OtherScalarType> > { 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> > @@ -169,88 +177,4 @@ Rotation2D<Scalar>::toRotationMatrix(void) const return (Matrix2() << cosA, -sinA, sinA, cosA).finished(); } -/** \class AngleAxis - * - * \brief Represents a rotation in a 3 dimensional space as a rotation angle around a 3D axis - * - * \param _Scalar the scalar type, i.e., the type of the coefficients. - * - * \sa class Quaternion, class Transform - */ -template<typename _Scalar> -class AngleAxis -{ -public: - enum { Dim = 3 }; - /** the scalar type of the coefficients */ - typedef _Scalar Scalar; - typedef Matrix<Scalar,3,3> Matrix3; - typedef Matrix<Scalar,3,1> Vector3; - -protected: - - Vector3 m_axis; - Scalar m_angle; - -public: - - AngleAxis() {} - template<typename Derived> - inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {} - - Scalar angle() const { return m_angle; } - Scalar& angle() { return m_angle; } - - const Vector3& axis() const { return m_axis; } - Vector3& axis() { return m_axis; } - - template<typename Derived> - AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m); - Matrix3 toRotationMatrix(void) const; -}; - -/** Set \c *this from a 3x3 rotation matrix \a mat. - * In other words, this function extract the rotation angle - * from the rotation matrix. - */ -template<typename Scalar> -template<typename Derived> -AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) -{ - // Since a direct conversion would not be really faster, - // let's use the robust Quaternion implementation: - Quaternion<Scalar>().fromRotationMatrix(mat).toAngleAxis(m_angle, m_axis); - - return *this; -} - -/** Constructs and \returns an equivalent 2x2 rotation matrix. - */ -template<typename Scalar> -typename AngleAxis<Scalar>::Matrix3 -AngleAxis<Scalar>::toRotationMatrix(void) const -{ - Matrix3 res; - Vector3 sin_axis = ei_sin(m_angle) * m_axis; - Scalar c = ei_cos(m_angle); - Vector3 cos1_axis = (Scalar(1)-c) * m_axis; - - Scalar tmp; - tmp = cos1_axis.x() * m_axis.y(); - res.coeffRef(0,1) = tmp - sin_axis.z(); - res.coeffRef(1,0) = tmp + sin_axis.z(); - - tmp = cos1_axis.x() * m_axis.z(); - res.coeffRef(0,2) = tmp + sin_axis.y(); - res.coeffRef(2,0) = tmp - sin_axis.y(); - - tmp = cos1_axis.y() * m_axis.z(); - res.coeffRef(1,2) = tmp - sin_axis.x(); - res.coeffRef(2,1) = tmp + sin_axis.x(); - - res.diagonal() = Vector3::constant(c) + cos1_axis.cwiseProduct(m_axis); - - return res; -} - #endif // EIGEN_ROTATION_H diff --git a/Eigen/src/Geometry/Transform.h b/Eigen/src/Geometry/Transform.h index 5d092951e..ed4e6f200 100644 --- a/Eigen/src/Geometry/Transform.h +++ b/Eigen/src/Geometry/Transform.h @@ -384,20 +384,20 @@ Transform<Scalar,Dim>::fromPositionOrientationScale(const MatrixBase<PositionDer template<typename Other, int Dim, int HDim> struct ei_transform_product_impl<Other,Dim,HDim, HDim,HDim> { - typedef Transform<typename Other::Scalar,Dim> Transform; - typedef typename Transform::MatrixType MatrixType; + typedef Transform<typename Other::Scalar,Dim> TransformType; + typedef typename TransformType::MatrixType MatrixType; typedef typename ProductReturnType<MatrixType,Other>::Type ResultType; - static ResultType run(const Transform& tr, const Other& other) + static ResultType run(const TransformType& tr, const Other& other) { return tr.matrix() * other; } }; template<typename Other, int Dim, int HDim> struct ei_transform_product_impl<Other,Dim,HDim, HDim,1> { - typedef Transform<typename Other::Scalar,Dim> Transform; - typedef typename Transform::MatrixType MatrixType; + typedef Transform<typename Other::Scalar,Dim> TransformType; + typedef typename TransformType::MatrixType MatrixType; typedef typename ProductReturnType<MatrixType,Other>::Type ResultType; - static ResultType run(const Transform& tr, const Other& other) + static ResultType run(const TransformType& tr, const Other& other) { return tr.matrix() * other; } }; @@ -405,17 +405,17 @@ template<typename Other, int Dim, int HDim> struct ei_transform_product_impl<Other,Dim,HDim, Dim,1> { typedef typename Other::Scalar Scalar; - typedef Transform<Scalar,Dim> Transform; - typedef typename Transform::AffineMatrixRef MatrixType; + typedef Transform<Scalar,Dim> TransformType; + typedef typename TransformType::AffineMatrixRef MatrixType; typedef const CwiseUnaryOp< ei_scalar_multiple_op<Scalar>, NestByValue<CwiseBinaryOp< ei_scalar_sum_op<Scalar>, NestByValue<typename ProductReturnType<NestByValue<MatrixType>,Other>::Type >, - NestByValue<typename Transform::VectorRef> > > + NestByValue<typename TransformType::VectorRef> > > > ResultType; // FIXME shall we offer an optimized version when the last row is known to be 0,0...,0,1 ? - static ResultType run(const Transform& tr, const Other& other) + static ResultType run(const TransformType& tr, const Other& other) { return ((tr.affine().nestByValue() * other).nestByValue() + tr.translation().nestByValue()).nestByValue() * (Scalar(1) / ( (tr.matrix().template block<1,Dim>(Dim,0) * other).coeff(0) + tr.matrix().coeff(Dim,Dim))); } }; diff --git a/test/geometry.cpp b/test/geometry.cpp index c4d8326a8..bf3268745 100644 --- a/test/geometry.cpp +++ b/test/geometry.cpp @@ -39,6 +39,8 @@ template<typename Scalar> void geometry(void) typedef Matrix<Scalar,3,1> Vector3; 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(), @@ -47,8 +49,8 @@ template<typename Scalar> void geometry(void) Scalar a = ei_random<Scalar>(-M_PI, M_PI); - q1.fromAngleAxis(ei_random<Scalar>(-M_PI, M_PI), v0.normalized()); - q2.fromAngleAxis(ei_random<Scalar>(-M_PI, M_PI), v1.normalized()); + q1 = AngleAxis(ei_random<Scalar>(-M_PI, M_PI), v0.normalized()); + q2 = AngleAxis(ei_random<Scalar>(-M_PI, M_PI), v1.normalized()); // rotation matrix conversion VERIFY_IS_APPROX(q1 * v2, q1.toRotationMatrix() * v2); @@ -56,23 +58,23 @@ template<typename Scalar> void geometry(void) q1.toRotationMatrix() * q2.toRotationMatrix() * v2); VERIFY_IS_NOT_APPROX(q2 * q1 * v2, q1.toRotationMatrix() * q2.toRotationMatrix() * v2); - q2.fromRotationMatrix(q1.toRotationMatrix()); + q2 = q1.toRotationMatrix(); VERIFY_IS_APPROX(q1*v1,q2*v1); // Euler angle conversion - VERIFY_IS_APPROX(q2.fromEulerAngles(q1.toEulerAngles()) * v1, q1 * v1); - v2 = q2.toEulerAngles(); - VERIFY_IS_APPROX(q2.fromEulerAngles(v2).toEulerAngles(), v2); - VERIFY_IS_NOT_APPROX(q2.fromEulerAngles(v2.cwiseProduct(Vector3(0.2,-0.2,1))).toEulerAngles(), v2); + 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.cwiseProduct(Vector3(0.2,-0.2,1))))).coeffs(), v2); // angle-axis conversion - q1.toAngleAxis(a, v2); - VERIFY_IS_APPROX(q1 * v1, q2.fromAngleAxis(a,v2) * v1); - VERIFY_IS_NOT_APPROX(q1 * v1, q2.fromAngleAxis(2*a,v2) * v1); + AngleAxis aa = q1; + VERIFY_IS_APPROX(q1 * v1, Quaternion(aa) * v1); + VERIFY_IS_NOT_APPROX(q1 * v1, Quaternion(AngleAxis(aa.angle()*2,aa.axis())) * v1); // from two vector creation - VERIFY_IS_APPROX(v2.normalized(),(q2.fromTwoVectors(v1,v2)*v1).normalized()); - VERIFY_IS_APPROX(v2.normalized(),(q2.fromTwoVectors(v1,v2)*v1).normalized()); + VERIFY_IS_APPROX(v2.normalized(),(q2.setFromTwoVectors(v1,v2)*v1).normalized()); + VERIFY_IS_APPROX(v2.normalized(),(q2.setFromTwoVectors(v1,v2)*v1).normalized()); // inverse and conjugate VERIFY_IS_APPROX(q1 * (q1.inverse() * v1), v1); @@ -87,15 +89,14 @@ template<typename Scalar> void geometry(void) VERIFY(m.isOrtho()); // AngleAxis - VERIFY_IS_APPROX(AngleAxis<Scalar>(a,v1.normalized()).toRotationMatrix(), - q2.fromAngleAxis(a,v1.normalized()).toRotationMatrix()); + VERIFY_IS_APPROX(AngleAxis(a,v1.normalized()).toRotationMatrix(), + Quaternion(AngleAxis(a,v1.normalized())).toRotationMatrix()); - AngleAxis<Scalar> aa1; + AngleAxis aa1; m = q1.toRotationMatrix(); - Vector3 tax; Scalar tan; - q2.fromRotationMatrix(m).toAngleAxis(tan,tax); - VERIFY_IS_APPROX(aa1.fromRotationMatrix(m).toRotationMatrix(), - q2.fromRotationMatrix(m).toRotationMatrix()); + aa1 = m; + VERIFY_IS_APPROX(AngleAxis(m).toRotationMatrix(), + Quaternion(m).toRotationMatrix()); // Transform @@ -106,7 +107,7 @@ template<typename Scalar> void geometry(void) a = 0; while (ei_abs(a)<0.1) a = ei_random<Scalar>(-0.4*M_PI, 0.4*M_PI); - q1.fromAngleAxis(a, v0.normalized()); + q1 = AngleAxis(a, v0.normalized()); Transform3 t0, t1, t2; t0.setIdentity(); t0.affine() = q1.toRotationMatrix(); |