1c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This file is part of Eigen, a lightweight C++ template library 2c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// for linear algebra. Eigen itself is part of the KDE project. 3c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 4c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr> 5c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 6c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This Source Code Form is subject to the terms of the Mozilla 7c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Public License v. 2.0. If a copy of the MPL was not distributed 8c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 10c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway 11c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 12c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace Eigen { 13c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 14c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \geometry_module \ingroup Geometry_Module 15c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 16c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \class AngleAxis 17c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 18c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \brief Represents a 3D rotation as a rotation angle around an arbitrary 3D axis 19c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 20c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param _Scalar the scalar type, i.e., the type of the coefficients. 21c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 22c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * The following two typedefs are provided for convenience: 23c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \li \c AngleAxisf for \c float 24c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \li \c AngleAxisd for \c double 25c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 26c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \addexample AngleAxisForEuler \label How to define a rotation from Euler-angles 27c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 28c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * Combined with MatrixBase::Unit{X,Y,Z}, AngleAxis can be used to easily 29c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * mimic Euler-angles. Here is an example: 30c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \include AngleAxis_mimic_euler.cpp 31c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * Output: \verbinclude AngleAxis_mimic_euler.out 32c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 33c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \note This class is not aimed to be used to store a rotation transformation, 34c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * but rather to make easier the creation of other rotation (Quaternion, rotation Matrix) 35c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * and transformation objects. 36c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 37c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa class Quaternion, class Transform, MatrixBase::UnitX() 38c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 39c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 40c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename _Scalar> struct ei_traits<AngleAxis<_Scalar> > 41c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 42c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef _Scalar Scalar; 43c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 44c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 45c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename _Scalar> 46c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathclass AngleAxis : public RotationBase<AngleAxis<_Scalar>,3> 47c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 48c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef RotationBase<AngleAxis<_Scalar>,3> Base; 49c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 50c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathpublic: 51c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 52c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath using Base::operator*; 53c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 54c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath enum { Dim = 3 }; 55c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** the scalar type of the coefficients */ 56c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef _Scalar Scalar; 57c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,3,3> Matrix3; 58c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,3,1> Vector3; 59c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Quaternion<Scalar> QuaternionType; 60c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 61c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathprotected: 62c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 63c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Vector3 m_axis; 64c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar m_angle; 65c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 66c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathpublic: 67c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 68c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Default constructor without initialization. */ 69c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath AngleAxis() {} 70c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Constructs and initialize the angle-axis rotation from an \a angle in radian 71c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * and an \a axis which must be normalized. */ 72c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename Derived> 73c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {} 74c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Constructs and initialize the angle-axis rotation from a quaternion \a q. */ 75c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline AngleAxis(const QuaternionType& q) { *this = q; } 76c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */ 77c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename Derived> 78c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; } 79c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 80c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar angle() const { return m_angle; } 81c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar& angle() { return m_angle; } 82c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 83c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath const Vector3& axis() const { return m_axis; } 84c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Vector3& axis() { return m_axis; } 85c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 86c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Concatenates two rotations */ 87c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline QuaternionType operator* (const AngleAxis& other) const 88c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return QuaternionType(*this) * QuaternionType(other); } 89c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 90c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Concatenates two rotations */ 91c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline QuaternionType operator* (const QuaternionType& other) const 92c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return QuaternionType(*this) * other; } 93c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 94c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Concatenates two rotations */ 95c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b) 96c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return a * QuaternionType(b); } 97c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 98c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Concatenates two rotations */ 99c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Matrix3 operator* (const Matrix3& other) const 100c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return toRotationMatrix() * other; } 101c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 102c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Concatenates two rotations */ 103c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline friend Matrix3 operator* (const Matrix3& a, const AngleAxis& b) 104c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return a * b.toRotationMatrix(); } 105c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 106c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Applies rotation to vector */ 107c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Vector3 operator* (const Vector3& other) const 108c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return toRotationMatrix() * other; } 109c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 110c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */ 111c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath AngleAxis inverse() const 112c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return AngleAxis(-m_angle, m_axis); } 113c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 114c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath AngleAxis& operator=(const QuaternionType& q); 115c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename Derived> 116c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath AngleAxis& operator=(const MatrixBase<Derived>& m); 117c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 118c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename Derived> 119c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m); 120c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix3 toRotationMatrix(void) const; 121c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 122c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns \c *this with scalar type casted to \a NewScalarType 123c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 124c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * Note that if \a NewScalarType is equal to the current scalar type of \c *this 125c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * then this function smartly returns a const reference to \c *this. 126c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 127c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename NewScalarType> 128c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const 129c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); } 130c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 131c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Copy constructor with scalar type conversion */ 132c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename OtherScalarType> 133c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other) 134c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 135c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath m_axis = other.axis().template cast<Scalar>(); 136c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath m_angle = Scalar(other.angle()); 137c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 138c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 139c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns \c true if \c *this is approximately equal to \a other, within the precision 140c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * determined by \a prec. 141c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 142c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa MatrixBase::isApprox() */ 143c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath bool isApprox(const AngleAxis& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const 144c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return m_axis.isApprox(other.m_axis, prec) && ei_isApprox(m_angle,other.m_angle, prec); } 145c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 146c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 147c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \ingroup Geometry_Module 148c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * single precision angle-axis type */ 149c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtypedef AngleAxis<float> AngleAxisf; 150c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \ingroup Geometry_Module 151c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * double precision angle-axis type */ 152c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtypedef AngleAxis<double> AngleAxisd; 153c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 154c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** Set \c *this from a quaternion. 155c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * The axis is normalized. 156c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 157c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Scalar> 158c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathAngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionType& q) 159c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 160c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar n2 = q.vec().squaredNorm(); 161c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath if (n2 < precision<Scalar>()*precision<Scalar>()) 162c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 163c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath m_angle = 0; 164c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath m_axis << 1, 0, 0; 165c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 166c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath else 167c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 168c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath m_angle = 2*std::acos(q.w()); 169c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath m_axis = q.vec() / ei_sqrt(n2); 170c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 171c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return *this; 172c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 173c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 174c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** Set \c *this from a 3x3 rotation matrix \a mat. 175c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 176c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Scalar> 177c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Derived> 178c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathAngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat) 179c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 180c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // Since a direct conversion would not be really faster, 181c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath // let's use the robust Quaternion implementation: 182c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return *this = QuaternionType(mat); 183c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 184c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 185c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** Constructs and \returns an equivalent 3x3 rotation matrix. 186c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 187c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Scalar> 188c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtypename AngleAxis<Scalar>::Matrix3 189c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathAngleAxis<Scalar>::toRotationMatrix(void) const 190c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 191c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix3 res; 192c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Vector3 sin_axis = ei_sin(m_angle) * m_axis; 193c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar c = ei_cos(m_angle); 194c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Vector3 cos1_axis = (Scalar(1)-c) * m_axis; 195c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 196c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar tmp; 197c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath tmp = cos1_axis.x() * m_axis.y(); 198c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res.coeffRef(0,1) = tmp - sin_axis.z(); 199c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res.coeffRef(1,0) = tmp + sin_axis.z(); 200c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 201c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath tmp = cos1_axis.x() * m_axis.z(); 202c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res.coeffRef(0,2) = tmp + sin_axis.y(); 203c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res.coeffRef(2,0) = tmp - sin_axis.y(); 204c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 205c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath tmp = cos1_axis.y() * m_axis.z(); 206c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res.coeffRef(1,2) = tmp - sin_axis.x(); 207c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res.coeffRef(2,1) = tmp + sin_axis.x(); 208c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 209c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath res.diagonal() = (cos1_axis.cwise() * m_axis).cwise() + c; 210c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 211c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return res; 212c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 213c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 214c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} // end namespace Eigen 215