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(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 
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  Scalar n2 = q.vec().squaredNorm();
166  {
167  m_angle = 0;
168  m_axis << 1, 0, 0;
169  }
170  else
171  {
172  m_angle = Scalar(2)*acos((min)((max)(Scalar(-1),q.w()),Scalar(1)));
173  m_axis = q.vec() / internal::sqrt(n2);
174  }
175  return *this;
176 }
177 
180 template<typename Scalar>
181 template<typename Derived>
183 {
184  // Since a direct conversion would not be really faster,
185  // let's use the robust Quaternion implementation:
186  return *this = QuaternionType(mat);
187 }
188 
192 template<typename Scalar>
193 template<typename Derived>
195 {
196  return *this = QuaternionType(mat);
197 }
198 
201 template<typename Scalar>
204 {
205  Matrix3 res;
206  Vector3 sin_axis = internal::sin(m_angle) * m_axis;
207  Scalar c = internal::cos(m_angle);
208  Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
209 
210  Scalar tmp;
211  tmp = cos1_axis.x() * m_axis.y();
212  res.coeffRef(0,1) = tmp - sin_axis.z();
213  res.coeffRef(1,0) = tmp + sin_axis.z();
214 
215  tmp = cos1_axis.x() * m_axis.z();
216  res.coeffRef(0,2) = tmp + sin_axis.y();
217  res.coeffRef(2,0) = tmp - sin_axis.y();
218 
219  tmp = cos1_axis.y() * m_axis.z();
220  res.coeffRef(1,2) = tmp - sin_axis.x();
221  res.coeffRef(2,1) = tmp + sin_axis.x();
222 
223  res.diagonal() = (cos1_axis.cwiseProduct(m_axis)).array() + c;
224 
225  return res;
226 }
227 
228 } // end namespace Eigen
229 
230 #endif // EIGEN_ANGLEAXIS_H