Eigen2Support/Geometry/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 <g.gael@free.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 // no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway
11 
12 namespace Eigen {
13 
40 template<typename _Scalar> struct ei_traits<AngleAxis<_Scalar> >
41 {
42  typedef _Scalar Scalar;
43 };
44 
45 template<typename _Scalar>
46 class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3>
47 {
49 
50 public:
51 
52  using Base::operator*;
53 
54  enum { Dim = 3 };
56  typedef _Scalar Scalar;
60 
61 protected:
62 
63  Vector3 m_axis;
64  Scalar m_angle;
65 
66 public:
67 
69  AngleAxis() {}
72  template<typename Derived>
73  inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
75  inline AngleAxis(const QuaternionType& q) { *this = q; }
77  template<typename Derived>
78  inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
79 
80  Scalar angle() const { return m_angle; }
81  Scalar& angle() { return m_angle; }
82 
83  const Vector3& axis() const { return m_axis; }
84  Vector3& axis() { return m_axis; }
85 
87  inline QuaternionType operator* (const AngleAxis& other) const
88  { return QuaternionType(*this) * QuaternionType(other); }
89 
91  inline QuaternionType operator* (const QuaternionType& other) const
92  { return QuaternionType(*this) * other; }
93 
95  friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
96  { return a * QuaternionType(b); }
97 
99  inline Matrix3 operator* (const Matrix3& other) const
100  { return toRotationMatrix() * other; }
101 
103  inline friend Matrix3 operator* (const Matrix3& a, const AngleAxis& b)
104  { return a * b.toRotationMatrix(); }
105 
107  inline Vector3 operator* (const Vector3& other) const
108  { return toRotationMatrix() * other; }
109 
112  { return AngleAxis(-m_angle, m_axis); }
113 
114  AngleAxis& operator=(const QuaternionType& q);
115  template<typename Derived>
116  AngleAxis& operator=(const MatrixBase<Derived>& m);
117 
118  template<typename Derived>
119  AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
120  Matrix3 toRotationMatrix(void) const;
121 
127  template<typename NewScalarType>
129  { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); }
130 
132  template<typename OtherScalarType>
133  inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other)
134  {
135  m_axis = other.axis().template cast<Scalar>();
136  m_angle = Scalar(other.angle());
137  }
138 
143  bool isApprox(const AngleAxis& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
144  { return m_axis.isApprox(other.m_axis, prec) && ei_isApprox(m_angle,other.m_angle, prec); }
145 };
146 
153 
157 template<typename Scalar>
159 {
160  Scalar n2 = q.vec().squaredNorm();
161  if (n2 < precision<Scalar>()*precision<Scalar>())
162  {
163  m_angle = 0;
164  m_axis << 1, 0, 0;
165  }
166  else
167  {
168  m_angle = 2*std::acos(q.w());
169  m_axis = q.vec() / ei_sqrt(n2);
170  }
171  return *this;
172 }
173 
176 template<typename Scalar>
177 template<typename Derived>
179 {
180  // Since a direct conversion would not be really faster,
181  // let's use the robust Quaternion implementation:
182  return *this = QuaternionType(mat);
183 }
184 
187 template<typename Scalar>
190 {
191  Matrix3 res;
192  Vector3 sin_axis = ei_sin(m_angle) * m_axis;
193  Scalar c = ei_cos(m_angle);
194  Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
195 
196  Scalar tmp;
197  tmp = cos1_axis.x() * m_axis.y();
198  res.coeffRef(0,1) = tmp - sin_axis.z();
199  res.coeffRef(1,0) = tmp + sin_axis.z();
200 
201  tmp = cos1_axis.x() * m_axis.z();
202  res.coeffRef(0,2) = tmp + sin_axis.y();
203  res.coeffRef(2,0) = tmp - sin_axis.y();
204 
205  tmp = cos1_axis.y() * m_axis.z();
206  res.coeffRef(1,2) = tmp - sin_axis.x();
207  res.coeffRef(2,1) = tmp + sin_axis.x();
208 
209  res.diagonal() = (cos1_axis.cwise() * m_axis).cwise() + c;
210 
211  return res;
212 }
213 
214 } // end namespace Eigen
AngleAxis< float > AngleAxisf
static Matrix< Scalar, 2, 2 > toRotationMatrix(const Scalar &s)
AngleAxis & operator=(const QuaternionType &q)
Matrix< Scalar, 3, 3 > Matrix3
iterative scaling algorithm to equilibrate rows and column norms in matrices
Definition: matrix.hpp:471
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:88
const internal::permut_matrix_product_retval< PermutationDerived, Derived, OnTheRight > operator*(const MatrixBase< Derived > &matrix, const PermutationBase< PermutationDerived > &permutation)
AngleAxis(const QuaternionType &q)
const Vector3 & axis() const
AngleAxis(Scalar angle, const MatrixBase< Derived > &axis)
AngleAxis< double > AngleAxisd
internal::cast_return_type< AngleAxis, AngleAxis< NewScalarType > >::type cast() const
IntermediateState acos(const Expression &arg)
EIGEN_STRONG_INLINE Scalar & coeffRef(Index rowId, Index colId)
Common base class for compact rotation representations.
Matrix< Scalar, 3, 1 > Vector3
bool ei_isApprox(const Scalar &x, const Scalar &y, typename NumTraits< Scalar >::Real precision=NumTraits< Scalar >::dummy_precision())
Matrix3 toRotationMatrix(void) const
#define AngleAxis
Definition: All.h:33
The quaternion class used to represent 3D orientations and rotations.
Quaternion< Scalar > QuaternionType
AngleAxis(const AngleAxis< OtherScalarType > &other)
bool isApprox(const AngleAxis &other, typename NumTraits< Scalar >::Real prec=precision< Scalar >()) const
The matrix class, also used for vectors and row-vectors.
Definition: Matrix.h:127
const Block< const Coefficients, 3, 1 > vec() const
AngleAxis(const MatrixBase< Derived > &m)
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.
RotationBase< AngleAxis< _Scalar >, 3 > Base


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:27