5 #ifndef __pinocchio_math_rotation_hpp__
6 #define __pinocchio_math_rotation_hpp__
12 #include <Eigen/Geometry>
23 template<
typename Vector3,
typename Scalar,
typename Matrix3>
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);
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);
66 typedef Eigen::Quaternion<Scalar,Options> Quaternion;
67 Quaternion
quat(rot);
quat.normalize();
68 rot_ =
quat.toRotationMatrix();
78 template<
typename Matrix3>
80 orthogonalProjection(
const Eigen::MatrixBase<Matrix3> & mat)
82 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
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__