Eigen  3.2.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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 {
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) {}
81  template<typename QuatDerived> inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; }
83  template<typename Derived>
84  inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
85 
86  Scalar angle() const { return m_angle; }
87  Scalar& angle() { return m_angle; }
88 
89  const Vector3& axis() const { return m_axis; }
90  Vector3& axis() { return m_axis; }
91 
93  inline QuaternionType operator* (const AngleAxis& other) const
94  { return QuaternionType(*this) * QuaternionType(other); }
95 
97  inline QuaternionType operator* (const QuaternionType& other) const
98  { return QuaternionType(*this) * other; }
99 
101  friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
102  { return a * QuaternionType(b); }
103 
106  { return AngleAxis(-m_angle, m_axis); }
107 
108  template<class QuatDerived>
110  template<typename Derived>
112 
113  template<typename Derived>
115  Matrix3 toRotationMatrix(void) const;
116 
122  template<typename NewScalarType>
123  inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const
124  { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); }
125 
127  template<typename OtherScalarType>
128  inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other)
129  {
130  m_axis = other.axis().template cast<Scalar>();
131  m_angle = Scalar(other.angle());
132  }
133 
134  static inline const AngleAxis Identity() { return AngleAxis(0, Vector3::UnitX()); }
135 
140  bool isApprox(const AngleAxis& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
141  { return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle,other.m_angle, prec); }
142 };
143 
150 
157 template<typename Scalar>
158 template<typename QuatDerived>
160 {
161  using std::acos;
162  using std::min;
163  using std::max;
164  using std::sqrt;
165  Scalar n2 = q.vec().squaredNorm();
167  {
168  m_angle = 0;
169  m_axis << 1, 0, 0;
170  }
171  else
172  {
173  m_angle = Scalar(2)*acos((min)((max)(Scalar(-1),q.w()),Scalar(1)));
174  m_axis = q.vec() / sqrt(n2);
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