autodiff/casadi/math/quaternion.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2020 INRIA
3 //
4 
5 #ifndef __pinocchio_autodiff_casadi_math_quaternion_hpp__
6 #define __pinocchio_autodiff_casadi_math_quaternion_hpp__
7 
8 #include "pinocchio/math/quaternion.hpp"
9 
10 namespace pinocchio
11 {
12  namespace quaternion
13  {
14  namespace internal
15  {
16 
17  template<typename _Scalar>
18  struct quaternionbase_assign_impl< ::casadi::Matrix<_Scalar>, false >
19  {
20  typedef ::casadi::Matrix<_Scalar> Scalar;
21  template<typename Matrix3, typename QuaternionDerived>
22  static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q,
23  const Matrix3 & mat)
24  {
25  typedef typename Eigen::internal::traits<QuaternionDerived>::Coefficients QuatCoefficients;
26 
27  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(QuatCoefficients) QuatCoefficientsPlainType;
28  typedef Eigen::Quaternion<Scalar,QuatCoefficientsPlainType::Options> QuaternionPlain;
29  QuaternionPlain quat_t_positive;
30 
31  Scalar t = mat.trace();
32  quaternionbase_assign_impl_if_t_positive::run(t,quat_t_positive,mat);
33 
34  QuaternionPlain quat_t_negative_0, quat_t_negative_1, quat_t_negative_2;
35 
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);
39 
40  // Build the expression graph
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));
44 
45  for(Eigen::DenseIndex k = 0; k < 4; ++k)
46  {
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),
52  t_is_negative_cond1);
53 
54  q.coeffs().coeffRef(k) = Scalar::if_else(t_greater_than_zero,
55  quat_t_positive.coeffs().coeff(k),
56  t_is_negative_cond2);
57  }
58  }
59  };
60 
61  } // namespace internal
62 
63  } // namespace quaternion
64 
65 } // namespace pinocchio
66 
67 #endif // ifndef __pinocchio_autodiff_casadi_math_quaternion_hpp__
static void run(Eigen::QuaternionBase< QuaternionDerived > &q, const Matrix3 &mat)
static void run(Scalar t, Eigen::QuaternionBase< QuaternionDerived > &q, const Matrix3 &mat)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
SE3::Scalar Scalar
Definition: conversions.cpp:13
Main pinocchio namespace.
Definition: timings.cpp:28
PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(const ModelTpl< Scalar
Integrate a configuration vector for the specified model for a tangent vector during one unit time...
Transform3f t
static void run(Scalar t, Eigen::QuaternionBase< QuaternionDerived > &q, const Matrix3 &mat)


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:32