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 TF2_QUATERNION_H_
18 #define TF2_QUATERNION_H_
19 
20 
21 #include "Vector3.h"
22 #include "QuadWord.h"
23 
24 #include <ros/macros.h>
25 
26 namespace tf2
27 {
28 
30 class Quaternion : public QuadWord {
31 public:
34 
35  // template <typename tf2Scalar>
36  // explicit Quaternion(const tf2Scalar *v) : Tuple4<tf2Scalar>(v) {}
38  Quaternion(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z, const tf2Scalar& w)
39  : QuadWord(x, y, z, w)
40  {}
44  Quaternion(const Vector3& axis, const tf2Scalar& angle)
45  {
46  setRotation(axis, angle);
47  }
52  ROS_DEPRECATED Quaternion(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll)
53  {
54 #ifndef TF2_EULER_DEFAULT_ZYX
55  setEuler(yaw, pitch, roll);
56 #else
57  setRPY(roll, pitch, yaw);
58 #endif
59  }
63  void setRotation(const Vector3& axis, const tf2Scalar& angle)
64  {
65  tf2Scalar d = axis.length();
66  tf2Assert(d != tf2Scalar(0.0));
67  tf2Scalar s = tf2Sin(angle * tf2Scalar(0.5)) / d;
68  setValue(axis.x() * s, axis.y() * s, axis.z() * s,
69  tf2Cos(angle * tf2Scalar(0.5)));
70  }
75  void setEuler(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll)
76  {
77  tf2Scalar halfYaw = tf2Scalar(yaw) * tf2Scalar(0.5);
78  tf2Scalar halfPitch = tf2Scalar(pitch) * tf2Scalar(0.5);
79  tf2Scalar halfRoll = tf2Scalar(roll) * tf2Scalar(0.5);
80  tf2Scalar cosYaw = tf2Cos(halfYaw);
81  tf2Scalar sinYaw = tf2Sin(halfYaw);
82  tf2Scalar cosPitch = tf2Cos(halfPitch);
83  tf2Scalar sinPitch = tf2Sin(halfPitch);
84  tf2Scalar cosRoll = tf2Cos(halfRoll);
85  tf2Scalar sinRoll = tf2Sin(halfRoll);
86  setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
87  cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
88  sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
89  cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
90  }
95  void setRPY(const tf2Scalar& roll, const tf2Scalar& pitch, const tf2Scalar& yaw)
96  {
97  tf2Scalar halfYaw = tf2Scalar(yaw) * tf2Scalar(0.5);
98  tf2Scalar halfPitch = tf2Scalar(pitch) * tf2Scalar(0.5);
99  tf2Scalar halfRoll = tf2Scalar(roll) * tf2Scalar(0.5);
100  tf2Scalar cosYaw = tf2Cos(halfYaw);
101  tf2Scalar sinYaw = tf2Sin(halfYaw);
102  tf2Scalar cosPitch = tf2Cos(halfPitch);
103  tf2Scalar sinPitch = tf2Sin(halfPitch);
104  tf2Scalar cosRoll = tf2Cos(halfRoll);
105  tf2Scalar sinRoll = tf2Sin(halfRoll);
106  setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
107  cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
108  cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
109  cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
110  }
115  ROS_DEPRECATED void setEulerZYX(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll)
116  {
117  setRPY(roll, pitch, yaw);
118  }
122  {
123  m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3];
124  return *this;
125  }
126 
130  {
131  m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3];
132  return *this;
133  }
134 
138  {
139  m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s;
140  return *this;
141  }
142 
147  {
148  setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
149  m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
150  m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
151  m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
152  return *this;
153  }
156  tf2Scalar dot(const Quaternion& q) const
157  {
158  return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
159  }
160 
163  {
164  return dot(*this);
165  }
166 
169  {
170  return tf2Sqrt(length2());
171  }
172 
176  {
177  return *this /= length();
178  }
179 
183  operator*(const tf2Scalar& s) const
184  {
185  return Quaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
186  }
187 
188 
192  {
193  tf2Assert(s != tf2Scalar(0.0));
194  return *this * (tf2Scalar(1.0) / s);
195  }
196 
200  {
201  tf2Assert(s != tf2Scalar(0.0));
202  return *this *= tf2Scalar(1.0) / s;
203  }
204 
207  {
208  return *this / length();
209  }
212  tf2Scalar angle(const Quaternion& q) const
213  {
214  tf2Scalar s = tf2Sqrt(length2() * q.length2());
215  tf2Assert(s != tf2Scalar(0.0));
216  return tf2Acos(dot(q) / s);
217  }
221  {
222  tf2Scalar s = tf2Sqrt(length2() * q.length2());
223  tf2Assert(s != tf2Scalar(0.0));
224  if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
225  return tf2Acos(dot(-q) / s) * tf2Scalar(2.0);
226  else
227  return tf2Acos(dot(q) / s) * tf2Scalar(2.0);
228  }
231  {
232  tf2Scalar s = tf2Scalar(2.) * tf2Acos(m_floats[3]);
233  return s;
234  }
235 
238  {
239  tf2Scalar s;
240  if (m_floats[3] >= 0)
241  s = tf2Scalar(2.) * tf2Acos(m_floats[3]);
242  else
243  s = tf2Scalar(2.) * tf2Acos(-m_floats[3]);
244 
245  return s;
246  }
247 
249  Vector3 getAxis() const
250  {
251  tf2Scalar s_squared = tf2Scalar(1.) - tf2Pow(m_floats[3], tf2Scalar(2.));
252  if (s_squared < tf2Scalar(10.) * TF2SIMD_EPSILON) //Check for divide by zero
253  return Vector3(1.0, 0.0, 0.0); // Arbitrary
254  tf2Scalar s = tf2Sqrt(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 tf2Scalar& t) const
317  {
318  tf2Scalar theta = angleShortestPath(q) / tf2Scalar(2.0);
319  if (theta != tf2Scalar(0.0))
320  {
321  tf2Scalar d = tf2Scalar(1.0) / tf2Sin(theta);
322  tf2Scalar s0 = tf2Sin((tf2Scalar(1.0) - t) * theta);
323  tf2Scalar s1 = tf2Sin(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(tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(1.));
345  return identityQuat;
346  }
347 
348  TF2SIMD_FORCE_INLINE const tf2Scalar& 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 tf2Scalar& t)
433 {
434  return q1.slerp(q2, t);
435 }
436 
437 TF2SIMD_FORCE_INLINE Vector3
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  tf2Scalar d = v0.dot(v1);
450 
451  if (d < -1.0 + TF2SIMD_EPSILON)
452  {
453  Vector3 n,unused;
454  tf2PlaneSpace1(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  tf2Scalar s = tf2Sqrt((1.0f + d) * 2.0f);
459  tf2Scalar rs = 1.0f / s;
460 
461  return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
462 }
463 
465 shortestArcQuatNormalize2(Vector3& v0,Vector3& v1)
466 {
467  v0.normalize();
468  v1.normalize();
469  return shortestArcQuat(v0,v1);
470 }
471 
472 }
473 #endif
474 
475 
476 
477 
Quaternion & operator/=(const tf2Scalar &s)
Inversely scale this quaternion.
Definition: Quaternion.h:199
d
#define TF2SIMD_FORCE_INLINE
Definition: Scalar.h:129
TF2SIMD_FORCE_INLINE Quaternion shortestArcQuat(const Vector3 &v0, const Vector3 &v1)
Definition: Quaternion.h:446
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
Set the quaternion using fixed axis RPY.
Definition: Quaternion.h:95
TF2SIMD_FORCE_INLINE Quaternion operator+(const Quaternion &q2) const
Return the sum of this quaternion and the other.
Definition: Quaternion.h:267
TF2SIMD_FORCE_INLINE tf2Scalar tf2Cos(tf2Scalar x)
Definition: Scalar.h:179
TF2SIMD_FORCE_INLINE tf2Scalar tf2Acos(tf2Scalar x)
Definition: Scalar.h:182
static const Quaternion & getIdentity()
Definition: Quaternion.h:342
tf2Scalar angle(const Quaternion &q) const
Return the half angle between this quaternion and the other.
Definition: Quaternion.h:212
TF2SIMD_FORCE_INLINE Quaternion operator-() const
Return the negative of this quaternion This simply negates each element.
Definition: Quaternion.h:284
tf2Scalar getAngleShortestPath() const
Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path...
Definition: Quaternion.h:237
double tf2Scalar
The tf2Scalar type abstracts floating point numbers, to easily switch between double and single float...
Definition: Scalar.h:159
Quaternion slerp(const Quaternion &q, const tf2Scalar &t) const
Return the quaternion which is the result of Spherical Linear Interpolation between this and the othe...
Definition: Quaternion.h:316
TF2SIMD_FORCE_INLINE Quaternion operator-(const Quaternion &q2) const
Return the difference between this quaternion and the other.
Definition: Quaternion.h:276
TF2SIMD_FORCE_INLINE void tf2PlaneSpace1(const Vector3 &n, Vector3 &p, Vector3 &q)
Definition: Vector3.h:656
Quaternion()
No initialization constructor.
Definition: Quaternion.h:33
tf2Scalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
Definition: Quaternion.h:230
TF2SIMD_FORCE_INLINE tf2Scalar tf2Pow(tf2Scalar x, tf2Scalar y)
Definition: Scalar.h:188
ROS_DEPRECATED Quaternion(const tf2Scalar &yaw, const tf2Scalar &pitch, const tf2Scalar &roll)
Constructor from Euler angles.
Definition: Quaternion.h:52
Quaternion operator/(const tf2Scalar &s) const
Return an inversely scaled versionof this quaternion.
Definition: Quaternion.h:191
Quaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: Quaternion.h:175
TF2SIMD_FORCE_INLINE Quaternion farthest(const Quaternion &qd) const
Definition: Quaternion.h:290
TF2SIMD_FORCE_INLINE Quaternion shortestArcQuatNormalize2(Vector3 &v0, Vector3 &v1)
Definition: Quaternion.h:465
Quaternion inverse() const
Return the inverse of this quaternion.
Definition: Quaternion.h:259
tf2Scalar dot(const Quaternion &q) const
Return the dot product between this quaternion and another.
Definition: Quaternion.h:156
Quaternion(const tf2Scalar &x, const tf2Scalar &y, const tf2Scalar &z, const tf2Scalar &w)
Constructor from scalars.
Definition: Quaternion.h:38
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sin(tf2Scalar x)
Definition: Scalar.h:180
Vector3 getAxis() const
Return the axis of the rotation represented by this quaternion.
Definition: Quaternion.h:249
void setEuler(const tf2Scalar &yaw, const tf2Scalar &pitch, const tf2Scalar &roll)
Set the quaternion using Euler angles.
Definition: Quaternion.h:75
TF2SIMD_FORCE_INLINE Quaternion & operator+=(const Quaternion &q)
Add two quaternions.
Definition: Quaternion.h:121
TF2SIMD_FORCE_INLINE const tf2Scalar & getW() const
Definition: Quaternion.h:348
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sqrt(tf2Scalar x)
Definition: Scalar.h:177
#define TF2SIMD_EPSILON
Definition: Scalar.h:202
#define ROS_DEPRECATED
ROS_DEPRECATED void setEulerZYX(const tf2Scalar &yaw, const tf2Scalar &pitch, const tf2Scalar &roll)
Set the quaternion using euler angles.
Definition: Quaternion.h:115
void setRotation(const Vector3 &axis, const tf2Scalar &angle)
Set the rotation using axis angle notation.
Definition: Quaternion.h:63
tf2Scalar length() const
Return the length of the quaternion.
Definition: Quaternion.h:168
TF2SIMD_FORCE_INLINE Vector3 quatRotate(const Quaternion &rotation, const Vector3 &v)
Definition: Quaternion.h:438
Quaternion & operator-=(const Quaternion &q)
Sutf2ract out a quaternion.
Definition: Quaternion.h:129
Quaternion normalized() const
Return a normalized version of this quaternion.
Definition: Quaternion.h:206
Quaternion & operator*=(const Quaternion &q)
Multiply this quaternion by q on the right.
Definition: Quaternion.h:146
TF2SIMD_FORCE_INLINE Quaternion operator*(const tf2Scalar &s) const
Return a scaled version of this quaternion.
Definition: Quaternion.h:183
tf2Scalar length2() const
Return the length squared of the quaternion.
Definition: Quaternion.h:162
tf2Scalar angleShortestPath(const Quaternion &q) const
Return the angle between this quaternion and the other along the shortest path.
Definition: Quaternion.h:220
#define tf2Assert(x)
Definition: Scalar.h:144
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:30
Quaternion(const Vector3 &axis, const tf2Scalar &angle)
Axis angle Constructor.
Definition: Quaternion.h:44
Quaternion & operator*=(const tf2Scalar &s)
Scale this quaternion.
Definition: Quaternion.h:137
TF2SIMD_FORCE_INLINE Quaternion nearest(const Quaternion &qd) const
Definition: Quaternion.h:301


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Jun 27 2022 02:43:06