| Directory: | ./ |
|---|---|
| File: | src/matrix/operator.cpp |
| Date: | 2024-11-13 12:35:17 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 3 | 63 | 4.8% |
| 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), ®Function_##name) | ||
| 27 | |||
| 28 | /* ---------------------------------------------------------------------------*/ | ||
| 29 | /* ---------------------------------------------------------------------------*/ | ||
| 30 | /* ---------------------------------------------------------------------------*/ | ||
| 31 | |||
| 32 | namespace dynamicgraph { | ||
| 33 | namespace sot { | ||
| 34 | |||
| 35 | /* ---------------------------------------------------------------------- */ | ||
| 36 | /* --- ALGEBRA SELECTORS ------------------------------------------------ */ | ||
| 37 | /* ---------------------------------------------------------------------- */ | ||
| 38 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | REGISTER_UNARY_OP(VectorSelecter, Selec_of_vector); |
| 39 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | REGISTER_UNARY_OP(VectorComponent, Component_of_vector); |
| 40 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
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), ®Function_##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), ®Function_##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.") | ||
| 229 |