GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/matrix/operator.cpp Lines: 3 63 4.8 %
Date: 2023-03-13 12:09:37 Branches: 3 126 2.4 %

Line Branch Exec Source
1
/*
2
 * Copyright 2010,
3
 * François Bleibel,
4
 * Olivier Stasse,
5
 * Nicolas Mansard
6
 * Joseph Mirabel
7
 *
8
 * CNRS/AIST
9
 *
10
 */
11
12
#include "operator.hh"
13
14
namespace dg = ::dynamicgraph;
15
16
/* ---------------------------------------------------------------------------*/
17
/* ------- GENERIC HELPERS -------------------------------------------------- */
18
/* ---------------------------------------------------------------------------*/
19
20
#define REGISTER_UNARY_OP(OpType, name)                               \
21
  template <>                                                         \
22
  const std::string UnaryOp<OpType>::CLASS_NAME = std::string(#name); \
23
  Entity *regFunction_##name(const std::string &objname) {            \
24
    return new UnaryOp<OpType>(objname);                              \
25
  }                                                                   \
26
  EntityRegisterer regObj_##name(std::string(#name), &regFunction_##name)
27
28
/* ---------------------------------------------------------------------------*/
29
/* ---------------------------------------------------------------------------*/
30
/* ---------------------------------------------------------------------------*/
31
32
namespace dynamicgraph {
33
namespace sot {
34
35
/* ---------------------------------------------------------------------- */
36
/* --- ALGEBRA SELECTORS ------------------------------------------------ */
37
/* ---------------------------------------------------------------------- */
38
1
REGISTER_UNARY_OP(VectorSelecter, Selec_of_vector);
39
1
REGISTER_UNARY_OP(VectorComponent, Component_of_vector);
40
1
REGISTER_UNARY_OP(MatrixSelector, Selec_of_matrix);
41
REGISTER_UNARY_OP(MatrixColumnSelector, Selec_column_of_matrix);
42
REGISTER_UNARY_OP(MatrixTranspose, MatrixTranspose);
43
REGISTER_UNARY_OP(Diagonalizer, MatrixDiagonal);
44
45
/* ---------------------------------------------------------------------- */
46
/* --- INVERSION -------------------------------------------------------- */
47
/* ---------------------------------------------------------------------- */
48
49
REGISTER_UNARY_OP(Inverser<Matrix>, Inverse_of_matrix);
50
REGISTER_UNARY_OP(Inverser<MatrixHomogeneous>, Inverse_of_matrixHomo);
51
REGISTER_UNARY_OP(Inverser<MatrixTwist>, Inverse_of_matrixtwist);
52
REGISTER_UNARY_OP(Normalize, Norm_of_vector);
53
REGISTER_UNARY_OP(InverserRotation, Inverse_of_matrixrotation);
54
REGISTER_UNARY_OP(InverserQuaternion, Inverse_of_unitquat);
55
56
/* ----------------------------------------------------------------------- */
57
/* --- SE3/SO3 conversions ----------------------------------------------- */
58
/* ----------------------------------------------------------------------- */
59
60
REGISTER_UNARY_OP(MatrixHomoToPoseUTheta, MatrixHomoToPoseUTheta);
61
REGISTER_UNARY_OP(MatrixHomoToSE3Vector, MatrixHomoToSE3Vector);
62
REGISTER_UNARY_OP(SE3VectorToMatrixHomo, SE3VectorToMatrixHomo);
63
REGISTER_UNARY_OP(SkewSymToVector, SkewSymToVector);
64
REGISTER_UNARY_OP(PoseUThetaToMatrixHomo, PoseUThetaToMatrixHomo);
65
REGISTER_UNARY_OP(MatrixHomoToPoseQuaternion, MatrixHomoToPoseQuaternion);
66
REGISTER_UNARY_OP(PoseQuaternionToMatrixHomo, PoseQuaternionToMatrixHomo);
67
REGISTER_UNARY_OP(MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseRollPitchYaw);
68
REGISTER_UNARY_OP(PoseRollPitchYawToMatrixHomo, PoseRollPitchYawToMatrixHomo);
69
REGISTER_UNARY_OP(PoseRollPitchYawToPoseUTheta, PoseRollPitchYawToPoseUTheta);
70
REGISTER_UNARY_OP(HomoToMatrix, HomoToMatrix);
71
REGISTER_UNARY_OP(MatrixToHomo, MatrixToHomo);
72
REGISTER_UNARY_OP(HomoToTwist, HomoToTwist);
73
REGISTER_UNARY_OP(HomoToRotation, HomoToRotation);
74
REGISTER_UNARY_OP(MatrixHomoToPose, MatrixHomoToPose);
75
REGISTER_UNARY_OP(RPYToMatrix, RPYToMatrix);
76
REGISTER_UNARY_OP(MatrixToRPY, MatrixToRPY);
77
REGISTER_UNARY_OP(RPYToQuaternion, RPYToQuaternion);
78
REGISTER_UNARY_OP(QuaternionToRPY, QuaternionToRPY);
79
REGISTER_UNARY_OP(QuaternionToMatrix, QuaternionToMatrix);
80
REGISTER_UNARY_OP(MatrixToQuaternion, MatrixToQuaternion);
81
REGISTER_UNARY_OP(MatrixToUTheta, MatrixToUTheta);
82
REGISTER_UNARY_OP(UThetaToQuaternion, UThetaToQuaternion);
83
84
/* ---------------------------------------------------------------------------*/
85
/* ---------------------------------------------------------------------------*/
86
/* ---------------------------------------------------------------------------*/
87
88
#define REGISTER_BINARY_OP(OpType, name)                               \
89
  template <>                                                          \
90
  const std::string BinaryOp<OpType>::CLASS_NAME = std::string(#name); \
91
  Entity *regFunction_##name(const std::string &objname) {             \
92
    return new BinaryOp<OpType>(objname);                              \
93
  }                                                                    \
94
  EntityRegisterer regObj_##name(std::string(#name), &regFunction_##name)
95
96
/* --- MULTIPLICATION --------------------------------------------------- */
97
98
REGISTER_BINARY_OP(Multiplier_double_vector, Multiply_double_vector);
99
REGISTER_BINARY_OP(Multiplier_matrix_vector, Multiply_matrix_vector);
100
REGISTER_BINARY_OP(Multiplier_matrixHomo_vector, Multiply_matrixHomo_vector);
101
REGISTER_BINARY_OP(Multiplier_matrixTwist_vector, Multiply_matrixTwist_vector);
102
103
/* --- SUBSTRACTION ----------------------------------------------------- */
104
REGISTER_BINARY_OP(Substraction<dynamicgraph::Matrix>, Substract_of_matrix);
105
REGISTER_BINARY_OP(Substraction<dynamicgraph::Vector>, Substract_of_vector);
106
REGISTER_BINARY_OP(Substraction<double>, Substract_of_double);
107
108
/* --- STACK ------------------------------------------------------------ */
109
REGISTER_BINARY_OP(VectorStack, Stack_of_vector);
110
111
/* ---------------------------------------------------------------------- */
112
113
REGISTER_BINARY_OP(Composer, Compose_R_and_T);
114
115
/* --- CONVOLUTION PRODUCT ---------------------------------------------- */
116
REGISTER_BINARY_OP(ConvolutionTemporal, ConvolutionTemporal);
117
118
/* --- BOOLEAN REDUCTION ------------------------------------------------ */
119
REGISTER_BINARY_OP(Comparison<double>, CompareDouble);
120
REGISTER_BINARY_OP(MatrixComparison<Vector>, CompareVector);
121
122
REGISTER_BINARY_OP(WeightedAdder<dynamicgraph::Matrix>, WeightAdd_of_matrix);
123
REGISTER_BINARY_OP(WeightedAdder<dynamicgraph::Vector>, WeightAdd_of_vector);
124
REGISTER_BINARY_OP(WeightedAdder<double>, WeightAdd_of_double);
125
126
#define REGISTER_VARIADIC_OP(OpType, name)                               \
127
  template <>                                                            \
128
  const std::string VariadicOp<OpType>::CLASS_NAME = std::string(#name); \
129
  Entity *regFunction_##name(const std::string &objname) {               \
130
    return new VariadicOp<OpType>(objname);                              \
131
  }                                                                      \
132
  EntityRegisterer regObj_##name(std::string(#name), &regFunction_##name)
133
134
/* --- VectorMix ------------------------------------------------------------ */
135
REGISTER_VARIADIC_OP(VectorMix, Mix_of_vector);
136
137
/* --- ADDITION --------------------------------------------------------- */
138
REGISTER_VARIADIC_OP(AdderVariadic<Matrix>, Add_of_matrix);
139
REGISTER_VARIADIC_OP(AdderVariadic<Vector>, Add_of_vector);
140
REGISTER_VARIADIC_OP(AdderVariadic<double>, Add_of_double);
141
142
/* --- MULTIPLICATION --------------------------------------------------- */
143
REGISTER_VARIADIC_OP(Multiplier<Matrix>, Multiply_of_matrix);
144
REGISTER_VARIADIC_OP(Multiplier<Vector>, Multiply_of_vector);
145
REGISTER_VARIADIC_OP(Multiplier<MatrixRotation>, Multiply_of_matrixrotation);
146
REGISTER_VARIADIC_OP(Multiplier<MatrixHomogeneous>, Multiply_of_matrixHomo);
147
REGISTER_VARIADIC_OP(Multiplier<MatrixTwist>, Multiply_of_matrixtwist);
148
REGISTER_VARIADIC_OP(Multiplier<VectorQuaternion>, Multiply_of_quaternion);
149
REGISTER_VARIADIC_OP(Multiplier<double>, Multiply_of_double);
150
151
/* --- BOOLEAN --------------------------------------------------------- */
152
REGISTER_VARIADIC_OP(BoolOp<0>, And);
153
REGISTER_VARIADIC_OP(BoolOp<1>, Or);
154
155
}  // namespace sot
156
}  // namespace dynamicgraph
157
158
/* --- TODO ------------------------------------------------------------------*/
159
// The following commented lines are sot-v1 entities that are still waiting
160
//   for conversion. Help yourself!
161
162
// /* --------------------------------------------------------------------------
163
// */
164
165
// struct WeightedDirection
166
// {
167
// public:
168
//   void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Vector&
169
//   v2,dynamicgraph::Vector& res ) const
170
//   {
171
//     const double norm1 = v1.norm();
172
//     const double norm2 = v2.norm();
173
//     res=v2; res*=norm1;
174
//     res*= (1/norm2);
175
//   }
176
// };
177
// typedef BinaryOp< Vector,Vector,Vector,WeightedDirection > weightdir;
178
// SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_ExE_E(weightdir,vector,weight_dir,"WeightDir")
179
180
// /* --------------------------------------------------------------------------
181
// */
182
183
// struct Nullificator
184
// {
185
// public:
186
//   void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Vector&
187
//   v2,dynamicgraph::Vector& res ) const
188
//   {
189
//     const unsigned int s = std::max( v1.size(),v2.size() );
190
//     res.resize(s);
191
//     for( unsigned int i=0;i<s;++i )
192
//       {
193
// 	if( v1(i)>v2(i) ) res(i)=v1(i)-v2(i);
194
// 	else 	if( v1(i)<-v2(i) ) res(i)=v1(i)+v2(i);
195
// 	else res(i)=0;
196
//       }
197
//   }
198
// };
199
// typedef BinaryOp< Vector,Vector,Vector,Nullificator > vectNil;
200
// SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_ExE_E(vectNil,vector,vectnil_,"Nullificator")
201
202
// /* --------------------------------------------------------------------------
203
// */
204
205
// struct VirtualSpring
206
// {
207
// public:
208
//   double spring;
209
210
//   void operator()( const dynamicgraph::Vector& pos,const
211
//   dynamicgraph::Vector& ref,dynamicgraph::Vector& res ) const
212
//   {
213
//     double norm = ref.norm();
214
//     double dist = ref.scalarProduct(pos) / (norm*norm);
215
216
//     res.resize( ref.size() );
217
//     res = ref;  res *= dist; res -= pos;
218
//     res *= spring;
219
//   }
220
// };
221
// typedef BinaryOp< Vector,Vector,Vector,VirtualSpring > virtspring;
222
// SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_ExE_E_CMD
223
// (virtspring,vector,virtspring_,
224
//  "VirtualSpring"
225
//  ,else if( cmdLine=="spring" ){  CMDARGS_INOUT(op.spring); }
226
//  ,"VirtualSpring<pos,ref> compute the virtual force of a spring attache "
227
//  "to the reference line <ref>. The eq is: k.(<ref|pos>/<ref|ref>.ref-pos)"
228
//  "Params:\n  - spring: get/set the spring factor.")