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