GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/cartesian-axis.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 39 52 75.0%
Branches: 0 8 0.0%
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 2208 static typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) cross(const Eigen::MatrixBase<V3> & vin)
30 {
31 2208 typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) res;
32 2208 cross(vin, res);
33 2208 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 ✗ alphaCross(const Scalar & s, const Eigen::MatrixBase<V3> & vin)
45 {
46 ✗ typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) res;
47 ✗ alphaCross(s, vin, res);
48 ✗ return res;
49 }
50
51 template<typename Scalar>
52 ✗ Eigen::Matrix<Scalar, dim, 1> operator*(const Scalar & s) const
53 {
54 typedef Eigen::Matrix<Scalar, dim, 1> ReturnType;
55 ✗ ReturnType res;
56 ✗ for (Eigen::DenseIndex i = 0; i < dim; ++i)
57 ✗ res[i] = i == axis ? s : Scalar(0);
58
59 ✗ return res;
60 }
61
62 template<typename Scalar>
63 ✗ friend inline Eigen::Matrix<Scalar, dim, 1> operator*(const Scalar & s, const CartesianAxis &)
64 {
65 ✗ 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 60 static Eigen::Matrix<Scalar, 3, 1> vector()
80 {
81 typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
82
0/2
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 60 return Vector3::Unit(axis);
83 }
84
85 ✗ static Vector3 vector()
86 {
87 ✗ return vector<double>();
88 }
89
90 }; // struct CartesianAxis
91
92 template<>
93 template<typename V3_in, typename V3_out>
94 812 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 812 V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
100 812 vout_[0] = 0.;
101 812 vout_[1] = -vin[2];
102 812 vout_[2] = vin[1];
103 812 }
104
105 template<>
106 template<typename V3_in, typename V3_out>
107 1200 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 1200 V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
113 1200 vout_[0] = vin[2];
114 1200 vout_[1] = 0.;
115 1200 vout_[2] = -vin[0];
116 1200 }
117
118 template<>
119 template<typename V3_in, typename V3_out>
120 422 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 422 V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
126 422 vout_[0] = -vin[1];
127 422 vout_[1] = vin[0];
128 422 vout_[2] = 0.;
129 422 }
130
131 template<>
132 template<typename Scalar, typename V3_in, typename V3_out>
133 56460 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 56460 V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
139 56460 vout_[0] = 0.;
140 56460 vout_[1] = -s * vin[2];
141 56460 vout_[2] = s * vin[1];
142 }
143
144 template<>
145 template<typename Scalar, typename V3_in, typename V3_out>
146 91652 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 91652 V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
152 91652 vout_[0] = s * vin[2];
153 91652 vout_[1] = 0.;
154 91652 vout_[2] = -s * vin[0];
155 }
156
157 template<>
158 template<typename Scalar, typename V3_in, typename V3_out>
159 35248 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 35248 V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
165 35248 vout_[0] = -s * vin[1];
166 35248 vout_[1] = s * vin[0];
167 35248 vout_[2] = 0.;
168 }
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