GCC Code Coverage Report

Directory: ./
File: include/pinocchio/math/rotation.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 30 30 100.0%
Branches: 74 155 47.7%
Line Branch Exec Source
1 //
2 // Copyright (c) 2019-2020 CNRS INRIA
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 "pinocchio/math/sincos.hpp"
11
12 #include <Eigen/Core>
13 #include <Eigen/Geometry>
14 #include <Eigen/SVD>
15
16 namespace pinocchio
17 {
18
19 ///
20 /// \brief Computes a rotation matrix from a vector and values of sin and cos
21 /// orientations values.
22 ///
23 /// \remarks This code is issue from Eigen::AxisAngle::toRotationMatrix
24 ///
25 template<typename Vector3, typename Scalar, typename Matrix3>
26 206294 void toRotationMatrix(
27 const Eigen::MatrixBase<Vector3> & axis,
28 const Scalar & cos_value,
29 const Scalar & sin_value,
30 const Eigen::MatrixBase<Matrix3> & res)
31 {
32 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3, 3);
33 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3);
34
35
5/11
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 206264 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 206274 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 206294 assert(isUnitary(axis) && "The axis is not unitary.");
36
37 206294 Matrix3 & res_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, res);
38
2/4
✓ Branch 1 taken 206274 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206274 times.
✗ Branch 5 not taken.
 206294 Vector3 sin_axis = sin_value * axis;
39
4/8
✓ Branch 1 taken 206274 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206274 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
 206314 Vector3 cos1_axis = (Scalar(1) - cos_value) * axis;
40
41
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 Scalar tmp;
42
4/8
✓ Branch 1 taken 206274 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206274 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
 206294 tmp = cos1_axis.x() * axis.y();
43
4/9
✓ Branch 1 taken 206274 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206274 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
 206294 res_.coeffRef(0, 1) = tmp - sin_axis.z();
44
4/9
✓ Branch 1 taken 206274 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206274 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
 206294 res_.coeffRef(1, 0) = tmp + sin_axis.z();
45
46
4/8
✓ Branch 1 taken 206274 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206274 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
 206294 tmp = cos1_axis.x() * axis.z();
47
4/9
✓ Branch 1 taken 206274 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206274 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
 206294 res_.coeffRef(0, 2) = tmp + sin_axis.y();
48
4/9
✓ Branch 1 taken 206274 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206274 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
 206294 res_.coeffRef(2, 0) = tmp - sin_axis.y();
49
50
4/8
✓ Branch 1 taken 206274 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206274 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
 206294 tmp = cos1_axis.y() * axis.z();
51
4/9
✓ Branch 1 taken 206274 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206274 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
 206294 res_.coeffRef(1, 2) = tmp - sin_axis.x();
52
4/9
✓ Branch 1 taken 206274 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206274 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
 206294 res_.coeffRef(2, 1) = tmp + sin_axis.x();
53
54
5/10
✓ Branch 1 taken 206274 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 206274 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 206274 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 206274 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 206274 times.
✗ Branch 14 not taken.
 206294 res_.diagonal() = (cos1_axis.cwiseProduct(axis)).array() + cos_value;
55 206294 }
56
57 ///
58 /// \brief Computes a rotation matrix from a vector and the angular value
59 /// orientations values.
60 ///
61 /// \remarks This code is issue from Eigen::AxisAngle::toRotationMatrix
62 ///
63 template<typename Vector3, typename Scalar, typename Matrix3>
64 106198 void toRotationMatrix(
65 const Eigen::MatrixBase<Vector3> & axis,
66 const Scalar & angle,
67 const Eigen::MatrixBase<Matrix3> & res)
68 {
69
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 Scalar sa, ca;
70
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 106198 SINCOS(angle, &sa, &ca);
71
1/2
✓ Branch 2 taken 106190 times.
✗ Branch 3 not taken.
 106198 toRotationMatrix(axis, ca, sa, PINOCCHIO_EIGEN_CONST_CAST(Matrix3, res));
72 106198 }
73
74 ///
75 /// \brief Orthogonormalization procedure for a rotation matrix (closed enough to SO(3)).
76 ///
77 /// \param[in,out] rot A 3x3 matrix to orthonormalize
78 ///
79 template<typename Matrix3>
80 void normalizeRotation(const Eigen::MatrixBase<Matrix3> & rot)
81 {
82 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3);
83 Matrix3 & rot_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, rot);
84
85 typedef typename Matrix3::Scalar Scalar;
86 enum
87 {
88 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Options
89 };
90 typedef Eigen::Quaternion<Scalar, Options> Quaternion;
91 Quaternion quat(rot);
92 normalize(quat.coeffs());
93 rot_ = quat.toRotationMatrix();
94 }
95
96 ///
97 /// \brief Orthogonal projection of a matrix on the SO(3) manifold.
98 ///
99 /// \param[in] mat A 3x3 matrix to project on SO(3).
100 ///
101 /// \returns the orthogonal projection of mat on SO(3)
102 ///
103 template<typename Matrix3>
104 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)
105 200344 orthogonalProjection(const Eigen::MatrixBase<Matrix3> & mat)
106 {
107 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3);
108 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3) ReturnType;
109
110 typedef Eigen::JacobiSVD<Matrix3> SVD;
111
2/4
✓ Branch 1 taken 200340 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 200340 times.
✗ Branch 5 not taken.
 200344 const SVD svd(mat, Eigen::ComputeFullU | Eigen::ComputeFullV);
112
113
1/2
✓ Branch 1 taken 200340 times.
✗ Branch 2 not taken.
 200344 ReturnType res;
114
3/6
✓ Branch 1 taken 200340 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 200340 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 200340 times.
✗ Branch 8 not taken.
 200344 res.template leftCols<2>().noalias() =
115
5/10
✓ Branch 1 taken 200340 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 200340 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 200340 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 200340 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 200340 times.
✗ Branch 14 not taken.
 200344 svd.matrixU() * svd.matrixV().transpose().template leftCols<2>();
116
6/12
✓ Branch 1 taken 200340 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 200340 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 200340 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 200340 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 200340 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 200340 times.
✗ Branch 17 not taken.
 200344 res.col(2).noalias() = res.col(0).cross(res.col(1));
117 400688 return res;
118 200344 }
119 } // namespace pinocchio
120
121 #endif // #ifndef __pinocchio_math_rotation_hpp__
122