5 #ifndef __pinocchio_autodiff_cppad_math_quaternion_hpp__
6 #define __pinocchio_autodiff_cppad_math_quaternion_hpp__
8 #include "pinocchio/math/quaternion.hpp"
17 template<
typename _Scalar>
18 struct quaternionbase_assign_impl<CppAD::AD<_Scalar>, false >
20 typedef _Scalar Scalar;
21 typedef CppAD::AD<Scalar> ADScalar;
22 template<
typename Matrix3,
typename QuaternionDerived>
23 static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q,
26 typedef typename Eigen::internal::traits<QuaternionDerived>::Coefficients QuatCoefficients;
28 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(QuatCoefficients) QuatCoefficientsPlainType;
29 typedef Eigen::Quaternion<ADScalar,QuatCoefficientsPlainType::Options> QuaternionPlain;
30 QuaternionPlain quat_t_positive;
32 ADScalar t = mat.trace();
33 quaternionbase_assign_impl_if_t_positive::run(t,quat_t_positive,mat);
35 QuaternionPlain quat_t_negative_0, quat_t_negative_1, quat_t_negative_2;
37 quaternionbase_assign_impl_if_t_negative<0>::run(t,quat_t_negative_0,mat);
38 quaternionbase_assign_impl_if_t_negative<1>::run(t,quat_t_negative_1,mat);
39 quaternionbase_assign_impl_if_t_negative<2>::run(t,quat_t_negative_2,mat);
42 for(Eigen::DenseIndex k = 0; k < 4; ++k)
44 ADScalar t_is_negative_cond1 = CppAD::CondExpGt<Scalar>(mat.coeff(1,1), mat.coeff(0,0),
45 quat_t_negative_1.coeffs().coeff(k),
46 quat_t_negative_0.coeffs().coeff(k));
48 ADScalar t_is_negative_cond2 = CppAD::CondExpGt<Scalar>(mat.coeff(2,2), mat.coeff(0,0),
49 quat_t_negative_2.coeffs().coeff(k),
50 CppAD::CondExpGt<Scalar>(mat.coeff(1,1), mat.coeff(0,0),
51 CppAD::CondExpGt<Scalar>(mat.coeff(2,2), mat.coeff(1,1),
52 quat_t_negative_2.coeffs().coeff(k),
54 t_is_negative_cond1));
55 q.coeffs().coeffRef(k) = CppAD::CondExpGt<Scalar>(t,ADScalar(0),
56 quat_t_positive.coeffs().coeff(k),
68 #endif // ifndef __pinocchio_autodiff_casadi_math_quaternion_hpp__