AngleAxis.h
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra. Eigen itself is part of the KDE project.
00003 //
00004 // Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
00005 //
00006 // Eigen is free software; you can redistribute it and/or
00007 // modify it under the terms of the GNU Lesser General Public
00008 // License as published by the Free Software Foundation; either
00009 // version 3 of the License, or (at your option) any later version.
00010 //
00011 // Alternatively, you can redistribute it and/or
00012 // modify it under the terms of the GNU General Public License as
00013 // published by the Free Software Foundation; either version 2 of
00014 // the License, or (at your option) any later version.
00015 //
00016 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00017 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00018 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00019 // GNU General Public License for more details.
00020 //
00021 // You should have received a copy of the GNU Lesser General Public
00022 // License and a copy of the GNU General Public License along with
00023 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00024 
00025 // no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway
00026 
00027 
00054 template<typename _Scalar> struct ei_traits<AngleAxis<_Scalar> >
00055 {
00056   typedef _Scalar Scalar;
00057 };
00058 
00059 template<typename _Scalar>
00060 class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3>
00061 {
00062   typedef RotationBase<AngleAxis<_Scalar>,3> Base;
00063 
00064 public:
00065 
00066   using Base::operator*;
00067 
00068   enum { Dim = 3 };
00070   typedef _Scalar Scalar;
00071   typedef Matrix<Scalar,3,3> Matrix3;
00072   typedef Matrix<Scalar,3,1> Vector3;
00073   typedef Quaternion<Scalar> QuaternionType;
00074 
00075 protected:
00076 
00077   Vector3 m_axis;
00078   Scalar m_angle;
00079 
00080 public:
00081 
00083   AngleAxis() {}
00086   template<typename Derived>
00087   inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
00089   inline AngleAxis(const QuaternionType& q) { *this = q; }
00091   template<typename Derived>
00092   inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
00093 
00094   Scalar angle() const { return m_angle; }
00095   Scalar& angle() { return m_angle; }
00096 
00097   const Vector3& axis() const { return m_axis; }
00098   Vector3& axis() { return m_axis; }
00099 
00101   inline QuaternionType operator* (const AngleAxis& other) const
00102   { return QuaternionType(*this) * QuaternionType(other); }
00103 
00105   inline QuaternionType operator* (const QuaternionType& other) const
00106   { return QuaternionType(*this) * other; }
00107 
00109   friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
00110   { return a * QuaternionType(b); }
00111 
00113   inline Matrix3 operator* (const Matrix3& other) const
00114   { return toRotationMatrix() * other; }
00115 
00117   inline friend Matrix3 operator* (const Matrix3& a, const AngleAxis& b)
00118   { return a * b.toRotationMatrix(); }
00119 
00121   inline Vector3 operator* (const Vector3& other) const
00122   { return toRotationMatrix() * other; }
00123 
00125   AngleAxis inverse() const
00126   { return AngleAxis(-m_angle, m_axis); }
00127 
00128   AngleAxis& operator=(const QuaternionType& q);
00129   template<typename Derived>
00130   AngleAxis& operator=(const MatrixBase<Derived>& m);
00131 
00132   template<typename Derived>
00133   AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
00134   Matrix3 toRotationMatrix(void) const;
00135 
00141   template<typename NewScalarType>
00142   inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const
00143   { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); }
00144 
00146   template<typename OtherScalarType>
00147   inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other)
00148   {
00149     m_axis = other.axis().template cast<Scalar>();
00150     m_angle = Scalar(other.angle());
00151   }
00152 
00157   bool isApprox(const AngleAxis& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
00158   { return m_axis.isApprox(other.m_axis, prec) && ei_isApprox(m_angle,other.m_angle, prec); }
00159 };
00160 
00163 typedef AngleAxis<float> AngleAxisf;
00166 typedef AngleAxis<double> AngleAxisd;
00167 
00171 template<typename Scalar>
00172 AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionType& q)
00173 {
00174   Scalar n2 = q.vec().squaredNorm();
00175   if (n2 < precision<Scalar>()*precision<Scalar>())
00176   {
00177     m_angle = 0;
00178     m_axis << 1, 0, 0;
00179   }
00180   else
00181   {
00182     m_angle = 2*std::acos(q.w());
00183     m_axis = q.vec() / ei_sqrt(n2);
00184   }
00185   return *this;
00186 }
00187 
00190 template<typename Scalar>
00191 template<typename Derived>
00192 AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat)
00193 {
00194   // Since a direct conversion would not be really faster,
00195   // let's use the robust Quaternion implementation:
00196   return *this = QuaternionType(mat);
00197 }
00198 
00201 template<typename Scalar>
00202 typename AngleAxis<Scalar>::Matrix3
00203 AngleAxis<Scalar>::toRotationMatrix(void) const
00204 {
00205   Matrix3 res;
00206   Vector3 sin_axis  = ei_sin(m_angle) * m_axis;
00207   Scalar c = ei_cos(m_angle);
00208   Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
00209 
00210   Scalar tmp;
00211   tmp = cos1_axis.x() * m_axis.y();
00212   res.coeffRef(0,1) = tmp - sin_axis.z();
00213   res.coeffRef(1,0) = tmp + sin_axis.z();
00214 
00215   tmp = cos1_axis.x() * m_axis.z();
00216   res.coeffRef(0,2) = tmp + sin_axis.y();
00217   res.coeffRef(2,0) = tmp - sin_axis.y();
00218 
00219   tmp = cos1_axis.y() * m_axis.z();
00220   res.coeffRef(1,2) = tmp - sin_axis.x();
00221   res.coeffRef(2,1) = tmp + sin_axis.x();
00222 
00223   res.diagonal() = (cos1_axis.cwise() * m_axis).cwise() + c;
00224 
00225   return res;
00226 }


libicr
Author(s): Robert Krug
autogenerated on Mon Jan 6 2014 11:32:28