GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 |
✓✗✓✗ |
114 |
Eigen::Vector3d _t = MH.translation(); |
90 |
✓✗✓✗ |
114 |
MatrixRotation R(MH.linear()); |
91 |
✓✗ | 114 |
Eigen::Matrix3d Tx; |
92 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
114 |
Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0; |
93 |
✓✗ | 114 |
Eigen::Matrix3d sk; |
94 |
✓✗✓✗ |
114 |
sk = Tx * R; |
95 |
|||
96 |
✓✗✓✗ |
114 |
MT.block<3, 3>(0, 0) = R; |
97 |
✓✗✓✗ |
114 |
MT.block<3, 3>(0, 3) = sk; |
98 |
✓✗✓✗ |
114 |
MT.block<3, 3>(3, 0).setZero(); |
99 |
✓✗✓✗ |
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__ */ |
Generated by: GCOVR (Version 4.2) |