1 |
|
|
/* |
2 |
|
|
* Copyright 2010, |
3 |
|
|
* François Bleibel, |
4 |
|
|
* Olivier Stasse, |
5 |
|
|
* |
6 |
|
|
* CNRS/AIST |
7 |
|
|
* |
8 |
|
|
*/ |
9 |
|
|
|
10 |
|
|
#include <sot/core/debug.hh> |
11 |
|
|
#include <sot/core/factory.hh> |
12 |
|
|
#include <sot/core/macros-signal.hh> |
13 |
|
|
#include <sot/core/macros.hh> |
14 |
|
|
#include <sot/core/vector-to-rotation.hh> |
15 |
|
|
|
16 |
|
|
using namespace std; |
17 |
|
|
using namespace dynamicgraph::sot; |
18 |
|
|
using namespace dynamicgraph; |
19 |
|
|
|
20 |
|
|
SOT_CORE_DISABLE_WARNING_PUSH |
21 |
|
|
SOT_CORE_DISABLE_WARNING_DEPRECATED |
22 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(VectorToRotation, "VectorToRotation"); |
23 |
|
|
SOT_CORE_DISABLE_WARNING_POP |
24 |
|
|
|
25 |
|
|
/* --------------------------------------------------------------------- */ |
26 |
|
|
/* --------------------------------------------------------------------- */ |
27 |
|
|
/* --------------------------------------------------------------------- */ |
28 |
|
|
|
29 |
|
|
VectorToRotation::VectorToRotation(const std::string &name) |
30 |
|
|
: Entity(name), |
31 |
|
|
size(0), |
32 |
|
|
axes(0), |
33 |
|
|
SIN(NULL, "sotVectorToRotation(" + name + ")::output(vector)::sin"), |
34 |
|
|
SOUT(SOT_MEMBER_SIGNAL_1(VectorToRotation::computeRotation, SIN, |
35 |
|
|
dynamicgraph::Vector), |
36 |
|
|
"sotVectorToRotation(" + name + ")::output(matrixRotation)::sout") { |
37 |
|
|
signalRegistration(SIN << SOUT); |
38 |
|
|
} |
39 |
|
|
/* --------------------------------------------------------------------- */ |
40 |
|
|
/* --------------------------------------------------------------------- */ |
41 |
|
|
/* --------------------------------------------------------------------- */ |
42 |
|
|
|
43 |
|
|
MatrixRotation &VectorToRotation::computeRotation( |
44 |
|
|
const dynamicgraph::Vector &angles, MatrixRotation &res) { |
45 |
|
|
res.setIdentity(); |
46 |
|
|
MatrixRotation Ra, Rtmp; |
47 |
|
|
for (unsigned int i = 0; i < size; ++i) { |
48 |
|
|
Ra.setIdentity(); |
49 |
|
|
const double ca = cos(angles(i)); |
50 |
|
|
const double sa = sin(angles(i)); |
51 |
|
|
const unsigned int i_X = 0, i_Y = 1, i_Z = 2; |
52 |
|
|
switch (axes[i]) { |
53 |
|
|
case AXIS_X: { |
54 |
|
|
Ra(i_Y, i_Y) = ca; |
55 |
|
|
Ra(i_Y, i_Z) = -sa; |
56 |
|
|
Ra(i_Z, i_Y) = sa; |
57 |
|
|
Ra(i_Z, i_Z) = ca; |
58 |
|
|
break; |
59 |
|
|
} |
60 |
|
|
case AXIS_Y: { |
61 |
|
|
Ra(i_Z, i_Z) = ca; |
62 |
|
|
Ra(i_Z, i_X) = -sa; |
63 |
|
|
Ra(i_X, i_Z) = sa; |
64 |
|
|
Ra(i_X, i_X) = ca; |
65 |
|
|
break; |
66 |
|
|
} |
67 |
|
|
case AXIS_Z: { |
68 |
|
|
Ra(i_X, i_X) = ca; |
69 |
|
|
Ra(i_X, i_Y) = -sa; |
70 |
|
|
Ra(i_Y, i_X) = sa; |
71 |
|
|
Ra(i_Y, i_Y) = ca; |
72 |
|
|
break; |
73 |
|
|
} |
74 |
|
|
} |
75 |
|
|
|
76 |
|
|
sotDEBUG(15) << "R" << i << " = " << Ra; |
77 |
|
|
Rtmp = res * Ra; |
78 |
|
|
res = Rtmp; |
79 |
|
|
} |
80 |
|
|
|
81 |
|
|
return res; |
82 |
|
|
} |