00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef EIGEN_ANGLEAXIS_H
00026 #define EIGEN_ANGLEAXIS_H
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 ei_cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const
00143 { return typename ei_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
00195
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 }
00227
00228 #endif // EIGEN_ANGLEAXIS_H