Quaternion.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
3 
4 This software is provided 'as-is', without any express or implied warranty.
5 In no event will the authors be held liable for any damages arising from the use of this software.
6 Permission is granted to anyone to use this software for any purpose,
7 including commercial applications, and to alter it and redistribute it freely,
8 subject to the following restrictions:
9 
10 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
12 3. This notice may not be removed or altered from any source distribution.
13 */
14 
15 
16 
17 #ifndef TF_QUATERNION_H_
18 #define TF_QUATERNION_H_
19 
20 
21 #include "Vector3.h"
22 #include "QuadWord.h"
23 
24 #include <ros/macros.h>
25 
26 namespace tf
27 {
28 
30 class Quaternion : public QuadWord {
31 public:
34 
35 
36  // template <typename tfScalar>
37  // explicit Quaternion(const tfScalar *v) : Tuple4<tfScalar>(v) {}
39  Quaternion(const tfScalar& x, const tfScalar& y, const tfScalar& z, const tfScalar& w)
40  : QuadWord(x, y, z, w)
41  {}
45  Quaternion(const Vector3& axis, const tfScalar& angle)
46  {
47  setRotation(axis, angle);
48  }
53  ROS_DEPRECATED Quaternion(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll)
54  {
55 #ifndef TF_EULER_DEFAULT_ZYX
56  setEuler(yaw, pitch, roll);
57 #else
58  setRPY(roll, pitch, yaw);
59 #endif
60  }
64  void setRotation(const Vector3& axis, const tfScalar& angle)
65  {
66  tfScalar d = axis.length();
67  tfAssert(d != tfScalar(0.0));
68  tfScalar s = tfSin(angle * tfScalar(0.5)) / d;
69  setValue(axis.x() * s, axis.y() * s, axis.z() * s,
70  tfCos(angle * tfScalar(0.5)));
71  }
76  void setEuler(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll)
77  {
78  tfScalar halfYaw = tfScalar(yaw) * tfScalar(0.5);
79  tfScalar halfPitch = tfScalar(pitch) * tfScalar(0.5);
80  tfScalar halfRoll = tfScalar(roll) * tfScalar(0.5);
81  tfScalar cosYaw = tfCos(halfYaw);
82  tfScalar sinYaw = tfSin(halfYaw);
83  tfScalar cosPitch = tfCos(halfPitch);
84  tfScalar sinPitch = tfSin(halfPitch);
85  tfScalar cosRoll = tfCos(halfRoll);
86  tfScalar sinRoll = tfSin(halfRoll);
87  setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
88  cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
89  sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
90  cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
91  }
96  void setRPY(const tfScalar& roll, const tfScalar& pitch, const tfScalar& yaw)
97  {
98  tfScalar halfYaw = tfScalar(yaw) * tfScalar(0.5);
99  tfScalar halfPitch = tfScalar(pitch) * tfScalar(0.5);
100  tfScalar halfRoll = tfScalar(roll) * tfScalar(0.5);
101  tfScalar cosYaw = tfCos(halfYaw);
102  tfScalar sinYaw = tfSin(halfYaw);
103  tfScalar cosPitch = tfCos(halfPitch);
104  tfScalar sinPitch = tfSin(halfPitch);
105  tfScalar cosRoll = tfCos(halfRoll);
106  tfScalar sinRoll = tfSin(halfRoll);
107  setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
108  cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
109  cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
110  cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
111  }
116  ROS_DEPRECATED void setEulerZYX(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll)
117  {
118  setRPY(roll, pitch, yaw);
119  }
123  {
124  m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3];
125  return *this;
126  }
127 
131  {
132  m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3];
133  return *this;
134  }
135 
139  {
140  m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s;
141  return *this;
142  }
143 
148  {
149  setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
150  m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
151  m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
152  m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
153  return *this;
154  }
157  tfScalar dot(const Quaternion& q) const
158  {
159  return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
160  }
161 
164  {
165  return dot(*this);
166  }
167 
169  tfScalar length() const
170  {
171  return tfSqrt(length2());
172  }
173 
177  {
178  return *this /= length();
179  }
180 
184  operator*(const tfScalar& s) const
185  {
186  return Quaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
187  }
188 
189 
192  Quaternion operator/(const tfScalar& s) const
193  {
194  tfAssert(s != tfScalar(0.0));
195  return *this * (tfScalar(1.0) / s);
196  }
197 
201  {
202  tfAssert(s != tfScalar(0.0));
203  return *this *= tfScalar(1.0) / s;
204  }
205 
208  {
209  return *this / length();
210  }
213  tfScalar angle(const Quaternion& q) const
214  {
215  tfScalar s = tfSqrt(length2() * q.length2());
216  tfAssert(s != tfScalar(0.0));
217  return tfAcos(dot(q) / s);
218  }
222  {
223  tfScalar s = tfSqrt(length2() * q.length2());
224  tfAssert(s != tfScalar(0.0));
225  if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
226  return tfAcos(dot(-q) / s) * tfScalar(2.0);
227  else
228  return tfAcos(dot(q) / s) * tfScalar(2.0);
229  }
232  {
233  tfScalar s = tfScalar(2.) * tfAcos(m_floats[3]);
234  return s;
235  }
236 
239  {
240  tfScalar s;
241  if (m_floats[3] < 0)
242  s = tfScalar(2.) * tfAcos(-m_floats[3]);
243  else
244  s = tfScalar(2.) * tfAcos(m_floats[3]);
245  return s;
246  }
247 
249  Vector3 getAxis() const
250  {
251  tfScalar s_squared = tfScalar(1.) - tfPow(m_floats[3], tfScalar(2.));
252  if (s_squared < tfScalar(10.) * TFSIMD_EPSILON) //Check for divide by zero
253  return Vector3(1.0, 0.0, 0.0); // Arbitrary
254  tfScalar s = tfSqrt(s_squared);
255  return Vector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s);
256  }
257 
260  {
261  return Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
262  }
263 
267  operator+(const Quaternion& q2) const
268  {
269  const Quaternion& q1 = *this;
270  return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
271  }
272 
276  operator-(const Quaternion& q2) const
277  {
278  const Quaternion& q1 = *this;
279  return Quaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
280  }
281 
285  {
286  const Quaternion& q2 = *this;
287  return Quaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]);
288  }
291  {
292  Quaternion diff,sum;
293  diff = *this - qd;
294  sum = *this + qd;
295  if( diff.dot(diff) > sum.dot(sum) )
296  return qd;
297  return (-qd);
298  }
299 
302  {
303  Quaternion diff,sum;
304  diff = *this - qd;
305  sum = *this + qd;
306  if( diff.dot(diff) < sum.dot(sum) )
307  return qd;
308  return (-qd);
309  }
310 
311 
316  Quaternion slerp(const Quaternion& q, const tfScalar& t) const
317  {
318  tfScalar theta = angleShortestPath(q) / tfScalar(2.0);
319  if (theta != tfScalar(0.0))
320  {
321  tfScalar d = tfScalar(1.0) / tfSin(theta);
322  tfScalar s0 = tfSin((tfScalar(1.0) - t) * theta);
323  tfScalar s1 = tfSin(t * theta);
324  if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
325  return Quaternion((m_floats[0] * s0 + -q.x() * s1) * d,
326  (m_floats[1] * s0 + -q.y() * s1) * d,
327  (m_floats[2] * s0 + -q.z() * s1) * d,
328  (m_floats[3] * s0 + -q.m_floats[3] * s1) * d);
329  else
330  return Quaternion((m_floats[0] * s0 + q.x() * s1) * d,
331  (m_floats[1] * s0 + q.y() * s1) * d,
332  (m_floats[2] * s0 + q.z() * s1) * d,
333  (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
334 
335  }
336  else
337  {
338  return *this;
339  }
340  }
341 
342  static const Quaternion& getIdentity()
343  {
344  static const Quaternion identityQuat(tfScalar(0.),tfScalar(0.),tfScalar(0.),tfScalar(1.));
345  return identityQuat;
346  }
347 
348  TFSIMD_FORCE_INLINE const tfScalar& getW() const { return m_floats[3]; }
349 
350 
351 };
352 
353 
357 {
358  return Quaternion(-q.x(), -q.y(), -q.z(), -q.w());
359 }
360 
361 
362 
365 operator*(const Quaternion& q1, const Quaternion& q2) {
366  return Quaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
367  q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
368  q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
369  q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
370 }
371 
373 operator*(const Quaternion& q, const Vector3& w)
374 {
375  return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
376  q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
377  q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
378  -q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
379 }
380 
382 operator*(const Vector3& w, const Quaternion& q)
383 {
384  return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
385  w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
386  w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
387  -w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
388 }
389 
392 dot(const Quaternion& q1, const Quaternion& q2)
393 {
394  return q1.dot(q2);
395 }
396 
397 
400 length(const Quaternion& q)
401 {
402  return q.length();
403 }
404 
407 angle(const Quaternion& q1, const Quaternion& q2)
408 {
409  return q1.angle(q2);
410 }
411 
415 {
416  return q1.angleShortestPath(q2);
417 }
418 
421 inverse(const Quaternion& q)
422 {
423  return q.inverse();
424 }
425 
432 slerp(const Quaternion& q1, const Quaternion& q2, const tfScalar& t)
433 {
434  return q1.slerp(q2, t);
435 }
436 
438 quatRotate(const Quaternion& rotation, const Vector3& v)
439 {
440  Quaternion q = rotation * v;
441  q *= rotation.inverse();
442  return Vector3(q.getX(),q.getY(),q.getZ());
443 }
444 
446 shortestArcQuat(const Vector3& v0, const Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
447 {
448  Vector3 c = v0.cross(v1);
449  tfScalar d = v0.dot(v1);
450 
451  if (d < -1.0 + TFSIMD_EPSILON)
452  {
453  Vector3 n,unused;
454  tfPlaneSpace1(v0,n,unused);
455  return Quaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
456  }
457 
458  tfScalar s = tfSqrt((1.0f + d) * 2.0f);
459  tfScalar rs = 1.0f / s;
460 
461  return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
462 }
463 
466 {
467  v0.normalize();
468  v1.normalize();
469  return shortestArcQuat(v0,v1);
470 }
471 
472 }
473 #endif
474 
475 
476 
477 
d
TFSIMD_FORCE_INLINE Quaternion nearest(const Quaternion &qd) const
Definition: Quaternion.h:301
Quaternion inverse() const
Return the inverse of this quaternion.
Definition: Quaternion.h:259
tfScalar getAngleShortestPath() const
Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path...
Definition: Quaternion.h:238
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:30
TFSIMD_FORCE_INLINE Quaternion operator-() const
Return the negative of this quaternion This simply negates each element.
Definition: Quaternion.h:284
TFSIMD_FORCE_INLINE Quaternion operator-(const Quaternion &q2) const
Return the difference between this quaternion and the other.
Definition: Quaternion.h:276
f
double tfScalar
The tfScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: Scalar.h:160
Quaternion operator/(const tfScalar &s) const
Return an inversely scaled versionof this quaternion.
Definition: Quaternion.h:192
XmlRpcServer s
#define TFSIMD_EPSILON
Definition: Scalar.h:204
ROS_DEPRECATED Quaternion(const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll)
Constructor from Euler angles.
Definition: Quaternion.h:53
Quaternion & operator/=(const tfScalar &s)
Inversely scale this quaternion.
Definition: Quaternion.h:200
Quaternion(const Vector3 &axis, const tfScalar &angle)
Axis angle Constructor.
Definition: Quaternion.h:45
tfScalar angle(const Quaternion &q) const
Return the half angle between this quaternion and the other.
Definition: Quaternion.h:213
Quaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: Quaternion.h:176
Definition: exceptions.h:38
tfScalar dot(const Quaternion &q) const
Return the dot product between this quaternion and another.
Definition: Quaternion.h:157
Quaternion & operator*=(const tfScalar &s)
Scale this quaternion.
Definition: Quaternion.h:138
Quaternion & operator*=(const Quaternion &q)
Multiply this quaternion by q on the right.
Definition: Quaternion.h:147
ROS_DEPRECATED void setEulerZYX(const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll)
Set the quaternion using euler angles.
Definition: Quaternion.h:116
Vector3 getAxis() const
Return the axis of the rotation represented by this quaternion.
Definition: Quaternion.h:249
TFSIMD_FORCE_INLINE void tfPlaneSpace1(const Vector3 &n, Vector3 &p, Vector3 &q)
Definition: Vector3.h:658
TFSIMD_FORCE_INLINE tfScalar tfCos(tfScalar x)
Definition: Scalar.h:181
TFSIMD_FORCE_INLINE const tfScalar & getW() const
Definition: Quaternion.h:348
TFSIMD_FORCE_INLINE Quaternion farthest(const Quaternion &qd) const
Definition: Quaternion.h:290
TFSIMD_FORCE_INLINE tfScalar tfAcos(tfScalar x)
Definition: Scalar.h:184
#define tfAssert(x)
Definition: Scalar.h:145
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
Set the quaternion using fixed axis RPY.
Definition: Quaternion.h:96
static const Quaternion & getIdentity()
Definition: Quaternion.h:342
Quaternion slerp(const Quaternion &q, const tfScalar &t) const
Return the quaternion which is the result of Spherical Linear Interpolation between this and the othe...
Definition: Quaternion.h:316
tfScalar length() const
Return the length of the quaternion.
Definition: Quaternion.h:169
TFSIMD_FORCE_INLINE Vector3 quatRotate(const Quaternion &rotation, const Vector3 &v)
Definition: Quaternion.h:438
#define TFSIMD_FORCE_INLINE
Definition: Scalar.h:130
tfScalar length2() const
Return the length squared of the quaternion.
Definition: Quaternion.h:163
void setEuler(const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll)
Set the quaternion using Euler angles.
Definition: Quaternion.h:76
#define ROS_DEPRECATED
TFSIMD_FORCE_INLINE Quaternion operator+(const Quaternion &q2) const
Return the sum of this quaternion and the other.
Definition: Quaternion.h:267
Quaternion()
No initialization constructor.
Definition: Quaternion.h:33
TFSIMD_FORCE_INLINE tfScalar tfSqrt(tfScalar x)
Definition: Scalar.h:179
Quaternion(const tfScalar &x, const tfScalar &y, const tfScalar &z, const tfScalar &w)
Constructor from scalars.
Definition: Quaternion.h:39
TFSIMD_FORCE_INLINE Quaternion shortestArcQuatNormalize2(Vector3 &v0, Vector3 &v1)
Definition: Quaternion.h:465
Quaternion & operator-=(const Quaternion &q)
Sutfract out a quaternion.
Definition: Quaternion.h:130
TFSIMD_FORCE_INLINE Quaternion & operator+=(const Quaternion &q)
Add two quaternions.
Definition: Quaternion.h:122
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...
tfScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
Definition: Quaternion.h:231
TFSIMD_FORCE_INLINE tfScalar tfSin(tfScalar x)
Definition: Scalar.h:182
TFSIMD_FORCE_INLINE Quaternion shortestArcQuat(const Vector3 &v0, const Vector3 &v1)
Definition: Quaternion.h:446
TFSIMD_FORCE_INLINE Quaternion operator*(const tfScalar &s) const
Return a scaled version of this quaternion.
Definition: Quaternion.h:184
tfScalar angleShortestPath(const Quaternion &q) const
Return the angle between this quaternion and the other along the shortest path.
Definition: Quaternion.h:221
void setRotation(const Vector3 &axis, const tfScalar &angle)
Set the rotation using axis angle notation.
Definition: Quaternion.h:64
TFSIMD_FORCE_INLINE tfScalar tfPow(tfScalar x, tfScalar y)
Definition: Scalar.h:190
Quaternion normalized() const
Return a normalized version of this quaternion.
Definition: Quaternion.h:207


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Feb 28 2022 22:26:19