Eigen  3.2.91
AngleAxis.h
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
5 //
6 // This Source Code Form is subject to the terms of the Mozilla
7 // Public License v. 2.0. If a copy of the MPL was not distributed
8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9 
10 #ifndef EIGEN_ANGLEAXIS_H
11 #define EIGEN_ANGLEAXIS_H
12 
13 namespace Eigen {
14 
41 namespace internal {
42 template<typename _Scalar> struct traits<AngleAxis<_Scalar> >
43 {
44  typedef _Scalar Scalar;
45 };
46 }
47 
48 template<typename _Scalar>
49 class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3>
50 {
51  typedef RotationBase<AngleAxis<_Scalar>,3> Base;
52 
53 public:
54 
55  using Base::operator*;
56 
57  enum { Dim = 3 };
59  typedef _Scalar Scalar;
63 
64 protected:
65 
66  Vector3 m_axis;
67  Scalar m_angle;
68 
69 public:
70 
72  AngleAxis() {}
78  template<typename Derived>
79  inline AngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
83  template<typename QuatDerived> inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; }
85  template<typename Derived>
86  inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
87 
88  Scalar angle() const { return m_angle; }
89  Scalar& angle() { return m_angle; }
90 
91  const Vector3& axis() const { return m_axis; }
92  Vector3& axis() { return m_axis; }
93 
95  inline QuaternionType operator* (const AngleAxis& other) const
96  { return QuaternionType(*this) * QuaternionType(other); }
97 
99  inline QuaternionType operator* (const QuaternionType& other) const
100  { return QuaternionType(*this) * other; }
101 
103  friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
104  { return a * QuaternionType(b); }
105 
108  { return AngleAxis(-m_angle, m_axis); }
109 
110  template<class QuatDerived>
111  AngleAxis& operator=(const QuaternionBase<QuatDerived>& q);
112  template<typename Derived>
113  AngleAxis& operator=(const MatrixBase<Derived>& m);
114 
115  template<typename Derived>
116  AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
117  Matrix3 toRotationMatrix(void) const;
118 
124  template<typename NewScalarType>
125  inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const
126  { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); }
127 
129  template<typename OtherScalarType>
130  inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other)
131  {
132  m_axis = other.axis().template cast<Scalar>();
133  m_angle = Scalar(other.angle());
134  }
135 
136  static inline const AngleAxis Identity() { return AngleAxis(0, Vector3::UnitX()); }
137 
142  bool isApprox(const AngleAxis& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
143  { return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle,other.m_angle, prec); }
144 };
145 
152 
158 template<typename Scalar>
159 template<typename QuatDerived>
161 {
162  using std::atan2;
163  Scalar n = q.vec().norm();
165  n = q.vec().stableNorm();
166  if (n > Scalar(0))
167  {
168  m_angle = Scalar(2)*atan2(n, q.w());
169  m_axis = q.vec() / n;
170  }
171  else
172  {
173  m_angle = 0;
174  m_axis << 1, 0, 0;
175  }
176  return *this;
177 }
178 
181 template<typename Scalar>
182 template<typename Derived>
184 {
185  // Since a direct conversion would not be really faster,
186  // let's use the robust Quaternion implementation:
187  return *this = QuaternionType(mat);
188 }
189 
193 template<typename Scalar>
194 template<typename Derived>
196 {
197  return *this = QuaternionType(mat);
198 }
199 
202 template<typename Scalar>
205 {
206  using std::sin;
207  using std::cos;
208  Matrix3 res;
209  Vector3 sin_axis = sin(m_angle) * m_axis;
210  Scalar c = cos(m_angle);
211  Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
212 
213  Scalar tmp;
214  tmp = cos1_axis.x() * m_axis.y();
215  res.coeffRef(0,1) = tmp - sin_axis.z();
216  res.coeffRef(1,0) = tmp + sin_axis.z();
217 
218  tmp = cos1_axis.x() * m_axis.z();
219  res.coeffRef(0,2) = tmp + sin_axis.y();
220  res.coeffRef(2,0) = tmp - sin_axis.y();
221 
222  tmp = cos1_axis.y() * m_axis.z();
223  res.coeffRef(1,2) = tmp - sin_axis.x();
224  res.coeffRef(2,1) = tmp + sin_axis.x();
225 
226  res.diagonal() = (cos1_axis.cwiseProduct(m_axis)).array() + c;
227 
228  return res;
229 }
230 
231 } // end namespace Eigen
232 
233 #endif // EIGEN_ANGLEAXIS_H
AngleAxis(const Scalar &angle, const MatrixBase< Derived > &axis)
Definition: AngleAxis.h:79
Definition: LDLT.h:16
const VectorBlock< const Coefficients, 3 > vec() const
Definition: Quaternion.h:79
AngleAxis< double > AngleAxisd
Definition: AngleAxis.h:151
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:107
QuaternionType operator*(const AngleAxis &other) const
Definition: AngleAxis.h:95
bool isApprox(const AngleAxis &other, const typename NumTraits< Scalar >::Real &prec=NumTraits< Scalar >::dummy_precision()) const
Definition: AngleAxis.h:142
AngleAxis()
Definition: AngleAxis.h:72
internal::cast_return_type< AngleAxis, AngleAxis< NewScalarType > >::type cast() const
Definition: AngleAxis.h:125
AngleAxis inverse() const
Definition: AngleAxis.h:107
AngleAxis< float > AngleAxisf
Definition: AngleAxis.h:148
_Scalar Scalar
Definition: AngleAxis.h:59
Base class for quaternion expressions.
Definition: ForwardDeclarations.h:264
Definition: Eigen_Colamd.h:54
Matrix3 toRotationMatrix(void) const
Definition: AngleAxis.h:204
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:270
Scalar w() const
Definition: Quaternion.h:67
AngleAxis(const AngleAxis< OtherScalarType > &other)
Definition: AngleAxis.h:130
DiagonalReturnType diagonal()
Definition: Diagonal.h:188
The matrix class, also used for vectors and row-vectors.
Definition: Matrix.h:178
AngleAxis(const QuaternionBase< QuatDerived > &q)
Definition: AngleAxis.h:83
AngleAxis(const MatrixBase< Derived > &m)
Definition: AngleAxis.h:86
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
Represents a 3D rotation as a rotation angle around an arbitrary 3D axis.
Definition: ForwardDeclarations.h:266