| 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 |