Go to the documentation of this file.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
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054 namespace internal {
00055 template<typename _Scalar> struct traits<AngleAxis<_Scalar> >
00056 {
00057 typedef _Scalar Scalar;
00058 };
00059 }
00060
00061 template<typename _Scalar>
00062 class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3>
00063 {
00064 typedef RotationBase<AngleAxis<_Scalar>,3> Base;
00065
00066 public:
00067
00068 using Base::operator*;
00069
00070 enum { Dim = 3 };
00071
00072 typedef _Scalar Scalar;
00073 typedef Matrix<Scalar,3,3> Matrix3;
00074 typedef Matrix<Scalar,3,1> Vector3;
00075 typedef Quaternion<Scalar> QuaternionType;
00076
00077 protected:
00078
00079 Vector3 m_axis;
00080 Scalar m_angle;
00081
00082 public:
00083
00084
00085 AngleAxis() {}
00086
00087
00088
00089
00090
00091 template<typename Derived>
00092 inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
00093
00094 template<typename QuatDerived> inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; }
00095
00096 template<typename Derived>
00097 inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
00098
00099 Scalar angle() const { return m_angle; }
00100 Scalar& angle() { return m_angle; }
00101
00102 const Vector3& axis() const { return m_axis; }
00103 Vector3& axis() { return m_axis; }
00104
00105
00106 inline QuaternionType operator* (const AngleAxis& other) const
00107 { return QuaternionType(*this) * QuaternionType(other); }
00108
00109
00110 inline QuaternionType operator* (const QuaternionType& other) const
00111 { return QuaternionType(*this) * other; }
00112
00113
00114 friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
00115 { return a * QuaternionType(b); }
00116
00117
00118 AngleAxis inverse() const
00119 { return AngleAxis(-m_angle, m_axis); }
00120
00121 template<class QuatDerived>
00122 AngleAxis& operator=(const QuaternionBase<QuatDerived>& q);
00123 template<typename Derived>
00124 AngleAxis& operator=(const MatrixBase<Derived>& m);
00125
00126 template<typename Derived>
00127 AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
00128 Matrix3 toRotationMatrix(void) const;
00129
00130
00131
00132
00133
00134
00135 template<typename NewScalarType>
00136 inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const
00137 { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); }
00138
00139
00140 template<typename OtherScalarType>
00141 inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other)
00142 {
00143 m_axis = other.axis().template cast<Scalar>();
00144 m_angle = Scalar(other.angle());
00145 }
00146
00147 inline static const AngleAxis Identity() { return AngleAxis(0, Vector3::UnitX()); }
00148
00149
00150
00151
00152
00153 bool isApprox(const AngleAxis& other, typename NumTraits<Scalar>::Real prec = NumTraits<Scalar>::dummy_precision()) const
00154 { return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle,other.m_angle, prec); }
00155 };
00156
00157
00158
00159 typedef AngleAxis<float> AngleAxisf;
00160
00161
00162 typedef AngleAxis<double> AngleAxisd;
00163
00164
00165
00166
00167
00168
00169
00170 template<typename Scalar>
00171 template<typename QuatDerived>
00172 AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q)
00173 {
00174 Scalar n2 = q.vec().squaredNorm();
00175 if (n2 < NumTraits<Scalar>::dummy_precision()*NumTraits<Scalar>::dummy_precision())
00176 {
00177 m_angle = 0;
00178 m_axis << 1, 0, 0;
00179 }
00180 else
00181 {
00182 m_angle = Scalar(2)*std::acos(std::min(std::max(Scalar(-1),q.w()),Scalar(1)));
00183 m_axis = q.vec() / internal::sqrt(n2);
00184 }
00185 return *this;
00186 }
00187
00188
00189
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
00199
00200
00201
00202 template<typename Scalar>
00203 template<typename Derived>
00204 AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat)
00205 {
00206 return *this = QuaternionType(mat);
00207 }
00208
00209
00210
00211 template<typename Scalar>
00212 typename AngleAxis<Scalar>::Matrix3
00213 AngleAxis<Scalar>::toRotationMatrix(void) const
00214 {
00215 Matrix3 res;
00216 Vector3 sin_axis = internal::sin(m_angle) * m_axis;
00217 Scalar c = internal::cos(m_angle);
00218 Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
00219
00220 Scalar tmp;
00221 tmp = cos1_axis.x() * m_axis.y();
00222 res.coeffRef(0,1) = tmp - sin_axis.z();
00223 res.coeffRef(1,0) = tmp + sin_axis.z();
00224
00225 tmp = cos1_axis.x() * m_axis.z();
00226 res.coeffRef(0,2) = tmp + sin_axis.y();
00227 res.coeffRef(2,0) = tmp - sin_axis.y();
00228
00229 tmp = cos1_axis.y() * m_axis.z();
00230 res.coeffRef(1,2) = tmp - sin_axis.x();
00231 res.coeffRef(2,1) = tmp + sin_axis.x();
00232
00233 res.diagonal() = (cos1_axis.cwiseProduct(m_axis)).array() + c;
00234
00235 return res;
00236 }
00237
00238 #endif // EIGEN_ANGLEAXIS_H