Directory: | ./ |
---|---|
File: | include/pinocchio/math/quaternion.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 28 | 61 | 45.9% |
Branches: | 26 | 58 | 44.8% |
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 | 6 | 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 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 2 times.
✓ Branch 10 taken 4 times.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✓ Branch 23 taken 2 times.
✗ Branch 24 not taken.
|
6 | 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 | 2428 | void firstOrderNormalize(const Eigen::QuaternionBase<D> & q) | |
90 | { | ||
91 | typedef typename D::Scalar Scalar; | ||
92 |
1/2✓ Branch 1 taken 2428 times.
✗ Branch 2 not taken.
|
2428 | const Scalar N2 = q.squaredNorm(); |
93 | #ifndef NDEBUG | ||
94 | 2428 | 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 |
2/4✓ Branch 2 taken 2428 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 2428 times.
|
2428 | assert(static_leq::op(math::fabs(static_cast<Scalar>(N2 - Scalar(1))), epsilon)); |
98 | #endif | ||
99 | 2428 | const Scalar alpha = ((Scalar)3 - N2) / Scalar(2); | |
100 |
1/2✓ Branch 3 taken 2428 times.
✗ Branch 4 not taken.
|
2428 | PINOCCHIO_EIGEN_CONST_CAST(D, q).coeffs() *= alpha; |
101 | #ifndef NDEBUG | ||
102 | 2428 | const Scalar M = | |
103 |
1/2✓ Branch 1 taken 2428 times.
✗ Branch 2 not taken.
|
2428 | Scalar(3) * math::pow(Scalar(1) - epsilon, ((Scalar)-Scalar(5)) / Scalar(2)) / Scalar(4); |
104 |
4/8✓ Branch 2 taken 2428 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2428 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2428 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 2428 times.
|
2428 | 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 | 2428 | } | |
111 | |||
112 | /// Uniformly random quaternion sphere. | ||
113 | template<typename Derived> | ||
114 | 5195 | void uniformRandom(Eigen::QuaternionBase<Derived> & q) | |
115 | { | ||
116 | typedef typename Derived::Scalar Scalar; | ||
117 | |||
118 | // Rotational part | ||
119 | 5195 | const Scalar u1 = (Scalar)rand() / RAND_MAX; | |
120 | 5195 | const Scalar u2 = (Scalar)rand() / RAND_MAX; | |
121 | 5195 | const Scalar u3 = (Scalar)rand() / RAND_MAX; | |
122 | |||
123 | 5195 | const Scalar mult1 = sqrt(Scalar(1) - u1); | |
124 | 5195 | const Scalar mult2 = sqrt(u1); | |
125 | |||
126 |
3/4✓ Branch 0 taken 58 times.
✓ Branch 1 taken 5137 times.
✓ Branch 3 taken 58 times.
✗ Branch 4 not taken.
|
5195 | static const Scalar PI_value = PI<Scalar>(); |
127 | Scalar s2, c2; | ||
128 | 5195 | SINCOS(Scalar(2) * PI_value * u2, &s2, &c2); | |
129 | Scalar s3, c3; | ||
130 | 5195 | SINCOS(Scalar(2) * PI_value * u3, &s3, &c3); | |
131 | |||
132 |
1/2✓ Branch 1 taken 5195 times.
✗ Branch 2 not taken.
|
5195 | q.w() = mult1 * s2; |
133 |
1/2✓ Branch 1 taken 5195 times.
✗ Branch 2 not taken.
|
5195 | q.x() = mult1 * c2; |
134 |
1/2✓ Branch 1 taken 5195 times.
✗ Branch 2 not taken.
|
5195 | q.y() = mult2 * s3; |
135 |
1/2✓ Branch 1 taken 5195 times.
✗ Branch 2 not taken.
|
5195 | q.z() = mult2 * c3; |
136 | 5195 | } | |
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 | ✗ | run(Scalar t, Eigen::QuaternionBase<QuaternionDerived> & q, const Matrix3 & mat) | |
150 | { | ||
151 | using pinocchio::math::sqrt; | ||
152 | |||
153 | ✗ | Eigen::DenseIndex j = (i + 1) % 3; | |
154 | ✗ | Eigen::DenseIndex k = (j + 1) % 3; | |
155 | |||
156 | ✗ | t = sqrt(mat.coeff(i, i) - mat.coeff(j, j) - mat.coeff(k, k) + Scalar(1.0)); | |
157 | ✗ | q.coeffs().coeffRef(i) = Scalar(0.5) * t; | |
158 | ✗ | t = Scalar(0.5) / t; | |
159 | ✗ | q.w() = (mat.coeff(k, j) - mat.coeff(j, k)) * t; | |
160 | ✗ | q.coeffs().coeffRef(j) = (mat.coeff(j, i) + mat.coeff(i, j)) * t; | |
161 | ✗ | q.coeffs().coeffRef(k) = (mat.coeff(k, i) + mat.coeff(i, k)) * t; | |
162 | } | ||
163 | }; | ||
164 | |||
165 | struct quaternionbase_assign_impl_if_t_positive | ||
166 | { | ||
167 | template<typename Scalar, typename Matrix3, typename QuaternionDerived> | ||
168 | static inline void | ||
169 | ✗ | run(Scalar t, Eigen::QuaternionBase<QuaternionDerived> & q, const Matrix3 & mat) | |
170 | { | ||
171 | using pinocchio::math::sqrt; | ||
172 | |||
173 | ✗ | t = sqrt(t + Scalar(1.0)); | |
174 | ✗ | q.w() = Scalar(0.5) * t; | |
175 | ✗ | t = Scalar(0.5) / t; | |
176 | ✗ | q.x() = (mat.coeff(2, 1) - mat.coeff(1, 2)) * t; | |
177 | ✗ | q.y() = (mat.coeff(0, 2) - mat.coeff(2, 0)) * t; | |
178 | ✗ | q.z() = (mat.coeff(1, 0) - mat.coeff(0, 1)) * t; | |
179 | } | ||
180 | }; | ||
181 | |||
182 | template<typename Scalar> | ||
183 | struct quaternionbase_assign_impl<Scalar, true> | ||
184 | { | ||
185 | template<typename Matrix3, typename QuaternionDerived> | ||
186 | ✗ | static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q, const Matrix3 & mat) | |
187 | { | ||
188 | using pinocchio::math::sqrt; | ||
189 | |||
190 | ✗ | Scalar t = mat.trace(); | |
191 | ✗ | if (t > Scalar(0.)) | |
192 | ✗ | quaternionbase_assign_impl_if_t_positive::run(t, q, mat); | |
193 | else | ||
194 | { | ||
195 | ✗ | Eigen::DenseIndex i = 0; | |
196 | ✗ | if (mat.coeff(1, 1) > mat.coeff(0, 0)) | |
197 | ✗ | i = 1; | |
198 | ✗ | if (mat.coeff(2, 2) > mat.coeff(i, i)) | |
199 | ✗ | i = 2; | |
200 | |||
201 | ✗ | if (i == 0) | |
202 | ✗ | quaternionbase_assign_impl_if_t_negative<0>::run(t, q, mat); | |
203 | ✗ | else if (i == 1) | |
204 | ✗ | quaternionbase_assign_impl_if_t_negative<1>::run(t, q, mat); | |
205 | else | ||
206 | ✗ | quaternionbase_assign_impl_if_t_negative<2>::run(t, q, mat); | |
207 | } | ||
208 | } | ||
209 | }; | ||
210 | |||
211 | } // namespace internal | ||
212 | |||
213 | template<typename D, typename Matrix3> | ||
214 | ✗ | void assignQuaternion(Eigen::QuaternionBase<D> & quat, const Eigen::MatrixBase<Matrix3> & R) | |
215 | { | ||
216 | ✗ | internal::quaternionbase_assign_impl<typename Matrix3::Scalar>::run( | |
217 | ✗ | quat.derived(), R.derived()); | |
218 | } | ||
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 | 6961 | 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 | 6961 | 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 |