5 #ifndef __pinocchio_math_quaternion_hpp__ 6 #define __pinocchio_math_quaternion_hpp__ 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> 13 #include <Eigen/Geometry> 27 template<
typename D1,
typename D2>
30 const Eigen::QuaternionBase<D2> & q2)
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>();
38 return PI_value - theta;
52 template<
typename D1,
typename D2>
54 const Eigen::QuaternionBase<D2> & q2,
55 const typename D1::RealScalar & prec = Eigen::NumTraits<typename D1::Scalar>::dummy_precision())
57 return (q1.coeffs().isApprox(q2.coeffs(), prec) || q1.coeffs().isApprox(-q2.coeffs(), prec) );
84 typedef typename D::Scalar Scalar;
85 const Scalar N2 = q.squaredNorm();
87 const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
89 assert(static_leq::op(math::fabs(N2-1.), epsilon));
91 const Scalar alpha = ((Scalar)3 - N2) / Scalar(2);
92 PINOCCHIO_EIGEN_CONST_CAST(D,q).coeffs() *= alpha;
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())));
101 template<
typename Derived>
104 typedef typename Derived::Scalar Scalar;
107 const Scalar u1 = (Scalar)rand() / RAND_MAX;
108 const Scalar u2 = (Scalar)rand() / RAND_MAX;
109 const Scalar u3 = (Scalar)rand() / RAND_MAX;
111 const Scalar mult1 = sqrt(Scalar(1)-u1);
112 const Scalar mult2 = sqrt(u1);
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);
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;
127 template<Eigen::DenseIndex i>
128 struct quaternionbase_assign_impl_if_t_negative
130 template<
typename Scalar,
typename Matrix3,
typename QuaternionDerived>
131 static inline void run(Scalar t,
132 Eigen::QuaternionBase<QuaternionDerived> & q,
135 using pinocchio::math::sqrt;
137 Eigen::DenseIndex j = (i+1)%3;
138 Eigen::DenseIndex k = (j+1)%3;
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;
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;
149 struct quaternionbase_assign_impl_if_t_positive
151 template<
typename Scalar,
typename Matrix3,
typename QuaternionDerived>
152 static inline void run(Scalar t,
153 Eigen::QuaternionBase<QuaternionDerived> & q,
156 using pinocchio::math::sqrt;
158 t = sqrt(t + Scalar(1.0));
159 q.w() = 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;
167 template<
typename Scalar>
168 struct quaternionbase_assign_impl
170 template<
typename Matrix3,
typename QuaternionDerived>
171 static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q,
174 using pinocchio::math::sqrt;
176 Scalar t = mat.trace();
178 quaternionbase_assign_impl_if_t_positive::run(t,q,mat);
181 Eigen::DenseIndex i = 0;
182 if (mat.coeff(1,1) > mat.coeff(0,0))
184 if (mat.coeff(2,2) > mat.coeff(i,i))
188 quaternionbase_assign_impl_if_t_negative<0>::run(t,q,mat);
190 quaternionbase_assign_impl_if_t_negative<1>::run(t,q,mat);
192 quaternionbase_assign_impl_if_t_negative<2>::run(t,q,mat);
199 template<
typename D,
typename Matrix3>
200 void assignQuaternion(Eigen::QuaternionBase<D> & quat,
201 const Eigen::MatrixBase<Matrix3> & R)
203 internal::quaternionbase_assign_impl<typename Matrix3::Scalar>::run(PINOCCHIO_EIGEN_CONST_CAST(D,quat),
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.
D1::Scalar angleBetweenQuaternions(const Eigen::QuaternionBase< D1 > &q1, const Eigen::QuaternionBase< D2 > &q2)
Compute the minimal angle between q1 and q2.
void firstOrderNormalize(const Eigen::QuaternionBase< D > &q)
void SINCOS(const Scalar &a, Scalar *sa, Scalar *ca)
Computes sin/cos values of a given input scalar.
void uniformRandom(const Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Main pinocchio namespace.