| Directory: | ./ |
|---|---|
| File: | include/pinocchio/autodiff/casadi/math/quaternion.hpp |
| Date: | 2025-02-12 21:03:38 |
| 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 | 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 | 1 | static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q, const Matrix3 & mat) | |
| 23 | { | ||
| 24 | typedef | ||
| 25 | 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 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | QuaternionPlain quat_t_positive; |
| 30 | |||
| 31 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Scalar t = mat.trace(); |
| 32 |
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); |
| 33 | |||
| 34 |
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; |
| 35 | |||
| 36 |
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); |
| 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<1>::run(t, quat_t_negative_1, 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<2>::run(t, quat_t_negative_2, mat); |
| 39 | |||
| 40 | // Build the expression graph | ||
| 41 |
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); |
| 42 |
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); |
| 43 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
3 | const Scalar cond2 = |
| 44 |
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)); |
| 45 | |||
| 46 |
2/2✓ Branch 1 taken 4 times.
✓ Branch 2 taken 1 times.
|
5 | for (Eigen::DenseIndex k = 0; k < 4; ++k) |
| 47 | { | ||
| 48 | 4 | Scalar t_is_negative_cond1 = Scalar::if_else( | |
| 49 |
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)); |
| 50 | 4 | Scalar t_is_negative_cond2 = | |
| 51 |
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); |
| 52 | |||
| 53 |
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( |
| 54 |
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); |
| 55 | } | ||
| 56 | 1 | } | |
| 57 | }; | ||
| 58 | |||
| 59 | } // namespace internal | ||
| 60 | |||
| 61 | } // namespace quaternion | ||
| 62 | |||
| 63 | } // namespace pinocchio | ||
| 64 | |||
| 65 | namespace Eigen | ||
| 66 | { | ||
| 67 | namespace internal | ||
| 68 | { | ||
| 69 | template<class Scalar, int Options> | ||
| 70 | struct quaternionbase_assign_impl<Eigen::Matrix<::casadi::Matrix<Scalar>, 3, 3, Options>, 3, 3> | ||
| 71 | { | ||
| 72 | template<typename QuaternionDerived> | ||
| 73 | ✗ | EIGEN_DEVICE_FUNC static inline void run( | |
| 74 | QuaternionBase<QuaternionDerived> & q, | ||
| 75 | const Eigen::Matrix<::casadi::Matrix<Scalar>, 3, 3, Options> & a_mat) | ||
| 76 | { | ||
| 77 | ::pinocchio::quaternion::internal::quaternionbase_assign_impl< | ||
| 78 | ✗ | ::casadi::Matrix<Scalar>, false>::run(q, a_mat); | |
| 79 | } | ||
| 80 | }; | ||
| 81 | } // namespace internal | ||
| 82 | } // namespace Eigen | ||
| 83 | |||
| 84 | #endif // ifndef __pinocchio_autodiff_casadi_math_quaternion_hpp__ | ||
| 85 |