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 namespace tf
25 {
26 
28 class Quaternion : public QuadWord {
29 public:
32 
33 
34  // template <typename tfScalar>
35  // explicit Quaternion(const tfScalar *v) : Tuple4<tfScalar>(v) {}
37  Quaternion(const tfScalar& x, const tfScalar& y, const tfScalar& z, const tfScalar& w)
38  : QuadWord(x, y, z, w)
39  {}
43  Quaternion(const Vector3& axis, const tfScalar& angle)
44  {
45  setRotation(axis, angle);
46  }
51  Quaternion(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll) __attribute__((deprecated))
52  {
53 #ifndef TF_EULER_DEFAULT_ZYX
54  setEuler(yaw, pitch, roll);
55 #else
56  setRPY(roll, pitch, yaw);
57 #endif
58  }
62  void setRotation(const Vector3& axis, const tfScalar& angle)
63  {
64  tfScalar d = axis.length();
65  tfAssert(d != tfScalar(0.0));
66  tfScalar s = tfSin(angle * tfScalar(0.5)) / d;
67  setValue(axis.x() * s, axis.y() * s, axis.z() * s,
68  tfCos(angle * tfScalar(0.5)));
69  }
74  void setEuler(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll)
75  {
76  tfScalar halfYaw = tfScalar(yaw) * tfScalar(0.5);
77  tfScalar halfPitch = tfScalar(pitch) * tfScalar(0.5);
78  tfScalar halfRoll = tfScalar(roll) * tfScalar(0.5);
79  tfScalar cosYaw = tfCos(halfYaw);
80  tfScalar sinYaw = tfSin(halfYaw);
81  tfScalar cosPitch = tfCos(halfPitch);
82  tfScalar sinPitch = tfSin(halfPitch);
83  tfScalar cosRoll = tfCos(halfRoll);
84  tfScalar sinRoll = tfSin(halfRoll);
85  setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
86  cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
87  sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
88  cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
89  }
94  void setRPY(const tfScalar& roll, const tfScalar& pitch, const tfScalar& yaw)
95  {
96  tfScalar halfYaw = tfScalar(yaw) * tfScalar(0.5);
97  tfScalar halfPitch = tfScalar(pitch) * tfScalar(0.5);
98  tfScalar halfRoll = tfScalar(roll) * tfScalar(0.5);
99  tfScalar cosYaw = tfCos(halfYaw);
100  tfScalar sinYaw = tfSin(halfYaw);
101  tfScalar cosPitch = tfCos(halfPitch);
102  tfScalar sinPitch = tfSin(halfPitch);
103  tfScalar cosRoll = tfCos(halfRoll);
104  tfScalar sinRoll = tfSin(halfRoll);
105  setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
106  cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
107  cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
108  cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
109  }
114  void setEulerZYX(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll) __attribute__((deprecated))
115  {
116  setRPY(roll, pitch, yaw);
117  }
121  {
122  m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3];
123  return *this;
124  }
125 
129  {
130  m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3];
131  return *this;
132  }
133 
137  {
138  m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s;
139  return *this;
140  }
141 
146  {
147  setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
148  m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
149  m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
150  m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
151  return *this;
152  }
155  tfScalar dot(const Quaternion& q) const
156  {
157  return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
158  }
159 
162  {
163  return dot(*this);
164  }
165 
167  tfScalar length() const
168  {
169  return tfSqrt(length2());
170  }
171 
175  {
176  return *this /= length();
177  }
178 
182  operator*(const tfScalar& s) const
183  {
184  return Quaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
185  }
186 
187 
190  Quaternion operator/(const tfScalar& s) const
191  {
192  tfAssert(s != tfScalar(0.0));
193  return *this * (tfScalar(1.0) / s);
194  }
195 
199  {
200  tfAssert(s != tfScalar(0.0));
201  return *this *= tfScalar(1.0) / s;
202  }
203 
206  {
207  return *this / length();
208  }
211  tfScalar angle(const Quaternion& q) const
212  {
213  tfScalar s = tfSqrt(length2() * q.length2());
214  tfAssert(s != tfScalar(0.0));
215  return tfAcos(dot(q) / s);
216  }
220  {
221  tfScalar s = tfSqrt(length2() * q.length2());
222  tfAssert(s != tfScalar(0.0));
223  if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
224  return tfAcos(dot(-q) / s) * tfScalar(2.0);
225  else
226  return tfAcos(dot(q) / s) * tfScalar(2.0);
227  }
230  {
231  tfScalar s = tfScalar(2.) * tfAcos(m_floats[3]);
232  return s;
233  }
234 
237  {
238  tfScalar s;
239  if (m_floats[3] < 0)
240  s = tfScalar(2.) * tfAcos(-m_floats[3]);
241  else
242  s = tfScalar(2.) * tfAcos(m_floats[3]);
243  return s;
244  }
245 
247  Vector3 getAxis() const
248  {
249  tfScalar s_squared = tfScalar(1.) - tfPow(m_floats[3], tfScalar(2.));
250  if (s_squared < tfScalar(10.) * TFSIMD_EPSILON) //Check for divide by zero
251  return Vector3(1.0, 0.0, 0.0); // Arbitrary
252  tfScalar s = tfSqrt(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 tfScalar& t) const
315  {
316  tfScalar theta = angleShortestPath(q) / tfScalar(2.0);
317  if (theta != tfScalar(0.0))
318  {
319  tfScalar d = tfScalar(1.0) / tfSin(theta);
320  tfScalar s0 = tfSin((tfScalar(1.0) - t) * theta);
321  tfScalar s1 = tfSin(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(tfScalar(0.),tfScalar(0.),tfScalar(0.),tfScalar(1.));
343  return identityQuat;
344  }
345 
346  TFSIMD_FORCE_INLINE const tfScalar& 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 tfScalar& t)
431 {
432  return q1.slerp(q2, t);
433 }
434 
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  tfScalar d = v0.dot(v1);
448 
449  if (d < -1.0 + TFSIMD_EPSILON)
450  {
451  Vector3 n,unused;
452  tfPlaneSpace1(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  tfScalar s = tfSqrt((1.0f + d) * 2.0f);
457  tfScalar rs = 1.0f / s;
458 
459  return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
460 }
461 
464 {
465  v0.normalize();
466  v1.normalize();
467  return shortestArcQuat(v0,v1);
468 }
469 
470 }
471 #endif
472 
473 
474 
475 
d
tfScalar length() const
Return the length of the quaternion.
Definition: Quaternion.h:167
TFSIMD_FORCE_INLINE Quaternion operator+(const Quaternion &q2) const
Return the sum of this quaternion and the other.
Definition: Quaternion.h:265
tfScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
Definition: Quaternion.h:229
TFSIMD_FORCE_INLINE Quaternion operator-() const
Return the negative of this quaternion This simply negates each element.
Definition: Quaternion.h:282
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:28
f
double tfScalar
The tfScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: Scalar.h:160
tfScalar length2() const
Return the length squared of the quaternion.
Definition: Quaternion.h:161
TFSIMD_FORCE_INLINE void setValue(const tfScalar &x, const tfScalar &y, const tfScalar &z)
Definition: Vector3.h:529
TFSIMD_FORCE_INLINE Quaternion operator*(const tfScalar &s) const
Return a scaled version of this quaternion.
Definition: Quaternion.h:182
XmlRpcServer s
#define TFSIMD_EPSILON
Definition: Scalar.h:204
Quaternion & operator/=(const tfScalar &s)
Inversely scale this quaternion.
Definition: Quaternion.h:198
tfScalar getAngleShortestPath() const
Return the angle [0, Pi] of rotation represented by this quaternion along the shortest path...
Definition: Quaternion.h:236
Quaternion(const Vector3 &axis, const tfScalar &angle)
Axis angle Constructor.
Definition: Quaternion.h:43
TFSIMD_FORCE_INLINE const tfScalar & getY() const
Return the y value.
Definition: Vector3.h:251
Quaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: Quaternion.h:174
Definition: exceptions.h:38
TFSIMD_FORCE_INLINE const tfScalar & getW() const
Definition: Quaternion.h:346
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
Definition: Vector3.h:486
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
Return the z value.
Definition: Vector3.h:253
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:314
Quaternion & operator*=(const tfScalar &s)
Scale this quaternion.
Definition: Quaternion.h:136
TFSIMD_FORCE_INLINE Vector3 cross(const Vector3 &v) const
Return the cross product between this and another vector.
Definition: Vector3.h:181
Quaternion & operator*=(const Quaternion &q)
Multiply this quaternion by q on the right.
Definition: Quaternion.h:145
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
Definition: Vector3.h:263
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 tfScalar dot(const Vector3 &v) const
Return the dot product.
Definition: Vector3.h:123
tfScalar angle(const Quaternion &q) const
Return the ***half*** angle between this quaternion and the other.
Definition: Quaternion.h:211
TFSIMD_FORCE_INLINE tfScalar tfAcos(tfScalar x)
Definition: Scalar.h:184
TFSIMD_FORCE_INLINE const tfScalar & z() const
Return the z value.
Definition: Vector3.h:267
#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:94
static const Quaternion & getIdentity()
Definition: Quaternion.h:340
TFSIMD_FORCE_INLINE Vector3()
No initialization constructor.
Definition: Vector3.h:293
Vector3 getAxis() const
Return the axis of the rotation represented by this quaternion.
Definition: Quaternion.h:247
TFSIMD_FORCE_INLINE const tfScalar & y() const
Return the y value.
Definition: Vector3.h:265
tfScalar dot(const Quaternion &q) const
Return the dot product between this quaternion and another.
Definition: Quaternion.h:155
TFSIMD_FORCE_INLINE Vector3 quatRotate(const Quaternion &rotation, const Vector3 &v)
Definition: Quaternion.h:436
void setEulerZYX(const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll) __attribute__((deprecated))
Set the quaternion using euler angles.
Definition: Quaternion.h:114
#define TFSIMD_FORCE_INLINE
Definition: Scalar.h:130
TFSIMD_FORCE_INLINE const tfScalar & x() const
Return the x value.
Definition: Vector3.h:484
Quaternion operator/(const tfScalar &s) const
Return an inversely scaled versionof this quaternion.
Definition: Quaternion.h:190
void setEuler(const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll)
Set the quaternion using Euler angles.
Definition: Quaternion.h:74
TFSIMD_FORCE_INLINE const tfScalar & z() const
Return the z value.
Definition: Vector3.h:488
TFSIMD_FORCE_INLINE Quaternion operator-(const Quaternion &q2) const
Return the difference between this quaternion and the other.
Definition: Quaternion.h:274
Quaternion inverse() const
Return the inverse of this quaternion.
Definition: Quaternion.h:257
TFSIMD_FORCE_INLINE Vector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: Vector3.h:150
TFSIMD_FORCE_INLINE Quaternion nearest(const Quaternion &qd) const
Definition: Quaternion.h:299
Quaternion()
No initialization constructor.
Definition: Quaternion.h:31
TFSIMD_FORCE_INLINE tfScalar tfSqrt(tfScalar x)
Definition: Scalar.h:179
TFSIMD_FORCE_INLINE const tfScalar & w() const
Return the w value.
Definition: Vector3.h:490
Quaternion(const tfScalar &x, const tfScalar &y, const tfScalar &z, const tfScalar &w)
Constructor from scalars.
Definition: Quaternion.h:37
TFSIMD_FORCE_INLINE Quaternion farthest(const Quaternion &qd) const
Definition: Quaternion.h:288
TFSIMD_FORCE_INLINE Quaternion shortestArcQuatNormalize2(Vector3 &v0, Vector3 &v1)
Definition: Quaternion.h:463
Quaternion & operator-=(const Quaternion &q)
Sutfract out a quaternion.
Definition: Quaternion.h:128
TFSIMD_FORCE_INLINE Quaternion & operator+=(const Quaternion &q)
Add two quaternions.
Definition: Quaternion.h:120
Quaternion(const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll) __attribute__((deprecated))
Constructor from Euler angles.
Definition: Quaternion.h:51
TFSIMD_FORCE_INLINE const tfScalar & getX() const
Return the x value.
Definition: Vector3.h:249
tfScalar m_floats[4]
Definition: Vector3.h:286
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:444
tfScalar angleShortestPath(const Quaternion &q) const
Return the angle between this quaternion and the other along the shortest path.
Definition: Quaternion.h:219
void setRotation(const Vector3 &axis, const tfScalar &angle)
Set the rotation using axis angle notation.
Definition: Quaternion.h:62
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:205
TFSIMD_FORCE_INLINE tfScalar length() const
Return the length of the vector.
Definition: Vector3.h:135
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...
Definition: Vector3.h:38
tf::tfVector4 __attribute__


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Jun 7 2019 22:00:28