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