YARP
Yet Another Robot Platform
Quaternion.cpp
Go to the documentation of this file.
1 /*
2  * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * SPDX-License-Identifier: BSD-3-Clause
4  */
5 
6 #include <yarp/math/Quaternion.h>
7 
10 #include <yarp/os/LogComponent.h>
11 #include <yarp/math/Math.h>
12 #include <cmath>
13 #include <cstdio>
14 
15 using namespace yarp::math;
16 
17 namespace {
18 YARP_LOG_COMPONENT(QUATERNION, "yarp.math.Quaternion")
19 }
20 
23 {
24 public:
25  yarp::os::NetInt32 listTag{0};
26  yarp::os::NetInt32 listLen{0};
28 };
30 
32 {
33  internal_data[0] = 1;
34  internal_data[1] = 0;
35  internal_data[2] = 0;
36  internal_data[3] = 0;
37 }
38 
39 Quaternion::Quaternion(double x, double y, double z, double w)
40 {
41  internal_data[0] = w;
42  internal_data[1] = x;
43  internal_data[2] = y;
44  internal_data[3] = z;
45 }
46 
47 const double* Quaternion::data() const
48 {
49  return internal_data;
50 }
51 
53 {
54  return internal_data;
55 }
56 
58 {
59  yarp::sig::Vector v(4);
60  v[0] = internal_data[0];
61  v[1] = internal_data[1];
62  v[2] = internal_data[2];
63  v[3] = internal_data[3];
64  return v;
65 }
66 
67 double Quaternion::w() const
68 {
69  return internal_data[0];
70 }
71 
72 double Quaternion::x() const
73 {
74  return internal_data[1];
75 }
76 
77 double Quaternion::y() const
78 {
79  return internal_data[2];
80 }
81 
82 double Quaternion::z() const
83 {
84  return internal_data[3];
85 }
86 
87 double& Quaternion::w()
88 {
89  return internal_data[0];
90 }
91 
92 double& Quaternion::x()
93 {
94  return internal_data[1];
95 }
96 
97 double& Quaternion::y()
98 {
99  return internal_data[2];
100 }
101 
102 double& Quaternion::z()
103 {
104  return internal_data[3];
105 }
106 
108 {
109  // auto-convert text mode interaction
110  connection.convertTextMode();
112  bool ok = connection.expectBlock((char*)&header, sizeof(header));
113  if (!ok) {
114  return false;
115  }
116 
117  if (header.listLen == 4 && header.listTag == (BOTTLE_TAG_LIST | BOTTLE_TAG_FLOAT64))
118  {
119  this->internal_data[0] = connection.expectFloat64();
120  this->internal_data[1] = connection.expectFloat64();
121  this->internal_data[2] = connection.expectFloat64();
122  this->internal_data[3] = connection.expectFloat64();
123  }
124  else
125  {
126  return false;
127  }
128 
129  return !connection.isError();
130 }
131 
133 {
135 
137  header.listLen = 4;
138 
139  connection.appendBlock((char*)&header, sizeof(header));
140 
141  connection.appendFloat64(this->internal_data[0]);
142  connection.appendFloat64(this->internal_data[1]);
143  connection.appendFloat64(this->internal_data[2]);
144  connection.appendFloat64(this->internal_data[3]);
145 
146  // if someone is foolish enough to connect in text mode,
147  // let them see something readable.
148  connection.convertTextMode();
149 
150  return !connection.isError();
151 }
152 
154 {
155  if ((R.rows()<3) || (R.cols()<3))
156  {
157  yCError(QUATERNION, "fromRotationMatrix() failed, matrix should be >= 3x3");
158  yCAssert(QUATERNION, R.rows() >= 3 && R.cols() >= 3);
159  }
160 
161  double tr = R(0, 0) + R(1, 1) + R(2, 2);
162 
163  if (tr>0.0)
164  {
165  double sqtrp1 = sqrt(tr + 1.0);
166  double sqtrp12 = 2.0*sqtrp1;
167  internal_data[0] = 0.5*sqtrp1;
168  internal_data[1] = (R(2, 1) - R(1, 2)) / sqtrp12;
169  internal_data[2] = (R(0, 2) - R(2, 0)) / sqtrp12;
170  internal_data[3] = (R(1, 0) - R(0, 1)) / sqtrp12;
171  }
172  else if ((R(1, 1)>R(0, 0)) && (R(1, 1)>R(2, 2)))
173  {
174  double sqdip1 = sqrt(R(1, 1) - R(0, 0) - R(2, 2) + 1.0);
175  internal_data[2] = 0.5*sqdip1;
176 
177  if (sqdip1 > 0.0) {
178  sqdip1 = 0.5 / sqdip1;
179  }
180 
181  internal_data[0] = (R(0, 2) - R(2, 0))*sqdip1;
182  internal_data[1] = (R(1, 0) + R(0, 1))*sqdip1;
183  internal_data[3] = (R(2, 1) + R(1, 2))*sqdip1;
184  }
185  else if (R(2, 2)>R(0, 0))
186  {
187  double sqdip1 = sqrt(R(2, 2) - R(0, 0) - R(1, 1) + 1.0);
188  internal_data[3] = 0.5*sqdip1;
189 
190  if (sqdip1 > 0.0) {
191  sqdip1 = 0.5 / sqdip1;
192  }
193 
194  internal_data[0] = (R(1, 0) - R(0, 1))*sqdip1;
195  internal_data[1] = (R(0, 2) + R(2, 0))*sqdip1;
196  internal_data[2] = (R(2, 1) + R(1, 2))*sqdip1;
197  }
198  else
199  {
200  double sqdip1 = sqrt(R(0, 0) - R(1, 1) - R(2, 2) + 1.0);
201  internal_data[1] = 0.5*sqdip1;
202 
203  if (sqdip1 > 0.0) {
204  sqdip1 = 0.5 / sqdip1;
205  }
206 
207  internal_data[0] = (R(2, 1) - R(1, 2))*sqdip1;
208  internal_data[2] = (R(1, 0) + R(0, 1))*sqdip1;
209  internal_data[3] = (R(0, 2) + R(2, 0))*sqdip1;
210  }
211 }
212 
214 {
215  yarp::sig::Vector q = this->toVector();
216  yarp::sig::Vector qin = (1.0 / yarp::math::norm(q))*q;
217 
219  R(0, 0) = qin[0] * qin[0] + qin[1] * qin[1] - qin[2] * qin[2] - qin[3] * qin[3];
220  R(1, 0) = 2.0*(qin[1] * qin[2] + qin[0] * qin[3]);
221  R(2, 0) = 2.0*(qin[1] * qin[3] - qin[0] * qin[2]);
222  R(0, 1) = 2.0*(qin[1] * qin[2] - qin[0] * qin[3]);
223  R(1, 1) = qin[0] * qin[0] - qin[1] * qin[1] + qin[2] * qin[2] - qin[3] * qin[3];
224  R(2, 1) = 2.0*(qin[2] * qin[3] + qin[0] * qin[1]);
225  R(0, 2) = 2.0*(qin[1] * qin[3] + qin[0] * qin[2]);
226  R(1, 2) = 2.0*(qin[2] * qin[3] - qin[0] * qin[1]);
227  R(2, 2) = qin[0] * qin[0] - qin[1] * qin[1] - qin[2] * qin[2] + qin[3] * qin[3];
228 
229  return R;
230 }
231 
233 {
234  yarp::sig::Vector q = this->toVector();
235  yarp::sig::Vector qin = (1.0 / yarp::math::norm(q))*q;
236 
238  R(0, 0) = qin[0] * qin[0] + qin[1] * qin[1] - qin[2] * qin[2] - qin[3] * qin[3];
239  R(1, 0) = 2.0*(qin[1] * qin[2] + qin[0] * qin[3]);
240  R(2, 0) = 2.0*(qin[1] * qin[3] - qin[0] * qin[2]);
241  R(0, 1) = 2.0*(qin[1] * qin[2] - qin[0] * qin[3]);
242  R(1, 1) = qin[0] * qin[0] - qin[1] * qin[1] + qin[2] * qin[2] - qin[3] * qin[3];
243  R(2, 1) = 2.0*(qin[2] * qin[3] + qin[0] * qin[1]);
244  R(0, 2) = 2.0*(qin[1] * qin[3] + qin[0] * qin[2]);
245  R(1, 2) = 2.0*(qin[2] * qin[3] - qin[0] * qin[1]);
246  R(2, 2) = qin[0] * qin[0] - qin[1] * qin[1] - qin[2] * qin[2] + qin[3] * qin[3];
247 
248  return R;
249 }
250 
251 std::string Quaternion::toString(int precision, int width) const
252 {
253  std::string ret;
254  char tmp[350];
255  if (width<0)
256  {
257  sprintf(tmp, "w=% .*lf\t", precision, internal_data[0]); ret += tmp;
258  sprintf(tmp, "x=% .*lf\t", precision, internal_data[1]); ret += tmp;
259  sprintf(tmp, "y=% .*lf\t", precision, internal_data[2]); ret += tmp;
260  sprintf(tmp, "z=% .*lf\t", precision, internal_data[3]); ret += tmp;
261  }
262  else
263  {
264  sprintf(tmp, "w=% *.*lf ", width, precision, internal_data[0]); ret += tmp;
265  sprintf(tmp, "x=% *.*lf ", width, precision, internal_data[1]); ret += tmp;
266  sprintf(tmp, "y=% *.*lf ", width, precision, internal_data[2]); ret += tmp;
267  sprintf(tmp, "z=% *.*lf ", width, precision, internal_data[3]); ret += tmp;
268  }
269 
270  return ret.substr(0, ret.length() - 1);
271 }
272 
274 {
276  Quaternion q;
277  q.fromRotationMatrix(m);
278  this->internal_data[0] = q.internal_data[0];
279  this->internal_data[1] = q.internal_data[1];
280  this->internal_data[2] = q.internal_data[2];
281  this->internal_data[3] = q.internal_data[3];
282 }
283 
284 void Quaternion::fromAxisAngle(const yarp::sig::Vector& axis, const double& angle)
285 {
286  yarp::sig::Vector v = axis;
287  v.resize(4); v[4] = angle;
289  Quaternion q;
290  q.fromRotationMatrix(m);
291  this->internal_data[0] = q.internal_data[0];
292  this->internal_data[1] = q.internal_data[1];
293  this->internal_data[2] = q.internal_data[2];
294  this->internal_data[3] = q.internal_data[3];
295 }
296 
298 {
301  return v;
302 }
303 
305 {
306  return sqrt(internal_data[0] * internal_data[0] +
307  internal_data[1] * internal_data[1] +
308  internal_data[2] * internal_data[2] +
309  internal_data[3] * internal_data[3]);
310 }
311 
313 {
314  double length = abs();
315  internal_data[0] /= length;
316  internal_data[1] /= length;
317  internal_data[2] /= length;
318  internal_data[3] /= length;
319  return;
320 }
321 
323 {
324  return atan2(sqrt(internal_data[1] * internal_data[1] +
325  internal_data[2] * internal_data[2] +
326  internal_data[3] * internal_data[3]),
327  internal_data[0]);
328 }
329 
331 {
332  // w x y z
333  return Quaternion(internal_data[0], -internal_data[1], -internal_data[2], -internal_data[3]);
334 }
#define BOTTLE_TAG_FLOAT64
Definition: Bottle.h:25
#define BOTTLE_TAG_LIST
Definition: Bottle.h:28
bool ret
yarp::os::NetInt32 listLen
Definition: Quaternion.cpp:26
yarp::os::NetInt32 listTag
Definition: Quaternion.cpp:25
void fromRotationMatrix(const yarp::sig::Matrix &R)
Converts a rotation matrix to a quaternion.
Definition: Quaternion.cpp:153
double abs()
Computes the modulus of the quaternion.
Definition: Quaternion.cpp:304
double x() const
Definition: Quaternion.cpp:72
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
Definition: Quaternion.cpp:107
yarp::sig::Vector toVector() const
Converts the quaternion to a vector of length 4.
Definition: Quaternion.cpp:57
yarp::sig::Matrix toRotationMatrix3x3() const
Converts a quaternion to a rotation matrix.
Definition: Quaternion.cpp:232
Quaternion inverse() const
Computes the inverse of the quaternion.
Definition: Quaternion.cpp:330
std::string toString(int precision=-1, int width=-1) const
Definition: Quaternion.cpp:251
void normalize()
Normalizes the quaternion elements.
Definition: Quaternion.cpp:312
void fromAxisAngle(const yarp::sig::Vector &v)
Computes the quaternion from an axis-angle representation.
Definition: Quaternion.cpp:273
double arg()
Computes the argument or phase of the quaternion in radians.
Definition: Quaternion.cpp:322
yarp::sig::Matrix toRotationMatrix4x4() const
Converts a quaternion to a rotation matrix.
Definition: Quaternion.cpp:213
double z() const
Definition: Quaternion.cpp:82
double w() const
Definition: Quaternion.cpp:67
yarp::sig::Vector toAxisAngle()
Definition: Quaternion.cpp:297
bool write(yarp::os::ConnectionWriter &connection) const override
Write vector to a connection.
Definition: Quaternion.cpp:132
double y() const
Definition: Quaternion.cpp:77
An interface for reading from a network connection.
virtual bool expectBlock(char *data, size_t len)=0
Read a block of data from the network connection.
virtual bool convertTextMode()=0
Reads in a standard description in text mode, and converts it to a standard description in binary.
virtual bool isError() const =0
virtual yarp::conf::float64_t expectFloat64()=0
Read a 64-bit floating point number from the network connection.
An interface for writing to a network connection.
virtual bool isError() const =0
virtual bool convertTextMode()=0
Converts a standard description in binary into a textual description, if the connection is in text-mo...
virtual void appendFloat64(yarp::conf::float64_t data)=0
Send a representation of a 64-bit floating point number to the network connection.
virtual void appendBlock(const char *data, size_t len)=0
Send a block of data to the network connection.
A class for a Matrix.
Definition: Matrix.h:43
size_t cols() const
Return number of columns.
Definition: Matrix.h:98
size_t rows() const
Return number of rows.
Definition: Matrix.h:92
void resize(size_t size) override
Resize the vector.
Definition: Vector.h:222
#define yCError(component,...)
Definition: LogComponent.h:154
#define yCAssert(component, x)
Definition: LogComponent.h:169
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:77
yarp::sig::Matrix eye(int r, int c)
Build an identity matrix (defined in Math.h).
Definition: math.cpp:586
double norm(const yarp::sig::Vector &v)
Returns the Euclidean norm of the vector (defined in Math.h).
Definition: math.cpp:538
yarp::sig::Vector zeros(int s)
Creates a vector of zeros (defined in Math.h).
Definition: math.cpp:576
yarp::sig::Matrix axis2dcm(const yarp::sig::Vector &v)
Returns a dcm (direction cosine matrix) rotation matrix R from axis/angle representation (defined in ...
Definition: math.cpp:715
yarp::sig::Vector dcm2axis(const yarp::sig::Matrix &R)
Converts a dcm (direction cosine matrix) rotation matrix R to axis/angle representation (defined in M...
Definition: math.cpp:673
std::int32_t NetInt32
Definition of the NetInt32 type.
Definition: NetInt32.h:30
#define YARP_END_PACK
Ends 1 byte packing for structs/classes.
Definition: system.h:191
#define YARP_BEGIN_PACK
Starts 1 byte packing for structs/classes.
Definition: system.h:190