YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
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
7
9#include <yarp/math/Math.h>
10#include <cmath>
11#include <cstdio>
12
13using namespace yarp::math;
14
15namespace {
16YARP_LOG_COMPONENT(QUATERNION, "yarp.math.Quaternion")
17}
18
20{
21 q_w = 1;
22 q_x = 0;
23 q_y = 0;
24 q_z = 0;
25}
26
27Quaternion::Quaternion(double _x, double _y, double _z, double _w)
28{
29 q_w = _w;
30 q_x = _x;
31 q_y = _y;
32 q_z = _z;
33}
34
36{
37 if (q_w == 0 &&
38 q_x == 0 &&
39 q_y == 0 &&
40 q_z == 0) {return false;}
41 return true;
42}
43
45{
47 v[0] = q_w;
48 v[1] = q_x;
49 v[2] = q_y;
50 v[3] = q_z;
51 return v;
52}
53
54double Quaternion::w() const
55{
56 return q_w;
57}
58
59double Quaternion::x() const
60{
61 return q_x;
62}
63
64double Quaternion::y() const
65{
66 return q_y;
67}
68
69double Quaternion::z() const
70{
71 return q_z;
72}
73
75{
76 return q_w;
77}
78
80{
81 return q_x;
82}
83
85{
86 return q_y;
87}
88
90{
91 return q_z;
92}
93
95{
96 if ((R.rows()<3) || (R.cols()<3))
97 {
98 yCError(QUATERNION, "fromRotationMatrix() failed, matrix should be >= 3x3");
99 yCAssert(QUATERNION, R.rows() >= 3 && R.cols() >= 3);
100 }
101
102 double tr = R(0, 0) + R(1, 1) + R(2, 2);
103
104 if (tr>0.0)
105 {
106 double sqtrp1 = sqrt(tr + 1.0);
107 double sqtrp12 = 2.0*sqtrp1;
108 q_w = 0.5*sqtrp1;
109 q_x = (R(2, 1) - R(1, 2)) / sqtrp12;
110 q_y = (R(0, 2) - R(2, 0)) / sqtrp12;
111 q_z = (R(1, 0) - R(0, 1)) / sqtrp12;
112 }
113 else if ((R(1, 1)>R(0, 0)) && (R(1, 1)>R(2, 2)))
114 {
115 double sqdip1 = sqrt(R(1, 1) - R(0, 0) - R(2, 2) + 1.0);
116 q_y = 0.5*sqdip1;
117
118 if (sqdip1 > 0.0) {
119 sqdip1 = 0.5 / sqdip1;
120 }
121
122 q_w = (R(0, 2) - R(2, 0))*sqdip1;
123 q_x = (R(1, 0) + R(0, 1))*sqdip1;
124 q_z = (R(2, 1) + R(1, 2))*sqdip1;
125 }
126 else if (R(2, 2)>R(0, 0))
127 {
128 double sqdip1 = sqrt(R(2, 2) - R(0, 0) - R(1, 1) + 1.0);
129 q_z = 0.5*sqdip1;
130
131 if (sqdip1 > 0.0) {
132 sqdip1 = 0.5 / sqdip1;
133 }
134
135 q_w = (R(1, 0) - R(0, 1))*sqdip1;
136 q_x = (R(0, 2) + R(2, 0))*sqdip1;
137 q_y = (R(2, 1) + R(1, 2))*sqdip1;
138 }
139 else
140 {
141 double sqdip1 = sqrt(R(0, 0) - R(1, 1) - R(2, 2) + 1.0);
142 q_x = 0.5*sqdip1;
143
144 if (sqdip1 > 0.0) {
145 sqdip1 = 0.5 / sqdip1;
146 }
147
148 q_w = (R(2, 1) - R(1, 2))*sqdip1;
149 q_y = (R(1, 0) + R(0, 1))*sqdip1;
150 q_z = (R(0, 2) + R(2, 0))*sqdip1;
151 }
152}
153
155{
156 yarp::sig::Vector q = this->toVector();
157 yarp::sig::Vector qin = (1.0 / yarp::math::norm(q))*q;
158
160 R(0, 0) = qin[0] * qin[0] + qin[1] * qin[1] - qin[2] * qin[2] - qin[3] * qin[3];
161 R(1, 0) = 2.0*(qin[1] * qin[2] + qin[0] * qin[3]);
162 R(2, 0) = 2.0*(qin[1] * qin[3] - qin[0] * qin[2]);
163 R(0, 1) = 2.0*(qin[1] * qin[2] - qin[0] * qin[3]);
164 R(1, 1) = qin[0] * qin[0] - qin[1] * qin[1] + qin[2] * qin[2] - qin[3] * qin[3];
165 R(2, 1) = 2.0*(qin[2] * qin[3] + qin[0] * qin[1]);
166 R(0, 2) = 2.0*(qin[1] * qin[3] + qin[0] * qin[2]);
167 R(1, 2) = 2.0*(qin[2] * qin[3] - qin[0] * qin[1]);
168 R(2, 2) = qin[0] * qin[0] - qin[1] * qin[1] - qin[2] * qin[2] + qin[3] * qin[3];
169
170 return R;
171}
172
174{
175 yarp::sig::Vector q = this->toVector();
176 yarp::sig::Vector qin = (1.0 / yarp::math::norm(q))*q;
177
179 R(0, 0) = qin[0] * qin[0] + qin[1] * qin[1] - qin[2] * qin[2] - qin[3] * qin[3];
180 R(1, 0) = 2.0*(qin[1] * qin[2] + qin[0] * qin[3]);
181 R(2, 0) = 2.0*(qin[1] * qin[3] - qin[0] * qin[2]);
182 R(0, 1) = 2.0*(qin[1] * qin[2] - qin[0] * qin[3]);
183 R(1, 1) = qin[0] * qin[0] - qin[1] * qin[1] + qin[2] * qin[2] - qin[3] * qin[3];
184 R(2, 1) = 2.0*(qin[2] * qin[3] + qin[0] * qin[1]);
185 R(0, 2) = 2.0*(qin[1] * qin[3] + qin[0] * qin[2]);
186 R(1, 2) = 2.0*(qin[2] * qin[3] - qin[0] * qin[1]);
187 R(2, 2) = qin[0] * qin[0] - qin[1] * qin[1] - qin[2] * qin[2] + qin[3] * qin[3];
188
189 return R;
190}
191
192std::string Quaternion::toString(int precision, int width) const
193{
194 std::string ret;
195 char tmp[350];
196 if (width<0)
197 {
198 sprintf(tmp, "w=% .*lf\t", precision, q_w); ret += tmp;
199 sprintf(tmp, "x=% .*lf\t", precision, q_x); ret += tmp;
200 sprintf(tmp, "y=% .*lf\t", precision, q_y); ret += tmp;
201 sprintf(tmp, "z=% .*lf\t", precision, q_z); ret += tmp;
202 }
203 else
204 {
205 sprintf(tmp, "w=% *.*lf ", width, precision, q_w); ret += tmp;
206 sprintf(tmp, "x=% *.*lf ", width, precision, q_x); ret += tmp;
207 sprintf(tmp, "y=% *.*lf ", width, precision, q_y); ret += tmp;
208 sprintf(tmp, "z=% *.*lf ", width, precision, q_z); ret += tmp;
209 }
210
211 return ret.substr(0, ret.length() - 1);
212}
213
215{
217 Quaternion q;
219 this->q_w = q.q_w;
220 this->q_x = q.q_x;
221 this->q_y = q.q_y;
222 this->q_z = q.q_z;
223}
224
225void Quaternion::fromAxisAngle(const yarp::sig::Vector& axis, const double& angle)
226{
227 yarp::sig::Vector v = axis;
228 v.resize(4); v[4] = angle;
230 Quaternion q;
232 this->q_w = q.q_w;
233 this->q_x = q.q_x;
234 this->q_y = q.q_y;
235 this->q_z = q.q_z;
236}
237
244
246{
247 return sqrt(q_w * q_w +
248 q_x * q_x +
249 q_y * q_y +
250 q_z * q_z);
251}
252
254{
255 double length = abs();
256 q_w /= length;
257 q_x /= length;
258 q_y /= length;
259 q_z /= length;
260 return;
261}
262
264{
265 return atan2(sqrt(q_x * q_x +
266 q_y * q_y +
267 q_z * q_z), q_w);
268}
269
271{
272 return Quaternion(q_w, -q_x, -q_y, -q_z);
273}
void fromRotationMatrix(const yarp::sig::Matrix &R)
Converts a rotation matrix to a quaternion.
double abs()
Computes the modulus of the quaternion.
yarp::sig::Vector toVector() const
Converts the quaternion to a vector of length 4.
yarp::sig::Matrix toRotationMatrix3x3() const
Converts a quaternion to a rotation matrix.
Quaternion inverse() const
Computes the inverse of the quaternion.
void normalize()
Normalizes the quaternion elements.
void fromAxisAngle(const yarp::sig::Vector &v)
Computes the quaternion from an axis-angle representation.
double arg()
Computes the argument or phase of the quaternion in radians.
yarp::sig::Matrix toRotationMatrix4x4() const
Converts a quaternion to a rotation matrix.
yarp::sig::Vector toAxisAngle()
bool isValid() const
Check if the quaternion is valid.
A class for a Matrix.
Definition Matrix.h:40
size_t cols() const
Return number of columns.
Definition Matrix.h:91
size_t rows() const
Return number of rows.
Definition Matrix.h:85
std::string toString() const
void resize(size_t size)
Resize the vector.
Definition VectorBase.h:106
#define yCError(component,...)
#define yCAssert(component, x)
#define YARP_LOG_COMPONENT(name,...)
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