GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/cartesian-axis.hpp
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 55 55 100.0%
Branches: 47 135 34.8%
Line Branch Exec Source
1 //
2 // Copyright (c) 2017-2020 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_cartesian_axis_hpp__
6 #define __pinocchio_cartesian_axis_hpp__
7
8 #include "pinocchio/fwd.hpp"
9
10 namespace pinocchio
11 {
12
13 template<int _axis>
14 struct CartesianAxis
15 {
16 enum
17 {
18 axis = _axis,
19 dim = 3
20 };
21
22 typedef Eigen::Matrix<double, 3, 1> Vector3;
23
24 template<typename V3_in, typename V3_out>
25 inline static void
26 cross(const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout);
27
28 template<typename V3>
29 7696 static typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) cross(const Eigen::MatrixBase<V3> & vin)
30 {
31 7696 typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) res;
32
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 7696 cross(vin, res);
33 7696 return res;
34 }
35
36 template<typename Scalar, typename V3_in, typename V3_out>
37 inline static void alphaCross(
38 const Scalar & s,
39 const Eigen::MatrixBase<V3_in> & vin,
40 const Eigen::MatrixBase<V3_out> & vout);
41
42 template<typename Scalar, typename V3>
43 static typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3)
44 6 alphaCross(const Scalar & s, const Eigen::MatrixBase<V3> & vin)
45 {
46 6 typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) res;
47 6 alphaCross(s, vin, res);
48 6 return res;
49 }
50
51 template<typename Scalar>
52 148 Eigen::Matrix<Scalar, dim, 1> operator*(const Scalar & s) const
53 {
54 typedef Eigen::Matrix<Scalar, dim, 1> ReturnType;
55 148 ReturnType res;
56
2/2
✓ Branch 0 taken 222 times.
✓ Branch 1 taken 74 times.
 592 for (Eigen::DenseIndex i = 0; i < dim; ++i)
57
6/10
✓ Branch 0 taken 74 times.
✓ Branch 1 taken 148 times.
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
 444 res[i] = i == axis ? s : Scalar(0);
58
59 148 return res;
60 }
61
62 template<typename Scalar>
63 6 friend inline Eigen::Matrix<Scalar, dim, 1> operator*(const Scalar & s, const CartesianAxis &)
64 {
65
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 12 return CartesianAxis() * s;
66 }
67
68 template<typename Vector3Like>
69 static void setTo(const Eigen::MatrixBase<Vector3Like> v3)
70 {
71 Vector3Like & v3_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3Like, v3);
72 typedef typename Vector3Like::Scalar Scalar;
73
74 for (Eigen::DenseIndex i = 0; i < dim; ++i)
75 v3_[i] = i == axis ? Scalar(1) : Scalar(0);
76 }
77
78 template<typename Scalar>
79 270 static Eigen::Matrix<Scalar, 3, 1> vector()
80 {
81 typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
82
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 270 return Vector3::Unit(axis);
83 }
84
85 44 static Vector3 vector()
86 {
87 44 return vector<double>();
88 }
89
90 }; // struct CartesianAxis
91
92 template<>
93 template<typename V3_in, typename V3_out>
94 3281 inline void CartesianAxis<0>::cross(
95 const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout)
96 {
97 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in, 3)
98 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out, 3)
99 3281 V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
100
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 3281 vout_[0] = 0.;
101
2/8
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 3281 vout_[1] = -vin[2];
102
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 3281 vout_[2] = vin[1];
103 3281 }
104
105 template<>
106 template<typename V3_in, typename V3_out>
107 4775 inline void CartesianAxis<1>::cross(
108 const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout)
109 {
110 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in, 3)
111 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out, 3)
112 4775 V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
113
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 4775 vout_[0] = vin[2];
114
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 4775 vout_[1] = 0.;
115
0/8
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 4775 vout_[2] = -vin[0];
116 4775 }
117
118 template<>
119 template<typename V3_in, typename V3_out>
120 2535 inline void CartesianAxis<2>::cross(
121 const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout)
122 {
123 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in, 3)
124 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out, 3)
125 2535 V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
126
0/8
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 2535 vout_[0] = -vin[1];
127
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 2535 vout_[1] = vin[0];
128
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 2535 vout_[2] = 0.;
129 2535 }
130
131 template<>
132 template<typename Scalar, typename V3_in, typename V3_out>
133 72681 inline void CartesianAxis<0>::alphaCross(
134 const Scalar & s, const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout)
135 {
136 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in, 3)
137 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out, 3)
138 72681 V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
139
2/4
✓ Branch 2 taken 178 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 178 times.
✗ Branch 6 not taken.
 72681 vout_[0] = 0.;
140
7/11
✓ Branch 2 taken 177 times.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 177 times.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 177 times.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 177 times.
✗ Branch 12 not taken.
 72681 vout_[1] = -s * vin[2];
141
5/8
✓ Branch 2 taken 177 times.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 177 times.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 177 times.
✗ Branch 9 not taken.
 72681 vout_[2] = s * vin[1];
142 72681 }
143
144 template<>
145 template<typename Scalar, typename V3_in, typename V3_out>
146 118396 inline void CartesianAxis<1>::alphaCross(
147 const Scalar & s, const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout)
148 {
149 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in, 3)
150 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out, 3)
151 118396 V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
152
3/8
✓ Branch 2 taken 288 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 288 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 288 times.
✗ Branch 9 not taken.
 118396 vout_[0] = s * vin[2];
153
2/4
✓ Branch 2 taken 288 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 288 times.
✗ Branch 6 not taken.
 118396 vout_[1] = 0.;
154
4/11
✓ Branch 2 taken 288 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 288 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 288 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 288 times.
✗ Branch 12 not taken.
 118396 vout_[2] = -s * vin[0];
155 118396 }
156
157 template<>
158 template<typename Scalar, typename V3_in, typename V3_out>
159 45776 inline void CartesianAxis<2>::alphaCross(
160 const Scalar & s, const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout)
161 {
162 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in, 3)
163 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out, 3)
164 45776 V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
165
4/11
✓ Branch 2 taken 110 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 110 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 110 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 110 times.
✗ Branch 12 not taken.
 45776 vout_[0] = -s * vin[1];
166
3/8
✓ Branch 2 taken 110 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 110 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 110 times.
✗ Branch 9 not taken.
 45776 vout_[1] = s * vin[0];
167
2/4
✓ Branch 2 taken 110 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 110 times.
✗ Branch 6 not taken.
 45776 vout_[2] = 0.;
168 45776 }
169
170 typedef CartesianAxis<0> XAxis;
171 typedef XAxis AxisX;
172
173 typedef CartesianAxis<1> YAxis;
174 typedef YAxis AxisY;
175
176 typedef CartesianAxis<2> ZAxis;
177 typedef ZAxis AxisZ;
178
179 } // namespace pinocchio
180
181 #endif // __pinocchio_cartesian_axis_hpp__
182