mgcquaternion.cpp

3D Game Engine Design Source Code非常棒

// Magic Software, Inc.
// http://www.magic-software.com
// Copyright (c) 2000, All Rights Reserved
//
// Source code from Magic Software is supplied under the terms of a license
// agreement and may not be copied or disclosed except in accordance with the
// terms of that agreement.  The various license agreements may be found at
// the Magic Software web site.  This file is subject to the license
//
// FREE SOURCE CODE
// http://www.magic-software.com/License/free.pdf

#include "MgcQuaternion.h"

MgcReal MgcQuaternion::ms_fEpsilon = 1e-03;
MgcQuaternion MgcQuaternion::ZERO(0.0,0.0,0.0,0.0);
MgcQuaternion MgcQuaternion::IDENTITY(1.0,0.0,0.0,0.0);

//---------------------------------------------------------------------------
MgcQuaternion::MgcQuaternion (MgcReal fW, MgcReal fX, MgcReal fY, MgcReal fZ)
{
    w = fW;
    x = fX;
    y = fY;
    z = fZ;
}
//---------------------------------------------------------------------------
MgcQuaternion::MgcQuaternion (const MgcQuaternion& rkQ)
{
    w = rkQ.w;
    x = rkQ.x;
    y = rkQ.y;
    z = rkQ.z;
}
//---------------------------------------------------------------------------
void MgcQuaternion::FromRotationMatrix (const MgcMatrix3& kRot)
{
    // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
    // article "Quaternion Calculus and Fast Animation".

    MgcReal fTrace = kRot[0][0]+kRot[1][1]+kRot[2][2];
    MgcReal fRoot;

    if ( fTrace > 0.0 )
    {
        // |w| > 1/2, may as well choose w > 1/2
        fRoot = MgcMath::Sqrt(fTrace + 1.0);  // 2w
        w = 0.5*fRoot;
        fRoot = 0.5/fRoot;  // 1/(4w)
        x = (kRot[2][1]-kRot[1][2])*fRoot;
        y = (kRot[0][2]-kRot[2][0])*fRoot;
        z = (kRot[1][0]-kRot[0][1])*fRoot;
    }
    else
    {
        // |w| <= 1/2
        static int s_iNext[3] = { 1, 2, 0 };
        int i = 0;
        if ( kRot[1][1] > kRot[0][0] )
            i = 1;
        if ( kRot[2][2] > kRot[i][i] )
            i = 2;
        int j = s_iNext[i];
        int k = s_iNext[j];

        fRoot = MgcMath::Sqrt(kRot[i][i]-kRot[j][j]-kRot[k][k] + 1.0);
        MgcReal* apkQuat[3] = { &x, &y, &z };
        *apkQuat[i] = 0.5*fRoot;
        fRoot = 0.5/fRoot;
        w = (kRot[k][j]-kRot[j][k])*fRoot;
        *apkQuat[j] = (kRot[j][i]+kRot[i][j])*fRoot;
        *apkQuat[k] = (kRot[k][i]+kRot[i][k])*fRoot;
    }
}
//---------------------------------------------------------------------------
void MgcQuaternion::ToRotationMatrix (MgcMatrix3& kRot) const
{
    MgcReal fTx  = 2.0*x;
    MgcReal fTy  = 2.0*y;
    MgcReal fTz  = 2.0*z;
    MgcReal fTwx = fTx*w;
    MgcReal fTwy = fTy*w;
    MgcReal fTwz = fTz*w;
    MgcReal fTxx = fTx*x;
    MgcReal fTxy = fTy*x;
    MgcReal fTxz = fTz*x;
    MgcReal fTyy = fTy*y;
    MgcReal fTyz = fTz*y;
    MgcReal fTzz = fTz*z;

    kRot[0][0] = 1.0-(fTyy+fTzz);
    kRot[0][1] = fTxy-fTwz;
    kRot[0][2] = fTxz+fTwy;
    kRot[1][0] = fTxy+fTwz;
    kRot[1][1] = 1.0-(fTxx+fTzz);
    kRot[1][2] = fTyz-fTwx;
    kRot[2][0] = fTxz-fTwy;
    kRot[2][1] = fTyz+fTwx;
    kRot[2][2] = 1.0-(fTxx+fTyy);
}
//---------------------------------------------------------------------------
void MgcQuaternion::FromAngleAxis (const MgcReal& rfAngle,
    const MgcVector3& rkAxis)
{
    // assert:  axis[] is unit length
    //
	// The quaternion representing the rotation is
	//   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

    MgcReal fHalfAngle = 0.5*rfAngle;
    MgcReal fSin = MgcMath::Sin(fHalfAngle);
    w = MgcMath::Cos(fHalfAngle);
    x = fSin*rkAxis.x;
    y = fSin*rkAxis.y;
    z = fSin*rkAxis.z;
}
//---------------------------------------------------------------------------
void MgcQuaternion::ToAngleAxis (MgcReal& rfAngle, MgcVector3& rkAxis) const
{
	// The quaternion representing the rotation is
	//   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

    MgcReal fSqrLength = x*x+y*y+z*z;
    if ( fSqrLength > 0.0 )
    {
        rfAngle = 2.0*MgcMath::ACos(w);
        MgcReal fInvLength = 1.0/MgcMath::Sqrt(fSqrLength);
        rkAxis.x = x*fInvLength;
        rkAxis.y = y*fInvLength;
        rkAxis.z = z*fInvLength;
    }
    else
    {
        // angle is 0 (mod 2*pi), so any axis will do
        rfAngle = 0.0;
        rkAxis.x = 1.0;
        rkAxis.y = 0.0;
        rkAxis.z = 0.0;
    }
}
//---------------------------------------------------------------------------
void MgcQuaternion::FromAxes (const MgcVector3* akAxis)
{
    MgcMatrix3 kRot;

    for (int iCol = 0; iCol < 3; iCol++)
    {
        kRot[0][iCol] = akAxis[iCol].x;
        kRot[1][iCol] = akAxis[iCol].y;
        kRot[2][iCol] = akAxis[iCol].z;
    }

    FromRotationMatrix(kRot);
}
//---------------------------------------------------------------------------
void MgcQuaternion::ToAxes (MgcVector3* akAxis) const
{
    MgcMatrix3 kRot;

    ToRotationMatrix(kRot);

    for (int iCol = 0; iCol < 3; iCol++)
    {
        akAxis[iCol].x = kRot[0][iCol];
        akAxis[iCol].y = kRot[1][iCol];
        akAxis[iCol].z = kRot[2][iCol];
    }
}
//---------------------------------------------------------------------------
MgcQuaternion& MgcQuaternion::operator= (const MgcQuaternion& rkQ)
{
    w = rkQ.w;
    x = rkQ.x;
    y = rkQ.y;
    z = rkQ.z;
    return *this;
}
//---------------------------------------------------------------------------
MgcQuaternion MgcQuaternion::operator+ (const MgcQuaternion& rkQ) const
{
    return MgcQuaternion(w+rkQ.w,x+rkQ.x,y+rkQ.y,z+rkQ.z);
}
//---------------------------------------------------------------------------
MgcQuaternion MgcQuaternion::operator- (const MgcQuaternion& rkQ) const
{
    return MgcQuaternion(w-rkQ.w,x-rkQ.x,y-rkQ.y,z-rkQ.z);
}
//---------------------------------------------------------------------------
MgcQuaternion MgcQuaternion::operator* (const MgcQuaternion& rkQ) const
{
    // NOTE:  Multiplication is not generally commutative, so in most
    // cases p*q != q*p.

    return MgcQuaternion
    (
		w*rkQ.w-x*rkQ.x-y*rkQ.y-z*rkQ.z,
		w*rkQ.x+x*rkQ.w+y*rkQ.z-z*rkQ.y,
		w*rkQ.y+y*rkQ.w+z*rkQ.x-x*rkQ.z,
		w*rkQ.z+z*rkQ.w+x*rkQ.y-y*rkQ.x
    );
}
//---------------------------------------------------------------------------
MgcQuaternion MgcQuaternion::operator* (MgcReal fScalar) const
{
    return MgcQuaternion(fScalar*w,fScalar*x,fScalar*y,fScalar*z);
}
//---------------------------------------------------------------------------
MgcQuaternion operator* (MgcReal fScalar, const MgcQuaternion& rkQ)
{
    return MgcQuaternion(fScalar*rkQ.w,fScalar*rkQ.x,fScalar*rkQ.y,
        fScalar*rkQ.z);
}
//---------------------------------------------------------------------------
MgcQuaternion MgcQuaternion::operator- () const
{
    return MgcQuaternion(-w,-x,-y,-z);
}
//---------------------------------------------------------------------------
MgcReal MgcQuaternion::Dot (const MgcQuaternion& rkQ) const
{
    return w*rkQ.w+x*rkQ.x+y*rkQ.y+z*rkQ.z;
}
//---------------------------------------------------------------------------
MgcReal MgcQuaternion::Norm () const
{
    return w*w+x*x+y*y+z*z;
}
//---------------------------------------------------------------------------
MgcQuaternion MgcQuaternion::Inverse () const
{
    MgcReal fNorm = w*w+x*x+y*y+z*z;
    if ( fNorm > 0.0 )
    {
        MgcReal fInvNorm = 1.0/fNorm;
        return MgcQuaternion(w*fInvNorm,-x*fInvNorm,-y*fInvNorm,-z*fInvNorm);
    }
    else
    {
        // return an invalid result to flag the error
        return ZERO;
    }
}
//---------------------------------------------------------------------------
MgcQuaternion MgcQuaternion::UnitInverse () const
{
    // assert:  'this' is unit length
    return MgcQuaternion(w,-x,-y,-z);
}
//---------------------------------------------------------------------------
MgcQuaternion MgcQuaternion::Exp () const
{
    // If q = A*(x*i+y*j+z*k) where (x,y,z) is unit length, then
    // exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k).  If sin(A) is near zero,
    // use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1.

    MgcReal fAngle = MgcMath::Sqrt(x*x+y*y+z*z);
    MgcReal fSin = MgcMath::Sin(fAngle);

    MgcQuaternion kResult;
    kResult.w = MgcMath::Cos(fAngle);

    if ( MgcMath::Abs(fSin) >= ms_fEpsilon )
    {
        MgcReal fCoeff = fSin/fAngle;
        kResult.x = fCoeff*x;
        kResult.y = fCoeff*y;
        kResult.z = fCoeff*z;
    }
    else
    {
        kResult.x = x;
        kResult.y = y;
        kResult.z = z;
    }

    return kResult;
}
//---------------------------------------------------------------------------
MgcQuaternion MgcQuaternion::Log () const
{
    // If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x,y,z) is unit length, then
    // log(q) = A*(x*i+y*j+z*k).  If sin(A) is near zero, use log(q) =
    // sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.

    MgcQuaternion kResult;
    kResult.w = 0.0;

    if ( MgcMath::Abs(w) < 1.0 )
    {
        MgcReal fAngle = MgcMath::ACos(w);
        MgcReal fSin = MgcMath::Sin(fAngle);
        if ( MgcMath::Abs(fSin) >= ms_fEpsilon )
        {
            MgcReal fCoeff = fAngle/fSin;
            kResult.x = fCoeff*x;
            kResult.y = fCoeff*y;
            kResult.z = fCoeff*z;
            return kResult;
        }
    }

    kResult.x = x;
    kResult.y = y;
    kResult.z = z;

    return kResult;
}
//---------------------------------------------------------------------------
MgcVector3 MgcQuaternion::operator* (const MgcVector3& rkVector) const
{
    // Given a vector u = (x0,y0,z0) and a unit length quaternion
    // q = <w,x,y,z>, the vector v = (x1,y1,z1) which represents the
    // rotation of u by q is v = q*u*q^{-1} where * indicates quaternion
    // multiplication and where u is treated as the quaternion <0,x0,y0,z0>.
    // Note that q^{-1} = <w,-x,-y,-z>, so no real work is required to
    // invert q.  Now
    //
    //   q*u*q^{-1} = q*<0,x0,y0,z0>*q^{-1}
    //     = q*(x0*i+y0*j+z0*k)*q^{-1}
    //     = x0*(q*i*q^{-1})+y0*(q*j*q^{-1})+z0*(q*k*q^{-1})
    //
    // As 3-vectors, q*i*q^{-1}, q*j*q^{-1}, and 2*k*q^{-1} are the columns
    // of the rotation matrix computed in MgcQuaternion::ToRotationMatrix.
    // The vector v is obtained as the product of that rotation matrix with
    // vector u.  As such, the quaternion representation of a rotation
    // matrix requires less space than the matrix and more time to compute
    // the rotated vector.  Typical space-time tradeoff...

    MgcMatrix3 kRot;
    ToRotationMatrix(kRot);
    return kRot*rkVector;
}
//---------------------------------------------------------------------------
MgcQuaternion MgcQuaternion::Slerp (MgcReal fT, const MgcQuaternion& rkP,
    const MgcQuaternion& rkQ)
{
    MgcReal fCos = rkP.Dot(rkQ);
    MgcReal fAngle = MgcMath::ACos(fCos);

    if ( MgcMath::Abs(fAngle) < ms_fEpsilon )
        return rkP;

    MgcReal fSin = MgcMath::Sin(fAngle);
    MgcReal fInvSin = 1.0/fSin;
    MgcReal fCoeff0 = MgcMath::Sin((1.0-fT)*fAngle)*fInvSin;
    MgcReal fCoeff1 = MgcMath::Sin(fT*fAngle)*fInvSin;
    return fCoeff0*rkP + fCoeff1*rkQ;
}
//---------------------------------------------------------------------------
MgcQuaternion MgcQuaternion::SlerpExtraSpins (MgcReal fT,
    const MgcQuaternion& rkP, const MgcQuaternion& rkQ, int iExtraSpins)
{
    MgcReal fCos = rkP.Dot(rkQ);
    MgcReal fAngle = MgcMath::ACos(fCos);

    if ( MgcMath::Abs(fAngle) < ms_fEpsilon )
        return rkP;

    MgcReal fSin = MgcMath::Sin(fAngle);
    MgcReal fPhase = MgcMath::PI*iExtraSpins*fT;
    MgcReal fInvSin = 1.0/fSin;
    MgcReal fCoeff0 = MgcMath::Sin((1.0-fT)*fAngle - fPhase)*fInvSin;
    MgcReal fCoeff1 = MgcMath::Sin(fT*fAngle + fPhase)*fInvSin;
    return fCoeff0*rkP + fCoeff1*rkQ;
}
//---------------------------------------------------------------------------
void MgcQuaternion::Intermediate (const MgcQuaternion& rkQ0,
    const MgcQuaternion& rkQ1, const MgcQuaternion& rkQ2,
    MgcQuaternion& rkA, MgcQuaternion& rkB)
{
    // assert:  q0, q1, q2 are unit quaternions

    MgcQuaternion kQ0inv = rkQ0.UnitInverse();
    MgcQuaternion kQ1inv = rkQ1.UnitInverse();
    MgcQuaternion rkP0 = kQ0inv*rkQ1;
    MgcQuaternion rkP1 = kQ1inv*rkQ2;
    MgcQuaternion kArg = 0.25*(rkP0.Log()-rkP1.Log());
    MgcQuaternion kMinusArg = -kArg;

    rkA = rkQ1*kArg.Exp();
    rkB = rkQ1*kMinusArg.Exp();
}
//---------------------------------------------------------------------------
MgcQuaternion MgcQuaternion::Squad (MgcReal fT,
    const MgcQuaternion& rkP, const MgcQuaternion& rkA,
    const MgcQuaternion& rkB, const MgcQuaternion& rkQ)
{
    MgcReal fSlerpT = 2.0*fT*(1.0-fT);
    MgcQuaternion kSlerpP = Slerp(fT,rkP,rkQ);
    MgcQuaternion kSlerpQ = Slerp(fT,rkA,rkB);
    return Slerp(fSlerpT,kSlerpP,kSlerpQ);
}
//---------------------------------------------------------------------------