Eigen  3.2.92
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 
89  Scalar angle() const { return m_angle; }
91  Scalar& angle() { return m_angle; }
92 
94  const Vector3& axis() const { return m_axis; }
99  Vector3& axis() { return m_axis; }
100 
102  inline QuaternionType operator* (const AngleAxis& other) const
103  { return QuaternionType(*this) * QuaternionType(other); }
104 
106  inline QuaternionType operator* (const QuaternionType& other) const
107  { return QuaternionType(*this) * other; }
108 
110  friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
111  { return a * QuaternionType(b); }
112 
115  { return AngleAxis(-m_angle, m_axis); }
116 
117  template<class QuatDerived>
118  AngleAxis& operator=(const QuaternionBase<QuatDerived>& q);
119  template<typename Derived>
120  AngleAxis& operator=(const MatrixBase<Derived>& m);
121 
122  template<typename Derived>
123  AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
124  Matrix3 toRotationMatrix(void) const;
125 
131  template<typename NewScalarType>
132  inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const
133  { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); }
134 
136  template<typename OtherScalarType>
137  inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other)
138  {
139  m_axis = other.axis().template cast<Scalar>();
140  m_angle = Scalar(other.angle());
141  }
142 
143  static inline const AngleAxis Identity() { return AngleAxis(Scalar(0), Vector3::UnitX()); }
144 
149  bool isApprox(const AngleAxis& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
150  { return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle,other.m_angle, prec); }
151 };
152 
159 
165 template<typename Scalar>
166 template<typename QuatDerived>
168 {
169  using std::atan2;
170  Scalar n = q.vec().norm();
172  n = q.vec().stableNorm();
173  if (n > Scalar(0))
174  {
175  m_angle = Scalar(2)*atan2(n, q.w());
176  m_axis = q.vec() / n;
177  }
178  else
179  {
180  m_angle = Scalar(0);
181  m_axis << Scalar(1), Scalar(0), Scalar(0);
182  }
183  return *this;
184 }
185 
188 template<typename Scalar>
189 template<typename Derived>
191 {
192  // Since a direct conversion would not be really faster,
193  // let's use the robust Quaternion implementation:
194  return *this = QuaternionType(mat);
195 }
196 
200 template<typename Scalar>
201 template<typename Derived>
203 {
204  return *this = QuaternionType(mat);
205 }
206 
209 template<typename Scalar>
212 {
213  using std::sin;
214  using std::cos;
215  Matrix3 res;
216  Vector3 sin_axis = sin(m_angle) * m_axis;
217  Scalar c = cos(m_angle);
218  Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
219 
220  Scalar tmp;
221  tmp = cos1_axis.x() * m_axis.y();
222  res.coeffRef(0,1) = tmp - sin_axis.z();
223  res.coeffRef(1,0) = tmp + sin_axis.z();
224 
225  tmp = cos1_axis.x() * m_axis.z();
226  res.coeffRef(0,2) = tmp + sin_axis.y();
227  res.coeffRef(2,0) = tmp - sin_axis.y();
228 
229  tmp = cos1_axis.y() * m_axis.z();
230  res.coeffRef(1,2) = tmp - sin_axis.x();
231  res.coeffRef(2,1) = tmp + sin_axis.x();
232 
233  res.diagonal() = (cos1_axis.cwiseProduct(m_axis)).array() + c;
234 
235  return res;
236 }
237 
238 } // end namespace Eigen
239 
240 #endif // EIGEN_ANGLEAXIS_H
AngleAxis(const Scalar &angle, const MatrixBase< Derived > &axis)
Definition: AngleAxis.h:79
Scalar angle() const
Definition: AngleAxis.h:89
Definition: LDLT.h:16
const VectorBlock< const Coefficients, 3 > vec() const
Definition: Quaternion.h:79
AngleAxis< double > AngleAxisd
Definition: AngleAxis.h:158
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:107
const Vector3 & axis() const
Definition: AngleAxis.h:94
bool isApprox(const AngleAxis &other, const typename NumTraits< Scalar >::Real &prec=NumTraits< Scalar >::dummy_precision()) const
Definition: AngleAxis.h:149
AngleAxis()
Definition: AngleAxis.h:72
internal::cast_return_type< AngleAxis, AngleAxis< NewScalarType > >::type cast() const
Definition: AngleAxis.h:132
AngleAxis inverse() const
Definition: AngleAxis.h:114
AngleAxis< float > AngleAxisf
Definition: AngleAxis.h:155
_Scalar Scalar
Definition: AngleAxis.h:59
Base class for quaternion expressions.
Definition: ForwardDeclarations.h:265
Vector3 & axis()
Definition: AngleAxis.h:99
Definition: Eigen_Colamd.h:54
Matrix3 toRotationMatrix(void) const
Definition: AngleAxis.h:211
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:137
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
Scalar & angle()
Definition: AngleAxis.h:91
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:267