1 // quaternion.h
2 //
3 // Copyright (C) 2000-2006, Chris Laurel <claurel@shatters.net>
4 //
5 // Template-ized quaternion math library.
6 //
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License
9 // as published by the Free Software Foundation; either version 2
10 // of the License, or (at your option) any later version.
11
12 #ifndef _QUATERNION_H_
13 #define _QUATERNION_H_
14
15 #include <limits>
16 #include <celmath/mathlib.h>
17 #include <celmath/vecmath.h>
18
19
20 template<class T> class Quaternion
21 {
22 public:
23 inline Quaternion();
24 inline Quaternion(const Quaternion<T>&);
25 inline Quaternion(T);
26 inline Quaternion(const Vector3<T>&);
27 inline Quaternion(T, const Vector3<T>&);
28 inline Quaternion(T, T, T, T);
29
30 inline Quaternion(const Matrix3<T>&);
31
32 inline Quaternion& operator+=(Quaternion);
33 inline Quaternion& operator-=(Quaternion);
34 inline Quaternion& operator*=(T);
35 Quaternion& operator*=(Quaternion);
36
37 inline Quaternion operator~() const; // conjugate
38 inline Quaternion operator-() const;
39 inline Quaternion operator+() const;
40
41 void setAxisAngle(Vector3<T> axis, T angle);
42
43 void getAxisAngle(Vector3<T>& axis, T& angle) const;
44 Matrix4<T> toMatrix4() const;
45 Matrix3<T> toMatrix3() const;
46
47 static Quaternion<T> slerp(const Quaternion<T>&, const Quaternion<T>&, T);
48 static Quaternion<T> vecToVecRotation(const Vector3<T>& v0,
49 const Vector3<T>& v1);
50 static Quaternion<T> matrixToQuaternion(const Matrix3<T>& m);
51
52 void rotate(Vector3<T> axis, T angle);
53 void xrotate(T angle);
54 void yrotate(T angle);
55 void zrotate(T angle);
56
57 bool isPure() const;
58 bool isReal() const;
59 T normalize();
60
61 static Quaternion<T> xrotation(T);
62 static Quaternion<T> yrotation(T);
63 static Quaternion<T> zrotation(T);
64
65 static Quaternion<T> lookAt(const Point3<T>& from, const Point3<T>& to, const Vector3<T>& up);
66
67 T w, x, y, z;
68 };
69
70
71 typedef Quaternion<float> Quatf;
72 typedef Quaternion<double> Quatd;
73
74
Quaternion()75 template<class T> Quaternion<T>::Quaternion() : w(0), x(0), y(0), z(0)
76 {
77 }
78
Quaternion(const Quaternion<T> & q)79 template<class T> Quaternion<T>::Quaternion(const Quaternion<T>& q) :
80 w(q.w), x(q.x), y(q.y), z(q.z)
81 {
82 }
83
Quaternion(T re)84 template<class T> Quaternion<T>::Quaternion(T re) :
85 w(re), x(0), y(0), z(0)
86 {
87 }
88
89 // Create a 'pure' quaternion
Quaternion(const Vector3<T> & im)90 template<class T> Quaternion<T>::Quaternion(const Vector3<T>& im) :
91 w(0), x(im.x), y(im.y), z(im.z)
92 {
93 }
94
Quaternion(T re,const Vector3<T> & im)95 template<class T> Quaternion<T>::Quaternion(T re, const Vector3<T>& im) :
96 w(re), x(im.x), y(im.y), z(im.z)
97 {
98 }
99
Quaternion(T _w,T _x,T _y,T _z)100 template<class T> Quaternion<T>::Quaternion(T _w, T _x, T _y, T _z) :
101 w(_w), x(_x), y(_y), z(_z)
102 {
103 }
104
105 // Create a quaternion from a rotation matrix
106 // TODO: purge this from code--it is replaced by the matrixToQuaternion()
107 // function.
Quaternion(const Matrix3<T> & m)108 template<class T> Quaternion<T>::Quaternion(const Matrix3<T>& m)
109 {
110 T trace = m[0][0] + m[1][1] + m[2][2];
111 T root;
112
113 if (trace >= (T) -1.0 + 1.0e-4f)
114 {
115 root = (T) sqrt(trace + 1);
116 w = (T) 0.5 * root;
117 root = (T) 0.5 / root;
118 x = (m[2][1] - m[1][2]) * root;
119 y = (m[0][2] - m[2][0]) * root;
120 z = (m[1][0] - m[0][1]) * root;
121 }
122 else
123 {
124 // Identify the largest element of the diagonal
125 int i = 0;
126 if (m[1][1] > m[i][i])
127 i = 1;
128 if (m[2][2] > m[i][i])
129 i = 2;
130 int j = (i == 2) ? 0 : i + 1;
131 int k = (j == 2) ? 0 : j + 1;
132
133 root = (T) sqrt(m[i][i] - m[j][j] - m[k][k] + 1);
134 T* xyz[3] = { &x, &y, &z };
135 *xyz[i] = (T) 0.5 * root;
136 root = (T) 0.5 / root;
137 w = (m[k][j] - m[j][k]) * root;
138 *xyz[j] = (m[j][i] + m[i][j]) * root;
139 *xyz[k] = (m[k][i] + m[i][k]) * root;
140 }
141 }
142
143 template<class T> Quaternion<T>& Quaternion<T>::operator+=(Quaternion<T> a)
144 {
145 x += a.x; y += a.y; z += a.z; w += a.w;
146 return *this;
147 }
148
149 template<class T> Quaternion<T>& Quaternion<T>::operator-=(Quaternion<T> a)
150 {
151 x -= a.x; y -= a.y; z -= a.z; w -= a.w;
152 return *this;
153 }
154
155 template<class T> Quaternion<T>& Quaternion<T>::operator*=(Quaternion<T> q)
156 {
157 *this = Quaternion<T>(w * q.w - x * q.x - y * q.y - z * q.z,
158 w * q.x + x * q.w + y * q.z - z * q.y,
159 w * q.y + y * q.w + z * q.x - x * q.z,
160 w * q.z + z * q.w + x * q.y - y * q.x);
161
162 return *this;
163 }
164
165 template<class T> Quaternion<T>& Quaternion<T>::operator*=(T s)
166 {
167 x *= s; y *= s; z *= s; w *= s;
168 return *this;
169 }
170
171 // conjugate operator
172 template<class T> Quaternion<T> Quaternion<T>::operator~() const
173 {
174 return Quaternion<T>(w, -x, -y, -z);
175 }
176
177 template<class T> Quaternion<T> Quaternion<T>::operator-() const
178 {
179 return Quaternion<T>(-w, -x, -y, -z);
180 }
181
182 template<class T> Quaternion<T> Quaternion<T>::operator+() const
183 {
184 return *this;
185 }
186
187
188 template<class T> Quaternion<T> operator+(Quaternion<T> a, Quaternion<T> b)
189 {
190 return Quaternion<T>(a.w + b.w, a.x + b.x, a.y + b.y, a.z + b.z);
191 }
192
193 template<class T> Quaternion<T> operator-(Quaternion<T> a, Quaternion<T> b)
194 {
195 return Quaternion<T>(a.w - b.w, a.x - b.x, a.y - b.y, a.z - b.z);
196 }
197
198 template<class T> Quaternion<T> operator*(Quaternion<T> a, Quaternion<T> b)
199 {
200 return Quaternion<T>(a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
201 a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
202 a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z,
203 a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x);
204 }
205
206 template<class T> Quaternion<T> operator*(T s, Quaternion<T> q)
207 {
208 return Quaternion<T>(s * q.w, s * q.x, s * q.y, s * q.z);
209 }
210
211 template<class T> Quaternion<T> operator*(Quaternion<T> q, T s)
212 {
213 return Quaternion<T>(s * q.w, s * q.x, s * q.y, s * q.z);
214 }
215
216 // equivalent to multiplying by the quaternion (0, v)
217 template<class T> Quaternion<T> operator*(Vector3<T> v, Quaternion<T> q)
218 {
219 return Quaternion<T>(-v.x * q.x - v.y * q.y - v.z * q.z,
220 v.x * q.w + v.y * q.z - v.z * q.y,
221 v.y * q.w + v.z * q.x - v.x * q.z,
222 v.z * q.w + v.x * q.y - v.y * q.x);
223 }
224
225 template<class T> Quaternion<T> operator/(Quaternion<T> q, T s)
226 {
227 return q * (1 / s);
228 }
229
230 template<class T> Quaternion<T> operator/(Quaternion<T> a, Quaternion<T> b)
231 {
232 return a * (~b / abs(b));
233 }
234
235
236 template<class T> bool operator==(Quaternion<T> a, Quaternion<T> b)
237 {
238 return a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w;
239 }
240
241 template<class T> bool operator!=(Quaternion<T> a, Quaternion<T> b)
242 {
243 return a.x != b.x || a.y != b.y || a.z != b.z || a.w != b.w;
244 }
245
246
247 // elementary functions
conjugate(Quaternion<T> q)248 template<class T> Quaternion<T> conjugate(Quaternion<T> q)
249 {
250 return Quaternion<T>(q.w, -q.x, -q.y, -q.z);
251 }
252
norm(Quaternion<T> q)253 template<class T> T norm(Quaternion<T> q)
254 {
255 return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
256 }
257
abs(Quaternion<T> q)258 template<class T> T abs(Quaternion<T> q)
259 {
260 return (T) sqrt(norm(q));
261 }
262
exp(Quaternion<T> q)263 template<class T> Quaternion<T> exp(Quaternion<T> q)
264 {
265 if (q.isReal())
266 {
267 return Quaternion<T>((T) exp(q.w));
268 }
269 else
270 {
271 T l = (T) sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
272 T s = (T) sin(l);
273 T c = (T) cos(l);
274 T e = (T) exp(q.w);
275 T t = e * s / l;
276 return Quaternion<T>(e * c, t * q.x, t * q.y, t * q.z);
277 }
278 }
279
log(Quaternion<T> q)280 template<class T> Quaternion<T> log(Quaternion<T> q)
281 {
282 if (q.isReal())
283 {
284 if (q.w > 0)
285 {
286 return Quaternion<T>((T) log(q.w));
287 }
288 else if (q.w < 0)
289 {
290 // The log of a negative purely real quaternion has
291 // infinitely many values, all of the form (ln(-w), PI * I),
292 // where I is any unit vector. We arbitrarily choose an I
293 // of (1, 0, 0) here and whereever else a similar choice is
294 // necessary. Geometrically, the set of roots is a sphere
295 // of radius PI centered at ln(-w) on the real axis.
296 return Quaternion<T>((T) log(-q.w), (T) PI, 0, 0);
297 }
298 else
299 {
300 // error . . . ln(0) not defined
301 return Quaternion<T>(0);
302 }
303 }
304 else
305 {
306 T l = (T) sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
307 T r = (T) sqrt(l * l + q.w * q.w);
308 T theta = (T) atan2(l, q.w);
309 T t = theta / l;
310 return Quaternion<T>((T) log(r), t * q.x, t * q.y, t * q.z);
311 }
312 }
313
314
pow(Quaternion<T> q,T s)315 template<class T> Quaternion<T> pow(Quaternion<T> q, T s)
316 {
317 return exp(s * log(q));
318 }
319
320
pow(Quaternion<T> q,Quaternion<T> p)321 template<class T> Quaternion<T> pow(Quaternion<T> q, Quaternion<T> p)
322 {
323 return exp(p * log(q));
324 }
325
326
sin(Quaternion<T> q)327 template<class T> Quaternion<T> sin(Quaternion<T> q)
328 {
329 if (q.isReal())
330 {
331 return Quaternion<T>((T) sin(q.w));
332 }
333 else
334 {
335 T l = (T) sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
336 T m = q.w;
337 T s = (T) sin(m);
338 T c = (T) cos(m);
339 T il = 1 / l;
340 T e0 = (T) exp(-l);
341 T e1 = (T) exp(l);
342
343 T c0 = (T) -0.5 * e0 * il * c;
344 T c1 = (T) 0.5 * e1 * il * c;
345
346 return Quaternion<T>((T) 0.5 * e0 * s, c0 * q.x, c0 * q.y, c0 * q.z) +
347 Quaternion<T>((T) 0.5 * e1 * s, c1 * q.x, c1 * q.y, c1 * q.z);
348 }
349 }
350
cos(Quaternion<T> q)351 template<class T> Quaternion<T> cos(Quaternion<T> q)
352 {
353 if (q.isReal())
354 {
355 return Quaternion<T>((T) cos(q.w));
356 }
357 else
358 {
359 T l = (T) sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
360 T m = q.w;
361 T s = (T) sin(m);
362 T c = (T) cos(m);
363 T il = 1 / l;
364 T e0 = (T) exp(-l);
365 T e1 = (T) exp(l);
366
367 T c0 = (T) 0.5 * e0 * il * s;
368 T c1 = (T) -0.5 * e1 * il * s;
369
370 return Quaternion<T>((T) 0.5 * e0 * c, c0 * q.x, c0 * q.y, c0 * q.z) +
371 Quaternion<T>((T) 0.5 * e1 * c, c1 * q.x, c1 * q.y, c1 * q.z);
372 }
373 }
374
sqrt(Quaternion<T> q)375 template<class T> Quaternion<T> sqrt(Quaternion<T> q)
376 {
377 // In general, the square root of a quaternion has two values, one
378 // of which is the negative of the other. However, any negative purely
379 // real quaternion has an infinite number of square roots.
380 // This function returns the positive root for positive reals and
381 // the root on the positive i axis for negative reals.
382 if (q.isReal())
383 {
384 if (q.w >= 0)
385 return Quaternion<T>((T) sqrt(q.w), 0, 0, 0);
386 else
387 return Quaternion<T>(0, (T) sqrt(-q.w), 0, 0);
388 }
389 else
390 {
391 T b = (T) sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
392 T r = (T) sqrt(q.w * q.w + b * b);
393 if (q.w >= 0)
394 {
395 T m = (T) sqrt((T) 0.5 * (r + q.w));
396 T l = b / (2 * m);
397 T t = l / b;
398 return Quaternion<T>(m, q.x * t, q.y * t, q.z * t);
399 }
400 else
401 {
402 T l = (T) sqrt((T) 0.5 * (r - q.w));
403 T m = b / (2 * l);
404 T t = l / b;
405 return Quaternion<T>(m, q.x * t, q.y * t, q.z * t);
406 }
407 }
408 }
409
real(Quaternion<T> q)410 template<class T> T real(Quaternion<T> q)
411 {
412 return q.w;
413 }
414
imag(Quaternion<T> q)415 template<class T> Vector3<T> imag(Quaternion<T> q)
416 {
417 return Vector3<T>(q.x, q.y, q.z);
418 }
419
420
421 // Quaternion methods
422
isReal()423 template<class T> bool Quaternion<T>::isReal() const
424 {
425 return (x == 0 && y == 0 && z == 0);
426 }
427
isPure()428 template<class T> bool Quaternion<T>::isPure() const
429 {
430 return w == 0;
431 }
432
normalize()433 template<class T> T Quaternion<T>::normalize()
434 {
435 T s = (T) sqrt(w * w + x * x + y * y + z * z);
436 T invs = (T) 1 / (T) s;
437 x *= invs;
438 y *= invs;
439 z *= invs;
440 w *= invs;
441
442 return s;
443 }
444
445 // Set to the unit quaternion representing an axis angle rotation. Assume
446 // that axis is a unit vector
setAxisAngle(Vector3<T> axis,T angle)447 template<class T> void Quaternion<T>::setAxisAngle(Vector3<T> axis, T angle)
448 {
449 T s, c;
450
451 Math<T>::sincos(angle * (T) 0.5, s, c);
452 x = s * axis.x;
453 y = s * axis.y;
454 z = s * axis.z;
455 w = c;
456 }
457
458
459 // Assuming that this a unit quaternion, return the in axis/angle form the
460 // orientation which it represents.
getAxisAngle(Vector3<T> & axis,T & angle)461 template<class T> void Quaternion<T>::getAxisAngle(Vector3<T>& axis,
462 T& angle) const
463 {
464 // The quaternion has the form:
465 // w = cos(angle/2), (x y z) = sin(angle/2)*axis
466
467 T magSquared = x * x + y * y + z * z;
468 if (magSquared > (T) 1e-10)
469 {
470 T s = (T) 1 / (T) sqrt(magSquared);
471 axis.x = x * s;
472 axis.y = y * s;
473 axis.z = z * s;
474 if (w <= -1 || w >= 1)
475 angle = 0;
476 else
477 angle = (T) acos(w) * 2;
478 }
479 else
480 {
481 // The angle is zero, so we pick an arbitrary unit axis
482 axis.x = 1;
483 axis.y = 0;
484 axis.z = 0;
485 angle = 0;
486 }
487 }
488
489
490 // Convert this (assumed to be normalized) quaternion to a rotation matrix
toMatrix4()491 template<class T> Matrix4<T> Quaternion<T>::toMatrix4() const
492 {
493 T wx = w * x * 2;
494 T wy = w * y * 2;
495 T wz = w * z * 2;
496 T xx = x * x * 2;
497 T xy = x * y * 2;
498 T xz = x * z * 2;
499 T yy = y * y * 2;
500 T yz = y * z * 2;
501 T zz = z * z * 2;
502
503 return Matrix4<T>(Vector4<T>(1 - yy - zz, xy - wz, xz + wy, 0),
504 Vector4<T>(xy + wz, 1 - xx - zz, yz - wx, 0),
505 Vector4<T>(xz - wy, yz + wx, 1 - xx - yy, 0),
506 Vector4<T>(0, 0, 0, 1));
507 }
508
509
510 // Convert this (assumed to be normalized) quaternion to a rotation matrix
toMatrix3()511 template<class T> Matrix3<T> Quaternion<T>::toMatrix3() const
512 {
513 T wx = w * x * 2;
514 T wy = w * y * 2;
515 T wz = w * z * 2;
516 T xx = x * x * 2;
517 T xy = x * y * 2;
518 T xz = x * z * 2;
519 T yy = y * y * 2;
520 T yz = y * z * 2;
521 T zz = z * z * 2;
522
523 return Matrix3<T>(Vector3<T>(1 - yy - zz, xy - wz, xz + wy),
524 Vector3<T>(xy + wz, 1 - xx - zz, yz - wx),
525 Vector3<T>(xz - wy, yz + wx, 1 - xx - yy));
526 }
527
528
dot(Quaternion<T> a,Quaternion<T> b)529 template<class T> T dot(Quaternion<T> a, Quaternion<T> b)
530 {
531 return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
532 }
533
534
535 /*! Spherical linear interpolation of two unit quaternions. Designed for
536 * interpolating rotations, so shortest path between rotations will be
537 * taken.
538 */
slerp(const Quaternion<T> & q0,const Quaternion<T> & q1,T t)539 template<class T> Quaternion<T> Quaternion<T>::slerp(const Quaternion<T>& q0,
540 const Quaternion<T>& q1,
541 T t)
542 {
543 const double Nlerp_Threshold = 0.99999;
544
545 T cosAngle = dot(q0, q1);
546
547 // Assuming the quaternions representat rotations, ensure that we interpolate
548 // through the shortest path by inverting one of the quaternions if the
549 // angle between them is negative.
550 Quaternion qstart;
551 if (cosAngle < 0)
552 {
553 qstart = -q0;
554 cosAngle = -cosAngle;
555 }
556 else
557 {
558 qstart = q0;
559 }
560
561 // Avoid precision troubles when we're near the limit of acos range and
562 // perform a linear interpolation followed by a normalize when interpolating
563 // very small angles.
564 if (cosAngle > (T) Nlerp_Threshold)
565 {
566 Quaternion<T> q = (1 - t) * qstart + t * q1;
567 q.normalize();
568 return q;
569 }
570
571 // Below code unnecessary since we've already inverted cosAngle if it's negative.
572 // It will be necessary if we change slerp to not assume that we want the shortest
573 // path between two rotations.
574 #if 0
575 // Because of potential rounding errors, we must clamp c to the domain of acos.
576 if (cosAngle < (T) -1.0)
577 cosAngle = (T) -1.0;
578 #endif
579
580 T angle = (T) acos(cosAngle);
581 T interpolatedAngle = t * angle;
582
583 // qstart and q2 will form an orthonormal basis in the plane of interpolation.
584 Quaternion q2 = q1 - qstart * cosAngle;
585 q2.normalize();
586
587 return qstart * (T) cos(interpolatedAngle) + q2 * (T) sin(interpolatedAngle);
588 #if 0
589 T s = (T) sin(angle);
590 T is = (T) 1.0 / s;
591
592 return q0 * ((T) sin((1 - t) * angle) * is) +
593 q1 * ((T) sin(t * angle) * is);
594 #endif
595 }
596
597
598 /*! Return a unit quaternion that representing a rotation that will
599 * rotation v0 to v1 about the axis perpendicular to them both. If the
600 * vectors point in opposite directions, there is no unique axis and
601 * (arbitrarily) a rotation about the x axis will be chosen.
602 */
vecToVecRotation(const Vector3<T> & v0,const Vector3<T> & v1)603 template<class T> Quaternion<T> Quaternion<T>::vecToVecRotation(const Vector3<T>& v0,
604 const Vector3<T>& v1)
605 {
606 // We need sine and cosine of half the angle between v0 and v1, so
607 // compute the vector halfway between v0 and v1. The cross product of
608 // half and v1 gives the imaginary part of the quaternion
609 // (axis * sin(angle/2)), and the dot product of half and v1 gives
610 // the real part.
611 Vector3<T> half = (v0 + v1) * (T) 0.5;
612
613 T hl = half.length();
614 if (hl > (T) 0.0)
615 {
616 half = half / hl; // normalize h
617
618 // The magnitude of rotAxis is the sine of half the angle between
619 // v0 and v1.
620 Vector3<T> rotAxis = half ^ v1;
621 T cosAngle = half * v1;
622 return Quaternion<T>(cosAngle, rotAxis.x, rotAxis.y, rotAxis.z);
623 }
624 else
625 {
626 // The vectors point in exactly opposite directions, so there is
627 // no unique axis of rotation. Rotating v0 180 degrees about any
628 // axis will map it to v1; we'll choose the x-axis.
629 return Quaternion<T>((T) 0.0, (T) 1.0, (T) 0.0, (T) 0.0);
630 }
631 }
632
633
634 /*! Create a quaternion from a rotation matrix
635 */
matrixToQuaternion(const Matrix3<T> & m)636 template<class T> Quaternion<T> Quaternion<T>::matrixToQuaternion(const Matrix3<T>& m)
637 {
638 Quaternion<T> q;
639 T trace = m[0][0] + m[1][1] + m[2][2];
640 T root;
641 T epsilon = std::numeric_limits<T>::epsilon() * (T) 1e3;
642
643 if (trace >= epsilon - 1)
644 {
645 root = (T) sqrt(trace + 1);
646 q.w = (T) 0.5 * root;
647 root = (T) 0.5 / root;
648 q.x = (m[2][1] - m[1][2]) * root;
649 q.y = (m[0][2] - m[2][0]) * root;
650 q.z = (m[1][0] - m[0][1]) * root;
651 }
652 else
653 {
654 // Set i to the largest element of the diagonal
655 int i = 0;
656 if (m[1][1] > m[i][i])
657 i = 1;
658 if (m[2][2] > m[i][i])
659 i = 2;
660 int j = (i == 2) ? 0 : i + 1;
661 int k = (j == 2) ? 0 : j + 1;
662
663 root = (T) sqrt(m[i][i] - m[j][j] - m[k][k] + 1);
664 T* xyz[3] = { &q.x, &q.y, &q.z };
665 *xyz[i] = (T) 0.5 * root;
666 root = (T) 0.5 / root;
667 q.w = (m[k][j] - m[j][k]) * root;
668 *xyz[j] = (m[j][i] + m[i][j]) * root;
669 *xyz[k] = (m[k][i] + m[i][k]) * root;
670 }
671
672 return q;
673 }
674
675
676 /*! Assuming that this is a unit quaternion representing an orientation,
677 * apply a rotation of angle radians about the specfied axis
678 */
rotate(Vector3<T> axis,T angle)679 template<class T> void Quaternion<T>::rotate(Vector3<T> axis, T angle)
680 {
681 Quaternion q;
682 q.setAxisAngle(axis, angle);
683 *this = q * *this;
684 }
685
686
687 // Assuming that this is a unit quaternion representing an orientation,
688 // apply a rotation of angle radians about the x-axis
xrotate(T angle)689 template<class T> void Quaternion<T>::xrotate(T angle)
690 {
691 T s, c;
692
693 Math<T>::sincos(angle * (T) 0.5, s, c);
694 *this = Quaternion<T>(c, s, 0, 0) * *this;
695 }
696
697 // Assuming that this is a unit quaternion representing an orientation,
698 // apply a rotation of angle radians about the y-axis
yrotate(T angle)699 template<class T> void Quaternion<T>::yrotate(T angle)
700 {
701 T s, c;
702
703 Math<T>::sincos(angle * (T) 0.5, s, c);
704 *this = Quaternion<T>(c, 0, s, 0) * *this;
705 }
706
707 // Assuming that this is a unit quaternion representing an orientation,
708 // apply a rotation of angle radians about the z-axis
zrotate(T angle)709 template<class T> void Quaternion<T>::zrotate(T angle)
710 {
711 T s, c;
712
713 Math<T>::sincos(angle * (T) 0.5, s, c);
714 *this = Quaternion<T>(c, 0, 0, s) * *this;
715 }
716
717
xrotation(T angle)718 template<class T> Quaternion<T> Quaternion<T>::xrotation(T angle)
719 {
720 T s, c;
721 Math<T>::sincos(angle * (T) 0.5, s, c);
722 return Quaternion<T>(c, s, 0, 0);
723 }
724
yrotation(T angle)725 template<class T> Quaternion<T> Quaternion<T>::yrotation(T angle)
726 {
727 T s, c;
728 Math<T>::sincos(angle * (T) 0.5, s, c);
729 return Quaternion<T>(c, 0, s, 0);
730 }
731
zrotation(T angle)732 template<class T> Quaternion<T> Quaternion<T>::zrotation(T angle)
733 {
734 T s, c;
735 Math<T>::sincos(angle * (T) 0.5, s, c);
736 return Quaternion<T>(c, 0, 0, s);
737 }
738
739 /*! Determine an orientation that will make the negative z-axis point from
740 * from the observer to the target, with the y-axis pointing in direction
741 * of the component of 'up' that is orthogonal to the z-axis.
742 */
743 template<class T> Quaternion<T>
lookAt(const Point3<T> & from,const Point3<T> & to,const Vector3<T> & up)744 Quaternion<T>::lookAt(const Point3<T>& from, const Point3<T>& to, const Vector3<T>& up)
745 {
746 Vector3<T> n = to - from;
747 n.normalize();
748 Vector3<T> v = n ^ up;
749 v.normalize();
750 Vector3<T> u = v ^ n;
751
752 return Quaternion<T>::matrixToQuaternion(Matrix3<T>(v, u, -n));
753 }
754
755 #endif // _QUATERNION_H_
756