lgsm: Rotations.h Source File

Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2009-2013 CEA LIST (DIASI/LSI) <xde-support@saxifrage.cea.fr>
00005 //
00006 // This Source Code Form is subject to the terms of the Mozilla
00007 // Public License v. 2.0. If a copy of the MPL was not distributed
00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00009 
00010 #ifndef EIGEN_LGSM_ROTATIONS_H
00011 #define EIGEN_LGSM_ROTATIONS_H
00012 
00013 template <typename QuatDerived, typename Derived>
00014 inline typename internal::traits<QuatDerived>::PlainObject projectRotation(
00015     const QuaternionBase<QuatDerived>& q,
00016     const MatrixBase<Derived>& n,
00017     typename internal::traits<Derived>::Scalar& theta)
00018 {
00019   typedef typename internal::traits<Derived>::Scalar Scalar;
00020   typedef typename internal::traits<QuatDerived>::PlainObject Quaternion;
00021   typedef Matrix<Scalar, 3, 3> AdjointMatrix;
00022 
00023   const AdjointMatrix& R = q.toRotationMatrix();
00024 
00025   //tr(R^t*[n])
00026   Scalar tr1 = (R(2,1)-R(1,2))*n[0] + (R(0,2)-R(2,0))*n[1] + (R(1,0)-R(0,1))*n[2];
00027   //tr(R^t*[n]^2) = tr(R^t*n*n^t) - tr(R^t) since n^2 = 1
00028   Scalar tr2 =  n[0] * (R(0,0)*n[0]+R(1,0)*n[1]+R(2,0)*n[2])
00029     + n[1] * (R(0,1)*n[0]+R(1,1)*n[1]+R(2,1)*n[2])
00030     + n[2] * (R(0,2)*n[0]+R(1,2)*n[1]+R(2,2)*n[2])
00031     - R(0,0) - R(1,1) - R(2,2);
00032 
00033   Scalar alpha = atan2(tr1, tr2);
00034   theta = M_PI - alpha;
00035   Scalar s = sin(theta);
00036   Scalar c = cos(theta);
00037   Scalar ic = 1-c;
00038 
00039   AdjointMatrix Rp;
00040   Rp(0,0) =          + ic*n[0]*n[0] + c;
00041   Rp(0,1) = -s*n[2] + ic*n[0]*n[1];
00042   Rp(0,2) =  s*n[1] + ic*n[0]*n[2];
00043   Rp(1,0) =  s*n[2] + ic*n[1]*n[0];
00044   Rp(1,1) =          + ic*n[1]*n[1] + c;
00045   Rp(1,2) = -s*n[0] + ic*n[1]*n[2];
00046   Rp(2,0) = -s*n[1] + ic*n[2]*n[0];
00047   Rp(2,1) =  s*n[0] + ic*n[2]*n[1];
00048   Rp(2,2) =          + ic*n[2]*n[2] + c;
00049 
00050   return Quaternion(Rp);
00051 }
00052 
00053 template <typename QuatDerived, typename Derived>
00054 inline typename internal::traits<QuatDerived>::PlainObject projectRotation(
00055     const QuaternionBase<QuatDerived>& q,
00056     const MatrixBase<Derived>& v)
00057 {
00058   typename internal::traits<Derived>::Scalar tmp;
00059   return projectRotation(q, v, tmp);
00060 }
00061 
00062   template <typename QuatDerived, typename Derived>
00063 inline bool isRotationAround(const QuaternionBase<QuatDerived>& q, const MatrixBase<Derived>& v)
00064 {
00065   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
00066   typedef typename QuaternionBase<QuatDerived>::Scalar Scalar;
00067   AngleAxis<Scalar> aa(q);
00068   return aa.axis().isApprox(v) || aa.axis().isApprox(-v) || aa.angle() == 0;  //if the rotation is very small, eigen put axis to (1,0,0) and angle to 0
00069 }
00070 
00071   template <typename QuatDerived, typename Derived>
00072 inline typename QuaternionBase<QuatDerived>::Scalar getAngleAround(const QuaternionBase<QuatDerived>& q, const MatrixBase<Derived>& v)
00073 {
00074   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
00075   assert(isRotationAround(q,v));
00076   typedef typename QuaternionBase<QuatDerived>::Scalar Scalar;
00077   AngleAxis<Scalar> aa(q);
00078   return aa.angle()*(aa.axis().dot(v));
00079 }
00080 
00081 #endif