GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/math/quaternion.hpp Lines: 63 63 100.0 %
Date: 2024-01-23 21:41:47 Branches: 36 58 62.1 %

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(const 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
      PINOCCHIO_EIGEN_CONST_CAST(Derived,q).w() = mult1 * s2;
125
118667
      PINOCCHIO_EIGEN_CONST_CAST(Derived,q).x() = mult1 * c2;
126
118667
      PINOCCHIO_EIGEN_CONST_CAST(Derived,q).y() = mult2 * s3;
127
118667
      PINOCCHIO_EIGEN_CONST_CAST(Derived,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__