AngleAxis.h
Go to the documentation of this file.
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 // Eigen is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation; either
9 // version 3 of the License, or (at your option) any later version.
10 //
11 // Alternatively, you can redistribute it and/or
12 // modify it under the terms of the GNU General Public License as
13 // published by the Free Software Foundation; either version 2 of
14 // the License, or (at your option) any later version.
15 //
16 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
17 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
18 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
19 // GNU General Public License for more details.
20 //
21 // You should have received a copy of the GNU Lesser General Public
22 // License and a copy of the GNU General Public License along with
23 // Eigen. If not, see <http://www.gnu.org/licenses/>.
24 
25 #ifndef EIGEN_ANGLEAXIS_H
26 #define EIGEN_ANGLEAXIS_H
27 
28 namespace Eigen {
29 
56 namespace internal {
57 template<typename _Scalar> struct traits<AngleAxis<_Scalar> >
58 {
59  typedef _Scalar Scalar;
60 };
61 }
62 
63 template<typename _Scalar>
64 class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3>
65 {
67 
68 public:
69 
70  using Base::operator*;
71 
72  enum { Dim = 3 };
74  typedef _Scalar Scalar;
78 
79 protected:
80 
83 
84 public:
85 
87  AngleAxis() {}
93  template<typename Derived>
94  inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
96  template<typename QuatDerived> inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; }
98  template<typename Derived>
99  inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
100 
101  Scalar angle() const { return m_angle; }
102  Scalar& angle() { return m_angle; }
103 
104  const Vector3& axis() const { return m_axis; }
105  Vector3& axis() { return m_axis; }
106 
108  inline QuaternionType operator* (const AngleAxis& other) const
109  { return QuaternionType(*this) * QuaternionType(other); }
110 
112  inline QuaternionType operator* (const QuaternionType& other) const
113  { return QuaternionType(*this) * other; }
114 
116  friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
117  { return a * QuaternionType(b); }
118 
121  { return AngleAxis(-m_angle, m_axis); }
122 
123  template<class QuatDerived>
125  template<typename Derived>
127 
128  template<typename Derived>
130  Matrix3 toRotationMatrix(void) const;
131 
137  template<typename NewScalarType>
138  inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const
139  { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); }
140 
142  template<typename OtherScalarType>
143  inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other)
144  {
145  m_axis = other.axis().template cast<Scalar>();
146  m_angle = Scalar(other.angle());
147  }
148 
149  static inline const AngleAxis Identity() { return AngleAxis(0, Vector3::UnitX()); }
150 
156  { return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle,other.m_angle, prec); }
157 };
158 
165 
172 template<typename Scalar>
173 template<typename QuatDerived>
175 {
176  using std::acos;
177  using std::min;
178  using std::max;
179  Scalar n2 = q.vec().squaredNorm();
181  {
182  m_angle = 0;
183  m_axis << 1, 0, 0;
184  }
185  else
186  {
187  m_angle = Scalar(2)*acos((min)((max)(Scalar(-1),q.w()),Scalar(1)));
188  m_axis = q.vec() / internal::sqrt(n2);
189  }
190  return *this;
191 }
192 
195 template<typename Scalar>
196 template<typename Derived>
198 {
199  // Since a direct conversion would not be really faster,
200  // let's use the robust Quaternion implementation:
201  return *this = QuaternionType(mat);
202 }
203 
207 template<typename Scalar>
208 template<typename Derived>
210 {
211  return *this = QuaternionType(mat);
212 }
213 
216 template<typename Scalar>
219 {
220  Matrix3 res;
221  Vector3 sin_axis = internal::sin(m_angle) * m_axis;
222  Scalar c = internal::cos(m_angle);
223  Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
224 
225  Scalar tmp;
226  tmp = cos1_axis.x() * m_axis.y();
227  res.coeffRef(0,1) = tmp - sin_axis.z();
228  res.coeffRef(1,0) = tmp + sin_axis.z();
229 
230  tmp = cos1_axis.x() * m_axis.z();
231  res.coeffRef(0,2) = tmp + sin_axis.y();
232  res.coeffRef(2,0) = tmp - sin_axis.y();
233 
234  tmp = cos1_axis.y() * m_axis.z();
235  res.coeffRef(1,2) = tmp - sin_axis.x();
236  res.coeffRef(2,1) = tmp + sin_axis.x();
237 
238  res.diagonal() = (cos1_axis.cwiseProduct(m_axis)).array() + c;
239 
240  return res;
241 }
242 
243 } // end namespace Eigen
244 
245 #endif // EIGEN_ANGLEAXIS_H