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 
355 TFSIMD_FORCE_INLINE Quaternion
357 {
358  return Quaternion(-q.x(), -q.y(), -q.z(), -q.w());
359 }
360 
361 
362 
364 TFSIMD_FORCE_INLINE Quaternion
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 
372 TFSIMD_FORCE_INLINE Quaternion
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 
381 TFSIMD_FORCE_INLINE Quaternion
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 
420 TFSIMD_FORCE_INLINE Quaternion
421 inverse(const Quaternion& q)
422 {
423  return q.inverse();
424 }
425 
431 TFSIMD_FORCE_INLINE Quaternion
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 
445 TFSIMD_FORCE_INLINE Quaternion
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 
464 TFSIMD_FORCE_INLINE Quaternion
466 {
467  v0.normalize();
468  v1.normalize();
469  return shortestArcQuat(v0,v1);
470 }
471 
472 }
473 #endif
474 
475 
476 
477 
tf::Quaternion::normalize
Quaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: Quaternion.h:176
tf::angle
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
Return the half angle between two quaternions.
Definition: Quaternion.h:407
tfAcos
TFSIMD_FORCE_INLINE tfScalar tfAcos(tfScalar x)
Definition: Scalar.h:184
tf::Quaternion::normalized
Quaternion normalized() const
Return a normalized version of this quaternion.
Definition: Quaternion.h:207
tf::Quaternion::setRPY
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
Set the quaternion using fixed axis RPY.
Definition: Quaternion.h:96
tf::Quaternion::getIdentity
static const Quaternion & getIdentity()
Definition: Quaternion.h:342
tf::Quaternion::operator+
TFSIMD_FORCE_INLINE Quaternion operator+(const Quaternion &q2) const
Return the sum of this quaternion and the other.
Definition: Quaternion.h:267
s
XmlRpcServer s
tf::angleShortestPath
TFSIMD_FORCE_INLINE tfScalar angleShortestPath(const Quaternion &q1, const Quaternion &q2)
Return the shortest angle between two quaternions.
Definition: Quaternion.h:414
tf::Quaternion::nearest
TFSIMD_FORCE_INLINE Quaternion nearest(const Quaternion &qd) const
Definition: Quaternion.h:301
tf::shortestArcQuat
TFSIMD_FORCE_INLINE Quaternion shortestArcQuat(const Vector3 &v0, const Vector3 &v1)
Definition: Quaternion.h:446
TFSIMD_FORCE_INLINE
#define TFSIMD_FORCE_INLINE
Definition: Scalar.h:130
tf::Quaternion::length
tfScalar length() const
Return the length of the quaternion.
Definition: Quaternion.h:169
tf::Quaternion::Quaternion
Quaternion()
No initialization constructor.
Definition: Quaternion.h:33
tf::Quaternion::inverse
Quaternion inverse() const
Return the inverse of this quaternion.
Definition: Quaternion.h:259
tfSqrt
TFSIMD_FORCE_INLINE tfScalar tfSqrt(tfScalar x)
Definition: Scalar.h:179
tf::Quaternion::angleShortestPath
tfScalar angleShortestPath(const Quaternion &q) const
Return the angle between this quaternion and the other along the shortest path.
Definition: Quaternion.h:221
tf::tfPlaneSpace1
TFSIMD_FORCE_INLINE void tfPlaneSpace1(const Vector3 &n, Vector3 &p, Vector3 &q)
Definition: Vector3.h:658
tf::operator*
TFSIMD_FORCE_INLINE Vector3 operator*(const Matrix3x3 &m, const Vector3 &v)
Definition: Matrix3x3.h:604
tf::slerp
TFSIMD_FORCE_INLINE Quaternion slerp(const Quaternion &q1, const Quaternion &q2, const tfScalar &t)
Return the result of spherical linear interpolation betwen two quaternions.
Definition: Quaternion.h:432
tfSin
TFSIMD_FORCE_INLINE tfScalar tfSin(tfScalar x)
Definition: Scalar.h:182
tf::Quaternion::operator+=
TFSIMD_FORCE_INLINE Quaternion & operator+=(const Quaternion &q)
Add two quaternions.
Definition: Quaternion.h:122
ROS_DEPRECATED
#define ROS_DEPRECATED
tf::Quaternion::operator*
TFSIMD_FORCE_INLINE Quaternion operator*(const tfScalar &s) const
Return a scaled version of this quaternion.
Definition: Quaternion.h:184
f
f
tf::Quaternion::setRotation
void setRotation(const Vector3 &axis, const tfScalar &angle)
Set the rotation using axis angle notation.
Definition: Quaternion.h:64
tf::Quaternion::operator-
TFSIMD_FORCE_INLINE Quaternion operator-(const Quaternion &q2) const
Return the difference between this quaternion and the other.
Definition: Quaternion.h:276
tf::Quaternion::operator/=
Quaternion & operator/=(const tfScalar &s)
Inversely scale this quaternion.
Definition: Quaternion.h:200
tf::Quaternion::slerp
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
tf::dot
TFSIMD_FORCE_INLINE tfScalar dot(const Quaternion &q1, const Quaternion &q2)
Calculate the dot product between two quaternions.
Definition: Quaternion.h:392
tfPow
TFSIMD_FORCE_INLINE tfScalar tfPow(tfScalar x, tfScalar y)
Definition: Scalar.h:190
tf::Quaternion::operator-
TFSIMD_FORCE_INLINE Quaternion operator-() const
Return the negative of this quaternion This simply negates each element.
Definition: Quaternion.h:284
tf::Quaternion::getAngle
tfScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
Definition: Quaternion.h:231
tf::Quaternion::dot
tfScalar dot(const Quaternion &q) const
Return the dot product between this quaternion and another.
Definition: Quaternion.h:157
tf::Quaternion::Quaternion
ROS_DEPRECATED Quaternion(const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll)
Constructor from Euler angles.
Definition: Quaternion.h:53
Vector3
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...
tf::Quaternion::Quaternion
Quaternion(const Vector3 &axis, const tfScalar &angle)
Axis angle Constructor.
Definition: Quaternion.h:45
tf::Quaternion::getAngleShortestPath
tfScalar getAngleShortestPath() const
Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path.
Definition: Quaternion.h:238
tf::Quaternion::Quaternion
Quaternion(const tfScalar &x, const tfScalar &y, const tfScalar &z, const tfScalar &w)
Constructor from scalars.
Definition: Quaternion.h:39
QuadWord.h
d
d
TFSIMD_EPSILON
#define TFSIMD_EPSILON
Definition: Scalar.h:204
tf::Quaternion::operator-=
Quaternion & operator-=(const Quaternion &q)
Sutfract out a quaternion.
Definition: Quaternion.h:130
tf::Quaternion::getW
const TFSIMD_FORCE_INLINE tfScalar & getW() const
Definition: Quaternion.h:348
tf::Quaternion::operator*=
Quaternion & operator*=(const Quaternion &q)
Multiply this quaternion by q on the right.
Definition: Quaternion.h:147
tf::Quaternion::farthest
TFSIMD_FORCE_INLINE Quaternion farthest(const Quaternion &qd) const
Definition: Quaternion.h:290
tf::operator-
TFSIMD_FORCE_INLINE Quaternion operator-(const Quaternion &q)
Return the negative of a quaternion.
Definition: Quaternion.h:356
tf::length
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
Return the length of a quaternion.
Definition: Quaternion.h:400
Vector3.h
tf::Quaternion::length2
tfScalar length2() const
Return the length squared of the quaternion.
Definition: Quaternion.h:163
tfScalar
double tfScalar
The tfScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: Scalar.h:160
tf::Quaternion::operator*=
Quaternion & operator*=(const tfScalar &s)
Scale this quaternion.
Definition: Quaternion.h:138
tf::Quaternion::angle
tfScalar angle(const Quaternion &q) const
Return the half angle between this quaternion and the other.
Definition: Quaternion.h:213
tf::Quaternion::setEuler
void setEuler(const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll)
Set the quaternion using Euler angles.
Definition: Quaternion.h:76
tf
Definition: exceptions.h:38
tf::inverse
TFSIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
Return the inverse of a quaternion.
Definition: Quaternion.h:421
tf::Quaternion::setEulerZYX
ROS_DEPRECATED void setEulerZYX(const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll)
Set the quaternion using euler angles.
Definition: Quaternion.h:116
tf::Quaternion::getAxis
Vector3 getAxis() const
Return the axis of the rotation represented by this quaternion.
Definition: Quaternion.h:249
tfCos
TFSIMD_FORCE_INLINE tfScalar tfCos(tfScalar x)
Definition: Scalar.h:181
macros.h
tf::quatRotate
TFSIMD_FORCE_INLINE Vector3 quatRotate(const Quaternion &rotation, const Vector3 &v)
Definition: Quaternion.h:438
tf::Quaternion
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:30
tf::shortestArcQuatNormalize2
TFSIMD_FORCE_INLINE Quaternion shortestArcQuatNormalize2(Vector3 &v0, Vector3 &v1)
Definition: Quaternion.h:465
tf::Quaternion::operator/
Quaternion operator/(const tfScalar &s) const
Return an inversely scaled versionof this quaternion.
Definition: Quaternion.h:192
tfAssert
#define tfAssert(x)
Definition: Scalar.h:145


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Sat Aug 19 2023 02:38:07