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 %

 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  19 #include  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 34  typename D1::Scalar 35  angleBetweenQuaternions(const Eigen::QuaternionBase & q1, 36  const Eigen::QuaternionBase & 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(); 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 59 9  bool defineSameRotation(const Eigen::QuaternionBase & q1, 60  const Eigen::QuaternionBase & q2, 61  const typename D1::RealScalar & prec = Eigen::NumTraits::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 88 13284  void firstOrderNormalize(const Eigen::QuaternionBase & 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::epsilon())); 94  typedef apply_op_if::value,true> static_leq; 95 ✓✗✗✓ 13284  assert(static_leq::op(math::fabs(static_cast(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(q.norm() - Scalar(1))), 102  math::max(M * sqrt(N2) * (N2 - Scalar(1))*(N2 - Scalar(1)) / Scalar(2), Eigen::NumTraits::dummy_precision()))); 103 #endif 104 13284  } 105 106  /// Uniformly random quaternion sphere. 107  template 108 118667  void uniformRandom(Eigen::QuaternionBase & 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(); 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::value> 134  struct quaternionbase_assign_impl; 135 136  template 137  struct quaternionbase_assign_impl_if_t_negative 138  { 139  template 140 132376  static inline void run(Scalar t, 141  Eigen::QuaternionBase & 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 161 41608  static inline void run(Scalar t, 162  Eigen::QuaternionBase & 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 177  struct quaternionbase_assign_impl 178  { 179  template 180 107796  static inline void run(Eigen::QuaternionBase & 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 209 107796  void assignQuaternion(Eigen::QuaternionBase & quat, 210  const Eigen::MatrixBase & R) 211  { 212 107796  internal::quaternionbase_assign_impl::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 225 64742  inline bool isNormalized(const Eigen::QuaternionBase & 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)