pinocchio  2.2.1-dirty
quaternion.hpp
1 //
2 // Copyright (c) 2016-2019 CNRS, INRIA
3 //
4 
5 #ifndef __pinocchio_math_quaternion_hpp__
6 #define __pinocchio_math_quaternion_hpp__
7 
8 #include "pinocchio/math/fwd.hpp"
9 #include "pinocchio/math/comparison-operators.hpp"
10 #include "pinocchio/math/sincos.hpp"
11 #include <boost/type_traits.hpp>
12 
13 #include <Eigen/Geometry>
14 
15 namespace pinocchio
16 {
17  namespace quaternion
18  {
27  template<typename D1, typename D2>
28  typename D1::Scalar
29  angleBetweenQuaternions(const Eigen::QuaternionBase<D1> & q1,
30  const Eigen::QuaternionBase<D2> & q2)
31  {
32  typedef typename D1::Scalar Scalar;
33  const Scalar innerprod = q1.dot(q2);
34  Scalar theta = math::acos(innerprod);
35  static const Scalar PI_value = PI<Scalar>();
36 
37  if(innerprod < 0)
38  return PI_value - theta;
39 
40  return theta;
41  }
42 
52  template<typename D1, typename D2>
53  bool defineSameRotation(const Eigen::QuaternionBase<D1> & q1,
54  const Eigen::QuaternionBase<D2> & q2,
55  const typename D1::RealScalar & prec = Eigen::NumTraits<typename D1::Scalar>::dummy_precision())
56  {
57  return (q1.coeffs().isApprox(q2.coeffs(), prec) || q1.coeffs().isApprox(-q2.coeffs(), prec) );
58  }
59 
81  template<typename D>
82  void firstOrderNormalize(const Eigen::QuaternionBase<D> & q)
83  {
84  typedef typename D::Scalar Scalar;
85  const Scalar N2 = q.squaredNorm();
86 #ifndef NDEBUG
87  const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
89  assert(static_leq::op(math::fabs(N2-1.), epsilon));
90 #endif
91  const Scalar alpha = ((Scalar)3 - N2) / Scalar(2);
92  PINOCCHIO_EIGEN_CONST_CAST(D,q).coeffs() *= alpha;
93 #ifndef NDEBUG
94  const Scalar M = Scalar(3) * math::pow(Scalar(1)-epsilon, ((Scalar)-Scalar(5))/Scalar(2)) / Scalar(4);
95  assert(static_leq::op(math::fabs(q.norm() - Scalar(1)),
96  math::max(M * sqrt(N2) * (N2 - Scalar(1))*(N2 - Scalar(1)) / Scalar(2), Eigen::NumTraits<Scalar>::dummy_precision())));
97 #endif
98  }
99 
101  template<typename Derived>
102  void uniformRandom(const Eigen::QuaternionBase<Derived> & q)
103  {
104  typedef typename Derived::Scalar Scalar;
105 
106  // Rotational part
107  const Scalar u1 = (Scalar)rand() / RAND_MAX;
108  const Scalar u2 = (Scalar)rand() / RAND_MAX;
109  const Scalar u3 = (Scalar)rand() / RAND_MAX;
110 
111  const Scalar mult1 = sqrt(Scalar(1)-u1);
112  const Scalar mult2 = sqrt(u1);
113 
114  static const Scalar PI_value = PI<Scalar>();
115  Scalar s2,c2; SINCOS(Scalar(2)*PI_value*u2,&s2,&c2);
116  Scalar s3,c3; SINCOS(Scalar(2)*PI_value*u3,&s3,&c3);
117 
118  PINOCCHIO_EIGEN_CONST_CAST(Derived,q).w() = mult1 * s2;
119  PINOCCHIO_EIGEN_CONST_CAST(Derived,q).x() = mult1 * c2;
120  PINOCCHIO_EIGEN_CONST_CAST(Derived,q).y() = mult2 * s3;
121  PINOCCHIO_EIGEN_CONST_CAST(Derived,q).z() = mult2 * c3;
122  }
123 
124  namespace internal
125  {
126 
127  template<Eigen::DenseIndex i>
128  struct quaternionbase_assign_impl_if_t_negative
129  {
130  template<typename Scalar, typename Matrix3, typename QuaternionDerived>
131  static inline void run(Scalar t,
132  Eigen::QuaternionBase<QuaternionDerived> & q,
133  const Matrix3 & mat)
134  {
135  using pinocchio::math::sqrt;
136 
137  Eigen::DenseIndex j = (i+1)%3;
138  Eigen::DenseIndex k = (j+1)%3;
139 
140  t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
141  q.coeffs().coeffRef(i) = Scalar(0.5) * t;
142  t = Scalar(0.5)/t;
143  q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
144  q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
145  q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
146  }
147  };
148 
149  struct quaternionbase_assign_impl_if_t_positive
150  {
151  template<typename Scalar, typename Matrix3, typename QuaternionDerived>
152  static inline void run(Scalar t,
153  Eigen::QuaternionBase<QuaternionDerived> & q,
154  const Matrix3 & mat)
155  {
156  using pinocchio::math::sqrt;
157 
158  t = sqrt(t + Scalar(1.0));
159  q.w() = Scalar(0.5)*t;
160  t = Scalar(0.5)/t;
161  q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
162  q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
163  q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
164  }
165  };
166 
167  template<typename Scalar>
168  struct quaternionbase_assign_impl
169  {
170  template<typename Matrix3, typename QuaternionDerived>
171  static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q,
172  const Matrix3 & mat)
173  {
174  using pinocchio::math::sqrt;
175 
176  Scalar t = mat.trace();
177  if (t > Scalar(0))
178  quaternionbase_assign_impl_if_t_positive::run(t,q,mat);
179  else
180  {
181  Eigen::DenseIndex i = 0;
182  if (mat.coeff(1,1) > mat.coeff(0,0))
183  i = 1;
184  if (mat.coeff(2,2) > mat.coeff(i,i))
185  i = 2;
186 
187  if(i==0)
188  quaternionbase_assign_impl_if_t_negative<0>::run(t,q,mat);
189  else if(i==1)
190  quaternionbase_assign_impl_if_t_negative<1>::run(t,q,mat);
191  else
192  quaternionbase_assign_impl_if_t_negative<2>::run(t,q,mat);
193  }
194  }
195  };
196 
197  } // namespace internal
198 
199  template<typename D, typename Matrix3>
200  void assignQuaternion(Eigen::QuaternionBase<D> & quat,
201  const Eigen::MatrixBase<Matrix3> & R)
202  {
203  internal::quaternionbase_assign_impl<typename Matrix3::Scalar>::run(PINOCCHIO_EIGEN_CONST_CAST(D,quat),
204  R.derived());
205  }
206 
207  } // namespace quaternion
208 
209 }
210 #endif //#ifndef __pinocchio_math_quaternion_hpp__
bool defineSameRotation(const Eigen::QuaternionBase< D1 > &q1, const Eigen::QuaternionBase< D2 > &q2, const typename D1::RealScalar &prec=Eigen::NumTraits< typename D1::Scalar >::dummy_precision())
Check if two quaternions define the same rotations.
Definition: quaternion.hpp:53
D1::Scalar angleBetweenQuaternions(const Eigen::QuaternionBase< D1 > &q1, const Eigen::QuaternionBase< D2 > &q2)
Compute the minimal angle between q1 and q2.
Definition: quaternion.hpp:29
void firstOrderNormalize(const Eigen::QuaternionBase< D > &q)
Definition: quaternion.hpp:82
void SINCOS(const Scalar &a, Scalar *sa, Scalar *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:27
void uniformRandom(const Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Definition: quaternion.hpp:102
Main pinocchio namespace.
Definition: treeview.dox:24