pinocchio  2.2.1-dirty
rotation.hpp
1 //
2 // Copyright (c) 2019 CNRS
3 //
4 
5 #ifndef __pinocchio_math_rotation_hpp__
6 #define __pinocchio_math_rotation_hpp__
7 
8 #include "pinocchio/fwd.hpp"
9 #include "pinocchio/math/matrix.hpp"
10 #include <Eigen/Core>
11 
12 namespace pinocchio
13 {
20  template<typename Vector3, typename Scalar, typename Matrix3>
21  void toRotationMatrix(const Eigen::MatrixBase<Vector3> & axis,
22  const Scalar & cos_value, const Scalar & sin_value,
23  const Eigen::MatrixBase<Matrix3> & res)
24  {
25  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
26  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
27 
28  assert(isUnitary(axis) && "The axis is not unitary.");
29 
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;
33 
34  Scalar tmp;
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();
38 
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();
42 
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();
46 
47  res_.diagonal() = (cos1_axis.cwiseProduct(axis)).array() + cos_value;
48  }
49 }
50 
51 #endif //#ifndef __pinocchio_math_rotation_hpp__
52 
Main pinocchio namespace.
Definition: treeview.dox:24
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.
Definition: rotation.hpp:21