SLProject  4.2.000
A platform independent 3D computer graphics framework for desktop OS, Android, iOS and online in web browsers
SLQuat4.h
Go to the documentation of this file.
1 /**
2  * \file SLQuat.h
3  * \brief Quaternion class for rotations
4  * \date July 2014
5  * \authors Marcus Hudritsch
6  * \copyright http://opensource.org/licenses/GPL-3.0
7  * \remarks Please use clangformat to format the code. See more code style on
8  * https://github.com/cpvrlab/SLProject4/wiki/SLProject-Coding-Style
9 */
10 
11 #ifndef SLQUAT_H
12 #define SLQUAT_H
13 
14 #include <SLMath.h>
15 #include <SLMat4.h>
16 #include <SLVec4.h>
17 #include <Utils.h>
18 
19 //-----------------------------------------------------------------------------
20 //!Quaternion class for angle-axis rotation representation.
21 /*!Quaternion class for angle-axis rotation representation. For rotations
22 quaternions must have unit length. See http://en.wikipedia.org/wiki/Quaternion
23 Quaternions can be interpolated at low cost with the method lerp or slerp.
24 */
25 // clang-format off
26 template <class T>
27 class SLQuat4
28 {
29  public: SLQuat4 ();
30  SLQuat4 (T x, T y, T z, T w);
31  explicit SLQuat4 (const SLMat3<T>& m);
32  SLQuat4 (T angleDEG, const SLVec3<T>& axis);
33  SLQuat4 (const SLVec3<T>& v0, const SLVec3<T>& v1);
34  SLQuat4 (const T xRotRAD, const T yRotRAD, const T zRotRAD);
35 
36  T x () const { return _x; }
37  T y () const { return _y; }
38  T z () const { return _z; }
39  T w () const { return _w; }
40 
41  void set (T x, T y, T z, T w);
42  void fromMat3 (const SLMat3<T>& m);
43  void fromAngleAxis (T angleRAD,
44  T axisX, T axisY, T axisZ);
45  void fromEulerAngles (const T xRotRAD, const T yRotRAD, const T zRotRAD);
46  void fromVec3 (const SLVec3<T>& v0, const SLVec3<T>& v1);
47 
48  static SLQuat4<T> fromLookRotation(const SLVec3<T>& forward, const SLVec3<T>& up);
49 
50  SLMat3<T> toMat3 () const;
51  SLMat4<T> toMat4 () const;
52  SLVec4<T> toVec4 () const;
53  void toAngleAxis (T& angleDEG, SLVec3<T>& axis) const;
54  void toEulerAnglesXYZ(T& xRotRAD, T& yRotRAD, T& zRotRAD) const;
55  void toEulerAnglesZYX(T& zRotRAD, T& yRotRAD, T& xRotRAD) const;
56  void toEulerAnglesZXY(T& zRotRAD, T& xRotRAD, T& yRotRAD) const;
57 
58  T dot (const SLQuat4<T>& q) const;
59  T length () const;
60  SLQuat4<T> normalized () const;
61  T normalize ();
62  SLQuat4<T> inverted () const;
63  void invert ();
64  SLQuat4<T> conjugated () const;
65  void conjugate ();
66  SLQuat4<T> rotated (const SLQuat4<T>& b) const;
67  void rotate (const SLQuat4<T>& q);
68  SLVec3<T> rotate (const SLVec3<T>& vec) const;
69  SLQuat4<T> scaled (T scale) const;
70  void scale (T scale);
71  SLQuat4<T> lerp (const SLQuat4<T>& q2, T t) const;
72  void lerp (const SLQuat4<T>& q1,
73  const SLQuat4<T>& q2, T t);
74  SLQuat4<T> slerp (const SLQuat4<T>& q2, T t) const;
75  void slerp (const SLQuat4<T>& q1,
76  const SLQuat4<T>& q2, T t);
77 
78  SLQuat4<T>& operator= (SLQuat4<T> q);
79  SLQuat4<T> operator- (const SLQuat4<T>& q) const;
80  SLQuat4<T> operator+ (const SLQuat4<T>& q) const;
81  SLQuat4<T> operator* (const SLQuat4<T>& q) const;
82  SLQuat4<T> operator* (T s) const;
83  SLVec3<T> operator* (const SLVec3<T>& v) const;
84  SLbool operator== (const SLQuat4<T>& q) const;
85  SLbool operator!= (const SLQuat4<T>& q) const;
86  SLQuat4<T>& operator*= (const SLQuat4<T>& q2);
87  SLQuat4<T>& operator*= (T s);
88 
89  static SLQuat4 IDENTITY;
90 
91  private:
92  T _x, _y, _z, _w;
93 };
94 
95 //-----------------------------------------------------------------------------
96 template<class T> SLQuat4<T> SLQuat4<T>::IDENTITY = SLQuat4<T>(0.0f, 0.0f, 0.0f, 1.0f);
97 
98 //-----------------------------------------------------------------------------
99 template <class T>
100 inline SLQuat4<T>::SLQuat4() : _x(0), _y(0), _z(0), _w(1)
101 {
102 }
103 
104 //-----------------------------------------------------------------------------
105 template <class T>
106 inline SLQuat4<T>::SLQuat4(T x, T y, T z, T w) : _x(x), _y(y), _z(z), _w(w)
107 {
108 }
109 
110 //-----------------------------------------------------------------------------
111 template <class T>
112 inline SLQuat4<T>::SLQuat4(const SLMat3<T>& m)
113 {
114  fromMat3(m);
115 }
116 
117 //-----------------------------------------------------------------------------
118 template <class T>
119 inline SLQuat4<T>::SLQuat4(const T angleDEG, const SLVec3<T>& axis)
120 {
121  fromAngleAxis(angleDEG * DEG2RAD, axis.x, axis.y, axis.z);
122 }
123 
124 //-----------------------------------------------------------------------------
125 template <class T>
126 inline SLQuat4<T>::SLQuat4(const SLVec3<T>& v0, const SLVec3<T>& v1)
127 {
128  fromVec3(v0, v1);
129 }
130 
131 //-----------------------------------------------------------------------------
132 template <class T>
133 inline SLQuat4<T>::SLQuat4(const T xRotRAD, const T yRotRAD, const T zRotRAD)
134 {
135  fromEulerAngles(xRotRAD, yRotRAD, zRotRAD);
136 }
137 
138 //-----------------------------------------------------------------------------
139 template <class T>
140 void SLQuat4<T>::set(T x, T y, T z, T w)
141 {
142  _x = x;
143  _y = y;
144  _z = z;
145  _w = w;
146 }
147 //-----------------------------------------------------------------------------
148 template <class T>
149 void SLQuat4<T>::fromMat3 (const SLMat3<T>& m)
150 {
151  // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
152  // article "Quaternion Calculus and Fast Animation".
153 
154  const int next[3] = {1, 2, 0};
155 
156  T trace = m.trace();
157  T root;
158 
159  if (trace > (T)0)
160  {
161  // |_w| > 1/2, may as well choose _w > 1/2
162  root = sqrt(trace + (T)1); // 2w
163  _w = ((T)0.5)*root;
164  root = ((T)0.5)/root; // 1/(4w)
165  _x = (m(2,1) - m(1,2)) * root;
166  _y = (m(0,2) - m(2,0)) * root;
167  _z = (m(1,0) - m(0,1)) * root;
168  }
169  else
170  {
171  // |_w| <= 1/2
172  int i = 0;
173  if (m(1,1) > m(0,0)) i = 1;
174  if (m(2,2) > m(i,i)) i = 2;
175  int j = next[i];
176  int k = next[j];
177 
178  root = sqrt(m(i,i) - m(j,j) - m(k,k) + (T)1);
179  T* quat[3] = { &_x, &_y, &_z };
180  *quat[i] = ((T)0.5)*root;
181  root = ((T)0.5)/root;
182  _w = (m(k,j) - m(j,k))*root;
183  *quat[j] = (m(j,i) + m(i,j))*root;
184  *quat[k] = (m(k,i) + m(i,k))*root;
185  }
186 }
187 
188 //-----------------------------------------------------------------------------
189 template <class T>
190 void SLQuat4<T>::fromVec3(const SLVec3<T>& v0, const SLVec3<T>& v1)
191 {
192  // Code from "The Shortest Arc Quaternion"
193  // by Stan Melax in "Game Programming Gems".
194  if (v0 == -v1)
195  {
196  fromAngleAxis(PI, 1, 0, 0);
197  return;
198  }
199 
200  SLVec3<T> c;
201  c.cross(v0, v1);
202  T d = v0.dot(v1);
203  T s = sqrt((1 + d) * (T)2);
204 
205  _x = c.x / s;
206  _y = c.y / s;
207  _z = c.z / s;
208  _w = s * (T)0.5;
209 }
210 
211 //-----------------------------------------------------------------------------
212 template <class T>
213 SLQuat4<T> SLQuat4<T>::fromLookRotation(const SLVec3<T>& forward, const SLVec3<T>& up)
214 {
215  SLVec3f vector = forward;
216  vector.normalize();
217  SLVec3f vector2;
218  vector2.cross(up, vector);
219  vector2.normalize();
220  SLVec3f vector3;
221  vector3.cross(vector, vector2);
222  vector3.normalize();
223  float m00 = vector2.x;
224  float m01 = vector2.y;
225  float m02 = vector2.z;
226  float m10 = vector3.x;
227  float m11 = vector3.y;
228  float m12 = vector3.z;
229  float m20 = vector.x;
230  float m21 = vector.y;
231  float m22 = vector.z;
232 
233  float num8 = (m00 + m11) + m22;
234  SLQuat4<T> quaternion;
235  if (num8 > 0.0f)
236  {
237  float num = (float)sqrt(num8 + 1.0f);
238  quaternion._w = num * 0.5f;
239  num = 0.5f / num;
240  quaternion._x = (m12 - m21) * num;
241  quaternion._y = (m20 - m02) * num;
242  quaternion._z = (m01 - m10) * num;
243  }
244  else if ((m00 >= m11) && (m00 >= m22))
245  {
246  float num7 = (float)sqrt(((1.0f + m00) - m11) - m22);
247  float num4 = 0.5f / num7;
248  quaternion._x = 0.5f * num7;
249  quaternion._y = (m01 + m10) * num4;
250  quaternion._z = (m02 + m20) * num4;
251  quaternion._w = (m12 - m21) * num4;
252  }
253  else if (m11 > m22)
254  {
255  float num6 = (float)sqrt(((1.0f + m11) - m00) - m22);
256  float num3 = 0.5f / num6;
257  quaternion._x = (m10+ m01) * num3;
258  quaternion._y = 0.5f * num6;
259  quaternion._z = (m21 + m12) * num3;
260  quaternion._w = (m20 - m02) * num3;
261  }
262  else
263  {
264  float num5 = (float)sqrt(((1.0f + m22) - m00) - m11);
265  float num2 = 0.5f / num5;
266  quaternion._x = (m20 + m02) * num2;
267  quaternion._y = (m21 + m12) * num2;
268  quaternion._z = 0.5f * num5;
269  quaternion._w = (m01 - m10) * num2;
270  }
271  return quaternion;
272 }
273 //-----------------------------------------------------------------------------
274 template <class T>
275 void SLQuat4<T>::fromAngleAxis(const T angleRAD,
276  const T axisX, const T axisY, const T axisZ)
277 {
278  _w = (T)cos(angleRAD * (T)0.5);
279  _x = _y = _z = (T)sin(angleRAD * (T)0.5);
280  _x *= axisX;
281  _y *= axisY;
282  _z *= axisZ;
283 }
284 
285 //-----------------------------------------------------------------------------
286 /*! Sets the quaternion from 3 Euler angles in radians
287 Source: Essential Mathematics for Games and Interactive Applications
288 A Programmer's Guide 2nd edition by James M. Van Verth and Lars M. Bishop
289 */
290 template <class T>
291 void SLQuat4<T>::fromEulerAngles(const T xRotRAD, const T yRotRAD, const T zRotRAD)
292 {
293  // Basically we create 3 Quaternions, one for (x), one for (y),
294  // one for (z) and multiply those together.
295  // The calculation below does the same, just shorter
296 
297  //ghm1: I checked function against https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
298  //and it is the same, only the sign is inverted (but this should be no problem)
299  T x = xRotRAD * (T)0.5;
300  T y = yRotRAD * (T)0.5;
301  T z = zRotRAD * (T)0.5;
302 
303  T Sx = (T)sin(x);
304  T Cx = (T)cos(x);
305  T Sy = (T)sin(y);
306  T Cy = (T)cos(y);
307  T Sz = (T)sin(z);
308  T Cz = (T)cos(z);
309 
310  _x = Sx*Cy*Cz + Cx*Sy*Sz;
311  _y = Cx*Sy*Cz - Sx*Cy*Sz;
312  _z = Cx*Cy*Sz + Sx*Sy*Cx;
313  _w = Cx*Cy*Cz - Sx*Sy*Sz;
314 }
315 
316 //-----------------------------------------------------------------------------
317 template <class T>
318 SLMat3<T> SLQuat4<T>::toMat3() const
319 {
320  T x2 = _x *(T)2;
321  T y2 = _y *(T)2;
322  T z2 = _z *(T)2;
323 
324  T wx2 = _w * x2; T wy2 = _w * y2; T wz2 = _w * z2;
325  T xx2 = _x * x2; T xy2 = _x * y2; T xz2 = _x * z2;
326  T yy2 = _y * y2; T yz2 = _y * z2; T zz2 = _z * z2;
327 
328  SLMat3<T> m(1 -(yy2 + zz2), xy2 - wz2, xz2 + wy2,
329  xy2 + wz2, 1 -(xx2 + zz2), yz2 - wx2,
330  xz2 - wy2, yz2 + wx2, 1 -(xx2 + yy2));
331 
332  return m;
333 }
334 
335 //-----------------------------------------------------------------------------
336 template <class T>
337 SLMat4<T> SLQuat4<T>::toMat4() const
338 {
339  T x2 = _x *(T)2;
340  T y2 = _y *(T)2;
341  T z2 = _z *(T)2;
342 
343  T wx2 = _w * x2; T wy2 = _w * y2; T wz2 = _w * z2;
344  T xx2 = _x * x2; T xy2 = _x * y2; T xz2 = _x * z2;
345  T yy2 = _y * y2; T yz2 = _y * z2; T zz2 = _z * z2;
346 
347  SLMat4<T> m(1 -(yy2 + zz2), xy2 - wz2, xz2 + wy2, 0,
348  xy2 + wz2, 1 -(xx2 + zz2), yz2 - wx2, 0,
349  xz2 - wy2, yz2 + wx2, 1 -(xx2 + yy2), 0,
350  0, 0, 0, 1);
351  return m;
352 }
353 
354 //-----------------------------------------------------------------------------
355 template <class T>
356 inline SLVec4<T> SLQuat4<T>::toVec4() const
357 {
358  return SLVec4<T>(_x, _y, _z, _w);
359 }
360 
361 //-----------------------------------------------------------------------------
362 template <typename T>
363 void SLQuat4<T>::toAngleAxis (T& angleDEG, SLVec3<T>& axis) const
364 {
365  // The quaternion representing the rotation is
366  // q = cos(A/2) + sin(A/2)*(_x*i+_y*j+_z*k)
367 
368  T sqrLen = _x*_x + _y*_y + _z*_z;
369 
370  if (sqrLen > FLT_EPSILON)
371  {
372  angleDEG = ((T)2) * acos(_w) * RAD2DEG;
373  T len = sqrt(sqrLen);
374  axis.x = _x / len;
375  axis.y = _y / len;
376  axis.z = _z / len;
377  }
378  else
379  {
380  // Angle is 0 (mod 2*pi), so any axis will do.
381  angleDEG = (T)0;
382  axis.x = (T)1;
383  axis.y = (T)0;
384  axis.z = (T)0;
385  }
386 }
387 //-----------------------------------------------------------------------------
388 template <typename T>
389 void SLQuat4<T>::toEulerAnglesZYX(T& zRotRAD, T& yRotRAD, T& xRotRAD) const
390 {
391  double sinz = +2.0 * (_w * _z + _x * _y);
392  double cosz = +1.0 - 2.0 * (_y *_y + _z *_z);
393  zRotRAD = (T)atan2(sinz, cosz);
394 
395  double siny = +2.0 * (_w *_y - _z *_x);
396  if (fabs(siny) >= 1)
397  yRotRAD = (T)copysign(PI / 2, siny); // use 90 degrees if out of range
398  else
399  yRotRAD = (T)asin(siny);
400 
401  double sinx = +2.0 * (_w * _x + _y * _z);
402  double cosx = +1.0 - 2.0 * (_x * _x + _y * _y);
403  xRotRAD = (T)atan2(sinx, cosx);
404 }
405 //-----------------------------------------------------------------------------
406 template <typename T>
407 void SLQuat4<T>::toEulerAnglesXYZ(T& xRotRAD, T& yRotRAD, T& zRotRAD) const
408 {
409  double sinx = -(T)2 * (_z * _y - _w * _x);
410  double cosx = +1.0 - (T)2 * (_x * _x + _y * _y);
411  xRotRAD = (T)atan2(sinx, cosx);
412 
413  double siny = (T)2 * (_w * _y + _x * _z);
414  if (fabs(siny) >= 1)
415  yRotRAD = (T)copysign(PI / 2, siny); // use 90 degrees if out of range
416  else
417  yRotRAD = (T)asin(siny);
418 
419  double sinz = -(T)2 * (_x * _y - _w * _z);
420  double cosz = +1.0 - (T)2 * (_y *_y + _z *_z);
421  zRotRAD = (T)atan2(sinz, cosz);
422 }
423 //-----------------------------------------------------------------------------
424 template <typename T>
425 void SLQuat4<T>::toEulerAnglesZXY(T& zRotRAD, T& xRotRAD, T& yRotRAD) const
426 {
427  double sinz = -(T)2 * (_x * _y - _w * _z);
428  double cosz = 1 - (T)2 * (_x * _x + _z * _z);
429  zRotRAD = (T)atan2(sinz, cosz);
430 
431  double sinx = (T)2 * (_y * _z + _w * _x);
432  if (fabs(sinx) >= 1)
433  xRotRAD = (T)copysign(PI / 2, sinx); // use 90 degrees if out of range
434  else
435  xRotRAD = (T)asin(sinx);
436 
437  double siny = -(T)2 * (_x * _z - _w * _y);
438  double cosy = 1 - (T)2 * (_x * _x + _y * _y);
439  yRotRAD = (T)atan2(siny, cosy);
440 }
441 //-----------------------------------------------------------------------------
442 template<class T>
443 SLQuat4<T>& SLQuat4<T>::operator= (const SLQuat4<T> q)
444 {
445  _x = q._x;
446  _y = q._y;
447  _z = q._z;
448  _w = q._w;
449  return(*this);
450 }
451 //-----------------------------------------------------------------------------
452 template <class T>
453 SLQuat4<T> SLQuat4<T>::operator- (const SLQuat4<T>& q) const
454 {
455  return SLQuat4<T>(_x - q._x, _y - q._y, _z - q._z, _w - q._w);
456 }
457 
458 //-----------------------------------------------------------------------------
459 template <class T>
460 SLQuat4<T> SLQuat4<T>::operator+ (const SLQuat4<T>& q) const
461 {
462  return SLQuat4<T>(_x + q._x, _y + q._y, _z + q._z, _w + q._w);
463 }
464 
465 //-----------------------------------------------------------------------------
466 template <class T>
467 SLQuat4<T> SLQuat4<T>::operator* (const SLQuat4<T>& q) const
468 {
469  return rotated(q);
470 }
471 
472 //-----------------------------------------------------------------------------
473 template <class T>
474 SLQuat4<T> SLQuat4<T>::operator* (const T s) const
475 {
476  return scaled(s);
477 }
478 
479 //-----------------------------------------------------------------------------
480 template <class T>
481 SLVec3<T> SLQuat4<T>::operator* (const SLVec3<T>& v) const
482 {
483  // nVidia SDK implementation
484  SLVec3<T> uv, uuv;
485  SLVec3<T> qvec(_x, _y, _z);
486  uv = qvec.crossProduct(v);
487  uuv = qvec.crossProduct(uv);
488  uv *= (2.0f * _w);
489  uuv *= 2.0f;
490 
491  return v + uv + uuv;
492 }
493 
494 //-----------------------------------------------------------------------------
495 template <class T>
496 bool SLQuat4<T>::operator== (const SLQuat4<T>& q) const
497 {
498  return _x == q._x && _y == q._y && _z == q._z && _w == q._w;
499 }
500 
501 //-----------------------------------------------------------------------------
502 template <class T>
503 bool SLQuat4<T>::operator!= (const SLQuat4<T>& q) const
504 {
505  return !(*this == q);
506 }
507 //-----------------------------------------------------------------------------
508 template<class T>
509 SLQuat4<T>& SLQuat4<T>::operator*= (const SLQuat4<T>& q2)
510 {
511  SLQuat4<T> q;
512  SLQuat4<T>& q1 = *this;
513 
514  q._w = q1._w * q2._w - q1._x * q2._x - q1._y * q2._y - q1._z * q2._z;
515  q._x = q1._w * q2._x + q1._x * q2._w + q1._y * q2._z - q1._z * q2._y;
516  q._y = q1._w * q2._y + q1._y * q2._w + q1._z * q2._x - q1._x * q2._z;
517  q._z = q1._w * q2._z + q1._z * q2._w + q1._x * q2._y - q1._y * q2._x;
518 
519  q.normalize();
520  *this = q;
521  return *this;
522 }
523 //-----------------------------------------------------------------------------
524 template<class T>
525 SLQuat4<T>& SLQuat4<T>::operator*= (const T s)
526 {
527  _x *= s;
528  _y *= s;
529  _z *= s;
530  _w *= s;
531  return *this;
532 }
533 
534 //-----------------------------------------------------------------------------
535 template <class T>
536 inline T SLQuat4<T>::dot(const SLQuat4<T>& q) const
537 {
538  return _x * q._x + _y * q._y + _z * q._z + _w * q._w;
539 }
540 
541 //-----------------------------------------------------------------------------
542 template <class T>
543 inline T SLQuat4<T>::length() const
544 {
545  return sqrt(_x*_x + _y*_y + _z*_z + _w*_w);
546 }
547 
548 //-----------------------------------------------------------------------------
549 template <class T>
550 SLQuat4<T> SLQuat4<T>::normalized() const
551 {
552  T len = length();
553  SLQuat4<T> norm;
554 
555  if (len > FLT_EPSILON)
556  {
557  T invLen = ((T)1)/len;
558  norm._x = _x * invLen;
559  norm._y = _y * invLen;
560  norm._z = _z * invLen;
561  norm._w = _w * invLen;
562  } else
563  { // set invalid result to flag the error.
564  norm._x = (T)0;
565  norm._y = (T)0;
566  norm._z = (T)0;
567  norm._w = (T)0;
568  }
569 
570  return norm;
571 }
572 
573 //-----------------------------------------------------------------------------
574 template <class T>
575 inline T SLQuat4<T>::normalize()
576 {
577  T len = length();
578 
579  if (len > FLT_EPSILON)
580  {
581  T invLen = ((T)1)/len;
582  _x *= invLen;
583  _y *= invLen;
584  _z *= invLen;
585  _w *= invLen;
586  } else
587  { // set invalid result to flag the error.
588  len = (T)0;
589  _x = (T)0;
590  _y = (T)0;
591  _z = (T)0;
592  _w = (T)0;
593  }
594 
595  return len;
596 }
597 
598 //----------------------------------------------------------------------------
599 template <class T>
600 SLQuat4<T> SLQuat4<T>::inverted () const
601 {
602  SLQuat4<T> inverse;
603  T norm = _x*_x + _y*_y + _z*_z + _w*_w;
604 
605  if (norm > (T)0)
606  {
607  // for non-unit quaternions we have to normalize
608  T invNorm = ((T)1) / norm;
609  inverse._x = -_x * invNorm;
610  inverse._y = -_y * invNorm;
611  inverse._z = -_z * invNorm;
612  inverse._w = _w * invNorm;
613  } else
614  {
615  // return an invalid result to flag the error.
616  inverse._x = (T)0;
617  inverse._y = (T)0;
618  inverse._z = (T)0;
619  inverse._w = (T)0;
620  }
621 
622  return inverse;
623 }
624 
625 //----------------------------------------------------------------------------
626 template <class T>
627 void SLQuat4<T>::invert()
628 {
629  T norm = _x*_x + _y*_y + _z*_z + _w*_w;
630 
631  if (norm > (T)0)
632  {
633  // for non-unit quaternions we have to normalize
634  T invNorm = ((T)1) / norm;
635  _x = -_x * invNorm;
636  _y = -_y * invNorm;
637  _z = -_z * invNorm;
638  _w = _w * invNorm;
639  } else
640  {
641  // return an invalid result to flag the error.
642  _x = (T)0;
643  _y = (T)0;
644  _z = (T)0;
645  _w = (T)0;
646  }
647 }
648 
649 //----------------------------------------------------------------------------
650 template <class T>
651 SLQuat4<T> SLQuat4<T>::conjugated() const
652 {
653  // for a unit quaternion the conjugate is equal to the inverse
654  return SLQuat4(-_x, -_y, -_z, _w);
655 }
656 
657 //----------------------------------------------------------------------------
658 template <class T>
659 void SLQuat4<T>::conjugate()
660 {
661  // for a unit quaternion the conjugate is equal to the inverse
662  _x = -_x;
663  _y = -_y;
664  _z = -_z;
665 }
666 
667 //-----------------------------------------------------------------------------
668 template <class T>
669 inline SLQuat4<T> SLQuat4<T>::rotated(const SLQuat4<T>& b) const
670 {
671  SLQuat4<T> q;
672  q._w = _w*b._w - _x*b._x - _y*b._y - _z*b._z;
673  q._x = _w*b._x + _x*b._w + _y*b._z - _z*b._y;
674  q._y = _w*b._y + _y*b._w + _z*b._x - _x*b._z;
675  q._z = _w*b._z + _z*b._w + _x*b._y - _y*b._x;
676  q.normalize();
677  return q;
678 }
679 
680 //-----------------------------------------------------------------------------
681 template <class T>
682 inline void SLQuat4<T>::rotate(const SLQuat4<T>& q2)
683 {
684  SLQuat4<T> q;
685  SLQuat4<T>& q1 = *this;
686 
687  q._w = q1._w * q2._w - q1._x * q2._x - q1._y * q2._y - q1._z * q2._z;
688  q._x = q1._w * q2._x + q1._x * q2._w + q1._y * q2._z - q1._z * q2._y;
689  q._y = q1._w * q2._y + q1._y * q2._w + q1._z * q2._x - q1._x * q2._z;
690  q._z = q1._w * q2._z + q1._z * q2._w + q1._x * q2._y - q1._y * q2._x;
691 
692  q.normalize();
693  *this = q;
694 }
695 
696 template <class T>
697 inline SLVec3<T> SLQuat4<T>::rotate(const SLVec3<T>& vec) const
698 {
699  SLMat3<T> rot = toMat3();
700  return rot * vec;
701 }
702 
703 //-----------------------------------------------------------------------------
704 template <class T>
705 inline SLQuat4<T> SLQuat4<T>::scaled(T s) const
706 {
707  return SLQuat4<T>(_x * s, _y * s, _z * s, _w * s);
708 }
709 
710 //-----------------------------------------------------------------------------
711 template <class T>
712 inline void SLQuat4<T>::scale(T s)
713 {
714  _x *= s;
715  _y *= s;
716  _z *= s;
717  _w *= s;
718 }
719 
720 //-----------------------------------------------------------------------------
721 //! Linear interpolation
722 template <class T>
723 inline SLQuat4<T> SLQuat4<T>::lerp(const SLQuat4<T>& q2, const T t) const
724 {
725  SLQuat4<T> q = scaled(1-t) + q2.scaled(t);
726  q.normalize();
727  return q;
728 }
729 //-----------------------------------------------------------------------------
730 //! Linear interpolation
731 template <class T>
732 inline void SLQuat4<T>::lerp(const SLQuat4<T>& q1,
733  const SLQuat4<T>& q2, const T t)
734 {
735  *this = q1.scaled(1-t) + q2.scaled(t);
736  normalize();
737 }
738 
739 //-----------------------------------------------------------------------------
740 /*! Spherical linear interpolation
741 @todo clean up the code below and find a working algorithm or check the
742  original shoemake implementation for errors.
743  */
744 template <class T>
745 inline SLQuat4<T> SLQuat4<T>::slerp(const SLQuat4<T>& q2, const T t) const
746 {
747  // Not 100% slerp, uses lerp in case of close angle! note the todo above this line!
748  SLfloat factor = t;
749  // calc cosine theta
750  T cosom = _x * q2._x + _y * q2._y + _z * q2._z + _w * q2._w;
751 
752  // adjust signs (if necessary)
753  SLQuat4<T> endCpy = q2;
754  if( cosom < static_cast<T>(0.0))
755  {
756  cosom = -cosom;
757  endCpy._x = -endCpy._x; // Reverse all signs
758  endCpy._y = -endCpy._y;
759  endCpy._z = -endCpy._z;
760  endCpy._w = -endCpy._w;
761  }
762 
763  // Calculate coefficients
764  T sclp, sclq;
765  if( (static_cast<T>(1.0) - cosom) > static_cast<T>(0.0001)) // 0.0001 -> some epsillon
766  {
767  // Standard case (slerp)
768  T omega, sinom;
769  omega = acos( cosom); // extract theta from dot product's cos theta
770  sinom = sin( omega);
771  sclp = sin( (static_cast<T>(1.0) - factor) * omega) / sinom;
772  sclq = sin( factor * omega) / sinom;
773  } else
774  {
775  // Very close, do linear interp (because it's faster)
776  sclp = static_cast<T>(1.0) - factor;
777  sclq = factor;
778  }
779 
780  SLQuat4<T> out;
781  out._x = sclp * _x + sclq * endCpy._x;
782  out._y = sclp * _y + sclq * endCpy._y;
783  out._z = sclp * _z + sclq * endCpy._z;
784  out._w = sclp * _w + sclq * endCpy._w;
785  return out;
786 
787  /*OLD
788  // Ken Shoemake's famous method.
789  assert(t>=0 && t<=1 && "Wrong t in SLQuat4::slerp");
790 
791  T cosAngle = dot(q2);
792 
793  if (cosAngle > 1 - FLT_EPSILON)
794  {
795  SLQuat4<T> result = q2 + (*this - q2).scaled(t);
796  result.normalize();
797  return result;
798  }
799 
800  if (cosAngle < 0) cosAngle = 0;
801  if (cosAngle > 1) cosAngle = 1;
802 
803  T theta0 = acos(cosAngle);
804  T theta = theta0 * t;
805 
806  SLQuat4<T> v2 = (q2 - scaled(cosAngle));
807  v2.normalize();
808 
809  SLQuat4<T> q = scaled(cos(theta)) + v2.scaled(sin(theta));
810  q.normalize();
811  return q;
812  */
813 }
814 //-----------------------------------------------------------------------------
815 //! Spherical linear interpolation
816 template <class T>
817 inline void SLQuat4<T>::slerp(const SLQuat4<T>& q1,
818  const SLQuat4<T>& q2, const T t)
819 {
820  // Ken Shoemake's famous method.
821  assert(t>=0 && t<=1 && "Wrong t in SLQuat4::slerp");
822 
823  T cosAngle = q1.dot(q2);
824 
825  if (cosAngle > 1 - FLT_EPSILON)
826  {
827  *this = q2 + (q1 - q2).scaled(t);
828  normalize();
829  return;
830  }
831 
832  if (cosAngle < 0) cosAngle = 0;
833  if (cosAngle > 1) cosAngle = 1;
834 
835  T theta0 = acos(cosAngle);
836  T theta = theta0 * t;
837 
838  SLQuat4<T> v2 = (q2 - q1.scaled(cosAngle));
839  v2.normalize();
840 
841  *this = q1.scaled(cos(theta)) + v2.scaled(sin(theta));
842  normalize();
843 }
844 
845 //-----------------------------------------------------------------------------
846 typedef SLQuat4<SLfloat> SLQuat4f;
847 //-----------------------------------------------------------------------------
848 #endif
SLfloat
float SLfloat
Definition: SL.h:173
SLbool
bool SLbool
Definition: SL.h:175
SLQuat4f
SLQuat4< SLfloat > SLQuat4f
Definition: SLQuat4.h:846
s
SLScene * s
Definition: SLScene.h:31
SLMat3
3x3 matrix template class
Definition: SLMat3.h:39
SLMat3::trace
T trace() const
Definition: SLMat3.h:486
SLMat4
4x4 matrix template class
Definition: SLMat4.h:52
SLQuat4
Quaternion class for angle-axis rotation representation.
Definition: SLQuat4.h:28
SLQuat4::inverted
SLQuat4< T > inverted() const
Definition: SLQuat4.h:600
SLQuat4::conjugate
void conjugate()
Definition: SLQuat4.h:659
SLQuat4::operator==
SLbool operator==(const SLQuat4< T > &q) const
Definition: SLQuat4.h:496
SLQuat4::w
T w() const
Definition: SLQuat4.h:39
SLQuat4::fromEulerAngles
void fromEulerAngles(const T xRotRAD, const T yRotRAD, const T zRotRAD)
Definition: SLQuat4.h:291
SLQuat4::operator*
SLQuat4< T > operator*(const SLQuat4< T > &q) const
Definition: SLQuat4.h:467
SLQuat4::operator+
SLQuat4< T > operator+(const SLQuat4< T > &q) const
Definition: SLQuat4.h:460
SLQuat4::rotated
SLQuat4< T > rotated(const SLQuat4< T > &b) const
Definition: SLQuat4.h:669
SLQuat4::scaled
SLQuat4< T > scaled(T scale) const
Definition: SLQuat4.h:705
SLQuat4::toEulerAnglesZYX
void toEulerAnglesZYX(T &zRotRAD, T &yRotRAD, T &xRotRAD) const
Definition: SLQuat4.h:389
SLQuat4::fromAngleAxis
void fromAngleAxis(T angleRAD, T axisX, T axisY, T axisZ)
Definition: SLQuat4.h:275
SLQuat4::lerp
void lerp(const SLQuat4< T > &q1, const SLQuat4< T > &q2, T t)
Linear interpolation.
Definition: SLQuat4.h:732
SLQuat4::_w
T _w
Definition: SLQuat4.h:92
SLQuat4::set
void set(T x, T y, T z, T w)
Definition: SLQuat4.h:140
SLQuat4::slerp
SLQuat4< T > slerp(const SLQuat4< T > &q2, T t) const
Definition: SLQuat4.h:745
SLQuat4::operator*=
SLQuat4< T > & operator*=(const SLQuat4< T > &q2)
Definition: SLQuat4.h:509
SLQuat4::z
T z() const
Definition: SLQuat4.h:38
SLQuat4::conjugated
SLQuat4< T > conjugated() const
Definition: SLQuat4.h:651
SLQuat4::scale
void scale(T scale)
Definition: SLQuat4.h:712
SLQuat4::toEulerAnglesZXY
void toEulerAnglesZXY(T &zRotRAD, T &xRotRAD, T &yRotRAD) const
Definition: SLQuat4.h:425
SLQuat4::toMat3
SLMat3< T > toMat3() const
Definition: SLQuat4.h:318
SLQuat4::SLQuat4
SLQuat4(const SLMat3< T > &m)
Definition: SLQuat4.h:112
SLQuat4::lerp
SLQuat4< T > lerp(const SLQuat4< T > &q2, T t) const
Linear interpolation.
Definition: SLQuat4.h:723
SLQuat4::SLQuat4
SLQuat4(T angleDEG, const SLVec3< T > &axis)
Definition: SLQuat4.h:119
SLQuat4::SLQuat4
SLQuat4(const T xRotRAD, const T yRotRAD, const T zRotRAD)
Definition: SLQuat4.h:133
SLQuat4::invert
void invert()
Definition: SLQuat4.h:627
SLQuat4::rotate
void rotate(const SLQuat4< T > &q)
Definition: SLQuat4.h:682
SLQuat4::_z
T _z
Definition: SLQuat4.h:92
SLQuat4::operator!=
SLbool operator!=(const SLQuat4< T > &q) const
Definition: SLQuat4.h:503
SLQuat4::IDENTITY
static SLQuat4 IDENTITY
Definition: SLQuat4.h:89
SLQuat4::operator-
SLQuat4< T > operator-(const SLQuat4< T > &q) const
Definition: SLQuat4.h:453
SLQuat4::fromMat3
void fromMat3(const SLMat3< T > &m)
Definition: SLQuat4.h:149
SLQuat4::SLQuat4
SLQuat4(const SLVec3< T > &v0, const SLVec3< T > &v1)
Definition: SLQuat4.h:126
SLQuat4::SLQuat4
SLQuat4(T x, T y, T z, T w)
Definition: SLQuat4.h:106
SLQuat4::slerp
void slerp(const SLQuat4< T > &q1, const SLQuat4< T > &q2, T t)
Spherical linear interpolation.
Definition: SLQuat4.h:817
SLQuat4::toAngleAxis
void toAngleAxis(T &angleDEG, SLVec3< T > &axis) const
Definition: SLQuat4.h:363
SLQuat4::dot
T dot(const SLQuat4< T > &q) const
Definition: SLQuat4.h:536
SLQuat4::length
T length() const
Definition: SLQuat4.h:543
SLQuat4::y
T y() const
Definition: SLQuat4.h:37
SLQuat4::SLQuat4
SLQuat4()
Definition: SLQuat4.h:100
SLQuat4::fromLookRotation
static SLQuat4< T > fromLookRotation(const SLVec3< T > &forward, const SLVec3< T > &up)
Definition: SLQuat4.h:213
SLQuat4::_x
T _x
Definition: SLQuat4.h:92
SLQuat4::x
T x() const
Definition: SLQuat4.h:36
SLQuat4::rotate
SLVec3< T > rotate(const SLVec3< T > &vec) const
Definition: SLQuat4.h:697
SLQuat4::normalized
SLQuat4< T > normalized() const
Definition: SLQuat4.h:550
SLQuat4::toVec4
SLVec4< T > toVec4() const
Definition: SLQuat4.h:356
SLQuat4::operator=
SLQuat4< T > & operator=(SLQuat4< T > q)
Definition: SLQuat4.h:443
SLQuat4::_y
T _y
Definition: SLQuat4.h:92
SLQuat4::toEulerAnglesXYZ
void toEulerAnglesXYZ(T &xRotRAD, T &yRotRAD, T &zRotRAD) const
Definition: SLQuat4.h:407
SLQuat4::fromVec3
void fromVec3(const SLVec3< T > &v0, const SLVec3< T > &v1)
Definition: SLQuat4.h:190
SLQuat4::normalize
T normalize()
Definition: SLQuat4.h:575
SLQuat4::toMat4
SLMat4< T > toMat4() const
Definition: SLQuat4.h:337
SLScene
The SLScene class represents the top level instance holding the scene structure.
Definition: SLScene.h:47
SLVec3
3D vector template class for standard 3D vector algebra.
Definition: SLVec3.h:40
SLVec3::y
T y
Definition: SLVec3.h:43
SLVec3::cross
void cross(const SLVec3 &a, const SLVec3 &b)
Definition: SLVec3.h:118
SLVec3::x
T x
Definition: SLVec3.h:43
SLVec3::normalize
SLVec3 & normalize()
Definition: SLVec3.h:124
SLVec3::dot
T dot(const SLVec3 &v) const
Definition: SLVec3.h:117
SLVec3::z
T z
Definition: SLVec3.h:43
SLVec4
4D vector template class for standard 4D vector algebra.
Definition: SLVec4.h:29
Utils::DEG2RAD
static const float DEG2RAD
Definition: Utils.h:239
Utils::RAD2DEG
static const float RAD2DEG
Definition: Utils.h:238
Utils::PI
static const float PI
Definition: Utils.h:237