5 #ifndef __pinocchio_math_rotation_hpp__ 6 #define __pinocchio_math_rotation_hpp__ 8 #include "pinocchio/fwd.hpp" 9 #include "pinocchio/math/matrix.hpp" 20 template<
typename Vector3,
typename Scalar,
typename Matrix3>
22 const Scalar & cos_value,
const Scalar & sin_value,
23 const Eigen::MatrixBase<Matrix3> & res)
25 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
26 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
28 assert(isUnitary(axis) &&
"The axis is not unitary.");
30 Matrix3 & res_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,res);
31 Vector3 sin_axis = sin_value * axis;
32 Vector3 cos1_axis = (Scalar(1)-cos_value) * axis;
35 tmp = cos1_axis.x() * axis.y();
36 res_.coeffRef(0,1) = tmp - sin_axis.z();
37 res_.coeffRef(1,0) = tmp + sin_axis.z();
39 tmp = cos1_axis.x() * axis.z();
40 res_.coeffRef(0,2) = tmp + sin_axis.y();
41 res_.coeffRef(2,0) = tmp - sin_axis.y();
43 tmp = cos1_axis.y() * axis.z();
44 res_.coeffRef(1,2) = tmp - sin_axis.x();
45 res_.coeffRef(2,1) = tmp + sin_axis.x();
47 res_.diagonal() = (cos1_axis.cwiseProduct(axis)).array() + cos_value;
51 #endif //#ifndef __pinocchio_math_rotation_hpp__ Main pinocchio namespace.
void toRotationMatrix(const Eigen::MatrixBase< Vector3 > &axis, const Scalar &cos_value, const Scalar &sin_value, const Eigen::MatrixBase< Matrix3 > &res)
Computes a rotation matrix from a vector and values of sin and cos orientations values.