| Directory: | ./ |
|---|---|
| File: | include/pinocchio/autodiff/casadi/math/quaternion.hpp |
| Date: | 2025-04-30 16:14:33 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 20 | 22 | 90.9% |
| Branches: | 37 | 74 | 50.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 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 | |||
| 15 | namespace internal | ||
| 16 | { | ||
| 17 | |||
| 18 | template<typename _Scalar> | ||
| 19 | struct quaternionbase_assign_impl<::casadi::Matrix<_Scalar>, false> | ||
| 20 | { | ||
| 21 | typedef ::casadi::Matrix<_Scalar> Scalar; | ||
| 22 | template<typename Matrix3, typename QuaternionDerived> | ||
| 23 | 1 | static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q, const Matrix3 & mat) | |
| 24 | { | ||
| 25 | typedef | ||
| 26 | typename Eigen::internal::traits<QuaternionDerived>::Coefficients QuatCoefficients; | ||
| 27 | |||
| 28 | typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(QuatCoefficients) QuatCoefficientsPlainType; | ||
| 29 | typedef Eigen::Quaternion<Scalar, QuatCoefficientsPlainType::Options> QuaternionPlain; | ||
| 30 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | QuaternionPlain quat_t_positive; |
| 31 | |||
| 32 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Scalar t = mat.trace(); |
| 33 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | quaternionbase_assign_impl_if_t_positive::run(t, quat_t_positive, mat); |
| 34 | |||
| 35 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | QuaternionPlain quat_t_negative_0, quat_t_negative_1, quat_t_negative_2; |
| 36 | |||
| 37 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | quaternionbase_assign_impl_if_t_negative<0>::run(t, quat_t_negative_0, mat); |
| 38 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | quaternionbase_assign_impl_if_t_negative<1>::run(t, quat_t_negative_1, mat); |
| 39 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | quaternionbase_assign_impl_if_t_negative<2>::run(t, quat_t_negative_2, mat); |
| 40 | |||
| 41 | // Build the expression graph | ||
| 42 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | const Scalar t_greater_than_zero = t > Scalar(0); |
| 43 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | const Scalar cond1 = mat.coeff(1, 1) > mat.coeff(0, 0); |
| 44 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
3 | const Scalar cond2 = |
| 45 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | (cond1 && mat.coeff(2, 2) > mat.coeff(1, 1)) || (mat.coeff(2, 2) > mat.coeff(0, 0)); |
| 46 | |||
| 47 |
2/2✓ Branch 1 taken 4 times.
✓ Branch 2 taken 1 times.
|
5 | for (Eigen::DenseIndex k = 0; k < 4; ++k) |
| 48 | { | ||
| 49 | 4 | Scalar t_is_negative_cond1 = Scalar::if_else( | |
| 50 |
3/6✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
|
4 | cond1, quat_t_negative_1.coeffs().coeff(k), quat_t_negative_0.coeffs().coeff(k)); |
| 51 | 4 | Scalar t_is_negative_cond2 = | |
| 52 |
2/4✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
|
4 | Scalar::if_else(cond2, quat_t_negative_2.coeffs().coeff(k), t_is_negative_cond1); |
| 53 | |||
| 54 |
2/6✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
|
4 | q.coeffs().coeffRef(k) = Scalar::if_else( |
| 55 |
2/4✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
|
4 | t_greater_than_zero, quat_t_positive.coeffs().coeff(k), t_is_negative_cond2); |
| 56 | } | ||
| 57 | 1 | } | |
| 58 | }; | ||
| 59 | |||
| 60 | } // namespace internal | ||
| 61 | |||
| 62 | } // namespace quaternion | ||
| 63 | |||
| 64 | } // namespace pinocchio | ||
| 65 | |||
| 66 | namespace Eigen | ||
| 67 | { | ||
| 68 | namespace internal | ||
| 69 | { | ||
| 70 | template<class Scalar, int Options> | ||
| 71 | struct quaternionbase_assign_impl<Eigen::Matrix<::casadi::Matrix<Scalar>, 3, 3, Options>, 3, 3> | ||
| 72 | { | ||
| 73 | template<typename QuaternionDerived> | ||
| 74 | ✗ | EIGEN_DEVICE_FUNC static inline void run( | |
| 75 | QuaternionBase<QuaternionDerived> & q, | ||
| 76 | const Eigen::Matrix<::casadi::Matrix<Scalar>, 3, 3, Options> & a_mat) | ||
| 77 | { | ||
| 78 | ::pinocchio::quaternion::internal::quaternionbase_assign_impl< | ||
| 79 | ✗ | ::casadi::Matrix<Scalar>, false>::run(q, a_mat); | |
| 80 | } | ||
| 81 | }; | ||
| 82 | } // namespace internal | ||
| 83 | } // namespace Eigen | ||
| 84 | |||
| 85 | #endif // ifndef __pinocchio_autodiff_casadi_math_quaternion_hpp__ | ||
| 86 |