source: OGRE/trunk/ogrenew/OgreMain/src/OgreQuaternion.cpp @ 692

Revision 692, 17.9 KB checked in by mattausch, 19 years ago (diff)

adding ogre 1.2 and dependencies

Line 
1/*
2-----------------------------------------------------------------------------
3This source file is part of OGRE
4    (Object-oriented Graphics Rendering Engine)
5For the latest info, see http://www.ogre3d.org/
6
7Copyright (c) 2000-2005 The OGRE Team
8Also see acknowledgements in Readme.html
9
10This program is free software; you can redistribute it and/or modify it under
11the terms of the GNU Lesser General Public License as published by the Free Software
12Foundation; either version 2 of the License, or (at your option) any later
13version.
14
15This program is distributed in the hope that it will be useful, but WITHOUT
16ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
17FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.
18
19You should have received a copy of the GNU Lesser General Public License along with
20this program; if not, write to the Free Software Foundation, Inc., 59 Temple
21Place - Suite 330, Boston, MA 02111-1307, USA, or go to
22http://www.gnu.org/copyleft/lesser.txt.
23-----------------------------------------------------------------------------
24*/
25#include "OgreStableHeaders.h"
26// NOTE THAT THIS FILE IS BASED ON MATERIAL FROM:
27
28// Magic Software, Inc.
29// http://www.geometrictools.com
30// Copyright (c) 2000, All Rights Reserved
31//
32// Source code from Magic Software is supplied under the terms of a license
33// agreement and may not be copied or disclosed except in accordance with the
34// terms of that agreement.  The various license agreements may be found at
35// the Magic Software web site.  This file is subject to the license
36//
37// FREE SOURCE CODE
38// http://www.geometrictools.com/License/WildMagic3License.pdf
39
40#include "OgreQuaternion.h"
41
42#include "OgreMath.h"
43#include "OgreMatrix3.h"
44#include "OgreVector3.h"
45
46namespace Ogre {
47
48    const Real Quaternion::ms_fEpsilon = 1e-03;
49    const Quaternion Quaternion::ZERO(0.0,0.0,0.0,0.0);
50    const Quaternion Quaternion::IDENTITY(1.0,0.0,0.0,0.0);
51
52    //-----------------------------------------------------------------------
53    void Quaternion::FromRotationMatrix (const Matrix3& kRot)
54    {
55        // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
56        // article "Quaternion Calculus and Fast Animation".
57
58        Real fTrace = kRot[0][0]+kRot[1][1]+kRot[2][2];
59        Real fRoot;
60
61        if ( fTrace > 0.0 )
62        {
63            // |w| > 1/2, may as well choose w > 1/2
64            fRoot = Math::Sqrt(fTrace + 1.0);  // 2w
65            w = 0.5*fRoot;
66            fRoot = 0.5/fRoot;  // 1/(4w)
67            x = (kRot[2][1]-kRot[1][2])*fRoot;
68            y = (kRot[0][2]-kRot[2][0])*fRoot;
69            z = (kRot[1][0]-kRot[0][1])*fRoot;
70        }
71        else
72        {
73            // |w| <= 1/2
74            static size_t s_iNext[3] = { 1, 2, 0 };
75            size_t i = 0;
76            if ( kRot[1][1] > kRot[0][0] )
77                i = 1;
78            if ( kRot[2][2] > kRot[i][i] )
79                i = 2;
80            size_t j = s_iNext[i];
81            size_t k = s_iNext[j];
82
83            fRoot = Math::Sqrt(kRot[i][i]-kRot[j][j]-kRot[k][k] + 1.0);
84            Real* apkQuat[3] = { &x, &y, &z };
85            *apkQuat[i] = 0.5*fRoot;
86            fRoot = 0.5/fRoot;
87            w = (kRot[k][j]-kRot[j][k])*fRoot;
88            *apkQuat[j] = (kRot[j][i]+kRot[i][j])*fRoot;
89            *apkQuat[k] = (kRot[k][i]+kRot[i][k])*fRoot;
90        }
91    }
92    //-----------------------------------------------------------------------
93    void Quaternion::ToRotationMatrix (Matrix3& kRot) const
94    {
95        Real fTx  = 2.0*x;
96        Real fTy  = 2.0*y;
97        Real fTz  = 2.0*z;
98        Real fTwx = fTx*w;
99        Real fTwy = fTy*w;
100        Real fTwz = fTz*w;
101        Real fTxx = fTx*x;
102        Real fTxy = fTy*x;
103        Real fTxz = fTz*x;
104        Real fTyy = fTy*y;
105        Real fTyz = fTz*y;
106        Real fTzz = fTz*z;
107
108        kRot[0][0] = 1.0-(fTyy+fTzz);
109        kRot[0][1] = fTxy-fTwz;
110        kRot[0][2] = fTxz+fTwy;
111        kRot[1][0] = fTxy+fTwz;
112        kRot[1][1] = 1.0-(fTxx+fTzz);
113        kRot[1][2] = fTyz-fTwx;
114        kRot[2][0] = fTxz-fTwy;
115        kRot[2][1] = fTyz+fTwx;
116        kRot[2][2] = 1.0-(fTxx+fTyy);
117    }
118    //-----------------------------------------------------------------------
119    void Quaternion::FromAngleAxis (const Radian& rfAngle,
120        const Vector3& rkAxis)
121    {
122        // assert:  axis[] is unit length
123        //
124        // The quaternion representing the rotation is
125        //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
126
127        Radian fHalfAngle ( 0.5*rfAngle );
128        Real fSin = Math::Sin(fHalfAngle);
129        w = Math::Cos(fHalfAngle);
130        x = fSin*rkAxis.x;
131        y = fSin*rkAxis.y;
132        z = fSin*rkAxis.z;
133    }
134    //-----------------------------------------------------------------------
135    void Quaternion::ToAngleAxis (Radian& rfAngle, Vector3& rkAxis) const
136    {
137        // The quaternion representing the rotation is
138        //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
139
140        Real fSqrLength = x*x+y*y+z*z;
141        if ( fSqrLength > 0.0 )
142        {
143            rfAngle = 2.0*Math::ACos(w);
144            Real fInvLength = Math::InvSqrt(fSqrLength);
145            rkAxis.x = x*fInvLength;
146            rkAxis.y = y*fInvLength;
147            rkAxis.z = z*fInvLength;
148        }
149        else
150        {
151            // angle is 0 (mod 2*pi), so any axis will do
152            rfAngle = Radian(0.0);
153            rkAxis.x = 1.0;
154            rkAxis.y = 0.0;
155            rkAxis.z = 0.0;
156        }
157    }
158    //-----------------------------------------------------------------------
159    void Quaternion::FromAxes (const Vector3* akAxis)
160    {
161        Matrix3 kRot;
162
163        for (size_t iCol = 0; iCol < 3; iCol++)
164        {
165            kRot[0][iCol] = akAxis[iCol].x;
166            kRot[1][iCol] = akAxis[iCol].y;
167            kRot[2][iCol] = akAxis[iCol].z;
168        }
169
170        FromRotationMatrix(kRot);
171    }
172    //-----------------------------------------------------------------------
173    void Quaternion::FromAxes (const Vector3& xaxis, const Vector3& yaxis, const Vector3& zaxis)
174    {
175        Matrix3 kRot;
176
177        kRot[0][0] = xaxis.x;
178        kRot[1][0] = xaxis.y;
179        kRot[2][0] = xaxis.z;
180
181        kRot[0][1] = yaxis.x;
182        kRot[1][1] = yaxis.y;
183        kRot[2][1] = yaxis.z;
184
185        kRot[0][2] = zaxis.x;
186        kRot[1][2] = zaxis.y;
187        kRot[2][2] = zaxis.z;
188
189        FromRotationMatrix(kRot);
190
191    }
192    //-----------------------------------------------------------------------
193    void Quaternion::ToAxes (Vector3* akAxis) const
194    {
195        Matrix3 kRot;
196
197        ToRotationMatrix(kRot);
198
199        for (size_t iCol = 0; iCol < 3; iCol++)
200        {
201            akAxis[iCol].x = kRot[0][iCol];
202            akAxis[iCol].y = kRot[1][iCol];
203            akAxis[iCol].z = kRot[2][iCol];
204        }
205    }
206    //-----------------------------------------------------------------------
207    Vector3 Quaternion::xAxis(void) const
208    {
209        //Real fTx  = 2.0*x;
210        Real fTy  = 2.0*y;
211        Real fTz  = 2.0*z;
212        Real fTwy = fTy*w;
213        Real fTwz = fTz*w;
214        Real fTxy = fTy*x;
215        Real fTxz = fTz*x;
216        Real fTyy = fTy*y;
217        Real fTzz = fTz*z;
218
219        return Vector3(1.0-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
220    }
221    //-----------------------------------------------------------------------
222    Vector3 Quaternion::yAxis(void) const
223    {
224        Real fTx  = 2.0*x;
225        Real fTy  = 2.0*y;
226        Real fTz  = 2.0*z;
227        Real fTwx = fTx*w;
228        Real fTwz = fTz*w;
229        Real fTxx = fTx*x;
230        Real fTxy = fTy*x;
231        Real fTyz = fTz*y;
232        Real fTzz = fTz*z;
233
234        return Vector3(fTxy-fTwz, 1.0-(fTxx+fTzz), fTyz+fTwx);
235    }
236    //-----------------------------------------------------------------------
237    Vector3 Quaternion::zAxis(void) const
238    {
239        Real fTx  = 2.0*x;
240        Real fTy  = 2.0*y;
241        Real fTz  = 2.0*z;
242        Real fTwx = fTx*w;
243        Real fTwy = fTy*w;
244        Real fTxx = fTx*x;
245        Real fTxz = fTz*x;
246        Real fTyy = fTy*y;
247        Real fTyz = fTz*y;
248
249        return Vector3(fTxz+fTwy, fTyz-fTwx, 1.0-(fTxx+fTyy));
250    }
251    //-----------------------------------------------------------------------
252    void Quaternion::ToAxes (Vector3& xaxis, Vector3& yaxis, Vector3& zaxis) const
253    {
254        Matrix3 kRot;
255
256        ToRotationMatrix(kRot);
257
258        xaxis.x = kRot[0][0];
259        xaxis.y = kRot[1][0];
260        xaxis.z = kRot[2][0];
261
262        yaxis.x = kRot[0][1];
263        yaxis.y = kRot[1][1];
264        yaxis.z = kRot[2][1];
265
266        zaxis.x = kRot[0][2];
267        zaxis.y = kRot[1][2];
268        zaxis.z = kRot[2][2];
269    }
270
271    //-----------------------------------------------------------------------
272    Quaternion Quaternion::operator+ (const Quaternion& rkQ) const
273    {
274        return Quaternion(w+rkQ.w,x+rkQ.x,y+rkQ.y,z+rkQ.z);
275    }
276    //-----------------------------------------------------------------------
277    Quaternion Quaternion::operator- (const Quaternion& rkQ) const
278    {
279        return Quaternion(w-rkQ.w,x-rkQ.x,y-rkQ.y,z-rkQ.z);
280    }
281    //-----------------------------------------------------------------------
282    Quaternion Quaternion::operator* (const Quaternion& rkQ) const
283    {
284        // NOTE:  Multiplication is not generally commutative, so in most
285        // cases p*q != q*p.
286
287        return Quaternion
288        (
289            w * rkQ.w - x * rkQ.x - y * rkQ.y - z * rkQ.z,
290            w * rkQ.x + x * rkQ.w + y * rkQ.z - z * rkQ.y,
291            w * rkQ.y + y * rkQ.w + z * rkQ.x - x * rkQ.z,
292            w * rkQ.z + z * rkQ.w + x * rkQ.y - y * rkQ.x
293        );
294    }
295    //-----------------------------------------------------------------------
296    Quaternion Quaternion::operator* (Real fScalar) const
297    {
298        return Quaternion(fScalar*w,fScalar*x,fScalar*y,fScalar*z);
299    }
300    //-----------------------------------------------------------------------
301    Quaternion operator* (Real fScalar, const Quaternion& rkQ)
302    {
303        return Quaternion(fScalar*rkQ.w,fScalar*rkQ.x,fScalar*rkQ.y,
304            fScalar*rkQ.z);
305    }
306    //-----------------------------------------------------------------------
307    Quaternion Quaternion::operator- () const
308    {
309        return Quaternion(-w,-x,-y,-z);
310    }
311    //-----------------------------------------------------------------------
312    Real Quaternion::Dot (const Quaternion& rkQ) const
313    {
314        return w*rkQ.w+x*rkQ.x+y*rkQ.y+z*rkQ.z;
315    }
316    //-----------------------------------------------------------------------
317    Real Quaternion::Norm () const
318    {
319        return w*w+x*x+y*y+z*z;
320    }
321    //-----------------------------------------------------------------------
322    Quaternion Quaternion::Inverse () const
323    {
324        Real fNorm = w*w+x*x+y*y+z*z;
325        if ( fNorm > 0.0 )
326        {
327            Real fInvNorm = 1.0/fNorm;
328            return Quaternion(w*fInvNorm,-x*fInvNorm,-y*fInvNorm,-z*fInvNorm);
329        }
330        else
331        {
332            // return an invalid result to flag the error
333            return ZERO;
334        }
335    }
336    //-----------------------------------------------------------------------
337    Quaternion Quaternion::UnitInverse () const
338    {
339        // assert:  'this' is unit length
340        return Quaternion(w,-x,-y,-z);
341    }
342    //-----------------------------------------------------------------------
343    Quaternion Quaternion::Exp () const
344    {
345        // If q = A*(x*i+y*j+z*k) where (x,y,z) is unit length, then
346        // exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k).  If sin(A) is near zero,
347        // use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1.
348
349        Radian fAngle ( Math::Sqrt(x*x+y*y+z*z) );
350        Real fSin = Math::Sin(fAngle);
351
352        Quaternion kResult;
353        kResult.w = Math::Cos(fAngle);
354
355        if ( Math::Abs(fSin) >= ms_fEpsilon )
356        {
357            Real fCoeff = fSin/(fAngle.valueRadians());
358            kResult.x = fCoeff*x;
359            kResult.y = fCoeff*y;
360            kResult.z = fCoeff*z;
361        }
362        else
363        {
364            kResult.x = x;
365            kResult.y = y;
366            kResult.z = z;
367        }
368
369        return kResult;
370    }
371    //-----------------------------------------------------------------------
372    Quaternion Quaternion::Log () const
373    {
374        // If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x,y,z) is unit length, then
375        // log(q) = A*(x*i+y*j+z*k).  If sin(A) is near zero, use log(q) =
376        // sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.
377
378        Quaternion kResult;
379        kResult.w = 0.0;
380
381        if ( Math::Abs(w) < 1.0 )
382        {
383            Radian fAngle ( Math::ACos(w) );
384            Real fSin = Math::Sin(fAngle);
385            if ( Math::Abs(fSin) >= ms_fEpsilon )
386            {
387                Real fCoeff = fAngle.valueRadians()/fSin;
388                kResult.x = fCoeff*x;
389                kResult.y = fCoeff*y;
390                kResult.z = fCoeff*z;
391                return kResult;
392            }
393        }
394
395        kResult.x = x;
396        kResult.y = y;
397        kResult.z = z;
398
399        return kResult;
400    }
401    //-----------------------------------------------------------------------
402    Vector3 Quaternion::operator* (const Vector3& v) const
403    {
404                // nVidia SDK implementation
405                Vector3 uv, uuv;
406                Vector3 qvec(x, y, z);
407                uv = qvec.crossProduct(v);
408                uuv = qvec.crossProduct(uv);
409                uv *= (2.0f * w);
410                uuv *= 2.0f;
411
412                return v + uv + uuv;
413
414    }
415    //-----------------------------------------------------------------------
416        bool Quaternion::equals(const Quaternion& rhs, const Radian& tolerance) const
417        {
418        Real fCos = Dot(rhs);
419        Radian angle = Math::ACos(fCos);
420
421                return (Math::Abs(angle.valueRadians()) <= tolerance.valueRadians())
422            || Math::RealEqual(angle.valueRadians(), Math::PI, tolerance.valueRadians());
423
424
425        }
426    //-----------------------------------------------------------------------
427    Quaternion Quaternion::Slerp (Real fT, const Quaternion& rkP,
428        const Quaternion& rkQ, bool shortestPath)
429    {
430        Real fCos = rkP.Dot(rkQ);
431        Radian fAngle ( Math::ACos(fCos) );
432
433        if ( Math::Abs(fAngle.valueRadians()) < ms_fEpsilon )
434            return rkP;
435
436        Real fSin = Math::Sin(fAngle);
437        Real fInvSin = 1.0/fSin;
438        Real fCoeff0 = Math::Sin((1.0-fT)*fAngle)*fInvSin;
439        Real fCoeff1 = Math::Sin(fT*fAngle)*fInvSin;
440        // Do we need to invert rotation?
441        if (fCos < 0.0f && shortestPath)
442        {
443            fCoeff0 = -fCoeff0;
444            // taking the complement requires renormalisation
445            Quaternion t(fCoeff0*rkP + fCoeff1*rkQ);
446            t.normalise();
447            return t;
448        }
449        else
450        {
451            return fCoeff0*rkP + fCoeff1*rkQ;
452        }
453    }
454    //-----------------------------------------------------------------------
455    Quaternion Quaternion::SlerpExtraSpins (Real fT,
456        const Quaternion& rkP, const Quaternion& rkQ, int iExtraSpins)
457    {
458        Real fCos = rkP.Dot(rkQ);
459        Radian fAngle ( Math::ACos(fCos) );
460
461        if ( Math::Abs(fAngle.valueRadians()) < ms_fEpsilon )
462            return rkP;
463
464        Real fSin = Math::Sin(fAngle);
465        Radian fPhase ( Math::PI*iExtraSpins*fT );
466        Real fInvSin = 1.0/fSin;
467        Real fCoeff0 = Math::Sin((1.0-fT)*fAngle - fPhase)*fInvSin;
468        Real fCoeff1 = Math::Sin(fT*fAngle + fPhase)*fInvSin;
469        return fCoeff0*rkP + fCoeff1*rkQ;
470    }
471    //-----------------------------------------------------------------------
472    void Quaternion::Intermediate (const Quaternion& rkQ0,
473        const Quaternion& rkQ1, const Quaternion& rkQ2,
474        Quaternion& rkA, Quaternion& rkB)
475    {
476        // assert:  q0, q1, q2 are unit quaternions
477
478        Quaternion kQ0inv = rkQ0.UnitInverse();
479        Quaternion kQ1inv = rkQ1.UnitInverse();
480        Quaternion rkP0 = kQ0inv*rkQ1;
481        Quaternion rkP1 = kQ1inv*rkQ2;
482        Quaternion kArg = 0.25*(rkP0.Log()-rkP1.Log());
483        Quaternion kMinusArg = -kArg;
484
485        rkA = rkQ1*kArg.Exp();
486        rkB = rkQ1*kMinusArg.Exp();
487    }
488    //-----------------------------------------------------------------------
489    Quaternion Quaternion::Squad (Real fT,
490        const Quaternion& rkP, const Quaternion& rkA,
491        const Quaternion& rkB, const Quaternion& rkQ, bool shortestPath)
492    {
493        Real fSlerpT = 2.0*fT*(1.0-fT);
494        Quaternion kSlerpP = Slerp(fT, rkP, rkQ, shortestPath);
495        Quaternion kSlerpQ = Slerp(fT, rkA, rkB);
496        return Slerp(fSlerpT, kSlerpP ,kSlerpQ);
497    }
498    //-----------------------------------------------------------------------
499    Real Quaternion::normalise(void)
500    {
501        Real len = Norm();
502        Real factor = 1.0f / Math::Sqrt(len);
503        *this = *this * factor;
504        return len;
505    }
506    //-----------------------------------------------------------------------
507        Radian Quaternion::getRoll(void) const
508        {
509                return Radian(Math::ATan2(2*(x*y + w*z), w*w + x*x - y*y - z*z));
510        }
511    //-----------------------------------------------------------------------
512        Radian Quaternion::getPitch(void) const
513        {
514                return Radian(Math::ATan2(2*(y*z + w*x), w*w - x*x - y*y + z*z));
515        }
516    //-----------------------------------------------------------------------
517        Radian Quaternion::getYaw(void) const
518        {
519                return Radian(Math::ASin(-2*(x*z - w*y)));
520        }
521    //-----------------------------------------------------------------------
522    Quaternion Quaternion::nlerp(Real fT, const Quaternion& rkP,
523        const Quaternion& rkQ, bool shortestPath)
524    {
525                Quaternion result;
526        Real fCos = rkP.Dot(rkQ);
527                if (fCos < 0.0f && shortestPath)
528                {
529                        result = rkP + fT * ((-rkQ) - rkP);
530                }
531                else
532                {
533                        result = rkP + fT * (rkQ - rkP);
534                }
535        result.normalise();
536        return result;
537    }
538}
Note: See TracBrowser for help on using the repository browser.