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 |
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 |
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 |
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 |
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> |
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 |
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> & |
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> |
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> |
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> & |
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> |
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 |
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 |
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> |
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> |
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> |
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> |
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> |
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> |
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> |
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 |
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> |
685 | Quat<T>::axis () const |
686 | { |
687 | return v.normalized(); |
688 | } |
689 | |
690 | |
691 | template <class T> |
692 | inline Quat<T> & |
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> & |
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 |
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> |
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> |
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 | |