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 <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>
109  AngleAxis& operator=(const QuaternionBase<QuatDerived>& q);
110  template<typename Derived>
111  AngleAxis& operator=(const MatrixBase<Derived>& m);
112 
113  template<typename Derived>
114  AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
115  Matrix3 toRotationMatrix(void) const;
116 
122  template<typename NewScalarType>
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
AngleAxis< float > AngleAxisf
USING_NAMESPACE_ACADO IntermediateState sin(const Expression &arg)
IntermediateState sqrt(const Expression &arg)
AngleAxis(const Scalar &angle, const MatrixBase< Derived > &axis)
static Matrix< Scalar, 2, 2 > toRotationMatrix(const Scalar &s)
AngleAxis & operator=(const QuaternionType &q)
AngleAxis & fromRotationMatrix(const MatrixBase< Derived > &m)
Scalar angle() const
Matrix< Scalar, 3, 3 > Matrix3
iterative scaling algorithm to equilibrate rows and column norms in matrices
Definition: matrix.hpp:471
const VectorBlock< const Coefficients, 3 > vec() const
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)
static const AngleAxis Identity()
const Vector3 & axis() const
IntermediateState cos(const Expression &arg)
bool isApprox(const AngleAxis &other, const typename NumTraits< Scalar >::Real &prec=NumTraits< Scalar >::dummy_precision()) const
AngleAxis< double > AngleAxisd
internal::cast_return_type< AngleAxis, AngleAxis< NewScalarType > >::type cast() const
IntermediateState acos(const Expression &arg)
AngleAxis inverse() const
EIGEN_STRONG_INLINE Scalar & coeffRef(Index rowId, Index colId)
bool isApprox(const Scalar &x, const Scalar &y, typename NumTraits< Scalar >::Real precision=NumTraits< Scalar >::dummy_precision())
Common base class for compact rotation representations.
Base class for quaternion expressions.
Matrix< Scalar, 3, 1 > Vector3
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)
The matrix class, also used for vectors and row-vectors.
Definition: Matrix.h:127
AngleAxis(const QuaternionBase< QuatDerived > &q)
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