1 ///////////////////////////////////////////////////////////////////////////
2 //
3 // Copyright (c) 2002-2012, Industrial Light & Magic, a division of Lucas
4 // Digital Ltd. LLC
5 //
6 // All rights reserved.
7 //
8 // Redistribution and use in source and binary forms, with or without
9 // modification, are permitted provided that the following conditions are
10 // met:
11 // * Redistributions of source code must retain the above copyright
12 // notice, this list of conditions and the following disclaimer.
13 // * Redistributions in binary form must reproduce the above
14 // copyright notice, this list of conditions and the following disclaimer
15 // in the documentation and/or other materials provided with the
16 // distribution.
17 // * Neither the name of Industrial Light & Magic nor the names of
18 // its contributors may be used to endorse or promote products derived
19 // from this software without specific prior written permission.
20 //
21 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
24 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
25 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
26 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
27 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
28 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
29 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
30 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
31 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32 //
33 ///////////////////////////////////////////////////////////////////////////
34
35
36
37 #ifndef INCLUDED_IMATHQUAT_H
38 #define INCLUDED_IMATHQUAT_H
39
40 //----------------------------------------------------------------------
41 //
42 // template class Quat<T>
43 //
44 // "Quaternions came from Hamilton ... and have been an unmixed
45 // evil to those who have touched them in any way. Vector is a
46 // useless survival ... and has never been of the slightest use
47 // to any creature."
48 //
49 // - Lord Kelvin
50 //
51 // This class implements the quaternion numerical type -- you
52 // will probably want to use this class to represent orientations
53 // in R3 and to convert between various euler angle reps. You
54 // should probably use Imath::Euler<> for that.
55 //
56 //----------------------------------------------------------------------
57
58 #include "ImathExc.h"
59 #include "ImathMatrix.h"
60 #include "ImathNamespace.h"
61
62 #include <iostream>
63
64 IMATH_INTERNAL_NAMESPACE_HEADER_ENTER
65
66 #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
67 // Disable MS VC++ warnings about conversion from double to float
68 #pragma warning(disable:4244)
69 #endif
70
71 template <class T>
72 class Quat
73 {
74 public:
75
76 T r; // real part
77 Vec3<T> v; // imaginary vector
78
79
80 //-----------------------------------------------------
81 // Constructors - default constructor is identity quat
82 //-----------------------------------------------------
83
84 Quat ();
85
86 template <class S>
87 Quat (const Quat<S> &q);
88
89 Quat (T s, T i, T j, T k);
90
91 Quat (T s, Vec3<T> d);
92
93 static Quat<T> identity ();
94
95
96 //-------------------------------------------------
97 // Basic Algebra - Operators and Methods
98 // The operator return values are *NOT* normalized
99 //
100 // operator^ and euclideanInnnerProduct() both
101 // implement the 4D dot product
102 //
103 // operator/ uses the inverse() quaternion
104 //
105 // operator~ is conjugate -- if (S+V) is quat then
106 // the conjugate (S+V)* == (S-V)
107 //
108 // some operators (*,/,*=,/=) treat the quat as
109 // a 4D vector when one of the operands is scalar
110 //-------------------------------------------------
111
112 const Quat<T> & operator = (const Quat<T> &q);
113 const Quat<T> & operator *= (const Quat<T> &q);
114 const Quat<T> & operator *= (T t);
115 const Quat<T> & operator /= (const Quat<T> &q);
116 const Quat<T> & operator /= (T t);
117 const Quat<T> & operator += (const Quat<T> &q);
118 const Quat<T> & operator -= (const Quat<T> &q);
119 T & operator [] (int index); // as 4D vector
120 T operator [] (int index) const;
121
122 template <class S> bool operator == (const Quat<S> &q) const;
123 template <class S> bool operator != (const Quat<S> &q) const;
124
125 Quat<T> & invert (); // this -> 1 / this
126 Quat<T> inverse () const;
127 Quat<T> & normalize (); // returns this
128 Quat<T> normalized () const;
129 T length () const; // in R4
130 Vec3<T> rotateVector(const Vec3<T> &original) const;
131 T euclideanInnerProduct(const Quat<T> &q) const;
132
133 //-----------------------
134 // Rotation conversion
135 //-----------------------
136
137 Quat<T> & setAxisAngle (const Vec3<T> &axis, T radians);
138
139 Quat<T> & setRotation (const Vec3<T> &fromDirection,
140 const Vec3<T> &toDirection);
141
142 T angle () const;
143 Vec3<T> axis () const;
144
145 Matrix33<T> toMatrix33 () const;
146 Matrix44<T> toMatrix44 () const;
147
148 Quat<T> log () const;
149 Quat<T> exp () const;
150
151
152 private:
153
154 void setRotationInternal (const Vec3<T> &f0,
155 const Vec3<T> &t0,
156 Quat<T> &q);
157 };
158
159
160 template<class T>
161 Quat<T> slerp (const Quat<T> &q1, const Quat<T> &q2, T t);
162
163 template<class T>
164 Quat<T> slerpShortestArc
165 (const Quat<T> &q1, const Quat<T> &q2, T t);
166
167
168 template<class T>
169 Quat<T> squad (const Quat<T> &q1, const Quat<T> &q2,
170 const Quat<T> &qa, const Quat<T> &qb, T t);
171
172 template<class T>
173 void intermediate (const Quat<T> &q0, const Quat<T> &q1,
174 const Quat<T> &q2, const Quat<T> &q3,
175 Quat<T> &qa, Quat<T> &qb);
176
177 template<class T>
178 Matrix33<T> operator * (const Matrix33<T> &M, const Quat<T> &q);
179
180 template<class T>
181 Matrix33<T> operator * (const Quat<T> &q, const Matrix33<T> &M);
182
183 template<class T>
184 std::ostream & operator << (std::ostream &o, const Quat<T> &q);
185
186 template<class T>
187 Quat<T> operator * (const Quat<T> &q1, const Quat<T> &q2);
188
189 template<class T>
190 Quat<T> operator / (const Quat<T> &q1, const Quat<T> &q2);
191
192 template<class T>
193 Quat<T> operator / (const Quat<T> &q, T t);
194
195 template<class T>
196 Quat<T> operator * (const Quat<T> &q, T t);
197
198 template<class T>
199 Quat<T> operator * (T t, const Quat<T> &q);
200
201 template<class T>
202 Quat<T> operator + (const Quat<T> &q1, const Quat<T> &q2);
203
204 template<class T>
205 Quat<T> operator - (const Quat<T> &q1, const Quat<T> &q2);
206
207 template<class T>
208 Quat<T> operator ~ (const Quat<T> &q);
209
210 template<class T>
211 Quat<T> operator - (const Quat<T> &q);
212
213 template<class T>
214 Vec3<T> operator * (const Vec3<T> &v, const Quat<T> &q);
215
216
217 //--------------------
218 // Convenient typedefs
219 //--------------------
220
221 typedef Quat<float> Quatf;
222 typedef Quat<double> Quatd;
223
224
225 //---------------
226 // Implementation
227 //---------------
228
229 template<class T>
230 inline
Quat()231 Quat<T>::Quat (): r (1), v (0, 0, 0)
232 {
233 // empty
234 }
235
236
237 template<class T>
238 template <class S>
239 inline
Quat(const Quat<S> & q)240 Quat<T>::Quat (const Quat<S> &q): r (q.r), v (q.v)
241 {
242 // empty
243 }
244
245
246 template<class T>
247 inline
Quat(T s,T i,T j,T k)248 Quat<T>::Quat (T s, T i, T j, T k): r (s), v (i, j, k)
249 {
250 // empty
251 }
252
253
254 template<class T>
255 inline
Quat(T s,Vec3<T> d)256 Quat<T>::Quat (T s, Vec3<T> d): r (s), v (d)
257 {
258 // empty
259 }
260
261
262 template<class T>
263 inline Quat<T>
identity()264 Quat<T>::identity ()
265 {
266 return Quat<T>();
267 }
268
269 template<class T>
270 inline const Quat<T> &
271 Quat<T>::operator = (const Quat<T> &q)
272 {
273 r = q.r;
274 v = q.v;
275 return *this;
276 }
277
278
279 template<class T>
280 inline const Quat<T> &
281 Quat<T>::operator *= (const Quat<T> &q)
282 {
283 T rtmp = r * q.r - (v ^ q.v);
284 v = r * q.v + v * q.r + v % q.v;
285 r = rtmp;
286 return *this;
287 }
288
289
290 template<class T>
291 inline const Quat<T> &
292 Quat<T>::operator *= (T t)
293 {
294 r *= t;
295 v *= t;
296 return *this;
297 }
298
299
300 template<class T>
301 inline const Quat<T> &
302 Quat<T>::operator /= (const Quat<T> &q)
303 {
304 *this = *this * q.inverse();
305 return *this;
306 }
307
308
309 template<class T>
310 inline const Quat<T> &
311 Quat<T>::operator /= (T t)
312 {
313 r /= t;
314 v /= t;
315 return *this;
316 }
317
318
319 template<class T>
320 inline const Quat<T> &
321 Quat<T>::operator += (const Quat<T> &q)
322 {
323 r += q.r;
324 v += q.v;
325 return *this;
326 }
327
328
329 template<class T>
330 inline const Quat<T> &
331 Quat<T>::operator -= (const Quat<T> &q)
332 {
333 r -= q.r;
334 v -= q.v;
335 return *this;
336 }
337
338
339 template<class T>
340 inline T &
341 Quat<T>::operator [] (int index)
342 {
343 return index ? v[index - 1] : r;
344 }
345
346
347 template<class T>
348 inline T
349 Quat<T>::operator [] (int index) const
350 {
351 return index ? v[index - 1] : r;
352 }
353
354
355 template <class T>
356 template <class S>
357 inline bool
358 Quat<T>::operator == (const Quat<S> &q) const
359 {
360 return r == q.r && v == q.v;
361 }
362
363
364 template <class T>
365 template <class S>
366 inline bool
367 Quat<T>::operator != (const Quat<S> &q) const
368 {
369 return r != q.r || v != q.v;
370 }
371
372
373 template<class T>
374 inline T
375 operator ^ (const Quat<T>& q1 ,const Quat<T>& q2)
376 {
377 return q1.r * q2.r + (q1.v ^ q2.v);
378 }
379
380
381 template <class T>
382 inline T
length()383 Quat<T>::length () const
384 {
385 return Math<T>::sqrt (r * r + (v ^ v));
386 }
387
388
389 template <class T>
390 inline Quat<T> &
normalize()391 Quat<T>::normalize ()
392 {
393 if (T l = length())
394 {
395 r /= l;
396 v /= l;
397 }
398 else
399 {
400 r = 1;
401 v = Vec3<T> (0);
402 }
403
404 return *this;
405 }
406
407
408 template <class T>
409 inline Quat<T>
normalized()410 Quat<T>::normalized () const
411 {
412 if (T l = length())
413 return Quat (r / l, v / l);
414
415 return Quat();
416 }
417
418
419 template<class T>
420 inline Quat<T>
inverse()421 Quat<T>::inverse () const
422 {
423 //
424 // 1 Q*
425 // - = ---- where Q* is conjugate (operator~)
426 // Q Q* Q and (Q* Q) == Q ^ Q (4D dot)
427 //
428
429 T qdot = *this ^ *this;
430 return Quat (r / qdot, -v / qdot);
431 }
432
433
434 template<class T>
435 inline Quat<T> &
invert()436 Quat<T>::invert ()
437 {
438 T qdot = (*this) ^ (*this);
439 r /= qdot;
440 v = -v / qdot;
441 return *this;
442 }
443
444
445 template<class T>
446 inline Vec3<T>
rotateVector(const Vec3<T> & original)447 Quat<T>::rotateVector(const Vec3<T>& original) const
448 {
449 //
450 // Given a vector p and a quaternion q (aka this),
451 // calculate p' = qpq*
452 //
453 // Assumes unit quaternions (because non-unit
454 // quaternions cannot be used to rotate vectors
455 // anyway).
456 //
457
458 Quat<T> vec (0, original); // temporarily promote grade of original
459 Quat<T> inv (*this);
460 inv.v *= -1; // unit multiplicative inverse
461 Quat<T> result = *this * vec * inv;
462 return result.v;
463 }
464
465
466 template<class T>
467 inline T
euclideanInnerProduct(const Quat<T> & q)468 Quat<T>::euclideanInnerProduct (const Quat<T> &q) const
469 {
470 return r * q.r + v.x * q.v.x + v.y * q.v.y + v.z * q.v.z;
471 }
472
473
474 template<class T>
475 T
angle4D(const Quat<T> & q1,const Quat<T> & q2)476 angle4D (const Quat<T> &q1, const Quat<T> &q2)
477 {
478 //
479 // Compute the angle between two quaternions,
480 // interpreting the quaternions as 4D vectors.
481 //
482
483 Quat<T> d = q1 - q2;
484 T lengthD = Math<T>::sqrt (d ^ d);
485
486 Quat<T> s = q1 + q2;
487 T lengthS = Math<T>::sqrt (s ^ s);
488
489 return 2 * Math<T>::atan2 (lengthD, lengthS);
490 }
491
492
493 template<class T>
494 Quat<T>
slerp(const Quat<T> & q1,const Quat<T> & q2,T t)495 slerp (const Quat<T> &q1, const Quat<T> &q2, T t)
496 {
497 //
498 // Spherical linear interpolation.
499 // Assumes q1 and q2 are normalized and that q1 != -q2.
500 //
501 // This method does *not* interpolate along the shortest
502 // arc between q1 and q2. If you desire interpolation
503 // along the shortest arc, and q1^q2 is negative, then
504 // consider calling slerpShortestArc(), below, or flipping
505 // the second quaternion explicitly.
506 //
507 // The implementation of squad() depends on a slerp()
508 // that interpolates as is, without the automatic
509 // flipping.
510 //
511 // Don Hatch explains the method we use here on his
512 // web page, The Right Way to Calculate Stuff, at
513 // http://www.plunk.org/~hatch/rightway.php
514 //
515
516 T a = angle4D (q1, q2);
517 T s = 1 - t;
518
519 Quat<T> q = sinx_over_x (s * a) / sinx_over_x (a) * s * q1 +
520 sinx_over_x (t * a) / sinx_over_x (a) * t * q2;
521
522 return q.normalized();
523 }
524
525
526 template<class T>
527 Quat<T>
slerpShortestArc(const Quat<T> & q1,const Quat<T> & q2,T t)528 slerpShortestArc (const Quat<T> &q1, const Quat<T> &q2, T t)
529 {
530 //
531 // Spherical linear interpolation along the shortest
532 // arc from q1 to either q2 or -q2, whichever is closer.
533 // Assumes q1 and q2 are unit quaternions.
534 //
535
536 if ((q1 ^ q2) >= 0)
537 return slerp (q1, q2, t);
538 else
539 return slerp (q1, -q2, t);
540 }
541
542
543 template<class T>
544 Quat<T>
spline(const Quat<T> & q0,const Quat<T> & q1,const Quat<T> & q2,const Quat<T> & q3,T t)545 spline (const Quat<T> &q0, const Quat<T> &q1,
546 const Quat<T> &q2, const Quat<T> &q3,
547 T t)
548 {
549 //
550 // Spherical Cubic Spline Interpolation -
551 // from Advanced Animation and Rendering
552 // Techniques by Watt and Watt, Page 366:
553 // A spherical curve is constructed using three
554 // spherical linear interpolations of a quadrangle
555 // of unit quaternions: q1, qa, qb, q2.
556 // Given a set of quaternion keys: q0, q1, q2, q3,
557 // this routine does the interpolation between
558 // q1 and q2 by constructing two intermediate
559 // quaternions: qa and qb. The qa and qb are
560 // computed by the intermediate function to
561 // guarantee the continuity of tangents across
562 // adjacent cubic segments. The qa represents in-tangent
563 // for q1 and the qb represents the out-tangent for q2.
564 //
565 // The q1 q2 is the cubic segment being interpolated.
566 // The q0 is from the previous adjacent segment and q3 is
567 // from the next adjacent segment. The q0 and q3 are used
568 // in computing qa and qb.
569 //
570
571 Quat<T> qa = intermediate (q0, q1, q2);
572 Quat<T> qb = intermediate (q1, q2, q3);
573 Quat<T> result = squad (q1, qa, qb, q2, t);
574
575 return result;
576 }
577
578
579 template<class T>
580 Quat<T>
squad(const Quat<T> & q1,const Quat<T> & qa,const Quat<T> & qb,const Quat<T> & q2,T t)581 squad (const Quat<T> &q1, const Quat<T> &qa,
582 const Quat<T> &qb, const Quat<T> &q2,
583 T t)
584 {
585 //
586 // Spherical Quadrangle Interpolation -
587 // from Advanced Animation and Rendering
588 // Techniques by Watt and Watt, Page 366:
589 // It constructs a spherical cubic interpolation as
590 // a series of three spherical linear interpolations
591 // of a quadrangle of unit quaternions.
592 //
593
594 Quat<T> r1 = slerp (q1, q2, t);
595 Quat<T> r2 = slerp (qa, qb, t);
596 Quat<T> result = slerp (r1, r2, 2 * t * (1 - t));
597
598 return result;
599 }
600
601
602 template<class T>
603 Quat<T>
intermediate(const Quat<T> & q0,const Quat<T> & q1,const Quat<T> & q2)604 intermediate (const Quat<T> &q0, const Quat<T> &q1, const Quat<T> &q2)
605 {
606 //
607 // From advanced Animation and Rendering
608 // Techniques by Watt and Watt, Page 366:
609 // computing the inner quadrangle
610 // points (qa and qb) to guarantee tangent
611 // continuity.
612 //
613
614 Quat<T> q1inv = q1.inverse();
615 Quat<T> c1 = q1inv * q2;
616 Quat<T> c2 = q1inv * q0;
617 Quat<T> c3 = (T) (-0.25) * (c2.log() + c1.log());
618 Quat<T> qa = q1 * c3.exp();
619 qa.normalize();
620 return qa;
621 }
622
623
624 template <class T>
625 inline Quat<T>
log()626 Quat<T>::log () const
627 {
628 //
629 // For unit quaternion, from Advanced Animation and
630 // Rendering Techniques by Watt and Watt, Page 366:
631 //
632
633 T theta = Math<T>::acos (std::min (r, (T) 1.0));
634
635 if (theta == 0)
636 return Quat<T> (0, v);
637
638 T sintheta = Math<T>::sin (theta);
639
640 T k;
641 if (abs (sintheta) < 1 && abs (theta) >= limits<T>::max() * abs (sintheta))
642 k = 1;
643 else
644 k = theta / sintheta;
645
646 return Quat<T> ((T) 0, v.x * k, v.y * k, v.z * k);
647 }
648
649
650 template <class T>
651 inline Quat<T>
exp()652 Quat<T>::exp () const
653 {
654 //
655 // For pure quaternion (zero scalar part):
656 // from Advanced Animation and Rendering
657 // Techniques by Watt and Watt, Page 366:
658 //
659
660 T theta = v.length();
661 T sintheta = Math<T>::sin (theta);
662
663 T k;
664 if (abs (theta) < 1 && abs (sintheta) >= limits<T>::max() * abs (theta))
665 k = 1;
666 else
667 k = sintheta / theta;
668
669 T costheta = Math<T>::cos (theta);
670
671 return Quat<T> (costheta, v.x * k, v.y * k, v.z * k);
672 }
673
674
675 template <class T>
676 inline T
angle()677 Quat<T>::angle () const
678 {
679 return 2 * Math<T>::atan2 (v.length(), r);
680 }
681
682
683 template <class T>
684 inline Vec3<T>
axis()685 Quat<T>::axis () const
686 {
687 return v.normalized();
688 }
689
690
691 template <class T>
692 inline Quat<T> &
setAxisAngle(const Vec3<T> & axis,T radians)693 Quat<T>::setAxisAngle (const Vec3<T> &axis, T radians)
694 {
695 r = Math<T>::cos (radians / 2);
696 v = axis.normalized() * Math<T>::sin (radians / 2);
697 return *this;
698 }
699
700
701 template <class T>
702 Quat<T> &
setRotation(const Vec3<T> & from,const Vec3<T> & to)703 Quat<T>::setRotation (const Vec3<T> &from, const Vec3<T> &to)
704 {
705 //
706 // Create a quaternion that rotates vector from into vector to,
707 // such that the rotation is around an axis that is the cross
708 // product of from and to.
709 //
710 // This function calls function setRotationInternal(), which is
711 // numerically accurate only for rotation angles that are not much
712 // greater than pi/2. In order to achieve good accuracy for angles
713 // greater than pi/2, we split large angles in half, and rotate in
714 // two steps.
715 //
716
717 //
718 // Normalize from and to, yielding f0 and t0.
719 //
720
721 Vec3<T> f0 = from.normalized();
722 Vec3<T> t0 = to.normalized();
723
724 if ((f0 ^ t0) >= 0)
725 {
726 //
727 // The rotation angle is less than or equal to pi/2.
728 //
729
730 setRotationInternal (f0, t0, *this);
731 }
732 else
733 {
734 //
735 // The angle is greater than pi/2. After computing h0,
736 // which is halfway between f0 and t0, we rotate first
737 // from f0 to h0, then from h0 to t0.
738 //
739
740 Vec3<T> h0 = (f0 + t0).normalized();
741
742 if ((h0 ^ h0) != 0)
743 {
744 setRotationInternal (f0, h0, *this);
745
746 Quat<T> q;
747 setRotationInternal (h0, t0, q);
748
749 *this *= q;
750 }
751 else
752 {
753 //
754 // f0 and t0 point in exactly opposite directions.
755 // Pick an arbitrary axis that is orthogonal to f0,
756 // and rotate by pi.
757 //
758
759 r = T (0);
760
761 Vec3<T> f02 = f0 * f0;
762
763 if (f02.x <= f02.y && f02.x <= f02.z)
764 v = (f0 % Vec3<T> (1, 0, 0)).normalized();
765 else if (f02.y <= f02.z)
766 v = (f0 % Vec3<T> (0, 1, 0)).normalized();
767 else
768 v = (f0 % Vec3<T> (0, 0, 1)).normalized();
769 }
770 }
771
772 return *this;
773 }
774
775
776 template <class T>
777 void
setRotationInternal(const Vec3<T> & f0,const Vec3<T> & t0,Quat<T> & q)778 Quat<T>::setRotationInternal (const Vec3<T> &f0, const Vec3<T> &t0, Quat<T> &q)
779 {
780 //
781 // The following is equivalent to setAxisAngle(n,2*phi),
782 // where the rotation axis, n, is orthogonal to the f0 and
783 // t0 vectors, and 2*phi is the angle between f0 and t0.
784 //
785 // This function is called by setRotation(), above; it assumes
786 // that f0 and t0 are normalized and that the angle between
787 // them is not much greater than pi/2. This function becomes
788 // numerically inaccurate if f0 and t0 point into nearly
789 // opposite directions.
790 //
791
792 //
793 // Find a normalized vector, h0, that is halfway between f0 and t0.
794 // The angle between f0 and h0 is phi.
795 //
796
797 Vec3<T> h0 = (f0 + t0).normalized();
798
799 //
800 // Store the rotation axis and rotation angle.
801 //
802
803 q.r = f0 ^ h0; // f0 ^ h0 == cos (phi)
804 q.v = f0 % h0; // (f0 % h0).length() == sin (phi)
805 }
806
807
808 template<class T>
809 Matrix33<T>
toMatrix33()810 Quat<T>::toMatrix33() const
811 {
812 return Matrix33<T> (1 - 2 * (v.y * v.y + v.z * v.z),
813 2 * (v.x * v.y + v.z * r),
814 2 * (v.z * v.x - v.y * r),
815
816 2 * (v.x * v.y - v.z * r),
817 1 - 2 * (v.z * v.z + v.x * v.x),
818 2 * (v.y * v.z + v.x * r),
819
820 2 * (v.z * v.x + v.y * r),
821 2 * (v.y * v.z - v.x * r),
822 1 - 2 * (v.y * v.y + v.x * v.x));
823 }
824
825 template<class T>
826 Matrix44<T>
toMatrix44()827 Quat<T>::toMatrix44() const
828 {
829 return Matrix44<T> (1 - 2 * (v.y * v.y + v.z * v.z),
830 2 * (v.x * v.y + v.z * r),
831 2 * (v.z * v.x - v.y * r),
832 0,
833 2 * (v.x * v.y - v.z * r),
834 1 - 2 * (v.z * v.z + v.x * v.x),
835 2 * (v.y * v.z + v.x * r),
836 0,
837 2 * (v.z * v.x + v.y * r),
838 2 * (v.y * v.z - v.x * r),
839 1 - 2 * (v.y * v.y + v.x * v.x),
840 0,
841 0,
842 0,
843 0,
844 1);
845 }
846
847
848 template<class T>
849 inline Matrix33<T>
850 operator * (const Matrix33<T> &M, const Quat<T> &q)
851 {
852 return M * q.toMatrix33();
853 }
854
855
856 template<class T>
857 inline Matrix33<T>
858 operator * (const Quat<T> &q, const Matrix33<T> &M)
859 {
860 return q.toMatrix33() * M;
861 }
862
863
864 template<class T>
865 std::ostream &
866 operator << (std::ostream &o, const Quat<T> &q)
867 {
868 return o << "(" << q.r
869 << " " << q.v.x
870 << " " << q.v.y
871 << " " << q.v.z
872 << ")";
873 }
874
875
876 template<class T>
877 inline Quat<T>
878 operator * (const Quat<T> &q1, const Quat<T> &q2)
879 {
880 return Quat<T> (q1.r * q2.r - (q1.v ^ q2.v),
881 q1.r * q2.v + q1.v * q2.r + q1.v % q2.v);
882 }
883
884
885 template<class T>
886 inline Quat<T>
887 operator / (const Quat<T> &q1, const Quat<T> &q2)
888 {
889 return q1 * q2.inverse();
890 }
891
892
893 template<class T>
894 inline Quat<T>
895 operator / (const Quat<T> &q, T t)
896 {
897 return Quat<T> (q.r / t, q.v / t);
898 }
899
900
901 template<class T>
902 inline Quat<T>
903 operator * (const Quat<T> &q, T t)
904 {
905 return Quat<T> (q.r * t, q.v * t);
906 }
907
908
909 template<class T>
910 inline Quat<T>
911 operator * (T t, const Quat<T> &q)
912 {
913 return Quat<T> (q.r * t, q.v * t);
914 }
915
916
917 template<class T>
918 inline Quat<T>
919 operator + (const Quat<T> &q1, const Quat<T> &q2)
920 {
921 return Quat<T> (q1.r + q2.r, q1.v + q2.v);
922 }
923
924
925 template<class T>
926 inline Quat<T>
927 operator - (const Quat<T> &q1, const Quat<T> &q2)
928 {
929 return Quat<T> (q1.r - q2.r, q1.v - q2.v);
930 }
931
932
933 template<class T>
934 inline Quat<T>
935 operator ~ (const Quat<T> &q)
936 {
937 return Quat<T> (q.r, -q.v);
938 }
939
940
941 template<class T>
942 inline Quat<T>
943 operator - (const Quat<T> &q)
944 {
945 return Quat<T> (-q.r, -q.v);
946 }
947
948
949 template<class T>
950 inline Vec3<T>
951 operator * (const Vec3<T> &v, const Quat<T> &q)
952 {
953 Vec3<T> a = q.v % v;
954 Vec3<T> b = q.v % a;
955 return v + T (2) * (q.r * a + b);
956 }
957
958 #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
959 #pragma warning(default:4244)
960 #endif
961
962 IMATH_INTERNAL_NAMESPACE_HEADER_EXIT
963
964 #endif // INCLUDED_IMATHQUAT_H
965