GCC Code Coverage Report

Directory: ./
File: include/sot/core/matrix-geometry.hh
Date: 2024-11-13 12:35:17
Exec Total Coverage
Lines: 12 12 100.0%
Branches: 31 62 50.0%
Line Branch Exec Source
1 /*
2 * Copyright 2010,2019
3 * CNRS/AIST
4 * François Bleibel, Olivier Stasse, François Bailly
5 *
6 */
7
8 #ifndef __SOT_MATRIX_GEOMETRY_H__
9 #define __SOT_MATRIX_GEOMETRY_H__
10
11 /* --- Matrix --- */
12 #include <dynamic-graph/eigen-io.h>
13 #include <dynamic-graph/linear-algebra.h>
14
15 #include <Eigen/Core>
16 #include <Eigen/Geometry>
17 #include <sot/core/api.hh>
18
19 #define MRAWDATA(x) x.data()
20
21 /* --------------------------------------------------------------------- */
22 /* --------------------------------------------------------------------- */
23 /* --------------------------------------------------------------------- */
24
25 namespace dynamicgraph {
26 namespace sot {
27
28 #define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \
29 /** \ingroup matrixtypedefs */ \
30 typedef Eigen::Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix; \
31 /** \ingroup matrixtypedefs */ \
32 typedef Eigen::Matrix<Type, Size, 1> Vector##SizeSuffix##TypeSuffix; \
33 /** \ingroup matrixtypedefs */ \
34 typedef Eigen::Matrix<Type, 1, Size> RowVector##SizeSuffix##TypeSuffix;
35
36 #define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size) \
37 /** \ingroup matrixtypedefs */ \
38 typedef Eigen::Matrix<Type, Size, Eigen::Dynamic> \
39 Matrix##Size##X##TypeSuffix; \
40 /** \ingroup matrixtypedefs */ \
41 typedef Eigen::Matrix<Type, Eigen::Dynamic, Size> Matrix##X##Size##TypeSuffix;
42
43 #define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
44 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1) \
45 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 5, 5) \
46 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 6, 6) \
47 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 7, 7) \
48 EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 1) \
49 EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 5) \
50 EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 6) \
51 EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 7)
52
53 EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i)
54 EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f)
55 EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d)
56 EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<float>, cf)
57 EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<double>, cd)
58
59 #undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
60 #undef EIGEN_MAKE_TYPEDEFS
61
62 typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
63 MatrixRXd;
64 typedef Eigen::Map<MatrixRXd> SigMatrixXd;
65 typedef Eigen::Map<Eigen::VectorXd> SigVectorXd;
66 typedef const Eigen::Map<const MatrixRXd> const_SigMatrixXd;
67 typedef const Eigen::Map<const Eigen::VectorXd> const_SigVectorXd;
68
69 typedef Eigen::Ref<Eigen::VectorXd> RefVector;
70 typedef const Eigen::Ref<const Eigen::VectorXd> &ConstRefVector;
71 typedef Eigen::Ref<Eigen::MatrixXd> RefMatrix;
72 typedef const Eigen::Ref<const Eigen::MatrixXd> ConstRefMatrix;
73
74 typedef Eigen::Transform<double, 3, Eigen::Affine> SOT_CORE_EXPORT
75 MatrixHomogeneous;
76 typedef Eigen::Matrix<double, 3, 3> SOT_CORE_EXPORT MatrixRotation;
77 typedef Eigen::AngleAxis<double> SOT_CORE_EXPORT VectorUTheta;
78 typedef Eigen::Quaternion<double> SOT_CORE_EXPORT VectorQuaternion;
79 typedef Eigen::Vector3d SOT_CORE_EXPORT VectorRotation;
80 typedef Eigen::Vector3d SOT_CORE_EXPORT VectorRollPitchYaw;
81 typedef Eigen::Matrix<double, 6, 6> SOT_CORE_EXPORT MatrixForce;
82 typedef Eigen::Matrix<double, 6, 6> SOT_CORE_EXPORT MatrixTwist;
83
84 typedef Eigen::Matrix<double, 7, 1> SOT_CORE_EXPORT Vector7;
85 typedef Eigen::Quaternion<double> SOT_CORE_EXPORT Quaternion;
86 typedef Eigen::Map<Quaternion> SOT_CORE_EXPORT QuaternionMap;
87
88 114 inline void buildFrom(const MatrixHomogeneous &MH, MatrixTwist &MT) {
89
2/4
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
 114 Eigen::Vector3d _t = MH.translation();
90
2/4
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
 114 MatrixRotation R(MH.linear());
91
1/2
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
 114 Eigen::Matrix3d Tx;
92
15/30
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 114 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 114 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 114 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 114 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 114 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 114 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 114 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 114 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 114 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 114 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 114 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 114 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 114 times.
✗ Branch 44 not taken.
 114 Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0;
93
1/2
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
 114 Eigen::Matrix3d sk;
94
2/4
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
 114 sk = Tx * R;
95
96
2/4
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
 114 MT.block<3, 3>(0, 0) = R;
97
2/4
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
 114 MT.block<3, 3>(0, 3) = sk;
98
2/4
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
 114 MT.block<3, 3>(3, 0).setZero();
99
2/4
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
 114 MT.block<3, 3>(3, 3) = R;
100 114 }
101
102 } // namespace sot
103 } // namespace dynamicgraph
104
105 #endif /* #ifndef __SOT_MATRIX_GEOMETRY_H__ */
106