5 #ifndef __pinocchio_autodiff_casadi_math_quaternion_hpp__ 6 #define __pinocchio_autodiff_casadi_math_quaternion_hpp__ 8 #include "pinocchio/math/quaternion.hpp" 17 template<
typename _Scalar>
18 struct quaternionbase_assign_impl< ::casadi::Matrix<_Scalar>, false >
20 typedef ::casadi::Matrix<_Scalar> Scalar;
21 template<
typename Matrix3,
typename QuaternionDerived>
22 static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q,
25 typedef typename Eigen::internal::traits<QuaternionDerived>::Coefficients QuatCoefficients;
27 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(QuatCoefficients) QuatCoefficientsPlainType;
28 typedef Eigen::Quaternion<Scalar,QuatCoefficientsPlainType::Options> QuaternionPlain;
29 QuaternionPlain quat_t_positive;
31 Scalar t = mat.trace();
32 quaternionbase_assign_impl_if_t_positive::run(t,quat_t_positive,mat);
34 QuaternionPlain quat_t_negative_0, quat_t_negative_1, quat_t_negative_2;
36 quaternionbase_assign_impl_if_t_negative<0>::run(t,quat_t_negative_0,mat);
37 quaternionbase_assign_impl_if_t_negative<1>::run(t,quat_t_negative_1,mat);
38 quaternionbase_assign_impl_if_t_negative<2>::run(t,quat_t_negative_2,mat);
41 const Scalar t_greater_than_zero = t > Scalar(0);
42 const Scalar cond1 = mat.coeff(1,1) > mat.coeff(0,0);
43 const Scalar cond2 = (cond1 && mat.coeff(2,2) > mat.coeff(1,1)) || (mat.coeff(2,2) > mat.coeff(0,0));
45 for(Eigen::DenseIndex k = 0; k < 4; ++k)
47 Scalar t_is_negative_cond1 = Scalar::if_else(cond1,
48 quat_t_negative_1.coeffs().coeff(k),
49 quat_t_negative_0.coeffs().coeff(k));
50 Scalar t_is_negative_cond2 = Scalar::if_else(cond2,
51 quat_t_negative_2.coeffs().coeff(k),
54 q.coeffs().coeffRef(k) = Scalar::if_else(t_greater_than_zero,
55 quat_t_positive.coeffs().coeff(k),
67 #endif // ifndef __pinocchio_autodiff_casadi_math_quaternion_hpp__ Main pinocchio namespace.