Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/math/quaternion.hpp	Lines:	63	63	100.0 %
Date:	2024-04-26 13:14:21	Branches:	36	58	62.1 %


//
// Copyright (c) 2016-2020 CNRS INRIA
//

#ifndef __pinocchio_math_quaternion_hpp__
#define __pinocchio_math_quaternion_hpp__

#ifndef PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE
  #define PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE 1e-8
#endif

#include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/comparison-operators.hpp"
#include "pinocchio/math/matrix.hpp"
#include "pinocchio/math/sincos.hpp"
#include "pinocchio/utils/static-if.hpp"

#include <boost/type_traits.hpp>
#include <Eigen/Geometry>

namespace pinocchio
{
  namespace quaternion
  {
    ///
    /// \brief Compute the minimal angle between q1 and q2.
    ///
    /// \param[in] q1 input quaternion.
    /// \param[in] q2 input quaternion.
    ///
    /// \return angle between the two quaternions
    ///
    template<typename D1, typename D2>
    typename D1::Scalar
    angleBetweenQuaternions(const Eigen::QuaternionBase<D1> & q1,
                            const Eigen::QuaternionBase<D2> & q2)
    {
      typedef typename D1::Scalar Scalar;
      const Scalar innerprod = q1.dot(q2);
      Scalar theta = math::acos(innerprod);
      static const Scalar PI_value = PI<Scalar>();

      theta = internal::if_then_else(internal::LT, innerprod, Scalar(0),
                                     PI_value - theta,
                                     theta);
      return theta;
    }

    ///
    /// \brief Check if two quaternions define the same rotations.
    /// \note Two quaternions define the same rotation iff q1 == q2 OR q1 == -q2.
    ///
    /// \param[in] q1 input quaternion.
    /// \param[in] q2 input quaternion.
    ///
    /// \return Return true if the two input quaternions define the same rotation.
    ///
    template<typename D1, typename D2>
    bool defineSameRotation(const Eigen::QuaternionBase<D1> & q1,
                            const Eigen::QuaternionBase<D2> & q2,
                            const typename D1::RealScalar & prec = Eigen::NumTraits<typename D1::Scalar>::dummy_precision())
    {
      return (q1.coeffs().isApprox(q2.coeffs(), prec) || q1.coeffs().isApprox(-q2.coeffs(), prec) );
    }

    /// Approximately normalize by applying the first order limited development
    /// of the normalization function.
    ///
    /// Only additions and multiplications are required. Neither square root nor
    /// division are used (except a division by 2). Let \f$ \delta = ||q||^2 - 1 \f$.
    /// Using the following limited development:
    /// \f[ \frac{1}{||q||} = (1 + \delta)^{-\frac{1}{2}} = 1 - \frac{\delta}{2} + \mathcal{O}(\delta^2) \f]
    ///
    /// The output is
    /// \f[ q_{out} = q \times \frac{3 - ||q_{in}||^2}{2} \f]
    ///
    /// The output quaternion is guaranted to statisfy the following:
    /// \f[ | ||q_{out}|| - 1 | \le \frac{M}{2} ||q_{in}|| ( ||q_{in}||^2 - 1 )^2 \f]
    /// where \f$ M = \frac{3}{4} (1 - \epsilon)^{-\frac{5}{2}} \f$
    /// and \f$ \epsilon \f$ is the maximum tolerance of \f$ ||q_{in}||^2 - 1 \f$.
    ///
    /// \warning \f$ ||q||^2 - 1 \f$ should already be close to zero.
    ///
    /// \note See
    /// http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html#title3
    /// to know the reason why the argument is const.
    template<typename D>
    void firstOrderNormalize(const Eigen::QuaternionBase<D> & q)
    {
      typedef typename D::Scalar Scalar;
      const Scalar N2 = q.squaredNorm();
#ifndef NDEBUG
      const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
      typedef apply_op_if<less_than_or_equal_to_op,is_floating_point<Scalar>::value,true> static_leq;
      assert(static_leq::op(math::fabs(static_cast<Scalar>(N2-Scalar(1))), epsilon));
#endif
      const Scalar alpha = ((Scalar)3 - N2) / Scalar(2);
      PINOCCHIO_EIGEN_CONST_CAST(D,q).coeffs() *= alpha;
#ifndef NDEBUG
      const Scalar M = Scalar(3) * math::pow(Scalar(1)-epsilon, ((Scalar)-Scalar(5))/Scalar(2)) / Scalar(4);
      assert(static_leq::op(math::fabs(static_cast<Scalar>(q.norm() - Scalar(1))),
                            math::max(M * sqrt(N2) * (N2 - Scalar(1))*(N2 - Scalar(1)) / Scalar(2), Eigen::NumTraits<Scalar>::dummy_precision())));
#endif
    }

    /// Uniformly random quaternion sphere.
    template<typename Derived>
    void uniformRandom(Eigen::QuaternionBase<Derived> & q)
    {
      typedef typename Derived::Scalar Scalar;

      // Rotational part
      const Scalar u1 = (Scalar)rand() / RAND_MAX;
      const Scalar u2 = (Scalar)rand() / RAND_MAX;
      const Scalar u3 = (Scalar)rand() / RAND_MAX;

      const Scalar mult1 = sqrt(Scalar(1)-u1);
      const Scalar mult2 = sqrt(u1);

      static const Scalar PI_value = PI<Scalar>();
      Scalar s2,c2; SINCOS(Scalar(2)*PI_value*u2,&s2,&c2);
      Scalar s3,c3; SINCOS(Scalar(2)*PI_value*u3,&s3,&c3);

      q.w() = mult1 * s2;
      q.x() = mult1 * c2;
      q.y() = mult2 * s3;
      q.z() = mult2 * c3;
    }

    namespace internal
    {

      template<typename Scalar, bool value = is_floating_point<Scalar>::value>
      struct quaternionbase_assign_impl;

      template<Eigen::DenseIndex i>
      struct quaternionbase_assign_impl_if_t_negative
      {
        template<typename Scalar, typename Matrix3, typename QuaternionDerived>
        static inline void run(Scalar t,
                               Eigen::QuaternionBase<QuaternionDerived> & q,
                               const Matrix3 & mat)
        {
          using pinocchio::math::sqrt;

          Eigen::DenseIndex j = (i+1)%3;
          Eigen::DenseIndex k = (j+1)%3;

          t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
          q.coeffs().coeffRef(i) = Scalar(0.5) * t;
          t = Scalar(0.5)/t;
          q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
          q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
          q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
        }
      };

      struct quaternionbase_assign_impl_if_t_positive
      {
        template<typename Scalar, typename Matrix3, typename QuaternionDerived>
        static inline void run(Scalar t,
                               Eigen::QuaternionBase<QuaternionDerived> & q,
                               const Matrix3 & mat)
        {
          using pinocchio::math::sqrt;

          t = sqrt(t + Scalar(1.0));
          q.w() = Scalar(0.5)*t;
          t = Scalar(0.5)/t;
          q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
          q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
          q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
        }
      };

      template<typename Scalar>
      struct quaternionbase_assign_impl<Scalar, true>
      {
        template<typename Matrix3, typename QuaternionDerived>
        static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q,
                               const Matrix3 & mat)
        {
          using pinocchio::math::sqrt;

          Scalar t = mat.trace();
          if (t > Scalar(0.))
            quaternionbase_assign_impl_if_t_positive::run(t,q,mat);
          else
          {
            Eigen::DenseIndex i = 0;
            if (mat.coeff(1,1) > mat.coeff(0,0))
              i = 1;
            if (mat.coeff(2,2) > mat.coeff(i,i))
              i = 2;

            if(i==0)
              quaternionbase_assign_impl_if_t_negative<0>::run(t,q,mat);
            else if(i==1)
              quaternionbase_assign_impl_if_t_negative<1>::run(t,q,mat);
            else
              quaternionbase_assign_impl_if_t_negative<2>::run(t,q,mat);
          }
        }
      };

    } // namespace internal

    template<typename D, typename Matrix3>
    void assignQuaternion(Eigen::QuaternionBase<D> & quat,
                          const Eigen::MatrixBase<Matrix3> & R)
    {
      internal::quaternionbase_assign_impl<typename Matrix3::Scalar>::run(PINOCCHIO_EIGEN_CONST_CAST(D,quat),
                                                                          R.derived());
    }

    ///
    /// \brief Check whether the input quaternion is Normalized within the given precision.
    ///
    /// \param[in] quat Input quaternion
    /// \param[in] prec Required precision
    ///
    /// \returns true if quat is normalized within the precision prec.
    ///
    template<typename Quaternion>
    inline bool isNormalized(const Eigen::QuaternionBase<Quaternion> & quat,
                             const typename Quaternion::Coefficients::RealScalar & prec =
                             Eigen::NumTraits< typename Quaternion::Coefficients::RealScalar >::dummy_precision())
    {
      return pinocchio::isNormalized(quat.coeffs(),prec);
    }

  } // namespace quaternion

}
#endif //#ifndef __pinocchio_math_quaternion_hpp__


Generated by: GCOVR (Version 4.2)

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_valueu2,&s2,&c2);
122		118667	Scalar s3,c3; SINCOS(Scalar(2)PI_valueu3,&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__