643 lines
20 KiB
C++
643 lines
20 KiB
C++
/*
|
||
-----------------------------------------------------------------------------
|
||
This source file is part of OGRE
|
||
(Object-oriented Graphics Rendering Engine)
|
||
For the latest info, see http://www.ogre3d.org/
|
||
|
||
Copyright (c) 2000-2009 Torus Knot Software Ltd
|
||
|
||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||
of this software and associated documentation files (the "Software"), to deal
|
||
in the Software without restriction, including without limitation the rights
|
||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||
copies of the Software, and to permit persons to whom the Software is
|
||
furnished to do so, subject to the following conditions:
|
||
|
||
The above copyright notice and this permission notice shall be included in
|
||
all copies or substantial portions of the Software.
|
||
|
||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||
THE SOFTWARE.
|
||
-----------------------------------------------------------------------------
|
||
*/
|
||
#include "stdneb.h"
|
||
// NOTE THAT THIS FILE IS BASED ON MATERIAL FROM:
|
||
|
||
// Geometric Tools, LLC
|
||
// Copyright (c) 1998-2010
|
||
// Distributed under the Boost Software License, Version 1.0.
|
||
// http://www.boost.org/LICENSE_1_0.txt
|
||
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
|
||
|
||
#include "OgreQuaternion.h"
|
||
|
||
#include "OgreMath.h"
|
||
#include "OgreMatrix3.h"
|
||
#include "OgreVector3.h"
|
||
|
||
namespace Ogre {
|
||
|
||
const Real Quaternion::ms_fEpsilon = Real(1e-03);
|
||
const Quaternion Quaternion::ZERO(0.0,0.0,0.0,0.0);
|
||
const Quaternion Quaternion::IDENTITY(1.0,0.0,0.0,0.0);
|
||
|
||
//-----------------------------------------------------------------------
|
||
void Quaternion::FromRotationMatrix (const Matrix3& kRot)
|
||
{
|
||
// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
|
||
// article "Quaternion Calculus and Fast Animation".
|
||
|
||
Real fTrace = kRot[0][0]+kRot[1][1]+kRot[2][2];
|
||
Real fRoot;
|
||
|
||
if ( fTrace > 0.0 )
|
||
{
|
||
// |w| > 1/2, may as well choose w > 1/2
|
||
fRoot = Math::Sqrt(fTrace + 1.0f); // 2w
|
||
w = 0.5f*fRoot;
|
||
fRoot = 0.5f/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 size_t s_iNext[3] = { 1, 2, 0 };
|
||
size_t i = 0;
|
||
if ( kRot[1][1] > kRot[0][0] )
|
||
i = 1;
|
||
if ( kRot[2][2] > kRot[i][i] )
|
||
i = 2;
|
||
size_t j = s_iNext[i];
|
||
size_t k = s_iNext[j];
|
||
|
||
fRoot = Math::Sqrt(kRot[i][i]-kRot[j][j]-kRot[k][k] + 1.0f);
|
||
Real* apkQuat[3] = { &x, &y, &z };
|
||
*apkQuat[i] = 0.5f*fRoot;
|
||
fRoot = 0.5f/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 Quaternion::ToRotationMatrix (Matrix3& kRot) const
|
||
{
|
||
Real fTx = x+x;
|
||
Real fTy = y+y;
|
||
Real fTz = z+z;
|
||
Real fTwx = fTx*w;
|
||
Real fTwy = fTy*w;
|
||
Real fTwz = fTz*w;
|
||
Real fTxx = fTx*x;
|
||
Real fTxy = fTy*x;
|
||
Real fTxz = fTz*x;
|
||
Real fTyy = fTy*y;
|
||
Real fTyz = fTz*y;
|
||
Real fTzz = fTz*z;
|
||
|
||
kRot[0][0] = 1.0f-(fTyy+fTzz);
|
||
kRot[0][1] = fTxy-fTwz;
|
||
kRot[0][2] = fTxz+fTwy;
|
||
kRot[1][0] = fTxy+fTwz;
|
||
kRot[1][1] = 1.0f-(fTxx+fTzz);
|
||
kRot[1][2] = fTyz-fTwx;
|
||
kRot[2][0] = fTxz-fTwy;
|
||
kRot[2][1] = fTyz+fTwx;
|
||
kRot[2][2] = 1.0f-(fTxx+fTyy);
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
void Quaternion::FromAngleAxis (const Radian& rfAngle,
|
||
const Vector3& 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)
|
||
|
||
Radian fHalfAngle ( 0.5*rfAngle );
|
||
Real fSin = Math::Sin(fHalfAngle);
|
||
w = Math::Cos(fHalfAngle);
|
||
x = fSin*rkAxis.x;
|
||
y = fSin*rkAxis.y;
|
||
z = fSin*rkAxis.z;
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
void Quaternion::ToAngleAxis (Radian& rfAngle, Vector3& rkAxis) const
|
||
{
|
||
// The quaternion representing the rotation is
|
||
// q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
|
||
|
||
Real fSqrLength = x*x+y*y+z*z;
|
||
if ( fSqrLength > 0.0 )
|
||
{
|
||
rfAngle = 2.0*Math::ACos(w);
|
||
Real fInvLength = Math::InvSqrt(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 = Radian(0.0);
|
||
rkAxis.x = 1.0;
|
||
rkAxis.y = 0.0;
|
||
rkAxis.z = 0.0;
|
||
}
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
void Quaternion::FromAxes (const Vector3* akAxis)
|
||
{
|
||
Matrix3 kRot;
|
||
|
||
for (size_t 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 Quaternion::FromAxes (const Vector3& xaxis, const Vector3& yaxis, const Vector3& zaxis)
|
||
{
|
||
Matrix3 kRot;
|
||
|
||
kRot[0][0] = xaxis.x;
|
||
kRot[1][0] = xaxis.y;
|
||
kRot[2][0] = xaxis.z;
|
||
|
||
kRot[0][1] = yaxis.x;
|
||
kRot[1][1] = yaxis.y;
|
||
kRot[2][1] = yaxis.z;
|
||
|
||
kRot[0][2] = zaxis.x;
|
||
kRot[1][2] = zaxis.y;
|
||
kRot[2][2] = zaxis.z;
|
||
|
||
FromRotationMatrix(kRot);
|
||
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
void Quaternion::ToAxes (Vector3* akAxis) const
|
||
{
|
||
Matrix3 kRot;
|
||
|
||
ToRotationMatrix(kRot);
|
||
|
||
for (size_t iCol = 0; iCol < 3; iCol++)
|
||
{
|
||
akAxis[iCol].x = kRot[0][iCol];
|
||
akAxis[iCol].y = kRot[1][iCol];
|
||
akAxis[iCol].z = kRot[2][iCol];
|
||
}
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Vector3 Quaternion::xAxis(void) const
|
||
{
|
||
//Real fTx = 2.0*x;
|
||
Real fTy = 2.0f*y;
|
||
Real fTz = 2.0f*z;
|
||
Real fTwy = fTy*w;
|
||
Real fTwz = fTz*w;
|
||
Real fTxy = fTy*x;
|
||
Real fTxz = fTz*x;
|
||
Real fTyy = fTy*y;
|
||
Real fTzz = fTz*z;
|
||
|
||
return Vector3(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Vector3 Quaternion::yAxis(void) const
|
||
{
|
||
Real fTx = 2.0f*x;
|
||
Real fTy = 2.0f*y;
|
||
Real fTz = 2.0f*z;
|
||
Real fTwx = fTx*w;
|
||
Real fTwz = fTz*w;
|
||
Real fTxx = fTx*x;
|
||
Real fTxy = fTy*x;
|
||
Real fTyz = fTz*y;
|
||
Real fTzz = fTz*z;
|
||
|
||
return Vector3(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Vector3 Quaternion::zAxis(void) const
|
||
{
|
||
Real fTx = 2.0f*x;
|
||
Real fTy = 2.0f*y;
|
||
Real fTz = 2.0f*z;
|
||
Real fTwx = fTx*w;
|
||
Real fTwy = fTy*w;
|
||
Real fTxx = fTx*x;
|
||
Real fTxz = fTz*x;
|
||
Real fTyy = fTy*y;
|
||
Real fTyz = fTz*y;
|
||
|
||
return Vector3(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
void Quaternion::ToAxes (Vector3& xaxis, Vector3& yaxis, Vector3& zaxis) const
|
||
{
|
||
Matrix3 kRot;
|
||
|
||
ToRotationMatrix(kRot);
|
||
|
||
xaxis.x = kRot[0][0];
|
||
xaxis.y = kRot[1][0];
|
||
xaxis.z = kRot[2][0];
|
||
|
||
yaxis.x = kRot[0][1];
|
||
yaxis.y = kRot[1][1];
|
||
yaxis.z = kRot[2][1];
|
||
|
||
zaxis.x = kRot[0][2];
|
||
zaxis.y = kRot[1][2];
|
||
zaxis.z = kRot[2][2];
|
||
}
|
||
|
||
Quaternion Quaternion::CalRollPitchYaw( float roll , float pitch , float yaw )
|
||
{
|
||
// in local coordinate, Y means up <20><> X means right <20><> Z means out of scrren
|
||
/*Vector3 axisZ( 0,0,1 ) ;
|
||
Quaternion q1 ;
|
||
q1.FromAngleAxis( Radian(roll), axisZ );
|
||
|
||
Vector3 axisX( 1 ,0 , 0 ) ;
|
||
Quaternion q2 ;
|
||
q2.FromAngleAxis( Radian(pitch), axisX ) ;
|
||
|
||
Quaternion q3 ;
|
||
Vector3 axisY( 0 , 1 , 0 ) ;
|
||
q3.FromAngleAxis( Radian(yaw), axisY ) ;*/
|
||
|
||
// return q3 * q2 * q1 ;
|
||
Matrix3 kRot;
|
||
Quaternion result;
|
||
ToRotationMatrix(kRot);
|
||
|
||
kRot.FromEulerAnglesYXZ(Radian(yaw), Radian(pitch), Radian(roll));
|
||
result.FromRotationMatrix(kRot);
|
||
return result;//q1 * q3 * q2 ;
|
||
}
|
||
|
||
void Quaternion::GetYawPitchRoll(Radian& yaw, Radian& pitch, Radian& roll) const
|
||
{
|
||
Ogre::Matrix3 kRot;
|
||
|
||
ToRotationMatrix(kRot);
|
||
kRot.ToEulerAnglesYXZ(yaw, pitch, roll);
|
||
}
|
||
|
||
//-----------------------------------------------------------------------
|
||
Quaternion Quaternion::operator+ (const Quaternion& rkQ) const
|
||
{
|
||
return Quaternion(w+rkQ.w,x+rkQ.x,y+rkQ.y,z+rkQ.z);
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Quaternion Quaternion::operator- (const Quaternion& rkQ) const
|
||
{
|
||
return Quaternion(w-rkQ.w,x-rkQ.x,y-rkQ.y,z-rkQ.z);
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Quaternion Quaternion::operator* (const Quaternion& rkQ) const
|
||
{
|
||
// NOTE: Multiplication is not generally commutative, so in most
|
||
// cases p*q != q*p.
|
||
|
||
return Quaternion
|
||
(
|
||
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
|
||
);
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Quaternion Quaternion::operator* (Real fScalar) const
|
||
{
|
||
return Quaternion(fScalar*w,fScalar*x,fScalar*y,fScalar*z);
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Quaternion operator* (Real fScalar, const Quaternion& rkQ)
|
||
{
|
||
return Quaternion(fScalar*rkQ.w,fScalar*rkQ.x,fScalar*rkQ.y,
|
||
fScalar*rkQ.z);
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Quaternion Quaternion::operator- () const
|
||
{
|
||
return Quaternion(-w,-x,-y,-z);
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Real Quaternion::Dot (const Quaternion& rkQ) const
|
||
{
|
||
return w*rkQ.w+x*rkQ.x+y*rkQ.y+z*rkQ.z;
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Real Quaternion::Norm () const
|
||
{
|
||
return w*w+x*x+y*y+z*z;
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Quaternion Quaternion::Inverse () const
|
||
{
|
||
Real fNorm = w*w+x*x+y*y+z*z;
|
||
if ( fNorm > 0.0 )
|
||
{
|
||
Real fInvNorm = 1.0f/fNorm;
|
||
return Quaternion(w*fInvNorm,-x*fInvNorm,-y*fInvNorm,-z*fInvNorm);
|
||
}
|
||
else
|
||
{
|
||
// return an invalid result to flag the error
|
||
return ZERO;
|
||
}
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Quaternion Quaternion::UnitInverse () const
|
||
{
|
||
// assert: 'this' is unit length
|
||
return Quaternion(w,-x,-y,-z);
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Quaternion Quaternion::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.
|
||
|
||
Radian fAngle ( Math::Sqrt(x*x+y*y+z*z) );
|
||
Real fSin = Math::Sin(fAngle);
|
||
|
||
Quaternion kResult;
|
||
kResult.w = Math::Cos(fAngle);
|
||
|
||
if ( Math::Abs(fSin) >= ms_fEpsilon )
|
||
{
|
||
Real fCoeff = fSin/(fAngle.valueRadians());
|
||
kResult.x = fCoeff*x;
|
||
kResult.y = fCoeff*y;
|
||
kResult.z = fCoeff*z;
|
||
}
|
||
else
|
||
{
|
||
kResult.x = x;
|
||
kResult.y = y;
|
||
kResult.z = z;
|
||
}
|
||
|
||
return kResult;
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Quaternion Quaternion::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.
|
||
|
||
Quaternion kResult;
|
||
kResult.w = 0.0;
|
||
|
||
if ( Math::Abs(w) < 1.0 )
|
||
{
|
||
Radian fAngle ( Math::ACos(w) );
|
||
Real fSin = Math::Sin(fAngle);
|
||
if ( Math::Abs(fSin) >= ms_fEpsilon )
|
||
{
|
||
Real fCoeff = fAngle.valueRadians()/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;
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Vector3 Quaternion::operator* (const Vector3& v) const
|
||
{
|
||
// nVidia SDK implementation
|
||
Vector3 uv, uuv;
|
||
Vector3 qvec(x, y, z);
|
||
uv = qvec.crossProduct(v);
|
||
uuv = qvec.crossProduct(uv);
|
||
uv *= (2.0f * w);
|
||
uuv *= 2.0f;
|
||
|
||
return v + uv + uuv;
|
||
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
bool Quaternion::equals(const Quaternion& rhs, const Radian& tolerance) const
|
||
{
|
||
Real fCos = Dot(rhs);
|
||
Radian angle = Math::ACos(fCos);
|
||
|
||
return (Math::Abs(angle.valueRadians()) <= tolerance.valueRadians())
|
||
|| Math::RealEqual(angle.valueRadians(), Math::OGRE_PI, tolerance.valueRadians());
|
||
|
||
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Quaternion Quaternion::Slerp (Real fT, const Quaternion& rkP,
|
||
const Quaternion& rkQ, bool shortestPath)
|
||
{
|
||
Real fCos = rkP.Dot(rkQ);
|
||
Quaternion rkT;
|
||
|
||
// Do we need to invert rotation?
|
||
if (fCos < 0.0f && shortestPath)
|
||
{
|
||
fCos = -fCos;
|
||
rkT = -rkQ;
|
||
}
|
||
else
|
||
{
|
||
rkT = rkQ;
|
||
}
|
||
|
||
if (Math::Abs(fCos) < 1 - ms_fEpsilon)
|
||
{
|
||
// Standard case (slerp)
|
||
Real fSin = Math::Sqrt(1 - Math::Sqr(fCos));
|
||
Radian fAngle = Math::ATan2(fSin, fCos);
|
||
Real fInvSin = 1.0f / fSin;
|
||
Real fCoeff0 = Math::Sin((1.0f - fT) * fAngle) * fInvSin;
|
||
Real fCoeff1 = Math::Sin(fT * fAngle) * fInvSin;
|
||
return fCoeff0 * rkP + fCoeff1 * rkT;
|
||
}
|
||
else
|
||
{
|
||
// There are two situations:
|
||
// 1. "rkP" and "rkQ" are very close (fCos ~= +1), so we can do a linear
|
||
// interpolation safely.
|
||
// 2. "rkP" and "rkQ" are almost inverse of each other (fCos ~= -1), there
|
||
// are an infinite number of possibilities interpolation. but we haven't
|
||
// have method to fix this case, so just use linear interpolation here.
|
||
Quaternion t = (1.0f - fT) * rkP + fT * rkT;
|
||
// taking the complement requires renormalisation
|
||
t.normalise();
|
||
return t;
|
||
}
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Quaternion Quaternion::SlerpExtraSpins (Real fT,
|
||
const Quaternion& rkP, const Quaternion& rkQ, int iExtraSpins)
|
||
{
|
||
Real fCos = rkP.Dot(rkQ);
|
||
Radian fAngle ( Math::ACos(fCos) );
|
||
|
||
if ( Math::Abs(fAngle.valueRadians()) < ms_fEpsilon )
|
||
return rkP;
|
||
|
||
Real fSin = Math::Sin(fAngle);
|
||
Radian fPhase ( Math::OGRE_PI*iExtraSpins*fT );
|
||
Real fInvSin = 1.0f/fSin;
|
||
Real fCoeff0 = Math::Sin((1.0f-fT)*fAngle - fPhase)*fInvSin;
|
||
Real fCoeff1 = Math::Sin(fT*fAngle + fPhase)*fInvSin;
|
||
return fCoeff0*rkP + fCoeff1*rkQ;
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
void Quaternion::Intermediate (const Quaternion& rkQ0,
|
||
const Quaternion& rkQ1, const Quaternion& rkQ2,
|
||
Quaternion& rkA, Quaternion& rkB)
|
||
{
|
||
// assert: q0, q1, q2 are unit quaternions
|
||
|
||
Quaternion kQ0inv = rkQ0.UnitInverse();
|
||
Quaternion kQ1inv = rkQ1.UnitInverse();
|
||
Quaternion rkP0 = kQ0inv*rkQ1;
|
||
Quaternion rkP1 = kQ1inv*rkQ2;
|
||
Quaternion kArg = 0.25*(rkP0.Log()-rkP1.Log());
|
||
Quaternion kMinusArg = -kArg;
|
||
|
||
rkA = rkQ1*kArg.Exp();
|
||
rkB = rkQ1*kMinusArg.Exp();
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Quaternion Quaternion::Squad (Real fT,
|
||
const Quaternion& rkP, const Quaternion& rkA,
|
||
const Quaternion& rkB, const Quaternion& rkQ, bool shortestPath)
|
||
{
|
||
Real fSlerpT = 2.0f*fT*(1.0f-fT);
|
||
Quaternion kSlerpP = Slerp(fT, rkP, rkQ, shortestPath);
|
||
Quaternion kSlerpQ = Slerp(fT, rkA, rkB);
|
||
return Slerp(fSlerpT, kSlerpP ,kSlerpQ);
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Real Quaternion::normalise(void)
|
||
{
|
||
Real len = Norm();
|
||
Real factor = 1.0f / Math::Sqrt(len);
|
||
*this = *this * factor;
|
||
return len;
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Radian Quaternion::getRoll(bool reprojectAxis) const
|
||
{
|
||
if (reprojectAxis)
|
||
{
|
||
// roll = atan2(localx.y, localx.x)
|
||
// pick parts of xAxis() implementation that we need
|
||
// Real fTx = 2.0*x;
|
||
Real fTy = 2.0f*y;
|
||
Real fTz = 2.0f*z;
|
||
Real fTwz = fTz*w;
|
||
Real fTxy = fTy*x;
|
||
Real fTyy = fTy*y;
|
||
Real fTzz = fTz*z;
|
||
|
||
// Vector3(1.0-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
|
||
|
||
return Radian(Math::ATan2(fTxy+fTwz, 1.0f-(fTyy+fTzz)));
|
||
|
||
}
|
||
else
|
||
{
|
||
return Radian(Math::ATan2(2*(x*y + w*z), w*w + x*x - y*y - z*z));
|
||
}
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Radian Quaternion::getPitch(bool reprojectAxis) const
|
||
{
|
||
if (reprojectAxis)
|
||
{
|
||
// pitch = atan2(localy.z, localy.y)
|
||
// pick parts of yAxis() implementation that we need
|
||
Real fTx = 2.0f*x;
|
||
// Real fTy = 2.0f*y;
|
||
Real fTz = 2.0f*z;
|
||
Real fTwx = fTx*w;
|
||
Real fTxx = fTx*x;
|
||
Real fTyz = fTz*y;
|
||
Real fTzz = fTz*z;
|
||
|
||
// Vector3(fTxy-fTwz, 1.0-(fTxx+fTzz), fTyz+fTwx);
|
||
return Radian(Math::ATan2(fTyz+fTwx, 1.0f-(fTxx+fTzz)));
|
||
}
|
||
else
|
||
{
|
||
// internal version
|
||
return Radian(Math::ATan2(2*(y*z + w*x), w*w - x*x - y*y + z*z));
|
||
}
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Radian Quaternion::getYaw(bool reprojectAxis) const
|
||
{
|
||
if (reprojectAxis)
|
||
{
|
||
// yaw = atan2(localz.x, localz.z)
|
||
// pick parts of zAxis() implementation that we need
|
||
Real fTx = 2.0f*x;
|
||
Real fTy = 2.0f*y;
|
||
Real fTz = 2.0f*z;
|
||
Real fTwy = fTy*w;
|
||
Real fTxx = fTx*x;
|
||
Real fTxz = fTz*x;
|
||
Real fTyy = fTy*y;
|
||
|
||
// Vector3(fTxz+fTwy, fTyz-fTwx, 1.0-(fTxx+fTyy));
|
||
|
||
return Radian(Math::ATan2(fTxz+fTwy, 1.0f-(fTxx+fTyy)));
|
||
|
||
}
|
||
else
|
||
{
|
||
// internal version
|
||
return Radian(Math::ASin(-2*(x*z - w*y)));
|
||
}
|
||
}
|
||
//-----------------------------------------------------------------------
|
||
Quaternion Quaternion::nlerp(Real fT, const Quaternion& rkP,
|
||
const Quaternion& rkQ, bool shortestPath)
|
||
{
|
||
Quaternion result;
|
||
Real fCos = rkP.Dot(rkQ);
|
||
if (fCos < 0.0f && shortestPath)
|
||
{
|
||
result = rkP + fT * ((-rkQ) - rkP);
|
||
}
|
||
else
|
||
{
|
||
result = rkP + fT * (rkQ - rkP);
|
||
}
|
||
result.normalise();
|
||
return result;
|
||
}
|
||
}
|