vector-quaternion.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2010,
3  * François Bleibel,
4  * Olivier Stasse,
5  *
6  * CNRS/AIST
7  *
8  */
9 
10 #include <sot/core/debug.hh>
12 
13 using namespace std;
14 using namespace dynamicgraph::sot;
15 
16 static const double ANGLE_MINIMUM = 0.0001;
17 static const double SINC_MINIMUM = 1e-8;
18 static const double COSC_MINIMUM = 2.5e-4;
19 
20 VectorRotation &VectorQuaternion::fromMatrix(const MatrixRotation &rot) {
21  sotDEBUGIN(15);
22 
23  const dynamicgraph::Matrix &rotmat = rot;
24 
25  double d0 = rotmat(0, 0), d1 = rotmat(1, 1), d2 = rotmat(2, 2);
26 
27  // The trace determines the method of decomposition
28  double rr = 1.0 + d0 + d1 + d2;
29 
30  double &_x = vector(1);
31  double &_y = vector(2);
32  double &_z = vector(3);
33  double &_r = vector(0);
34 
35  if (rr > 0) {
36  double s = 0.5 / sqrt(rr);
37  _x = (rotmat(2, 1) - rotmat(1, 2)) * s;
38  _y = (rotmat(0, 2) - rotmat(2, 0)) * s;
39  _z = (rotmat(1, 0) - rotmat(0, 1)) * s;
40  _r = 0.25 / s;
41  } else {
42  // Trace is less than zero, so need to determine which
43  // major diagonal is largest
44  if ((d0 > d1) && (d0 > d2)) {
45  double s = 0.5 / sqrt(1 + d0 - d1 - d2);
46  _x = 0.5 * s;
47  _y = (rotmat(0, 1) + rotmat(1, 0)) * s;
48  _z = (rotmat(0, 2) + rotmat(2, 0)) * s;
49  _r = (rotmat(1, 2) + rotmat(2, 1)) * s;
50  } else if (d1 > d2) {
51  double s = 0.5 / sqrt(1 + d0 - d1 - d2);
52  _x = (rotmat(0, 1) + rotmat(1, 0)) * s;
53  _y = 0.5 * s;
54  _z = (rotmat(1, 2) + rotmat(2, 1)) * s;
55  _r = (rotmat(0, 2) + rotmat(2, 0)) * s;
56  } else {
57  double s = 0.5 / sqrt(1 + d0 - d1 - d2);
58  _x = (rotmat(0, 2) + rotmat(2, 0)) * s;
59  _y = (rotmat(1, 2) + rotmat(2, 1)) * s;
60  _z = 0.5 * s;
61  _r = (rotmat(0, 1) + rotmat(1, 0)) * s;
62  }
63  }
64 
65  sotDEBUGOUT(15);
66  return *this;
67 }
68 
69 VectorRotation &VectorQuaternion::fromVector(const VectorUTheta &ut) {
70  sotDEBUGIN(15);
71 
72  double theta = sqrt(ut(0) * ut(0) + ut(1) * ut(1) + ut(2) * ut(2));
73  double si = sin(theta);
74  double co = cos(theta);
75  vector(0) = ut(0) / si;
76  vector(1) = ut(1) / si;
77  vector(2) = ut(2) / si;
78  vector(3) = co;
79 
80  sotDEBUGOUT(15);
81  return *this;
82 }
83 
84 MatrixRotation &VectorQuaternion::toMatrix(MatrixRotation &rot) const {
85  sotDEBUGIN(15);
86 
87  dynamicgraph::Matrix &rotmat = rot;
88 
89  const double &_x = vector(1);
90  const double &_y = vector(2);
91  const double &_z = vector(3);
92  const double &_r = vector(0);
93 
94  double x2 = _x * _x;
95  double y2 = _y * _y;
96  double z2 = _z * _z;
97  double r2 = _r * _r;
98 
99  rotmat(0, 0) = r2 + x2 - y2 - z2; // fill diagonal terms
100  rotmat(1, 1) = r2 - x2 + y2 - z2;
101  rotmat(2, 2) = r2 - x2 - y2 + z2;
102 
103  double xy = _x * _y;
104  double yz = _y * _z;
105  double zx = _z * _x;
106  double rx = _r * _x;
107  double ry = _r * _y;
108  double rz = _r * _z;
109 
110  rotmat(0, 1) = 2 * (xy - rz); // fill off diagonal terms
111  rotmat(0, 2) = 2 * (zx + ry);
112  rotmat(1, 0) = 2 * (xy + rz);
113  rotmat(1, 2) = 2 * (yz - rx);
114  rotmat(2, 0) = 2 * (zx - ry);
115  rotmat(2, 1) = 2 * (yz + rx);
116 
117  sotDEBUGOUT(15);
118  return rot;
119 }
120 
121 VectorQuaternion &VectorQuaternion::conjugate(VectorQuaternion &res) const {
122  res.vector(0) = vector(0);
123  res.vector(1) = -vector(1);
124  res.vector(2) = -vector(2);
125  res.vector(3) = -vector(3);
126  return res;
127 }
128 
129 VectorQuaternion &VectorQuaternion::multiply(const VectorQuaternion &q2,
130  VectorQuaternion &res) const {
131  double &a1 = vector(0);
132  double &b1 = vector(1);
133  double &c1 = vector(2);
134  double &d1 = vector(3);
135 
136  double &a2 = q2.vector(0);
137  double &b2 = q2.vector(1);
138  double &c2 = q2.vector(2);
139  double &d2 = q2.vector(3);
140 
141  res.vector(0) = a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2;
142  res.vector(1) = a1 * b2 + b1 * a2 + c1 * d2 - d1 * c2;
143  res.vector(2) = a1 * c2 + c1 * a2 + d1 * b2 - b1 * d2;
144  res.vector(3) = a1 * d2 + d1 * a2 + b1 * c2 - c1 * b2;
145 
146  return res;
147 }
s
Eigen::Quaternion< double > SOT_CORE_EXPORT VectorQuaternion
Eigen::Matrix< double, 3, 3 > MatrixRotation
#define sotDEBUGOUT(level)
Definition: debug.hh:212
x2
#define sotDEBUGIN(level)
Definition: debug.hh:211
float d1
float d0
Eigen::Vector3d SOT_CORE_EXPORT VectorRotation
q2
Eigen::AngleAxis< double > VectorUTheta
r2
res
static const double ANGLE_MINIMUM
static const double COSC_MINIMUM
Eigen::MatrixXd Matrix
c2
static const double SINC_MINIMUM


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Wed Jun 21 2023 02:51:26