source: NonGTP/OpenEXR/include/Imath/ImathQuat.h @ 855

Revision 855, 16.6 KB checked in by igarcia, 19 years ago (diff)
Line 
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
63namespace Imath {
64
65#if defined PLATFORM_WINDOWS && _MSC_VER
66// Disable MS VC++ warnings about conversion from double to float
67#pragma warning(disable:4244)
68#endif
69
70template <class T>
71class Quat;
72
73template<class T>
74Quat<T> slerp (const Quat<T> &q1,const Quat<T> &q2, T t);
75
76template<class T>
77Quat<T> squad (const Quat<T> &q1,const Quat<T> &q2,
78               const Quat<T> &qa,const Quat<T> &qb, T t);
79
80template<class T>
81void intermediate (const Quat<T> &q0, const Quat<T> &q1,
82                   const Quat<T> &q2, const Quat<T> &q3,
83                   Quat<T> &qa, Quat<T> &qb);
84
85template <class T>
86class Quat
87{
88  public:
89
90    T                       r;      // real part
91    Vec3<T>                 v;      // imaginary vector
92
93    //-----------------------------------------------------
94    //  Constructors - default constructor is identity quat
95    //-----------------------------------------------------
96
97    Quat()                          : r(1), v(0,0,0) {}
98
99    template <class S>
100    Quat( const Quat<S>& q)         : r(q.r), v(q.v) {}
101
102    Quat( T s, T i, T j, T k )      : r(s), v(i,j,k) {}
103
104    Quat( T s, Vec3<T> d )          : r(s), v(d) {}
105
106    static Quat<T> identity()   { return Quat<T>(); }
107
108    //------------------------------------------------
109    //  Basic Algebra - Operators and Methods
110    //  The operator return values are *NOT* normalized
111    //
112    //  operator^ is 4D dot product
113    //  operator/ uses the inverse() quaternion
114    //  operator~ is conjugate -- if (S+V) is quat then
115    //            the conjugate (S+V)* == (S-V)
116    //
117    //  some operators (*,/,*=,/=) treat the quat as
118    //  a 4D vector when one of the operands is scalar
119    //------------------------------------------------
120
121    const Quat<T>&          operator=   (const Quat<T>&);
122    const Quat<T>&          operator*=  (const Quat<T>&);
123    const Quat<T>&          operator*=  (T);
124    const Quat<T>&          operator/=  (const Quat<T>&);
125    const Quat<T>&          operator/=  (T);
126    const Quat<T>&          operator+=  (const Quat<T>&);
127    const Quat<T>&          operator-=  (const Quat<T>&);
128    T&                      operator[]  (int index);    // as 4D vector
129    T                       operator[]  (int index) const;
130
131    template <class S> bool operator == (const Quat<S> &q) const;
132    template <class S> bool operator != (const Quat<S> &q) const;
133
134    Quat<T>&                invert();               // this -> 1 / this
135    Quat<T>                 inverse() const;
136    Quat<T>&                normalize();            // returns this
137    Quat<T>                 normalized() const;
138    T                       length() const;         // in R4
139
140    //-----------------------
141    //  Rotation conversion
142    //-----------------------
143
144    Quat<T>&                setAxisAngle(const Vec3<T>& axis, T radians);
145    Quat<T>&                setRotation(const Vec3<T>& fromDirection,
146                                        const Vec3<T>& toDirection);
147
148    T                       angle() const;
149    Vec3<T>                 axis() const;
150
151    Matrix33<T>             toMatrix33() const;
152    Matrix44<T>             toMatrix44() const;
153
154    Quat<T>                 log() const;
155    Quat<T>                 exp() const;
156};
157
158
159//--------------------
160// Convenient typedefs
161//--------------------
162
163typedef Quat<float>     Quatf;
164typedef Quat<double>    Quatd;
165
166
167//---------------
168// Implementation
169//---------------
170
171template<class T>
172inline const Quat<T>& Quat<T>::operator= (const Quat<T>& q)
173{
174    r = q.r;
175    v = q.v;
176    return *this;
177}
178
179template<class T>
180inline const Quat<T>& Quat<T>::operator*= (const Quat<T>& q)
181{
182    T rtmp = r * q.r - (v ^ q.v);
183    v = r * q.v + v * q.r + v % q.v;
184    r = rtmp;
185    return *this;
186}
187
188template<class T>
189inline const Quat<T>& Quat<T>::operator*= (T t)
190{
191    r *= t;
192    v *= t;
193    return *this;
194}
195
196template<class T>
197inline const Quat<T>& Quat<T>::operator/= (const Quat<T>& q)
198{
199    *this = *this * q.inverse();
200    return *this;
201}
202
203template<class T>
204inline const Quat<T>& Quat<T>::operator/= (T t)
205{
206    r /= t;
207    v /= t;
208    return *this;
209}
210
211template<class T>
212inline const Quat<T>& Quat<T>::operator+= (const Quat<T>& q)
213{
214    r += q.r;
215    v += q.v;
216    return *this;
217}
218
219template<class T>
220inline const Quat<T>& Quat<T>::operator-= (const Quat<T>& q)
221{
222    r -= q.r;
223    v -= q.v;
224    return *this;
225}
226template<class T>
227inline T& Quat<T>::operator[] (int index)
228{
229    return index ? v[index-1] : r;
230}
231
232template<class T>
233inline T Quat<T>::operator[] (int index) const
234{
235    return index ? v[index-1] : r;
236}
237
238template <class T>
239template <class S>
240inline bool
241Quat<T>::operator == (const Quat<S> &q) const
242{
243    return r == q.r && v == q.v;
244}
245
246template <class T>
247template <class S>
248inline bool
249Quat<T>::operator != (const Quat<S> &q) const
250{
251    return r != q.r || v != q.v;
252}
253
254template<class T>
255inline T operator^ (const Quat<T>& q1,const Quat<T>& q2)
256{
257    return q1.r * q2.r + (q1.v ^ q2.v);
258}
259
260template <class T>
261inline T Quat<T>::length() const
262{
263    return Math<T>::sqrt( r * r + (v ^ v) );
264}
265
266template <class T>
267inline Quat<T>& Quat<T>::normalize()
268{
269    if ( T l = length() ) { r /= l; v /= l; }
270    else { r = 1; v = Vec3<T>(0); }
271    return *this;
272}
273
274template <class T>
275inline Quat<T> Quat<T>::normalized() const
276{
277    if ( T l = length() ) return Quat( r / l, v / l );
278    return Quat();
279}
280
281template<class T>
282inline Quat<T> Quat<T>::inverse() const
283{
284    // 1    Q*
285    // - = ----   where Q* is conjugate (operator~)
286    // Q   Q* Q   and (Q* Q) == Q ^ Q (4D dot)
287
288    T qdot = *this ^ *this;
289    return Quat( r / qdot, -v / qdot );
290}
291
292template<class T>
293inline Quat<T>& Quat<T>::invert()
294{
295    T qdot = (*this) ^ (*this);
296    r /= qdot;
297    v = -v / qdot;
298    return *this;
299}
300
301template<class T>
302Quat<T>
303slerp(const Quat<T> &q1,const Quat<T> &q2, T t)
304{
305    //
306    // Spherical linear interpolation.
307    //
308    // NOTE: Assumes q1 and q2 are normalized and that 0 <= t <= 1.
309    //
310    // This method does *not* interpolate along the shortest arc
311    // between q1 and q2. If you desire interpolation along the
312    // shortest arc, then consider flipping the second quaternion
313    // explicitly before calling slerp. The implementation of squad()
314    // depends on a slerp() that interpolates as is, without the
315    // automatic flipping.
316    //
317
318    T cosomega = q1 ^ q2;
319    if (cosomega >= (T) 1.0)
320    {
321        //
322        // Special case: q1 and q2 are the same, so just return one of them.
323        // This also catches the case where cosomega is very slightly > 1.0
324        //
325
326        return q1;
327    }
328   
329    T sinomega = Math<T>::sqrt (1 - cosomega * cosomega);
330
331    Quat<T> result;
332
333    if (sinomega * limits<T>::max() > 1)
334    {
335        T omega = Math<T>::acos (cosomega);
336        T s1 = Math<T>::sin ((1.0 - t) * omega) / sinomega;
337        T s2 = Math<T>::sin (t * omega) / sinomega;
338
339        result  = s1 * q1 + s2 * q2;
340    }
341    else if (cosomega > 0)
342    {
343        //
344        // omega == 0
345        //
346
347        T s1 = 1.0 - t;
348        T s2 = t;
349
350        result = s1 * q1 + s2 * q2;
351    }
352    else
353    {
354        //
355        // omega == -pi
356        //
357
358        result.v.x  = - q1.v.y;
359        result.v.y  =   q1.v.x;
360        result.v.z  = - q1.r;
361        result.r    =   q1.v.z;
362
363        T s1 = Math<T>::sin ((0.5 - t) * M_PI);
364        T s2 = Math<T>::sin (t * M_PI);
365
366        result  = s1 * q1 + s2 * result;
367    }
368
369    return result;
370}
371
372template<class T>
373Quat<T> spline(const Quat<T> &q0, const Quat<T> &q1,
374               const Quat<T> &q2, const Quat<T> &q3,
375               T t)
376{
377    // Spherical Cubic Spline Interpolation -
378    // from Advanced Animation and Rendering
379    // Techniques by Watt and Watt, Page 366:
380    // A spherical curve is constructed using three
381    // spherical linear interpolations of a quadrangle
382    // of unit quaternions: q1, qa, qb, q2.
383    // Given a set of quaternion keys: q0, q1, q2, q3,
384    // this routine does the interpolation between
385    // q1 and q2 by constructing two intermediate
386    // quaternions: qa and qb. The qa and qb are
387    // computed by the intermediate function to
388    // guarantee the continuity of tangents across
389    // adjacent cubic segments. The qa represents in-tangent
390    // for q1 and the qb represents the out-tangent for q2.
391    //
392    // The q1 q2 is the cubic segment being interpolated.
393    // The q0 is from the previous adjacent segment and q3 is
394    // from the next adjacent segment. The q0 and q3 are used
395    // in computing qa and qb.
396    //
397
398    Quat<T> qa = intermediate (q0, q1, q2);
399    Quat<T> qb = intermediate (q1, q2, q3);
400    Quat<T> result = squad(q1, qa, qb, q2, t);
401
402    return result;
403}
404
405template<class T>
406Quat<T> squad(const Quat<T> &q1, const Quat<T> &qa,
407              const Quat<T> &qb, const Quat<T> &q2,
408              T t)
409{
410    // Spherical Quadrangle Interpolation -
411    // from Advanced Animation and Rendering
412    // Techniques by Watt and Watt, Page 366:
413    // It constructs a spherical cubic interpolation as
414    // a series of three spherical linear interpolations
415    // of a quadrangle of unit quaternions.
416    //     
417 
418    Quat<T> r1 = slerp(q1, q2, t);
419    Quat<T> r2 = slerp(qa, qb, t);
420    Quat<T> result = slerp(r1, r2, 2*t*(1-t));
421
422    return result;
423}
424
425template<class T>
426Quat<T> intermediate(const Quat<T> &q0, const Quat<T> &q1, const Quat<T> &q2)
427{
428    // From advanced Animation and Rendering
429    // Techniques by Watt and Watt, Page 366:
430    // computing the inner quadrangle
431    // points (qa and qb) to guarantee tangent
432    // continuity.
433    //
434    Quat<T> q1inv = q1.inverse();
435    Quat<T> c1 = q1inv*q2;
436    Quat<T> c2 = q1inv*q0;
437    Quat<T> c3 = (T) (-0.25) * (c2.log() + c1.log());
438    Quat<T> qa = q1 * c3.exp();
439    qa.normalize();
440    return qa;
441}
442
443template <class T>
444inline Quat<T> Quat<T>::log() const
445{
446    //
447    // For unit quaternion, from Advanced Animation and
448    // Rendering Techniques by Watt and Watt, Page 366:
449    //
450
451    T theta = Math<T>::acos (std::min (r, (T) 1.0));
452    if (theta == 0)
453        return Quat<T> (0, v);
454   
455    T sintheta = Math<T>::sin (theta);
456   
457    T k;
458    if (abs (sintheta) < 1 && abs (theta) >= limits<T>::max() * abs (sintheta))
459        k = 0;
460    else
461        k = theta / sintheta;
462
463    return Quat<T> ((T) 0, v.x * k, v.y * k, v.z * k);
464}
465
466template <class T>
467inline Quat<T> Quat<T>::exp() const
468{
469    //
470    // For pure quaternion (zero scalar part):
471    // from Advanced Animation and Rendering
472    // Techniques by Watt and Watt, Page 366:
473    //
474
475    T theta = v.length();
476    T sintheta = Math<T>::sin (theta);
477   
478    T k;
479    if (abs (theta) < 1 && abs (sintheta) >= limits<T>::max() * abs (theta))
480        k = 0;
481    else
482        k = sintheta / theta;
483
484    T costheta = Math<T>::cos (theta);
485
486    return Quat<T> (costheta, v.x * k, v.y * k, v.z * k);
487}
488
489template <class T>
490inline T Quat<T>::angle() const
491{
492    return 2.0*Math<T>::acos(r);
493}
494
495template <class T>
496inline Vec3<T> Quat<T>::axis() const
497{
498    return v.normalized();
499}
500
501template <class T>
502inline Quat<T>& Quat<T>::setAxisAngle(const Vec3<T>& axis, T radians)
503{
504    r = Math<T>::cos(radians/2);
505    v = axis.normalized() * Math<T>::sin(radians/2);
506    return *this;
507}
508
509
510template <class T>
511Quat<T>&
512Quat<T>::setRotation(const Vec3<T>& from, const Vec3<T>& to)
513{
514    //
515    // Ported from SbRotation
516    //
517
518    T cost = from.dot(to) / Math<T>::sqrt(from.dot(from) * to.dot(to));
519
520    // check for degeneracies
521    if (cost > 0.99999)
522    {
523        //
524        // Vectors are parallel.
525        //
526
527        r = 1.0;
528        v = Vec3<T>(0);
529    }
530    else if (cost < -0.99999)
531    {
532        //
533        // Vectors are opposite. Find an axis to rotate around,
534        // which should be perpendicular to the original axis.
535        //
536
537        Vec3<T> frm = from.normalized();
538        v = frm.cross(Vec3<T>(1, 0, 0));
539        if (v.length() < 0.00001)
540            v   = frm.cross(Vec3<T>(0, 1, 0));
541        r = 0;
542        v.normalize();
543    }
544    else
545    {
546        //
547        // Use half-angle formulae:
548        // cos^2 t = ( 1 + cos (2t) ) / 2
549        // w part is cosine of half the rotation angle
550        //
551
552        r = Math<T>::sqrt(0.5 * (1.0 + cost));
553
554        //
555        // sin^2 t = ( 1 - cos (2t) ) / 2
556        // Do the normalization of the axis vector at the same time so
557        // we only call sqrt once.
558        //
559
560        v = from.cross(to);
561        v *= Math<T>::sqrt((0.5 * (1.0 - cost))/(v.dot(v)));
562    }
563
564    return *this;
565}
566
567template<class T>
568Matrix33<T> Quat<T>::toMatrix33() const
569{
570    return Matrix33<T>(1. - 2.0 * (v.y * v.y + v.z * v.z),
571                            2.0 * (v.x * v.y + v.z * r),
572                            2.0 * (v.z * v.x - v.y * r),
573
574                            2.0 * (v.x * v.y - v.z * r),
575                       1. - 2.0 * (v.z * v.z + v.x * v.x),
576                            2.0 * (v.y * v.z + v.x * r),
577
578                            2.0 * (v.z * v.x + v.y * r),
579                            2.0 * (v.y * v.z - v.x * r),
580                       1. - 2.0 * (v.y * v.y + v.x * v.x));
581}
582
583template<class T>
584Matrix44<T> Quat<T>::toMatrix44() const
585{
586    return Matrix44<T>(1. - 2.0 * (v.y * v.y + v.z * v.z),
587                            2.0 * (v.x * v.y + v.z * r),
588                            2.0 * (v.z * v.x - v.y * r),
589                            0.,
590                            2.0 * (v.x * v.y - v.z * r),
591                       1. - 2.0 * (v.z * v.z + v.x * v.x),
592                            2.0 * (v.y * v.z + v.x * r),
593                            0.,
594                            2.0 * (v.z * v.x + v.y * r),
595                            2.0 * (v.y * v.z - v.x * r),
596                       1. - 2.0 * (v.y * v.y + v.x * v.x),
597                            0.,
598                            0.,
599                            0.,
600                            0.,
601                            1.0 );
602}
603
604
605template<class T>
606inline Matrix33<T> operator* (const Matrix33<T> &M, const Quat<T> &q)
607{
608    return M * q.toMatrix33();
609}
610
611template<class T>
612inline Matrix33<T> operator* (const Quat<T> &q, const Matrix33<T> &M)
613{
614    return q.toMatrix33() * M;
615}
616
617template<class T>
618std::ostream& operator<< (std::ostream &o, const Quat<T> &q)
619{
620    return o << "(" << q.r
621             << " " << q.v.x
622             << " " << q.v.y
623             << " " << q.v.z
624             << ")";
625
626}
627
628template<class T>
629inline Quat<T> operator* (const Quat<T>& q1, const Quat<T>& q2)
630{
631    // (S1+V1) (S2+V2) = S1 S2 - V1.V2 + S1 V2 + V1 S2 + V1 x V2
632    return Quat<T>( q1.r * q2.r - (q1.v ^ q2.v),
633                    q1.r * q2.v + q1.v * q2.r + q1.v % q2.v );
634}
635
636template<class T>
637inline Quat<T> operator/ (const Quat<T>& q1, const Quat<T>& q2)
638{
639    return q1 * q2.inverse();
640}
641
642template<class T>
643inline Quat<T> operator/ (const Quat<T>& q,T t)
644{
645    return Quat<T>(q.r/t,q.v/t);
646}
647
648template<class T>
649inline Quat<T> operator* (const Quat<T>& q,T t)
650{
651    return Quat<T>(q.r*t,q.v*t);
652}
653
654template<class T>
655inline Quat<T> operator* (T t, const Quat<T>& q)
656{
657    return Quat<T>(q.r*t,q.v*t);
658}
659
660template<class T>
661inline Quat<T> operator+ (const Quat<T>& q1, const Quat<T>& q2)
662{
663    return Quat<T>( q1.r + q2.r, q1.v + q2.v );
664}
665
666template<class T>
667inline Quat<T> operator- (const Quat<T>& q1, const Quat<T>& q2)
668{
669    return Quat<T>( q1.r - q2.r, q1.v - q2.v );
670}
671
672template<class T>
673inline Quat<T> operator~ (const Quat<T>& q)
674{
675    return Quat<T>( q.r, -q.v );        // conjugate: (S+V)* = S-V
676}
677
678template<class T>
679inline Quat<T> operator- (const Quat<T>& q)
680{
681    return Quat<T>( -q.r, -q.v );
682}
683
684#if defined PLATFORM_WINDOWS && _MSC_VER
685#pragma warning(default:4244)
686#endif
687
688} // namespace Imath
689
690#endif
Note: See TracBrowser for help on using the repository browser.