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 namespace tf2
25 {
26 
28 class Quaternion : public QuadWord {
29 public:
32 
33  // template <typename tf2Scalar>
34  // explicit Quaternion(const tf2Scalar *v) : Tuple4<tf2Scalar>(v) {}
36  Quaternion(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z, const tf2Scalar& w)
37  : QuadWord(x, y, z, w)
38  {}
42  Quaternion(const Vector3& axis, const tf2Scalar& angle)
43  {
44  setRotation(axis, angle);
45  }
50  Quaternion(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) __attribute__((deprecated))
51  {
52 #ifndef TF2_EULER_DEFAULT_ZYX
53  setEuler(yaw, pitch, roll);
54 #else
55  setRPY(roll, pitch, yaw);
56 #endif
57  }
61  void setRotation(const Vector3& axis, const tf2Scalar& angle)
62  {
63  tf2Scalar d = axis.length();
64  tf2Assert(d != tf2Scalar(0.0));
65  tf2Scalar s = tf2Sin(angle * tf2Scalar(0.5)) / d;
66  setValue(axis.x() * s, axis.y() * s, axis.z() * s,
67  tf2Cos(angle * tf2Scalar(0.5)));
68  }
73  void setEuler(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll)
74  {
75  tf2Scalar halfYaw = tf2Scalar(yaw) * tf2Scalar(0.5);
76  tf2Scalar halfPitch = tf2Scalar(pitch) * tf2Scalar(0.5);
77  tf2Scalar halfRoll = tf2Scalar(roll) * tf2Scalar(0.5);
78  tf2Scalar cosYaw = tf2Cos(halfYaw);
79  tf2Scalar sinYaw = tf2Sin(halfYaw);
80  tf2Scalar cosPitch = tf2Cos(halfPitch);
81  tf2Scalar sinPitch = tf2Sin(halfPitch);
82  tf2Scalar cosRoll = tf2Cos(halfRoll);
83  tf2Scalar sinRoll = tf2Sin(halfRoll);
84  setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
85  cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
86  sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
87  cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
88  }
93  void setRPY(const tf2Scalar& roll, const tf2Scalar& pitch, const tf2Scalar& yaw)
94  {
95  tf2Scalar halfYaw = tf2Scalar(yaw) * tf2Scalar(0.5);
96  tf2Scalar halfPitch = tf2Scalar(pitch) * tf2Scalar(0.5);
97  tf2Scalar halfRoll = tf2Scalar(roll) * tf2Scalar(0.5);
98  tf2Scalar cosYaw = tf2Cos(halfYaw);
99  tf2Scalar sinYaw = tf2Sin(halfYaw);
100  tf2Scalar cosPitch = tf2Cos(halfPitch);
101  tf2Scalar sinPitch = tf2Sin(halfPitch);
102  tf2Scalar cosRoll = tf2Cos(halfRoll);
103  tf2Scalar sinRoll = tf2Sin(halfRoll);
104  setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
105  cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
106  cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
107  cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
108  }
113  void setEulerZYX(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) __attribute__((deprecated))
114  {
115  setRPY(roll, pitch, yaw);
116  }
120  {
121  m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3];
122  return *this;
123  }
124 
128  {
129  m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3];
130  return *this;
131  }
132 
136  {
137  m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s;
138  return *this;
139  }
140 
145  {
146  setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
147  m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
148  m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
149  m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
150  return *this;
151  }
154  tf2Scalar dot(const Quaternion& q) const
155  {
156  return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
157  }
158 
161  {
162  return dot(*this);
163  }
164 
167  {
168  return tf2Sqrt(length2());
169  }
170 
174  {
175  return *this /= length();
176  }
177 
181  operator*(const tf2Scalar& s) const
182  {
183  return Quaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
184  }
185 
186 
190  {
191  tf2Assert(s != tf2Scalar(0.0));
192  return *this * (tf2Scalar(1.0) / s);
193  }
194 
198  {
199  tf2Assert(s != tf2Scalar(0.0));
200  return *this *= tf2Scalar(1.0) / s;
201  }
202 
205  {
206  return *this / length();
207  }
210  tf2Scalar angle(const Quaternion& q) const
211  {
212  tf2Scalar s = tf2Sqrt(length2() * q.length2());
213  tf2Assert(s != tf2Scalar(0.0));
214  return tf2Acos(dot(q) / s);
215  }
219  {
220  tf2Scalar s = tf2Sqrt(length2() * q.length2());
221  tf2Assert(s != tf2Scalar(0.0));
222  if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
223  return tf2Acos(dot(-q) / s) * tf2Scalar(2.0);
224  else
225  return tf2Acos(dot(q) / s) * tf2Scalar(2.0);
226  }
229  {
230  tf2Scalar s = tf2Scalar(2.) * tf2Acos(m_floats[3]);
231  return s;
232  }
233 
236  {
237  tf2Scalar s;
238  if (m_floats[3] >= 0)
239  s = tf2Scalar(2.) * tf2Acos(m_floats[3]);
240  else
241  s = tf2Scalar(2.) * tf2Acos(-m_floats[3]);
242 
243  return s;
244  }
245 
247  Vector3 getAxis() const
248  {
249  tf2Scalar s_squared = tf2Scalar(1.) - tf2Pow(m_floats[3], tf2Scalar(2.));
250  if (s_squared < tf2Scalar(10.) * TF2SIMD_EPSILON) //Check for divide by zero
251  return Vector3(1.0, 0.0, 0.0); // Arbitrary
252  tf2Scalar s = tf2Sqrt(s_squared);
253  return Vector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s);
254  }
255 
258  {
259  return Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
260  }
261 
265  operator+(const Quaternion& q2) const
266  {
267  const Quaternion& q1 = *this;
268  return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
269  }
270 
274  operator-(const Quaternion& q2) const
275  {
276  const Quaternion& q1 = *this;
277  return Quaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
278  }
279 
283  {
284  const Quaternion& q2 = *this;
285  return Quaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]);
286  }
289  {
290  Quaternion diff,sum;
291  diff = *this - qd;
292  sum = *this + qd;
293  if( diff.dot(diff) > sum.dot(sum) )
294  return qd;
295  return (-qd);
296  }
297 
300  {
301  Quaternion diff,sum;
302  diff = *this - qd;
303  sum = *this + qd;
304  if( diff.dot(diff) < sum.dot(sum) )
305  return qd;
306  return (-qd);
307  }
308 
309 
314  Quaternion slerp(const Quaternion& q, const tf2Scalar& t) const
315  {
316  tf2Scalar theta = angleShortestPath(q) / tf2Scalar(2.0);
317  if (theta != tf2Scalar(0.0))
318  {
319  tf2Scalar d = tf2Scalar(1.0) / tf2Sin(theta);
320  tf2Scalar s0 = tf2Sin((tf2Scalar(1.0) - t) * theta);
321  tf2Scalar s1 = tf2Sin(t * theta);
322  if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
323  return Quaternion((m_floats[0] * s0 + -q.x() * s1) * d,
324  (m_floats[1] * s0 + -q.y() * s1) * d,
325  (m_floats[2] * s0 + -q.z() * s1) * d,
326  (m_floats[3] * s0 + -q.m_floats[3] * s1) * d);
327  else
328  return Quaternion((m_floats[0] * s0 + q.x() * s1) * d,
329  (m_floats[1] * s0 + q.y() * s1) * d,
330  (m_floats[2] * s0 + q.z() * s1) * d,
331  (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
332 
333  }
334  else
335  {
336  return *this;
337  }
338  }
339 
340  static const Quaternion& getIdentity()
341  {
342  static const Quaternion identityQuat(tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(1.));
343  return identityQuat;
344  }
345 
346  TF2SIMD_FORCE_INLINE const tf2Scalar& getW() const { return m_floats[3]; }
347 
348 
349 };
350 
351 
355 {
356  return Quaternion(-q.x(), -q.y(), -q.z(), -q.w());
357 }
358 
359 
360 
363 operator*(const Quaternion& q1, const Quaternion& q2) {
364  return Quaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
365  q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
366  q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
367  q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
368 }
369 
371 operator*(const Quaternion& q, const Vector3& w)
372 {
373  return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
374  q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
375  q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
376  -q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
377 }
378 
380 operator*(const Vector3& w, const Quaternion& q)
381 {
382  return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
383  w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
384  w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
385  -w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
386 }
387 
390 dot(const Quaternion& q1, const Quaternion& q2)
391 {
392  return q1.dot(q2);
393 }
394 
395 
398 length(const Quaternion& q)
399 {
400  return q.length();
401 }
402 
405 angle(const Quaternion& q1, const Quaternion& q2)
406 {
407  return q1.angle(q2);
408 }
409 
413 {
414  return q1.angleShortestPath(q2);
415 }
416 
419 inverse(const Quaternion& q)
420 {
421  return q.inverse();
422 }
423 
430 slerp(const Quaternion& q1, const Quaternion& q2, const tf2Scalar& t)
431 {
432  return q1.slerp(q2, t);
433 }
434 
435 TF2SIMD_FORCE_INLINE Vector3
436 quatRotate(const Quaternion& rotation, const Vector3& v)
437 {
438  Quaternion q = rotation * v;
439  q *= rotation.inverse();
440  return Vector3(q.getX(),q.getY(),q.getZ());
441 }
442 
444 shortestArcQuat(const Vector3& v0, const Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
445 {
446  Vector3 c = v0.cross(v1);
447  tf2Scalar d = v0.dot(v1);
448 
449  if (d < -1.0 + TF2SIMD_EPSILON)
450  {
451  Vector3 n,unused;
452  tf2PlaneSpace1(v0,n,unused);
453  return Quaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
454  }
455 
456  tf2Scalar s = tf2Sqrt((1.0f + d) * 2.0f);
457  tf2Scalar rs = 1.0f / s;
458 
459  return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
460 }
461 
463 shortestArcQuatNormalize2(Vector3& v0,Vector3& v1)
464 {
465  v0.normalize();
466  v1.normalize();
467  return shortestArcQuat(v0,v1);
468 }
469 
470 }
471 #endif
472 
473 
474 
475 
Quaternion & operator/=(const tf2Scalar &s)
Inversely scale this quaternion.
Definition: Quaternion.h:197
d
#define TF2SIMD_FORCE_INLINE
Definition: Scalar.h:129
TF2SIMD_FORCE_INLINE Quaternion shortestArcQuat(const Vector3 &v0, const Vector3 &v1)
Definition: Quaternion.h:444
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
Set the quaternion using fixed axis RPY.
Definition: Quaternion.h:93
TF2SIMD_FORCE_INLINE tf2Scalar tf2Cos(tf2Scalar x)
Definition: Scalar.h:179
Quaternion normalized() const
Return a normalized version of this quaternion.
Definition: Quaternion.h:204
TF2SIMD_FORCE_INLINE tf2Scalar tf2Acos(tf2Scalar x)
Definition: Scalar.h:182
static const Quaternion & getIdentity()
Definition: Quaternion.h:340
TF2SIMD_FORCE_INLINE Quaternion farthest(const Quaternion &qd) const
Definition: Quaternion.h:288
TF2SIMD_FORCE_INLINE const tf2Scalar & getW() const
Definition: Quaternion.h:346
Quaternion operator/(const tf2Scalar &s) const
Return an inversely scaled versionof this quaternion.
Definition: Quaternion.h:189
tf2Scalar length() const
Return the length of the quaternion.
Definition: Quaternion.h:166
double tf2Scalar
The tf2Scalar type abstracts floating point numbers, to easily switch between double and single float...
Definition: Scalar.h:159
Quaternion inverse() const
Return the inverse of this quaternion.
Definition: Quaternion.h:257
tf2Scalar getAngleShortestPath() const
Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path...
Definition: Quaternion.h:235
TF2SIMD_FORCE_INLINE void tf2PlaneSpace1(const Vector3 &n, Vector3 &p, Vector3 &q)
Definition: Vector3.h:656
Quaternion()
No initialization constructor.
Definition: Quaternion.h:31
TF2SIMD_FORCE_INLINE tf2Scalar tf2Pow(tf2Scalar x, tf2Scalar y)
Definition: Scalar.h:188
Quaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: Quaternion.h:173
tf2Scalar length2() const
Return the length squared of the quaternion.
Definition: Quaternion.h:160
tf2Scalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
Definition: Quaternion.h:228
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:314
void setEulerZYX(const tf2Scalar &yaw, const tf2Scalar &pitch, const tf2Scalar &roll) __attribute__((deprecated))
Set the quaternion using euler angles.
Definition: Quaternion.h:113
TF2SIMD_FORCE_INLINE Quaternion nearest(const Quaternion &qd) const
Definition: Quaternion.h:299
TF2SIMD_FORCE_INLINE Quaternion shortestArcQuatNormalize2(Vector3 &v0, Vector3 &v1)
Definition: Quaternion.h:463
Quaternion(const tf2Scalar &x, const tf2Scalar &y, const tf2Scalar &z, const tf2Scalar &w)
Constructor from scalars.
Definition: Quaternion.h:36
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sin(tf2Scalar x)
Definition: Scalar.h:180
tf2Scalar dot(const Quaternion &q) const
Return the dot product between this quaternion and another.
Definition: Quaternion.h:154
void setEuler(const tf2Scalar &yaw, const tf2Scalar &pitch, const tf2Scalar &roll)
Set the quaternion using Euler angles.
Definition: Quaternion.h:73
TF2SIMD_FORCE_INLINE Quaternion & operator+=(const Quaternion &q)
Add two quaternions.
Definition: Quaternion.h:119
TF2SIMD_FORCE_INLINE tf2Scalar tf2Sqrt(tf2Scalar x)
Definition: Scalar.h:177
#define TF2SIMD_EPSILON
Definition: Scalar.h:202
void setRotation(const Vector3 &axis, const tf2Scalar &angle)
Set the rotation using axis angle notation.
Definition: Quaternion.h:61
TF2SIMD_FORCE_INLINE Vector3 quatRotate(const Quaternion &rotation, const Vector3 &v)
Definition: Quaternion.h:436
TF2SIMD_FORCE_INLINE Quaternion operator-() const
Return the negative of this quaternion This simply negates each element.
Definition: Quaternion.h:282
Quaternion & operator-=(const Quaternion &q)
Sutf2ract out a quaternion.
Definition: Quaternion.h:127
Quaternion & operator*=(const Quaternion &q)
Multiply this quaternion by q on the right.
Definition: Quaternion.h:144
Vector3 getAxis() const
Return the axis of the rotation represented by this quaternion.
Definition: Quaternion.h:247
tf2Scalar angle(const Quaternion &q) const
Return the ***half*** angle between this quaternion and the other.
Definition: Quaternion.h:210
tf2Scalar angleShortestPath(const Quaternion &q) const
Return the angle between this quaternion and the other along the shortest path.
Definition: Quaternion.h:218
#define tf2Assert(x)
Definition: Scalar.h:144
TF2SIMD_FORCE_INLINE Quaternion operator+(const Quaternion &q2) const
Return the sum of this quaternion and the other.
Definition: Quaternion.h:265
TF2SIMD_FORCE_INLINE Quaternion operator*(const tf2Scalar &s) const
Return a scaled version of this quaternion.
Definition: Quaternion.h:181
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:28
Quaternion(const Vector3 &axis, const tf2Scalar &angle)
Axis angle Constructor.
Definition: Quaternion.h:42
Quaternion & operator*=(const tf2Scalar &s)
Scale this quaternion.
Definition: Quaternion.h:135
TF2SIMD_FORCE_INLINE Quaternion operator-(const Quaternion &q2) const
Return the difference between this quaternion and the other.
Definition: Quaternion.h:274
Quaternion(const tf2Scalar &yaw, const tf2Scalar &pitch, const tf2Scalar &roll) __attribute__((deprecated))
Constructor from Euler angles.
Definition: Quaternion.h:50


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Jun 7 2019 21:45:39