5 #ifndef __pinocchio_math_rotation_hpp__
6 #define __pinocchio_math_rotation_hpp__
8 #include "pinocchio/fwd.hpp"
9 #include "pinocchio/math/matrix.hpp"
12 #include <Eigen/Geometry>
23 template<
typename Vector3,
typename Scalar,
typename Matrix3>
25 const Scalar & cos_value,
const Scalar & sin_value,
26 const Eigen::MatrixBase<Matrix3> & res)
28 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
29 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
31 assert(
isUnitary(axis) &&
"The axis is not unitary.");
33 Matrix3 & res_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,res);
34 Vector3 sin_axis = sin_value * axis;
35 Vector3 cos1_axis = (Scalar(1)-cos_value) * axis;
38 tmp = cos1_axis.x() * axis.y();
39 res_.coeffRef(0,1) = tmp - sin_axis.z();
40 res_.coeffRef(1,0) = tmp + sin_axis.z();
42 tmp = cos1_axis.x() * axis.z();
43 res_.coeffRef(0,2) = tmp + sin_axis.y();
44 res_.coeffRef(2,0) = tmp - sin_axis.y();
46 tmp = cos1_axis.y() * axis.z();
47 res_.coeffRef(1,2) = tmp - sin_axis.x();
48 res_.coeffRef(2,1) = tmp + sin_axis.x();
50 res_.diagonal() = (cos1_axis.cwiseProduct(axis)).array() + cos_value;
58 template<
typename Matrix3>
61 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
62 Matrix3 & rot_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,rot);
64 typedef typename Matrix3::Scalar Scalar;
65 enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Options };
66 typedef Eigen::Quaternion<Scalar,Options> Quaternion;
67 Quaternion quat(rot); quat.normalize();
68 rot_ = quat.toRotationMatrix();
78 template<
typename Matrix3>
79 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)
82 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
83 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3) ReturnType;
85 typedef Eigen::JacobiSVD<Matrix3> SVD;
86 const SVD svd(mat,Eigen::ComputeFullU | Eigen::ComputeFullV);
89 res.template leftCols<2>().noalias() = svd.matrixU() * svd.matrixV().transpose().template leftCols<2>();
90 res.col(2).noalias() = res.col(0).cross(res.col(1));
95 #endif //#ifndef __pinocchio_math_rotation_hpp__