GCC Code Coverage Report

Directory: ./
File: include/pinocchio/math/quaternion.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 67 67 100.0%
Branches: 147 278 52.9%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_math_quaternion_hpp__
6 #define __pinocchio_math_quaternion_hpp__
7
8 #ifndef PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE
9 #define PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE 1e-8
10 #endif
11
12 #include "pinocchio/math/fwd.hpp"
13 #include "pinocchio/math/comparison-operators.hpp"
14 #include "pinocchio/math/matrix.hpp"
15 #include "pinocchio/math/sincos.hpp"
16 #include "pinocchio/utils/static-if.hpp"
17
18 #include <boost/type_traits.hpp>
19 #include <Eigen/Geometry>
20
21 namespace pinocchio
22 {
23 namespace quaternion
24 {
25 ///
26 /// \brief Compute the minimal angle between q1 and q2.
27 ///
28 /// \param[in] q1 input quaternion.
29 /// \param[in] q2 input quaternion.
30 ///
31 /// \return angle between the two quaternions
32 ///
33 template<typename D1, typename D2>
34 typename D1::Scalar angleBetweenQuaternions(
35 const Eigen::QuaternionBase<D1> & q1, const Eigen::QuaternionBase<D2> & q2)
36 {
37 typedef typename D1::Scalar Scalar;
38 const Scalar innerprod = q1.dot(q2);
39 Scalar theta = math::acos(innerprod);
40 static const Scalar PI_value = PI<Scalar>();
41
42 theta = internal::if_then_else(
43 internal::LT, innerprod, Scalar(0), static_cast<Scalar>(PI_value - theta), theta);
44 return theta;
45 }
46
47 ///
48 /// \brief Check if two quaternions define the same rotations.
49 /// \note Two quaternions define the same rotation iff q1 == q2 OR q1 == -q2.
50 ///
51 /// \param[in] q1 input quaternion.
52 /// \param[in] q2 input quaternion.
53 ///
54 /// \return Return true if the two input quaternions define the same rotation.
55 ///
56 template<typename D1, typename D2>
57 10011 bool defineSameRotation(
58 const Eigen::QuaternionBase<D1> & q1,
59 const Eigen::QuaternionBase<D2> & q2,
60 const typename D1::RealScalar & prec =
61 Eigen::NumTraits<typename D1::Scalar>::dummy_precision())
62 {
63
10/18
✓ Branch 1 taken 10011 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10011 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10011 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 515 times.
✓ Branch 10 taken 9496 times.
✓ Branch 12 taken 515 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 515 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 515 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 515 times.
✗ Branch 22 not taken.
✓ Branch 23 taken 515 times.
✗ Branch 24 not taken.
 10011 return (q1.coeffs().isApprox(q2.coeffs(), prec) || q1.coeffs().isApprox(-q2.coeffs(), prec));
64 }
65
66 /// Approximately normalize by applying the first order limited development
67 /// of the normalization function.
68 ///
69 /// Only additions and multiplications are required. Neither square root nor
70 /// division are used (except a division by 2). Let \f$ \delta = ||q||^2 - 1 \f$.
71 /// Using the following limited development:
72 /// \f[ \frac{1}{||q||} = (1 + \delta)^{-\frac{1}{2}} = 1 - \frac{\delta}{2} +
73 /// \mathcal{O}(\delta^2) \f]
74 ///
75 /// The output is
76 /// \f[ q_{out} = q \times \frac{3 - ||q_{in}||^2}{2} \f]
77 ///
78 /// The output quaternion is guaranted to statisfy the following:
79 /// \f[ | ||q_{out}|| - 1 | \le \frac{M}{2} ||q_{in}|| ( ||q_{in}||^2 - 1 )^2 \f]
80 /// where \f$ M = \frac{3}{4} (1 - \epsilon)^{-\frac{5}{2}} \f$
81 /// and \f$ \epsilon \f$ is the maximum tolerance of \f$ ||q_{in}||^2 - 1 \f$.
82 ///
83 /// \warning \f$ ||q||^2 - 1 \f$ should already be close to zero.
84 ///
85 /// \note See
86 /// http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html#title3
87 /// to know the reason why the argument is const.
88 template<typename D>
89 20346 void firstOrderNormalize(const Eigen::QuaternionBase<D> & q)
90 {
91 typedef typename D::Scalar Scalar;
92
1/2
✓ Branch 1 taken 20344 times.
✗ Branch 2 not taken.
 20346 const Scalar N2 = q.squaredNorm();
93 #ifndef NDEBUG
94
3/6
✓ Branch 1 taken 334 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 334 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 334 times.
✗ Branch 8 not taken.
 20681 const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
95 typedef apply_op_if<less_than_or_equal_to_op, is_floating_point<Scalar>::value, true>
96 static_leq;
97
6/9
✓ Branch 1 taken 334 times.
✓ Branch 2 taken 20010 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 334 times.
✓ Branch 5 taken 20010 times.
✓ Branch 7 taken 334 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 334 times.
 20346 assert(static_leq::op(math::fabs(static_cast<Scalar>(N2 - Scalar(1))), epsilon));
98 #endif
99
4/8
✓ Branch 1 taken 334 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 334 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 334 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 334 times.
✗ Branch 11 not taken.
 20681 const Scalar alpha = ((Scalar)3 - N2) / Scalar(2);
100
1/2
✓ Branch 3 taken 20344 times.
✗ Branch 4 not taken.
 20346 PINOCCHIO_EIGEN_CONST_CAST(D, q).coeffs() *= alpha;
101 #ifndef NDEBUG
102
5/10
✓ Branch 1 taken 334 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 334 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 334 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 334 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 334 times.
✗ Branch 14 not taken.
 22021 const Scalar M =
103
6/12
✓ Branch 1 taken 20344 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 334 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 334 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 334 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 334 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 334 times.
✗ Branch 17 not taken.
 22021 Scalar(3) * math::pow(Scalar(1) - epsilon, ((Scalar)-Scalar(5)) / Scalar(2)) / Scalar(4);
104
21/38
✓ Branch 1 taken 334 times.
✓ Branch 2 taken 20010 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 334 times.
✓ Branch 5 taken 20010 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 334 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 20010 times.
✓ Branch 10 taken 334 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 20010 times.
✓ Branch 13 taken 334 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 334 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 334 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 334 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 334 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 334 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 334 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 334 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 334 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 334 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 334 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 334 times.
✗ Branch 47 not taken.
✗ Branch 49 not taken.
✓ Branch 50 taken 334 times.
 20346 assert(static_leq::op(
105 math::fabs(static_cast<Scalar>(q.norm() - Scalar(1))),
106 math::max(
107 M * sqrt(N2) * (N2 - Scalar(1)) * (N2 - Scalar(1)) / Scalar(2),
108 Eigen::NumTraits<Scalar>::dummy_precision())));
109 #endif
110 20346 }
111
112 /// Uniformly random quaternion sphere.
113 template<typename Derived>
114 340298 void uniformRandom(Eigen::QuaternionBase<Derived> & q)
115 {
116 typedef typename Derived::Scalar Scalar;
117
118 // Rotational part
119
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 340300 const Scalar u1 = (Scalar)rand() / RAND_MAX;
120
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 340300 const Scalar u2 = (Scalar)rand() / RAND_MAX;
121
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 340300 const Scalar u3 = (Scalar)rand() / RAND_MAX;
122
123
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 340300 const Scalar mult1 = sqrt(Scalar(1) - u1);
124
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 340298 const Scalar mult2 = sqrt(u1);
125
126
4/8
✓ Branch 0 taken 193 times.
✓ Branch 1 taken 329951 times.
✓ Branch 3 taken 193 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 340298 static const Scalar PI_value = PI<Scalar>();
127
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar s2, c2;
128
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 340298 SINCOS(Scalar(2) * PI_value * u2, &s2, &c2);
129
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Scalar s3, c3;
130
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 340298 SINCOS(Scalar(2) * PI_value * u3, &s3, &c3);
131
132
3/6
✓ Branch 1 taken 330144 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 340298 q.w() = mult1 * s2;
133
3/6
✓ Branch 1 taken 330144 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 340298 q.x() = mult1 * c2;
134
3/6
✓ Branch 1 taken 330144 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 340298 q.y() = mult2 * s3;
135
3/6
✓ Branch 1 taken 330144 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 340298 q.z() = mult2 * c3;
136 340298 }
137
138 namespace internal
139 {
140
141 template<typename Scalar, bool value = is_floating_point<Scalar>::value>
142 struct quaternionbase_assign_impl;
143
144 template<Eigen::DenseIndex i>
145 struct quaternionbase_assign_impl_if_t_negative
146 {
147 template<typename Scalar, typename Matrix3, typename QuaternionDerived>
148 static inline void
149 121854 run(Scalar t, Eigen::QuaternionBase<QuaternionDerived> & q, const Matrix3 & mat)
150 {
151 using pinocchio::math::sqrt;
152
153 121854 Eigen::DenseIndex j = (i + 1) % 3;
154 121854 Eigen::DenseIndex k = (j + 1) % 3;
155
156
8/16
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 3 times.
✗ Branch 24 not taken.
 121854 t = sqrt(mat.coeff(i, i) - mat.coeff(j, j) - mat.coeff(k, k) + Scalar(1.0));
157
3/7
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
 121854 q.coeffs().coeffRef(i) = Scalar(0.5) * t;
158
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 121854 t = Scalar(0.5) / t;
159
3/6
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
 121854 q.w() = (mat.coeff(k, j) - mat.coeff(j, k)) * t;
160
3/7
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
 121854 q.coeffs().coeffRef(j) = (mat.coeff(j, i) + mat.coeff(i, j)) * t;
161
3/7
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
 121854 q.coeffs().coeffRef(k) = (mat.coeff(k, i) + mat.coeff(i, k)) * t;
162 121854 }
163 };
164
165 struct quaternionbase_assign_impl_if_t_positive
166 {
167 template<typename Scalar, typename Matrix3, typename QuaternionDerived>
168 static inline void
169 39077 run(Scalar t, Eigen::QuaternionBase<QuaternionDerived> & q, const Matrix3 & mat)
170 {
171 using pinocchio::math::sqrt;
172
173
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 39077 t = sqrt(t + Scalar(1.0));
174
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 39077 q.w() = Scalar(0.5) * t;
175
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 39077 t = Scalar(0.5) / t;
176
3/6
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 39077 q.x() = (mat.coeff(2, 1) - mat.coeff(1, 2)) * t;
177
3/6
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 39077 q.y() = (mat.coeff(0, 2) - mat.coeff(2, 0)) * t;
178
3/6
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 39077 q.z() = (mat.coeff(1, 0) - mat.coeff(0, 1)) * t;
179 39077 }
180 };
181
182 template<typename Scalar>
183 struct quaternionbase_assign_impl<Scalar, true>
184 {
185 template<typename Matrix3, typename QuaternionDerived>
186 100000 static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q, const Matrix3 & mat)
187 {
188 using pinocchio::math::sqrt;
189
190 100000 Scalar t = mat.trace();
191
2/2
✓ Branch 0 taken 39076 times.
✓ Branch 1 taken 60924 times.
 100000 if (t > Scalar(0.))
192 39076 quaternionbase_assign_impl_if_t_positive::run(t, q, mat);
193 else
194 {
195 60924 Eigen::DenseIndex i = 0;
196
2/2
✓ Branch 2 taken 30300 times.
✓ Branch 3 taken 30624 times.
 60924 if (mat.coeff(1, 1) > mat.coeff(0, 0))
197 30300 i = 1;
198
2/2
✓ Branch 2 taken 20270 times.
✓ Branch 3 taken 40654 times.
 60924 if (mat.coeff(2, 2) > mat.coeff(i, i))
199 20270 i = 2;
200
201
2/2
✓ Branch 0 taken 20376 times.
✓ Branch 1 taken 40548 times.
 60924 if (i == 0)
202 20376 quaternionbase_assign_impl_if_t_negative<0>::run(t, q, mat);
203
2/2
✓ Branch 0 taken 20278 times.
✓ Branch 1 taken 20270 times.
 40548 else if (i == 1)
204 20278 quaternionbase_assign_impl_if_t_negative<1>::run(t, q, mat);
205 else
206 20270 quaternionbase_assign_impl_if_t_negative<2>::run(t, q, mat);
207 }
208 100000 }
209 };
210
211 } // namespace internal
212
213 template<typename D, typename Matrix3>
214 100001 void assignQuaternion(Eigen::QuaternionBase<D> & quat, const Eigen::MatrixBase<Matrix3> & R)
215 {
216 200002 internal::quaternionbase_assign_impl<typename Matrix3::Scalar>::run(
217 100001 quat.derived(), R.derived());
218 100001 }
219
220 ///
221 /// \brief Check whether the input quaternion is Normalized within the given precision.
222 ///
223 /// \param[in] quat Input quaternion
224 /// \param[in] prec Required precision
225 ///
226 /// \returns true if quat is normalized within the precision prec.
227 ///
228 template<typename Quaternion>
229 92126 inline bool isNormalized(
230 const Eigen::QuaternionBase<Quaternion> & quat,
231 const typename Quaternion::Coefficients::RealScalar & prec =
232 Eigen::NumTraits<typename Quaternion::Coefficients::RealScalar>::dummy_precision())
233 {
234 92126 return pinocchio::isNormalized(quat.coeffs(), prec);
235 }
236
237 ///
238 /// \brief Normalize the input quaternion.
239 ///
240 /// \param[in] quat Input quaternion
241 ///
242 template<typename Quaternion>
243 inline void normalize(const Eigen::QuaternionBase<Quaternion> & quat)
244 {
245 return pinocchio::normalize(quat.const_cast_derived().coeffs());
246 }
247
248 ///
249 /// \returns the spherical linear interpolation between the two quaternions
250 ///
251 /// \param[in] u Interpolation factor
252 /// \param[in] quat Input quaternion
253 ///
254 template<
255 typename Scalar,
256 typename QuaternionIn1,
257 typename QuaternionIn2,
258 typename QuaternionOut>
259 inline void slerp(
260 const Scalar & u,
261 const Eigen::QuaternionBase<QuaternionIn1> & quat0,
262 const Eigen::QuaternionBase<QuaternionIn2> & quat1,
263 const Eigen::QuaternionBase<QuaternionOut> & res)
264 {
265 const Scalar one = Scalar(1) - Eigen::NumTraits<Scalar>::epsilon();
266 const Scalar d = quat0.dot(quat1);
267 const Scalar absD = fabs(d);
268
269 const Scalar theta = acos(absD);
270 const Scalar sinTheta = sin(theta);
271
272 using namespace pinocchio::internal;
273
274 const Scalar scale0 = if_then_else(
275 pinocchio::internal::GE, absD, one,
276 static_cast<Scalar>(Scalar(1) - u), // then
277 static_cast<Scalar>(sin((Scalar(1) - u) * theta) / sinTheta) // else
278 );
279
280 const Scalar scale1_factor =
281 if_then_else(pinocchio::internal::LT, d, Scalar(0), Scalar(-1), Scalar(1));
282 const Scalar scale1 = if_then_else(
283 pinocchio::internal::GE, absD, one,
284 u, // then
285 static_cast<Scalar>(sin((u * theta)) / sinTheta) // else
286 )
287 * scale1_factor;
288
289 PINOCCHIO_EIGEN_CONST_CAST(QuaternionOut, res.derived()).coeffs() =
290 scale0 * quat0.coeffs() + scale1 * quat1.coeffs();
291 }
292
293 } // namespace quaternion
294
295 } // namespace pinocchio
296 #endif // #ifndef __pinocchio_math_quaternion_hpp__
297