diff options
author | Campbell Barton <ideasman42@gmail.com> | 2011-10-23 21:52:20 +0400 |
---|---|---|
committer | Campbell Barton <ideasman42@gmail.com> | 2011-10-23 21:52:20 +0400 |
commit | 4a04f7206914a49f5f95adc5eb786237f1a9f547 (patch) | |
tree | 78aed2fa481f972fac0965f814bebebe9d71ae65 /extern/Eigen3/Eigen/src/Geometry/AngleAxis.h | |
parent | f1cea89d99f0c80bdccd2ba1359142b5ff14cdb9 (diff) |
remove $Id: tags after discussion on the mailign list: http://markmail.org/message/fp7ozcywxum3ar7n
Diffstat (limited to 'extern/Eigen3/Eigen/src/Geometry/AngleAxis.h')
-rw-r--r-- | extern/Eigen3/Eigen/src/Geometry/AngleAxis.h | 241 |
1 files changed, 241 insertions, 0 deletions
diff --git a/extern/Eigen3/Eigen/src/Geometry/AngleAxis.h b/extern/Eigen3/Eigen/src/Geometry/AngleAxis.h new file mode 100644 index 00000000000..0ec4624cf98 --- /dev/null +++ b/extern/Eigen3/Eigen/src/Geometry/AngleAxis.h @@ -0,0 +1,241 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.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 + +/** \geometry_module \ingroup Geometry_Module + * + * \class AngleAxis + * + * \brief Represents a 3D rotation as a rotation angle around an arbitrary 3D axis + * + * \param _Scalar the scalar type, i.e., the type of the coefficients. + * + * \warning When setting up an AngleAxis object, the axis vector \b must \b be \b normalized. + * + * The following two typedefs are provided for convenience: + * \li \c AngleAxisf for \c float + * \li \c AngleAxisd for \c double + * + * Combined with MatrixBase::Unit{X,Y,Z}, AngleAxis can be used to easily + * mimic Euler-angles. Here is an example: + * \include AngleAxis_mimic_euler.cpp + * Output: \verbinclude AngleAxis_mimic_euler.out + * + * \note This class is not aimed to be used to store a rotation transformation, + * but rather to make easier the creation of other rotation (Quaternion, rotation Matrix) + * and transformation objects. + * + * \sa class Quaternion, class Transform, MatrixBase::UnitX() + */ + +namespace internal { +template<typename _Scalar> struct traits<AngleAxis<_Scalar> > +{ + typedef _Scalar Scalar; +}; +} + +template<typename _Scalar> +class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3> +{ + typedef RotationBase<AngleAxis<_Scalar>,3> Base; + +public: + + using Base::operator*; + + 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; + +protected: + + Vector3 m_axis; + Scalar m_angle; + +public: + + /** Default constructor without initialization. */ + AngleAxis() {} + /** Constructs and initialize the angle-axis rotation from an \a angle in radian + * and an \a axis which \b must \b be \b normalized. + * + * \warning If the \a axis vector is not normalized, then the angle-axis object + * represents an invalid rotation. */ + template<typename Derived> + inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {} + /** Constructs and initialize the angle-axis rotation from a quaternion \a q. */ + template<typename QuatDerived> inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; } + /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */ + template<typename Derived> + inline explicit 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; } + + /** Concatenates two rotations */ + inline QuaternionType operator* (const AngleAxis& other) const + { return QuaternionType(*this) * QuaternionType(other); } + + /** Concatenates two rotations */ + inline QuaternionType operator* (const QuaternionType& other) const + { return QuaternionType(*this) * other; } + + /** Concatenates two rotations */ + friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b) + { return a * QuaternionType(b); } + + /** \returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */ + AngleAxis inverse() const + { return AngleAxis(-m_angle, m_axis); } + + template<class QuatDerived> + AngleAxis& operator=(const QuaternionBase<QuatDerived>& q); + template<typename Derived> + AngleAxis& operator=(const MatrixBase<Derived>& m); + + template<typename Derived> + AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m); + Matrix3 toRotationMatrix(void) const; + + /** \returns \c *this with scalar type casted to \a NewScalarType + * + * Note that if \a NewScalarType is equal to the current scalar type of \c *this + * then this function smartly returns a const reference to \c *this. + */ + template<typename NewScalarType> + inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const + { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); } + + /** Copy constructor with scalar type conversion */ + template<typename OtherScalarType> + inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other) + { + m_axis = other.axis().template cast<Scalar>(); + m_angle = Scalar(other.angle()); + } + + inline static const AngleAxis Identity() { return AngleAxis(0, Vector3::UnitX()); } + + /** \returns \c true if \c *this is approximately equal to \a other, within the precision + * determined by \a prec. + * + * \sa MatrixBase::isApprox() */ + bool isApprox(const AngleAxis& other, typename NumTraits<Scalar>::Real prec = NumTraits<Scalar>::dummy_precision()) const + { return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle,other.m_angle, prec); } +}; + +/** \ingroup Geometry_Module + * single precision angle-axis type */ +typedef AngleAxis<float> AngleAxisf; +/** \ingroup Geometry_Module + * double precision angle-axis type */ +typedef AngleAxis<double> AngleAxisd; + +/** Set \c *this from a \b unit quaternion. + * The axis is normalized. + * + * \warning As any other method dealing with quaternion, if the input quaternion + * is not normalized then the result is undefined. + */ +template<typename Scalar> +template<typename QuatDerived> +AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q) +{ + using std::acos; + using std::min; + using std::max; + Scalar n2 = q.vec().squaredNorm(); + if (n2 < NumTraits<Scalar>::dummy_precision()*NumTraits<Scalar>::dummy_precision()) + { + m_angle = 0; + m_axis << 1, 0, 0; + } + else + { + m_angle = Scalar(2)*acos((min)((max)(Scalar(-1),q.w()),Scalar(1))); + m_axis = q.vec() / internal::sqrt(n2); + } + return *this; +} + +/** 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); +} + +/** +* \brief Sets \c *this from a 3x3 rotation matrix. +**/ +template<typename Scalar> +template<typename Derived> +AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) +{ + 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 = internal::sin(m_angle) * m_axis; + Scalar c = internal::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() = (cos1_axis.cwiseProduct(m_axis)).array() + c; + + return res; +} + +#endif // EIGEN_ANGLEAXIS_H |