Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: sandbox_light/src/external/ogremath/OgreQuaternion.cpp @ 6417

Last change on this file since 6417 was 5789, checked in by rgrieder, 15 years ago

Stripped sandbox further down to get a light version that excludes almost all core features.
Also, the Ogre dependency has been removed and a little ogremath library been added.
  • Property svn:eol-style set to native
File size: 19.4 KB
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-2006 Torus Knot Software Ltd
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
24You may alternatively use this source under the terms of a specific version of
25the OGRE Unrestricted License provided you have obtained such a license from
26Torus Knot Software Ltd.
27-----------------------------------------------------------------------------
28*/
29// NOTE THAT THIS FILE IS BASED ON MATERIAL FROM:
30
31// Magic Software, Inc.
32// http://www.geometrictools.com
33// Copyright (c) 2000, All Rights Reserved
34//
35// Source code from Magic Software is supplied under the terms of a license
36// agreement and may not be copied or disclosed except in accordance with the
37// terms of that agreement.  The various license agreements may be found at
38// the Magic Software web site.  This file is subject to the license
39//
40// FREE SOURCE CODE
41// http://www.geometrictools.com/License/WildMagic3License.pdf
42
43#include "OgreQuaternion.h"
44
45#include "OgreMath.h"
46#include "OgreMatrix3.h"
47#include "OgreVector3.h"
48
49namespace Ogre {
50
51    const Real Quaternion::ms_fEpsilon = 1e-03;
52    const Quaternion Quaternion::ZERO(0.0,0.0,0.0,0.0);
53    const Quaternion Quaternion::IDENTITY(1.0,0.0,0.0,0.0);
54
55    //-----------------------------------------------------------------------
56    void Quaternion::FromRotationMatrix (const Matrix3& kRot)
57    {
58        // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
59        // article "Quaternion Calculus and Fast Animation".
60
61        Real fTrace = kRot[0][0]+kRot[1][1]+kRot[2][2];
62        Real fRoot;
63
64        if ( fTrace > 0.0 )
65        {
66            // |w| > 1/2, may as well choose w > 1/2
67            fRoot = Math::Sqrt(fTrace + 1.0);  // 2w
68            w = 0.5*fRoot;
69            fRoot = 0.5/fRoot;  // 1/(4w)
70            x = (kRot[2][1]-kRot[1][2])*fRoot;
71            y = (kRot[0][2]-kRot[2][0])*fRoot;
72            z = (kRot[1][0]-kRot[0][1])*fRoot;
73        }
74        else
75        {
76            // |w| <= 1/2
77            static size_t s_iNext[3] = { 1, 2, 0 };
78            size_t i = 0;
79            if ( kRot[1][1] > kRot[0][0] )
80                i = 1;
81            if ( kRot[2][2] > kRot[i][i] )
82                i = 2;
83            size_t j = s_iNext[i];
84            size_t k = s_iNext[j];
85
86            fRoot = Math::Sqrt(kRot[i][i]-kRot[j][j]-kRot[k][k] + 1.0);
87            Real* apkQuat[3] = { &x, &y, &z };
88            *apkQuat[i] = 0.5*fRoot;
89            fRoot = 0.5/fRoot;
90            w = (kRot[k][j]-kRot[j][k])*fRoot;
91            *apkQuat[j] = (kRot[j][i]+kRot[i][j])*fRoot;
92            *apkQuat[k] = (kRot[k][i]+kRot[i][k])*fRoot;
93        }
94    }
95    //-----------------------------------------------------------------------
96    void Quaternion::ToRotationMatrix (Matrix3& kRot) const
97    {
98        Real fTx  = 2.0*x;
99        Real fTy  = 2.0*y;
100        Real fTz  = 2.0*z;
101        Real fTwx = fTx*w;
102        Real fTwy = fTy*w;
103        Real fTwz = fTz*w;
104        Real fTxx = fTx*x;
105        Real fTxy = fTy*x;
106        Real fTxz = fTz*x;
107        Real fTyy = fTy*y;
108        Real fTyz = fTz*y;
109        Real fTzz = fTz*z;
110
111        kRot[0][0] = 1.0-(fTyy+fTzz);
112        kRot[0][1] = fTxy-fTwz;
113        kRot[0][2] = fTxz+fTwy;
114        kRot[1][0] = fTxy+fTwz;
115        kRot[1][1] = 1.0-(fTxx+fTzz);
116        kRot[1][2] = fTyz-fTwx;
117        kRot[2][0] = fTxz-fTwy;
118        kRot[2][1] = fTyz+fTwx;
119        kRot[2][2] = 1.0-(fTxx+fTyy);
120    }
121    //-----------------------------------------------------------------------
122    void Quaternion::FromAngleAxis (const Radian& rfAngle,
123        const Vector3& rkAxis)
124    {
125        // assert:  axis[] is unit length
126        //
127        // The quaternion representing the rotation is
128        //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
129
130        Radian fHalfAngle ( 0.5*rfAngle );
131        Real fSin = Math::Sin(fHalfAngle);
132        w = Math::Cos(fHalfAngle);
133        x = fSin*rkAxis.x;
134        y = fSin*rkAxis.y;
135        z = fSin*rkAxis.z;
136    }
137    //-----------------------------------------------------------------------
138    void Quaternion::ToAngleAxis (Radian& rfAngle, Vector3& rkAxis) const
139    {
140        // The quaternion representing the rotation is
141        //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
142
143        Real fSqrLength = x*x+y*y+z*z;
144        if ( fSqrLength > 0.0 )
145        {
146            rfAngle = 2.0*Math::ACos(w);
147            Real fInvLength = Math::InvSqrt(fSqrLength);
148            rkAxis.x = x*fInvLength;
149            rkAxis.y = y*fInvLength;
150            rkAxis.z = z*fInvLength;
151        }
152        else
153        {
154            // angle is 0 (mod 2*pi), so any axis will do
155            rfAngle = Radian(0.0);
156            rkAxis.x = 1.0;
157            rkAxis.y = 0.0;
158            rkAxis.z = 0.0;
159        }
160    }
161    //-----------------------------------------------------------------------
162    void Quaternion::FromAxes (const Vector3* akAxis)
163    {
164        Matrix3 kRot;
165
166        for (size_t iCol = 0; iCol < 3; iCol++)
167        {
168            kRot[0][iCol] = akAxis[iCol].x;
169            kRot[1][iCol] = akAxis[iCol].y;
170            kRot[2][iCol] = akAxis[iCol].z;
171        }
172
173        FromRotationMatrix(kRot);
174    }
175    //-----------------------------------------------------------------------
176    void Quaternion::FromAxes (const Vector3& xaxis, const Vector3& yaxis, const Vector3& zaxis)
177    {
178        Matrix3 kRot;
179
180        kRot[0][0] = xaxis.x;
181        kRot[1][0] = xaxis.y;
182        kRot[2][0] = xaxis.z;
183
184        kRot[0][1] = yaxis.x;
185        kRot[1][1] = yaxis.y;
186        kRot[2][1] = yaxis.z;
187
188        kRot[0][2] = zaxis.x;
189        kRot[1][2] = zaxis.y;
190        kRot[2][2] = zaxis.z;
191
192        FromRotationMatrix(kRot);
193
194    }
195    //-----------------------------------------------------------------------
196    void Quaternion::ToAxes (Vector3* akAxis) const
197    {
198        Matrix3 kRot;
199
200        ToRotationMatrix(kRot);
201
202        for (size_t iCol = 0; iCol < 3; iCol++)
203        {
204            akAxis[iCol].x = kRot[0][iCol];
205            akAxis[iCol].y = kRot[1][iCol];
206            akAxis[iCol].z = kRot[2][iCol];
207        }
208    }
209    //-----------------------------------------------------------------------
210    Vector3 Quaternion::xAxis(void) const
211    {
212        //Real fTx  = 2.0*x;
213        Real fTy  = 2.0*y;
214        Real fTz  = 2.0*z;
215        Real fTwy = fTy*w;
216        Real fTwz = fTz*w;
217        Real fTxy = fTy*x;
218        Real fTxz = fTz*x;
219        Real fTyy = fTy*y;
220        Real fTzz = fTz*z;
221
222        return Vector3(1.0-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
223    }
224    //-----------------------------------------------------------------------
225    Vector3 Quaternion::yAxis(void) const
226    {
227        Real fTx  = 2.0*x;
228        Real fTy  = 2.0*y;
229        Real fTz  = 2.0*z;
230        Real fTwx = fTx*w;
231        Real fTwz = fTz*w;
232        Real fTxx = fTx*x;
233        Real fTxy = fTy*x;
234        Real fTyz = fTz*y;
235        Real fTzz = fTz*z;
236
237        return Vector3(fTxy-fTwz, 1.0-(fTxx+fTzz), fTyz+fTwx);
238    }
239    //-----------------------------------------------------------------------
240    Vector3 Quaternion::zAxis(void) const
241    {
242        Real fTx  = 2.0*x;
243        Real fTy  = 2.0*y;
244        Real fTz  = 2.0*z;
245        Real fTwx = fTx*w;
246        Real fTwy = fTy*w;
247        Real fTxx = fTx*x;
248        Real fTxz = fTz*x;
249        Real fTyy = fTy*y;
250        Real fTyz = fTz*y;
251
252        return Vector3(fTxz+fTwy, fTyz-fTwx, 1.0-(fTxx+fTyy));
253    }
254    //-----------------------------------------------------------------------
255    void Quaternion::ToAxes (Vector3& xaxis, Vector3& yaxis, Vector3& zaxis) const
256    {
257        Matrix3 kRot;
258
259        ToRotationMatrix(kRot);
260
261        xaxis.x = kRot[0][0];
262        xaxis.y = kRot[1][0];
263        xaxis.z = kRot[2][0];
264
265        yaxis.x = kRot[0][1];
266        yaxis.y = kRot[1][1];
267        yaxis.z = kRot[2][1];
268
269        zaxis.x = kRot[0][2];
270        zaxis.y = kRot[1][2];
271        zaxis.z = kRot[2][2];
272    }
273
274    //-----------------------------------------------------------------------
275    Quaternion Quaternion::operator+ (const Quaternion& rkQ) const
276    {
277        return Quaternion(w+rkQ.w,x+rkQ.x,y+rkQ.y,z+rkQ.z);
278    }
279    //-----------------------------------------------------------------------
280    Quaternion Quaternion::operator- (const Quaternion& rkQ) const
281    {
282        return Quaternion(w-rkQ.w,x-rkQ.x,y-rkQ.y,z-rkQ.z);
283    }
284    //-----------------------------------------------------------------------
285    Quaternion Quaternion::operator* (const Quaternion& rkQ) const
286    {
287        // NOTE:  Multiplication is not generally commutative, so in most
288        // cases p*q != q*p.
289
290        return Quaternion
291        (
292            w * rkQ.w - x * rkQ.x - y * rkQ.y - z * rkQ.z,
293            w * rkQ.x + x * rkQ.w + y * rkQ.z - z * rkQ.y,
294            w * rkQ.y + y * rkQ.w + z * rkQ.x - x * rkQ.z,
295            w * rkQ.z + z * rkQ.w + x * rkQ.y - y * rkQ.x
296        );
297    }
298    //-----------------------------------------------------------------------
299    Quaternion Quaternion::operator* (Real fScalar) const
300    {
301        return Quaternion(fScalar*w,fScalar*x,fScalar*y,fScalar*z);
302    }
303    //-----------------------------------------------------------------------
304    Quaternion operator* (Real fScalar, const Quaternion& rkQ)
305    {
306        return Quaternion(fScalar*rkQ.w,fScalar*rkQ.x,fScalar*rkQ.y,
307            fScalar*rkQ.z);
308    }
309    //-----------------------------------------------------------------------
310    Quaternion Quaternion::operator- () const
311    {
312        return Quaternion(-w,-x,-y,-z);
313    }
314    //-----------------------------------------------------------------------
315    Real Quaternion::Dot (const Quaternion& rkQ) const
316    {
317        return w*rkQ.w+x*rkQ.x+y*rkQ.y+z*rkQ.z;
318    }
319    //-----------------------------------------------------------------------
320    Real Quaternion::Norm () const
321    {
322        return w*w+x*x+y*y+z*z;
323    }
324    //-----------------------------------------------------------------------
325    Quaternion Quaternion::Inverse () const
326    {
327        Real fNorm = w*w+x*x+y*y+z*z;
328        if ( fNorm > 0.0 )
329        {
330            Real fInvNorm = 1.0/fNorm;
331            return Quaternion(w*fInvNorm,-x*fInvNorm,-y*fInvNorm,-z*fInvNorm);
332        }
333        else
334        {
335            // return an invalid result to flag the error
336            return ZERO;
337        }
338    }
339    //-----------------------------------------------------------------------
340    Quaternion Quaternion::UnitInverse () const
341    {
342        // assert:  'this' is unit length
343        return Quaternion(w,-x,-y,-z);
344    }
345    //-----------------------------------------------------------------------
346    Quaternion Quaternion::Exp () const
347    {
348        // If q = A*(x*i+y*j+z*k) where (x,y,z) is unit length, then
349        // exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k).  If sin(A) is near zero,
350        // use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1.
351
352        Radian fAngle ( Math::Sqrt(x*x+y*y+z*z) );
353        Real fSin = Math::Sin(fAngle);
354
355        Quaternion kResult;
356        kResult.w = Math::Cos(fAngle);
357
358        if ( Math::Abs(fSin) >= ms_fEpsilon )
359        {
360            Real fCoeff = fSin/(fAngle.valueRadians());
361            kResult.x = fCoeff*x;
362            kResult.y = fCoeff*y;
363            kResult.z = fCoeff*z;
364        }
365        else
366        {
367            kResult.x = x;
368            kResult.y = y;
369            kResult.z = z;
370        }
371
372        return kResult;
373    }
374    //-----------------------------------------------------------------------
375    Quaternion Quaternion::Log () const
376    {
377        // If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x,y,z) is unit length, then
378        // log(q) = A*(x*i+y*j+z*k).  If sin(A) is near zero, use log(q) =
379        // sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.
380
381        Quaternion kResult;
382        kResult.w = 0.0;
383
384        if ( Math::Abs(w) < 1.0 )
385        {
386            Radian fAngle ( Math::ACos(w) );
387            Real fSin = Math::Sin(fAngle);
388            if ( Math::Abs(fSin) >= ms_fEpsilon )
389            {
390                Real fCoeff = fAngle.valueRadians()/fSin;
391                kResult.x = fCoeff*x;
392                kResult.y = fCoeff*y;
393                kResult.z = fCoeff*z;
394                return kResult;
395            }
396        }
397
398        kResult.x = x;
399        kResult.y = y;
400        kResult.z = z;
401
402        return kResult;
403    }
404    //-----------------------------------------------------------------------
405    Vector3 Quaternion::operator* (const Vector3& v) const
406    {
407                // nVidia SDK implementation
408                Vector3 uv, uuv;
409                Vector3 qvec(x, y, z);
410                uv = qvec.crossProduct(v);
411                uuv = qvec.crossProduct(uv);
412                uv *= (2.0f * w);
413                uuv *= 2.0f;
414
415                return v + uv + uuv;
416
417    }
418    //-----------------------------------------------------------------------
419        bool Quaternion::equals(const Quaternion& rhs, const Radian& tolerance) const
420        {
421        Real fCos = Dot(rhs);
422        Radian angle = Math::ACos(fCos);
423
424                return (Math::Abs(angle.valueRadians()) <= tolerance.valueRadians())
425            || Math::RealEqual(angle.valueRadians(), Math::PI, tolerance.valueRadians());
426
427
428        }
429    //-----------------------------------------------------------------------
430    Quaternion Quaternion::Slerp (Real fT, const Quaternion& rkP,
431        const Quaternion& rkQ, bool shortestPath)
432    {
433        Real fCos = rkP.Dot(rkQ);
434        Quaternion rkT;
435
436        // Do we need to invert rotation?
437        if (fCos < 0.0f && shortestPath)
438        {
439            fCos = -fCos;
440            rkT = -rkQ;
441        }
442        else
443        {
444            rkT = rkQ;
445        }
446
447        if (Math::Abs(fCos) < 1 - ms_fEpsilon)
448        {
449            // Standard case (slerp)
450            Real fSin = Math::Sqrt(1 - Math::Sqr(fCos));
451            Radian fAngle = Math::ATan2(fSin, fCos);
452            Real fInvSin = 1.0f / fSin;
453            Real fCoeff0 = Math::Sin((1.0f - fT) * fAngle) * fInvSin;
454            Real fCoeff1 = Math::Sin(fT * fAngle) * fInvSin;
455            return fCoeff0 * rkP + fCoeff1 * rkT;
456        }
457        else
458        {
459            // There are two situations:
460            // 1. "rkP" and "rkQ" are very close (fCos ~= +1), so we can do a linear
461            //    interpolation safely.
462            // 2. "rkP" and "rkQ" are almost inverse of each other (fCos ~= -1), there
463            //    are an infinite number of possibilities interpolation. but we haven't
464            //    have method to fix this case, so just use linear interpolation here.
465            Quaternion t = (1.0f - fT) * rkP + fT * rkT;
466            // taking the complement requires renormalisation
467            t.normalise();
468            return t;
469        }
470    }
471    //-----------------------------------------------------------------------
472    Quaternion Quaternion::SlerpExtraSpins (Real fT,
473        const Quaternion& rkP, const Quaternion& rkQ, int iExtraSpins)
474    {
475        Real fCos = rkP.Dot(rkQ);
476        Radian fAngle ( Math::ACos(fCos) );
477
478        if ( Math::Abs(fAngle.valueRadians()) < ms_fEpsilon )
479            return rkP;
480
481        Real fSin = Math::Sin(fAngle);
482        Radian fPhase ( Math::PI*iExtraSpins*fT );
483        Real fInvSin = 1.0/fSin;
484        Real fCoeff0 = Math::Sin((1.0-fT)*fAngle - fPhase)*fInvSin;
485        Real fCoeff1 = Math::Sin(fT*fAngle + fPhase)*fInvSin;
486        return fCoeff0*rkP + fCoeff1*rkQ;
487    }
488    //-----------------------------------------------------------------------
489    void Quaternion::Intermediate (const Quaternion& rkQ0,
490        const Quaternion& rkQ1, const Quaternion& rkQ2,
491        Quaternion& rkA, Quaternion& rkB)
492    {
493        // assert:  q0, q1, q2 are unit quaternions
494
495        Quaternion kQ0inv = rkQ0.UnitInverse();
496        Quaternion kQ1inv = rkQ1.UnitInverse();
497        Quaternion rkP0 = kQ0inv*rkQ1;
498        Quaternion rkP1 = kQ1inv*rkQ2;
499        Quaternion kArg = 0.25*(rkP0.Log()-rkP1.Log());
500        Quaternion kMinusArg = -kArg;
501
502        rkA = rkQ1*kArg.Exp();
503        rkB = rkQ1*kMinusArg.Exp();
504    }
505    //-----------------------------------------------------------------------
506    Quaternion Quaternion::Squad (Real fT,
507        const Quaternion& rkP, const Quaternion& rkA,
508        const Quaternion& rkB, const Quaternion& rkQ, bool shortestPath)
509    {
510        Real fSlerpT = 2.0*fT*(1.0-fT);
511        Quaternion kSlerpP = Slerp(fT, rkP, rkQ, shortestPath);
512        Quaternion kSlerpQ = Slerp(fT, rkA, rkB);
513        return Slerp(fSlerpT, kSlerpP ,kSlerpQ);
514    }
515    //-----------------------------------------------------------------------
516    Real Quaternion::normalise(void)
517    {
518        Real len = Norm();
519        Real factor = 1.0f / Math::Sqrt(len);
520        *this = *this * factor;
521        return len;
522    }
523    //-----------------------------------------------------------------------
524        Radian Quaternion::getRoll(bool reprojectAxis) const
525        {
526                if (reprojectAxis)
527                {
528                        // roll = atan2(localx.y, localx.x)
529                        // pick parts of xAxis() implementation that we need
530                        Real fTx  = 2.0*x;
531                        Real fTy  = 2.0*y;
532                        Real fTz  = 2.0*z;
533                        Real fTwz = fTz*w;
534                        Real fTxy = fTy*x;
535                        Real fTyy = fTy*y;
536                        Real fTzz = fTz*z;
537
538                        // Vector3(1.0-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
539
540                        return Radian(Math::ATan2(fTxy+fTwz, 1.0-(fTyy+fTzz)));
541
542                }
543                else
544                {
545                        return Radian(Math::ATan2(2*(x*y + w*z), w*w + x*x - y*y - z*z));
546                }
547        }
548    //-----------------------------------------------------------------------
549        Radian Quaternion::getPitch(bool reprojectAxis) const
550        {
551                if (reprojectAxis)
552                {
553                        // pitch = atan2(localy.z, localy.y)
554                        // pick parts of yAxis() implementation that we need
555                        Real fTx  = 2.0*x;
556                        Real fTy  = 2.0*y;
557                        Real fTz  = 2.0*z;
558                        Real fTwx = fTx*w;
559                        Real fTxx = fTx*x;
560                        Real fTyz = fTz*y;
561                        Real fTzz = fTz*z;
562
563                        // Vector3(fTxy-fTwz, 1.0-(fTxx+fTzz), fTyz+fTwx);
564                        return Radian(Math::ATan2(fTyz+fTwx, 1.0-(fTxx+fTzz)));
565                }
566                else
567                {
568                        // internal version
569                        return Radian(Math::ATan2(2*(y*z + w*x), w*w - x*x - y*y + z*z));
570                }
571        }
572    //-----------------------------------------------------------------------
573        Radian Quaternion::getYaw(bool reprojectAxis) const
574        {
575                if (reprojectAxis)
576                {
577                        // yaw = atan2(localz.x, localz.z)
578                        // pick parts of zAxis() implementation that we need
579                        Real fTx  = 2.0*x;
580                        Real fTy  = 2.0*y;
581                        Real fTz  = 2.0*z;
582                        Real fTwy = fTy*w;
583                        Real fTxx = fTx*x;
584                        Real fTxz = fTz*x;
585                        Real fTyy = fTy*y;
586
587                        // Vector3(fTxz+fTwy, fTyz-fTwx, 1.0-(fTxx+fTyy));
588
589                        return Radian(Math::ATan2(fTxz+fTwy, 1.0-(fTxx+fTyy)));
590
591                }
592                else
593                {
594                        // internal version
595                        return Radian(Math::ASin(-2*(x*z - w*y)));
596                }
597        }
598    //-----------------------------------------------------------------------
599    Quaternion Quaternion::nlerp(Real fT, const Quaternion& rkP,
600        const Quaternion& rkQ, bool shortestPath)
601    {
602                Quaternion result;
603        Real fCos = rkP.Dot(rkQ);
604                if (fCos < 0.0f && shortestPath)
605                {
606                        result = rkP + fT * ((-rkQ) - rkP);
607                }
608                else
609                {
610                        result = rkP + fT * (rkQ - rkP);
611                }
612        result.normalise();
613        return result;
614    }
615}
Note: See TracBrowser for help on using the repository browser.