GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 |
||
35 |
angleBetweenQuaternions(const Eigen::QuaternionBase<D1> & q1, |
||
36 |
const Eigen::QuaternionBase<D2> & q2) |
||
37 |
{ |
||
38 |
typedef typename D1::Scalar Scalar; |
||
39 |
const Scalar innerprod = q1.dot(q2); |
||
40 |
Scalar theta = math::acos(innerprod); |
||
41 |
static const Scalar PI_value = PI<Scalar>(); |
||
42 |
|||
43 |
theta = internal::if_then_else(internal::LT, innerprod, Scalar(0), |
||
44 |
PI_value - theta, |
||
45 |
theta); |
||
46 |
return theta; |
||
47 |
} |
||
48 |
|||
49 |
/// |
||
50 |
/// \brief Check if two quaternions define the same rotations. |
||
51 |
/// \note Two quaternions define the same rotation iff q1 == q2 OR q1 == -q2. |
||
52 |
/// |
||
53 |
/// \param[in] q1 input quaternion. |
||
54 |
/// \param[in] q2 input quaternion. |
||
55 |
/// |
||
56 |
/// \return Return true if the two input quaternions define the same rotation. |
||
57 |
/// |
||
58 |
template<typename D1, typename D2> |
||
59 |
9 |
bool defineSameRotation(const Eigen::QuaternionBase<D1> & q1, |
|
60 |
const Eigen::QuaternionBase<D2> & q2, |
||
61 |
const typename D1::RealScalar & prec = Eigen::NumTraits<typename D1::Scalar>::dummy_precision()) |
||
62 |
{ |
||
63 |
✓✗✓✗ ✓✗✓✓ ✓✗✓✗ ✓✗✓✗ ✓✗ |
9 |
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} + \mathcal{O}(\delta^2) \f] |
||
73 |
/// |
||
74 |
/// The output is |
||
75 |
/// \f[ q_{out} = q \times \frac{3 - ||q_{in}||^2}{2} \f] |
||
76 |
/// |
||
77 |
/// The output quaternion is guaranted to statisfy the following: |
||
78 |
/// \f[ | ||q_{out}|| - 1 | \le \frac{M}{2} ||q_{in}|| ( ||q_{in}||^2 - 1 )^2 \f] |
||
79 |
/// where \f$ M = \frac{3}{4} (1 - \epsilon)^{-\frac{5}{2}} \f$ |
||
80 |
/// and \f$ \epsilon \f$ is the maximum tolerance of \f$ ||q_{in}||^2 - 1 \f$. |
||
81 |
/// |
||
82 |
/// \warning \f$ ||q||^2 - 1 \f$ should already be close to zero. |
||
83 |
/// |
||
84 |
/// \note See |
||
85 |
/// http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html#title3 |
||
86 |
/// to know the reason why the argument is const. |
||
87 |
template<typename D> |
||
88 |
13284 |
void firstOrderNormalize(const Eigen::QuaternionBase<D> & q) |
|
89 |
{ |
||
90 |
typedef typename D::Scalar Scalar; |
||
91 |
✓✗ | 13284 |
const Scalar N2 = q.squaredNorm(); |
92 |
#ifndef NDEBUG |
||
93 |
13284 |
const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon())); |
|
94 |
typedef apply_op_if<less_than_or_equal_to_op,is_floating_point<Scalar>::value,true> static_leq; |
||
95 |
✓✗✗✓ |
13284 |
assert(static_leq::op(math::fabs(static_cast<Scalar>(N2-Scalar(1))), epsilon)); |
96 |
#endif |
||
97 |
13284 |
const Scalar alpha = ((Scalar)3 - N2) / Scalar(2); |
|
98 |
✓✗ | 13284 |
PINOCCHIO_EIGEN_CONST_CAST(D,q).coeffs() *= alpha; |
99 |
#ifndef NDEBUG |
||
100 |
✓✗ | 13284 |
const Scalar M = Scalar(3) * math::pow(Scalar(1)-epsilon, ((Scalar)-Scalar(5))/Scalar(2)) / Scalar(4); |
101 |
✓✗✓✗ ✓✗✗✓ |
13284 |
assert(static_leq::op(math::fabs(static_cast<Scalar>(q.norm() - Scalar(1))), |
102 |
math::max(M * sqrt(N2) * (N2 - Scalar(1))*(N2 - Scalar(1)) / Scalar(2), Eigen::NumTraits<Scalar>::dummy_precision()))); |
||
103 |
#endif |
||
104 |
13284 |
} |
|
105 |
|||
106 |
/// Uniformly random quaternion sphere. |
||
107 |
template<typename Derived> |
||
108 |
118667 |
void uniformRandom(Eigen::QuaternionBase<Derived> & q) |
|
109 |
{ |
||
110 |
typedef typename Derived::Scalar Scalar; |
||
111 |
|||
112 |
// Rotational part |
||
113 |
118667 |
const Scalar u1 = (Scalar)rand() / RAND_MAX; |
|
114 |
118667 |
const Scalar u2 = (Scalar)rand() / RAND_MAX; |
|
115 |
118667 |
const Scalar u3 = (Scalar)rand() / RAND_MAX; |
|
116 |
|||
117 |
118667 |
const Scalar mult1 = sqrt(Scalar(1)-u1); |
|
118 |
118667 |
const Scalar mult2 = sqrt(u1); |
|
119 |
|||
120 |
✓✓✓✗ |
118667 |
static const Scalar PI_value = PI<Scalar>(); |
121 |
118667 |
Scalar s2,c2; SINCOS(Scalar(2)*PI_value*u2,&s2,&c2); |
|
122 |
118667 |
Scalar s3,c3; SINCOS(Scalar(2)*PI_value*u3,&s3,&c3); |
|
123 |
|||
124 |
✓✗ | 118667 |
q.w() = mult1 * s2; |
125 |
✓✗ | 118667 |
q.x() = mult1 * c2; |
126 |
✓✗ | 118667 |
q.y() = mult2 * s3; |
127 |
✓✗ | 118667 |
q.z() = mult2 * c3; |
128 |
118667 |
} |
|
129 |
|||
130 |
namespace internal |
||
131 |
{ |
||
132 |
|||
133 |
template<typename Scalar, bool value = is_floating_point<Scalar>::value> |
||
134 |
struct quaternionbase_assign_impl; |
||
135 |
|||
136 |
template<Eigen::DenseIndex i> |
||
137 |
struct quaternionbase_assign_impl_if_t_negative |
||
138 |
{ |
||
139 |
template<typename Scalar, typename Matrix3, typename QuaternionDerived> |
||
140 |
132376 |
static inline void run(Scalar t, |
|
141 |
Eigen::QuaternionBase<QuaternionDerived> & q, |
||
142 |
const Matrix3 & mat) |
||
143 |
{ |
||
144 |
using pinocchio::math::sqrt; |
||
145 |
|||
146 |
132376 |
Eigen::DenseIndex j = (i+1)%3; |
|
147 |
132376 |
Eigen::DenseIndex k = (j+1)%3; |
|
148 |
|||
149 |
132376 |
t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0)); |
|
150 |
132376 |
q.coeffs().coeffRef(i) = Scalar(0.5) * t; |
|
151 |
132376 |
t = Scalar(0.5)/t; |
|
152 |
132376 |
q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t; |
|
153 |
132376 |
q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t; |
|
154 |
132376 |
q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t; |
|
155 |
132376 |
} |
|
156 |
}; |
||
157 |
|||
158 |
struct quaternionbase_assign_impl_if_t_positive |
||
159 |
{ |
||
160 |
template<typename Scalar, typename Matrix3, typename QuaternionDerived> |
||
161 |
41608 |
static inline void run(Scalar t, |
|
162 |
Eigen::QuaternionBase<QuaternionDerived> & q, |
||
163 |
const Matrix3 & mat) |
||
164 |
{ |
||
165 |
using pinocchio::math::sqrt; |
||
166 |
|||
167 |
41608 |
t = sqrt(t + Scalar(1.0)); |
|
168 |
41608 |
q.w() = Scalar(0.5)*t; |
|
169 |
41608 |
t = Scalar(0.5)/t; |
|
170 |
41608 |
q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t; |
|
171 |
41608 |
q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t; |
|
172 |
41608 |
q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t; |
|
173 |
41608 |
} |
|
174 |
}; |
||
175 |
|||
176 |
template<typename Scalar> |
||
177 |
struct quaternionbase_assign_impl<Scalar, true> |
||
178 |
{ |
||
179 |
template<typename Matrix3, typename QuaternionDerived> |
||
180 |
107796 |
static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q, |
|
181 |
const Matrix3 & mat) |
||
182 |
{ |
||
183 |
using pinocchio::math::sqrt; |
||
184 |
|||
185 |
107796 |
Scalar t = mat.trace(); |
|
186 |
✓✓ | 107796 |
if (t > Scalar(0.)) |
187 |
41608 |
quaternionbase_assign_impl_if_t_positive::run(t,q,mat); |
|
188 |
else |
||
189 |
{ |
||
190 |
66188 |
Eigen::DenseIndex i = 0; |
|
191 |
✓✓ | 66188 |
if (mat.coeff(1,1) > mat.coeff(0,0)) |
192 |
32360 |
i = 1; |
|
193 |
✓✓ | 66188 |
if (mat.coeff(2,2) > mat.coeff(i,i)) |
194 |
22549 |
i = 2; |
|
195 |
|||
196 |
✓✓ | 66188 |
if(i==0) |
197 |
22040 |
quaternionbase_assign_impl_if_t_negative<0>::run(t,q,mat); |
|
198 |
✓✓ | 44148 |
else if(i==1) |
199 |
21599 |
quaternionbase_assign_impl_if_t_negative<1>::run(t,q,mat); |
|
200 |
else |
||
201 |
22549 |
quaternionbase_assign_impl_if_t_negative<2>::run(t,q,mat); |
|
202 |
} |
||
203 |
107796 |
} |
|
204 |
}; |
||
205 |
|||
206 |
} // namespace internal |
||
207 |
|||
208 |
template<typename D, typename Matrix3> |
||
209 |
107796 |
void assignQuaternion(Eigen::QuaternionBase<D> & quat, |
|
210 |
const Eigen::MatrixBase<Matrix3> & R) |
||
211 |
{ |
||
212 |
107796 |
internal::quaternionbase_assign_impl<typename Matrix3::Scalar>::run(PINOCCHIO_EIGEN_CONST_CAST(D,quat), |
|
213 |
R.derived()); |
||
214 |
107796 |
} |
|
215 |
|||
216 |
/// |
||
217 |
/// \brief Check whether the input quaternion is Normalized within the given precision. |
||
218 |
/// |
||
219 |
/// \param[in] quat Input quaternion |
||
220 |
/// \param[in] prec Required precision |
||
221 |
/// |
||
222 |
/// \returns true if quat is normalized within the precision prec. |
||
223 |
/// |
||
224 |
template<typename Quaternion> |
||
225 |
64742 |
inline bool isNormalized(const Eigen::QuaternionBase<Quaternion> & quat, |
|
226 |
const typename Quaternion::Coefficients::RealScalar & prec = |
||
227 |
Eigen::NumTraits< typename Quaternion::Coefficients::RealScalar >::dummy_precision()) |
||
228 |
{ |
||
229 |
64742 |
return pinocchio::isNormalized(quat.coeffs(),prec); |
|
230 |
} |
||
231 |
|||
232 |
} // namespace quaternion |
||
233 |
|||
234 |
} |
||
235 |
#endif //#ifndef __pinocchio_math_quaternion_hpp__ |
Generated by: GCOVR (Version 4.2) |