mirror of
https://github.com/Mauler125/r5sdk.git
synced 2025-02-09 19:15:03 +01:00
1265 lines
38 KiB
C++
1265 lines
38 KiB
C++
//===== Copyright <20> 1996-2005, Valve Corporation, All rights reserved. ======//
|
||
//
|
||
// Purpose: - defines SIMD "structure of arrays" classes and functions.
|
||
//
|
||
//===========================================================================//
|
||
#ifndef SSEQUATMATH_H
|
||
#define SSEQUATMATH_H
|
||
|
||
#ifdef _WIN32
|
||
#pragma once
|
||
#endif
|
||
|
||
|
||
#include "mathlib/ssemath.h"
|
||
|
||
// Use this #define to allow SSE versions of Quaternion math
|
||
// to exist on PC.
|
||
// On PC, certain horizontal vector operations are not supported.
|
||
// This causes the SSE implementation of quaternion math to mix the
|
||
// vector and scalar floating point units, which is extremely
|
||
// performance negative if you don't compile to native SSE2 (which
|
||
// we don't as of Sept 1, 2007). So, it's best not to allow these
|
||
// functions to exist at all. It's not good enough to simply replace
|
||
// the contents of the functions with scalar math, because each call
|
||
// to LoadAligned and StoreAligned will result in an unnecessary copy
|
||
// of the quaternion, and several moves to and from the XMM registers.
|
||
//
|
||
// Basically, the problem you run into is that for efficient SIMD code,
|
||
// you need to load the quaternions and vectors into SIMD registers and
|
||
// keep them there as long as possible while doing only SIMD math,
|
||
// whereas for efficient scalar code, each time you copy onto or ever
|
||
// use a fltx4, it hoses your pipeline. So the difference has to be
|
||
// in the management of temporary variables in the calling function,
|
||
// not inside the math functions.
|
||
//
|
||
// If you compile assuming the presence of SSE2, the MSVC will abandon
|
||
// the traditional x87 FPU operations altogether and make everything use
|
||
// the SSE2 registers, which lessens this problem a little.
|
||
|
||
// permitted only on 360, as we've done careful tuning on its Altivec math.
|
||
// FourQuaternions, however, are always allowed, because vertical ops are
|
||
// fine on SSE.
|
||
#ifdef PLATFORM_PPC
|
||
#define ALLOW_SIMD_QUATERNION_MATH 1 // not on PC!
|
||
#endif
|
||
|
||
|
||
|
||
//---------------------------------------------------------------------
|
||
// Load/store quaternions
|
||
//---------------------------------------------------------------------
|
||
#ifndef _X360
|
||
// Using STDC or SSE
|
||
FORCEINLINE fltx4 LoadAlignedSIMD(const QuaternionAligned& pSIMD)
|
||
{
|
||
fltx4 retval = LoadAlignedSIMD(pSIMD.Base());
|
||
return retval;
|
||
}
|
||
|
||
FORCEINLINE fltx4 LoadAlignedSIMD(const QuaternionAligned* RESTRICT pSIMD)
|
||
{
|
||
fltx4 retval = LoadAlignedSIMD(pSIMD->Base());
|
||
return retval;
|
||
}
|
||
|
||
FORCEINLINE void StoreAlignedSIMD(QuaternionAligned* RESTRICT pSIMD, const fltx4& a)
|
||
{
|
||
StoreAlignedSIMD(pSIMD->Base(), a);
|
||
}
|
||
#else
|
||
|
||
// for the transitional class -- load a QuaternionAligned
|
||
FORCEINLINE fltx4 LoadAlignedSIMD(const QuaternionAligned& pSIMD)
|
||
{
|
||
fltx4 retval = XMLoadVector4A(pSIMD.Base());
|
||
return retval;
|
||
}
|
||
|
||
FORCEINLINE fltx4 LoadAlignedSIMD(const QuaternionAligned* RESTRICT pSIMD)
|
||
{
|
||
fltx4 retval = XMLoadVector4A(pSIMD);
|
||
return retval;
|
||
}
|
||
|
||
FORCEINLINE void StoreAlignedSIMD(QuaternionAligned* RESTRICT pSIMD, const fltx4& a)
|
||
{
|
||
XMStoreVector4A(pSIMD->Base(), a);
|
||
}
|
||
|
||
// From a RadianEuler packed onto a fltx4, to a quaternion
|
||
fltx4 AngleQuaternionSIMD(FLTX4 vAngles);
|
||
|
||
#endif
|
||
|
||
|
||
#if ALLOW_SIMD_QUATERNION_MATH
|
||
//---------------------------------------------------------------------
|
||
// Make sure quaternions are within 180 degrees of one another, if not, reverse q
|
||
//---------------------------------------------------------------------
|
||
FORCEINLINE fltx4 QuaternionAlignSIMD(const fltx4& p, const fltx4& q)
|
||
{
|
||
// decide if one of the quaternions is backwards
|
||
fltx4 a = SubSIMD(p, q);
|
||
fltx4 b = AddSIMD(p, q);
|
||
a = Dot4SIMD(a, a);
|
||
b = Dot4SIMD(b, b);
|
||
fltx4 cmp = (fltx4)CmpGtSIMD(a, b);
|
||
fltx4 result = MaskedAssign(cmp, NegSIMD(q), q);
|
||
return result;
|
||
}
|
||
|
||
//---------------------------------------------------------------------
|
||
// Normalize Quaternion
|
||
//---------------------------------------------------------------------
|
||
#if USE_STDC_FOR_SIMD
|
||
|
||
FORCEINLINE fltx4 QuaternionNormalizeSIMD(const fltx4& q)
|
||
{
|
||
fltx4 radius, result;
|
||
radius = Dot4SIMD(q, q);
|
||
|
||
if (SubFloat(radius, 0)) // > FLT_EPSILON && ((radius < 1.0f - 4*FLT_EPSILON) || (radius > 1.0f + 4*FLT_EPSILON))
|
||
{
|
||
float iradius = 1.0f / sqrt(SubFloat(radius, 0));
|
||
result = ReplicateX4(iradius);
|
||
result = MulSIMD(result, q);
|
||
return result;
|
||
}
|
||
return q;
|
||
}
|
||
|
||
#else
|
||
|
||
// SSE + X360 implementation
|
||
FORCEINLINE fltx4 QuaternionNormalizeSIMD(const fltx4& q)
|
||
{
|
||
fltx4 radius, result, mask;
|
||
radius = Dot4SIMD(q, q);
|
||
mask = (fltx4)CmpEqSIMD(radius, Four_Zeros); // all ones iff radius = 0
|
||
result = ReciprocalSqrtSIMD(radius);
|
||
result = MulSIMD(result, q);
|
||
return MaskedAssign(mask, q, result); // if radius was 0, just return q
|
||
}
|
||
|
||
#endif
|
||
|
||
|
||
//---------------------------------------------------------------------
|
||
// 0.0 returns p, 1.0 return q.
|
||
//---------------------------------------------------------------------
|
||
FORCEINLINE fltx4 QuaternionBlendNoAlignSIMD(const fltx4& p, const fltx4& q, float t)
|
||
{
|
||
fltx4 sclp, sclq, result;
|
||
sclq = ReplicateX4(t);
|
||
sclp = SubSIMD(Four_Ones, sclq);
|
||
result = MulSIMD(sclp, p);
|
||
result = MaddSIMD(sclq, q, result);
|
||
return QuaternionNormalizeSIMD(result);
|
||
}
|
||
|
||
|
||
//---------------------------------------------------------------------
|
||
// Blend Quaternions
|
||
//---------------------------------------------------------------------
|
||
FORCEINLINE fltx4 QuaternionBlendSIMD(const fltx4& p, const fltx4& q, float t)
|
||
{
|
||
// decide if one of the quaternions is backwards
|
||
fltx4 q2, result;
|
||
q2 = QuaternionAlignSIMD(p, q);
|
||
result = QuaternionBlendNoAlignSIMD(p, q2, t);
|
||
return result;
|
||
}
|
||
|
||
|
||
//---------------------------------------------------------------------
|
||
// Multiply Quaternions
|
||
//---------------------------------------------------------------------
|
||
#ifndef _X360
|
||
|
||
// SSE and STDC
|
||
FORCEINLINE fltx4 QuaternionMultSIMD(const fltx4& p, const fltx4& q)
|
||
{
|
||
// decide if one of the quaternions is backwards
|
||
fltx4 q2, result;
|
||
q2 = QuaternionAlignSIMD(p, q);
|
||
SubFloat(result, 0) = SubFloat(p, 0) * SubFloat(q2, 3) + SubFloat(p, 1) * SubFloat(q2, 2) - SubFloat(p, 2) * SubFloat(q2, 1) + SubFloat(p, 3) * SubFloat(q2, 0);
|
||
SubFloat(result, 1) = -SubFloat(p, 0) * SubFloat(q2, 2) + SubFloat(p, 1) * SubFloat(q2, 3) + SubFloat(p, 2) * SubFloat(q2, 0) + SubFloat(p, 3) * SubFloat(q2, 1);
|
||
SubFloat(result, 2) = SubFloat(p, 0) * SubFloat(q2, 1) - SubFloat(p, 1) * SubFloat(q2, 0) + SubFloat(p, 2) * SubFloat(q2, 3) + SubFloat(p, 3) * SubFloat(q2, 2);
|
||
SubFloat(result, 3) = -SubFloat(p, 0) * SubFloat(q2, 0) - SubFloat(p, 1) * SubFloat(q2, 1) - SubFloat(p, 2) * SubFloat(q2, 2) + SubFloat(p, 3) * SubFloat(q2, 3);
|
||
return result;
|
||
}
|
||
|
||
#else
|
||
|
||
// X360
|
||
extern const fltx4 g_QuatMultRowSign[4];
|
||
FORCEINLINE fltx4 QuaternionMultSIMD(const fltx4& p, const fltx4& q)
|
||
{
|
||
fltx4 q2, row, result;
|
||
q2 = QuaternionAlignSIMD(p, q);
|
||
|
||
row = XMVectorSwizzle(q2, 3, 2, 1, 0);
|
||
row = MulSIMD(row, g_QuatMultRowSign[0]);
|
||
result = Dot4SIMD(row, p);
|
||
|
||
row = XMVectorSwizzle(q2, 2, 3, 0, 1);
|
||
row = MulSIMD(row, g_QuatMultRowSign[1]);
|
||
row = Dot4SIMD(row, p);
|
||
result = __vrlimi(result, row, 4, 0);
|
||
|
||
row = XMVectorSwizzle(q2, 1, 0, 3, 2);
|
||
row = MulSIMD(row, g_QuatMultRowSign[2]);
|
||
row = Dot4SIMD(row, p);
|
||
result = __vrlimi(result, row, 2, 0);
|
||
|
||
row = MulSIMD(q2, g_QuatMultRowSign[3]);
|
||
row = Dot4SIMD(row, p);
|
||
result = __vrlimi(result, row, 1, 0);
|
||
return result;
|
||
}
|
||
|
||
#endif
|
||
|
||
|
||
//---------------------------------------------------------------------
|
||
// Quaternion scale
|
||
//---------------------------------------------------------------------
|
||
#ifdef _X360
|
||
|
||
// X360
|
||
FORCEINLINE fltx4 QuaternionScaleSIMD(const fltx4& p, float t)
|
||
{
|
||
fltx4 sinom = Dot3SIMD(p, p);
|
||
sinom = SqrtSIMD(sinom);
|
||
sinom = MinSIMD(sinom, Four_Ones);
|
||
fltx4 sinsom = ArcSinSIMD(sinom);
|
||
fltx4 t4 = ReplicateX4(t);
|
||
sinsom = MulSIMD(sinsom, t4);
|
||
sinsom = SinSIMD(sinsom);
|
||
sinom = AddSIMD(sinom, Four_Epsilons);
|
||
sinom = ReciprocalSIMD(sinom);
|
||
t4 = MulSIMD(sinsom, sinom);
|
||
fltx4 result = MulSIMD(p, t4);
|
||
|
||
// rescale rotation
|
||
sinsom = MulSIMD(sinsom, sinsom);
|
||
fltx4 r = SubSIMD(Four_Ones, sinsom);
|
||
r = MaxSIMD(r, Four_Zeros);
|
||
r = SqrtSIMD(r);
|
||
|
||
// keep sign of rotation
|
||
fltx4 cmp = CmpGeSIMD(p, Four_Zeros);
|
||
r = MaskedAssign(cmp, r, NegSIMD(r));
|
||
|
||
result = __vrlimi(result, r, 1, 0);
|
||
return result;
|
||
}
|
||
|
||
// X360
|
||
// assumes t4 contains a float replicated to each slot
|
||
FORCEINLINE fltx4 QuaternionScaleSIMD(const fltx4& p, const fltx4& t4)
|
||
{
|
||
fltx4 sinom = Dot3SIMD(p, p);
|
||
sinom = SqrtSIMD(sinom);
|
||
sinom = MinSIMD(sinom, Four_Ones);
|
||
fltx4 sinsom = ArcSinSIMD(sinom);
|
||
sinsom = MulSIMD(sinsom, t4);
|
||
sinsom = SinSIMD(sinsom);
|
||
sinom = AddSIMD(sinom, Four_Epsilons);
|
||
sinom = ReciprocalSIMD(sinom);
|
||
fltx4 result = MulSIMD(p, MulSIMD(sinsom, sinom));
|
||
|
||
// rescale rotation
|
||
sinsom = MulSIMD(sinsom, sinsom);
|
||
fltx4 r = SubSIMD(Four_Ones, sinsom);
|
||
r = MaxSIMD(r, Four_Zeros);
|
||
r = SqrtSIMD(r);
|
||
|
||
// keep sign of rotation
|
||
fltx4 cmp = CmpGeSIMD(p, Four_Zeros);
|
||
r = MaskedAssign(cmp, r, NegSIMD(r));
|
||
|
||
result = __vrlimi(result, r, 1, 0);
|
||
return result;
|
||
}
|
||
|
||
#elif defined(_PS3)
|
||
|
||
// X360
|
||
FORCEINLINE fltx4 QuaternionScaleSIMD(const fltx4& p, float t)
|
||
{
|
||
fltx4 sinom = Dot3SIMD(p, p);
|
||
sinom = SqrtSIMD(sinom);
|
||
sinom = MinSIMD(sinom, Four_Ones);
|
||
fltx4 sinsom = ArcSinSIMD(sinom);
|
||
fltx4 t4 = ReplicateX4(t);
|
||
sinsom = MulSIMD(sinsom, t4);
|
||
sinsom = SinSIMD(sinsom);
|
||
sinom = AddSIMD(sinom, Four_Epsilons);
|
||
sinom = ReciprocalSIMD(sinom);
|
||
t4 = MulSIMD(sinsom, sinom);
|
||
fltx4 result = MulSIMD(p, t4);
|
||
|
||
// rescale rotation
|
||
sinsom = MulSIMD(sinsom, sinsom);
|
||
fltx4 r = SubSIMD(Four_Ones, sinsom);
|
||
r = MaxSIMD(r, Four_Zeros);
|
||
r = SqrtSIMD(r);
|
||
|
||
// keep sign of rotation
|
||
r = MaskedAssign(CmpGeSIMD(p, Four_Zeros), r, NegSIMD(r));
|
||
// set just the w component of result
|
||
result = MaskedAssign(LoadAlignedSIMD(g_SIMD_ComponentMask[3]), r, result);
|
||
|
||
return result;
|
||
}
|
||
|
||
// X360
|
||
// assumes t4 contains a float replicated to each slot
|
||
FORCEINLINE fltx4 QuaternionScaleSIMD(const fltx4& p, const fltx4& t4)
|
||
{
|
||
fltx4 sinom = Dot3SIMD(p, p);
|
||
sinom = SqrtSIMD(sinom);
|
||
sinom = MinSIMD(sinom, Four_Ones);
|
||
fltx4 sinsom = ArcSinSIMD(sinom);
|
||
sinsom = MulSIMD(sinsom, t4);
|
||
sinsom = SinSIMD(sinsom);
|
||
sinom = AddSIMD(sinom, Four_Epsilons);
|
||
sinom = ReciprocalSIMD(sinom);
|
||
fltx4 result = MulSIMD(p, MulSIMD(sinsom, sinom));
|
||
|
||
// rescale rotation
|
||
sinsom = MulSIMD(sinsom, sinsom);
|
||
fltx4 r = SubSIMD(Four_Ones, sinsom);
|
||
r = MaxSIMD(r, Four_Zeros);
|
||
r = SqrtSIMD(r);
|
||
|
||
// keep sign of rotation
|
||
r = MaskedAssign(CmpGeSIMD(p, Four_Zeros), r, NegSIMD(r));
|
||
// set just the w component of result
|
||
result = MaskedAssign(LoadAlignedSIMD(g_SIMD_ComponentMask[3]), r, result);
|
||
|
||
return result;
|
||
}
|
||
|
||
#else
|
||
|
||
// SSE and STDC
|
||
FORCEINLINE fltx4 QuaternionScaleSIMD(const fltx4& p, float t)
|
||
{
|
||
float r;
|
||
fltx4 q;
|
||
|
||
// FIXME: nick, this isn't overly sensitive to accuracy, and it may be faster to
|
||
// use the cos part (w) of the quaternion (sin(omega)*N,cos(omega)) to figure the new scale.
|
||
float sinom = sqrt(SubFloat(p, 0) * SubFloat(p, 0) + SubFloat(p, 1) * SubFloat(p, 1) + SubFloat(p, 2) * SubFloat(p, 2));
|
||
sinom = fmin(sinom, 1.f);
|
||
|
||
float sinsom = sin(asin(sinom) * t);
|
||
|
||
t = sinsom / (sinom + FLT_EPSILON);
|
||
SubFloat(q, 0) = t * SubFloat(p, 0);
|
||
SubFloat(q, 1) = t * SubFloat(p, 1);
|
||
SubFloat(q, 2) = t * SubFloat(p, 2);
|
||
|
||
// rescale rotation
|
||
r = 1.0f - sinsom * sinsom;
|
||
|
||
// Assert( r >= 0 );
|
||
if (r < 0.0f)
|
||
r = 0.0f;
|
||
r = sqrt(r);
|
||
|
||
// keep sign of rotation
|
||
SubFloat(q, 3) = fsel(SubFloat(p, 3), r, -r);
|
||
return q;
|
||
}
|
||
|
||
#endif
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Quaternion spherical linear interpolation
|
||
//-----------------------------------------------------------------------------
|
||
#ifndef _X360
|
||
|
||
// SSE and STDC
|
||
FORCEINLINE fltx4 QuaternionSlerpNoAlignSIMD(const fltx4& p, const fltx4& q, float t)
|
||
{
|
||
float omega, cosom, sinom, sclp, sclq;
|
||
|
||
fltx4 result;
|
||
|
||
// 0.0 returns p, 1.0 return q.
|
||
cosom = SubFloat(p, 0) * SubFloat(q, 0) + SubFloat(p, 1) * SubFloat(q, 1) +
|
||
SubFloat(p, 2) * SubFloat(q, 2) + SubFloat(p, 3) * SubFloat(q, 3);
|
||
|
||
if ((1.0f + cosom) > 0.000001f)
|
||
{
|
||
if ((1.0f - cosom) > 0.000001f)
|
||
{
|
||
omega = acos(cosom);
|
||
sinom = sin(omega);
|
||
sclp = sin((1.0f - t) * omega) / sinom;
|
||
sclq = sin(t * omega) / sinom;
|
||
}
|
||
else
|
||
{
|
||
// TODO: add short circuit for cosom == 1.0f?
|
||
sclp = 1.0f - t;
|
||
sclq = t;
|
||
}
|
||
SubFloat(result, 0) = sclp * SubFloat(p, 0) + sclq * SubFloat(q, 0);
|
||
SubFloat(result, 1) = sclp * SubFloat(p, 1) + sclq * SubFloat(q, 1);
|
||
SubFloat(result, 2) = sclp * SubFloat(p, 2) + sclq * SubFloat(q, 2);
|
||
SubFloat(result, 3) = sclp * SubFloat(p, 3) + sclq * SubFloat(q, 3);
|
||
}
|
||
else
|
||
{
|
||
SubFloat(result, 0) = -SubFloat(q, 1);
|
||
SubFloat(result, 1) = SubFloat(q, 0);
|
||
SubFloat(result, 2) = -SubFloat(q, 3);
|
||
SubFloat(result, 3) = SubFloat(q, 2);
|
||
sclp = sin((1.0f - t) * (0.5f * M_PI));
|
||
sclq = sin(t * (0.5f * M_PI));
|
||
SubFloat(result, 0) = sclp * SubFloat(p, 0) + sclq * SubFloat(result, 0);
|
||
SubFloat(result, 1) = sclp * SubFloat(p, 1) + sclq * SubFloat(result, 1);
|
||
SubFloat(result, 2) = sclp * SubFloat(p, 2) + sclq * SubFloat(result, 2);
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
#else
|
||
|
||
// X360
|
||
FORCEINLINE fltx4 QuaternionSlerpNoAlignSIMD(const fltx4& p, const fltx4& q, float t)
|
||
{
|
||
return XMQuaternionSlerp(p, q, t);
|
||
}
|
||
|
||
#endif
|
||
|
||
|
||
FORCEINLINE fltx4 QuaternionSlerpSIMD(const fltx4& p, const fltx4& q, float t)
|
||
{
|
||
fltx4 q2, result;
|
||
q2 = QuaternionAlignSIMD(p, q);
|
||
result = QuaternionSlerpNoAlignSIMD(p, q2, t);
|
||
return result;
|
||
}
|
||
|
||
|
||
#endif // ALLOW_SIMD_QUATERNION_MATH
|
||
|
||
|
||
/// class FourVectors stores 4 independent vectors for use in SIMD processing. These vectors are
|
||
/// stored in the format x x x x y y y y z z z z so that they can be efficiently SIMD-accelerated.
|
||
class ALIGN16 FourQuaternions
|
||
{
|
||
public:
|
||
fltx4 x, y, z, w;
|
||
|
||
FourQuaternions(void)
|
||
{
|
||
}
|
||
|
||
FourQuaternions(const fltx4& _x,
|
||
const fltx4& _y,
|
||
const fltx4& _z,
|
||
const fltx4& _w)
|
||
: x(_x), y(_y), z(_z), w(_w)
|
||
{}
|
||
|
||
#if !defined(__SPU__)
|
||
// four rotations around the same axis. angles should be in radians.
|
||
FourQuaternions(const fltx4& axis,
|
||
const float& angle0, const float& angle1, const float& angle2, const float& angle3)
|
||
{
|
||
FromAxisAndAngles(axis, angle0, angle1, angle2, angle3);
|
||
}
|
||
#endif
|
||
|
||
FourQuaternions(FourQuaternions const& src)
|
||
{
|
||
x = src.x;
|
||
y = src.y;
|
||
z = src.z;
|
||
w = src.w;
|
||
}
|
||
|
||
FORCEINLINE void operator=(FourQuaternions const& src)
|
||
{
|
||
x = src.x;
|
||
y = src.y;
|
||
z = src.z;
|
||
w = src.w;
|
||
}
|
||
|
||
/// this = this * q;
|
||
FORCEINLINE FourQuaternions Mul(FourQuaternions const& q) const;
|
||
|
||
/// negate the vector part
|
||
FORCEINLINE FourQuaternions Conjugate() const;
|
||
|
||
/// for a quaternion representing a rotation of angle theta, return
|
||
/// one of angle s*theta
|
||
/// scale is four floats -- one for each quat
|
||
FORCEINLINE FourQuaternions ScaleAngle(const fltx4& scale) const;
|
||
|
||
/// ret = this * ( s * q )
|
||
/// In other words, for a quaternion representing a rotation of angle theta, return
|
||
/// one of angle s*theta
|
||
/// s is four floats in a fltx4 -- one for each quaternion
|
||
FORCEINLINE FourQuaternions MulAc(const fltx4& s, const FourQuaternions& q) const;
|
||
|
||
/// ret = ( s * this ) * q
|
||
FORCEINLINE FourQuaternions ScaleMul(const fltx4& s, const FourQuaternions& q) const;
|
||
|
||
/// Slerp four quaternions at once, FROM me TO the specified out.
|
||
FORCEINLINE FourQuaternions Slerp(const FourQuaternions& to, const fltx4& t);
|
||
|
||
FORCEINLINE FourQuaternions SlerpNoAlign(const FourQuaternions& originalto, const fltx4& t);
|
||
|
||
#if !defined(__SPU__)
|
||
/// given an axis and four angles, populate this quaternion with the equivalent rotations
|
||
/// (ie, make these four quaternions represent four different rotations around the same axis)
|
||
/// angles should be in RADIANS
|
||
FORCEINLINE FourQuaternions& FromAxisAndAngles(const fltx4& axis,
|
||
const float& angle0, const float& angle1, const float& angle2, const float& angle3);
|
||
FORCEINLINE FourQuaternions& FromAxisAndAngles(const fltx4& axis, const fltx4& angles);
|
||
// one convenience imp if you're doing this in degrees
|
||
FORCEINLINE FourQuaternions& FromAxisAndAnglesInDegrees(const fltx4& axis, const fltx4& angles)
|
||
{
|
||
return FromAxisAndAngles(axis, MulSIMD(angles, Four_DegToRad));
|
||
}
|
||
#endif
|
||
|
||
// rotate (in place) a FourVectors by this quaternion. there's a corresponding RotateBy in FourVectors.
|
||
FORCEINLINE void RotateFourVectors(FourVectors* RESTRICT vecs) const RESTRICT;
|
||
|
||
|
||
/// LoadAndSwizzleAligned - load 4 QuaternionAligneds into a FourQuaternions, performing transpose op.
|
||
/// all 4 vectors must be 128 bit boundary
|
||
FORCEINLINE void LoadAndSwizzleAligned(const float* RESTRICT a, const float* RESTRICT b, const float* RESTRICT c, const float* RESTRICT d)
|
||
{
|
||
#if defined( _X360 )
|
||
fltx4 tx = LoadAlignedSIMD(a);
|
||
fltx4 ty = LoadAlignedSIMD(b);
|
||
fltx4 tz = LoadAlignedSIMD(c);
|
||
fltx4 tw = LoadAlignedSIMD(d);
|
||
fltx4 r0 = __vmrghw(tx, tz);
|
||
fltx4 r1 = __vmrghw(ty, tw);
|
||
fltx4 r2 = __vmrglw(tx, tz);
|
||
fltx4 r3 = __vmrglw(ty, tw);
|
||
|
||
x = __vmrghw(r0, r1);
|
||
y = __vmrglw(r0, r1);
|
||
z = __vmrghw(r2, r3);
|
||
w = __vmrglw(r2, r3);
|
||
#else
|
||
x = LoadAlignedSIMD(a);
|
||
y = LoadAlignedSIMD(b);
|
||
z = LoadAlignedSIMD(c);
|
||
w = LoadAlignedSIMD(d);
|
||
// now, matrix is:
|
||
// x y z w
|
||
// x y z w
|
||
// x y z w
|
||
// x y z w
|
||
TransposeSIMD(x, y, z, w);
|
||
#endif
|
||
}
|
||
|
||
FORCEINLINE void LoadAndSwizzleAligned(const QuaternionAligned* RESTRICT a,
|
||
const QuaternionAligned* RESTRICT b,
|
||
const QuaternionAligned* RESTRICT c,
|
||
const QuaternionAligned* RESTRICT d)
|
||
{
|
||
LoadAndSwizzleAligned(a->Base(), b->Base(), c->Base(), d->Base());
|
||
}
|
||
|
||
|
||
/// LoadAndSwizzleAligned - load 4 consecutive QuaternionAligneds into a FourQuaternions,
|
||
/// performing transpose op.
|
||
/// all 4 vectors must be 128 bit boundary
|
||
FORCEINLINE void LoadAndSwizzleAligned(const QuaternionAligned* qs)
|
||
{
|
||
#if defined( _X360 )
|
||
fltx4 tx = LoadAlignedSIMD(qs++);
|
||
fltx4 ty = LoadAlignedSIMD(qs++);
|
||
fltx4 tz = LoadAlignedSIMD(qs++);
|
||
fltx4 tw = LoadAlignedSIMD(qs);
|
||
fltx4 r0 = __vmrghw(tx, tz);
|
||
fltx4 r1 = __vmrghw(ty, tw);
|
||
fltx4 r2 = __vmrglw(tx, tz);
|
||
fltx4 r3 = __vmrglw(ty, tw);
|
||
|
||
x = __vmrghw(r0, r1);
|
||
y = __vmrglw(r0, r1);
|
||
z = __vmrghw(r2, r3);
|
||
w = __vmrglw(r2, r3);
|
||
#else
|
||
x = LoadAlignedSIMD(qs++);
|
||
y = LoadAlignedSIMD(qs++);
|
||
z = LoadAlignedSIMD(qs++);
|
||
w = LoadAlignedSIMD(qs++);
|
||
// now, matrix is:
|
||
// x y z w
|
||
// x y z w
|
||
// x y z w
|
||
// x y z w
|
||
TransposeSIMD(x, y, z, w);
|
||
#endif
|
||
}
|
||
|
||
// Store the FourQuaternions out to four nonconsecutive ordinary quaternions in memory.
|
||
FORCEINLINE void SwizzleAndStoreAligned(QuaternionAligned* a, QuaternionAligned* b, QuaternionAligned* c, QuaternionAligned* d)
|
||
{
|
||
#if defined( _X360 )
|
||
fltx4 r0 = __vmrghw(x, z);
|
||
fltx4 r1 = __vmrghw(y, w);
|
||
fltx4 r2 = __vmrglw(x, z);
|
||
fltx4 r3 = __vmrglw(y, w);
|
||
|
||
fltx4 rx = __vmrghw(r0, r1);
|
||
fltx4 ry = __vmrglw(r0, r1);
|
||
fltx4 rz = __vmrghw(r2, r3);
|
||
fltx4 rw = __vmrglw(r2, r3);
|
||
|
||
StoreAlignedSIMD(a, rx);
|
||
StoreAlignedSIMD(b, ry);
|
||
StoreAlignedSIMD(c, rz);
|
||
StoreAlignedSIMD(d, rw);
|
||
#else
|
||
fltx4 dupes[4] = { x, y, z, w };
|
||
TransposeSIMD(dupes[0], dupes[1], dupes[2], dupes[3]);
|
||
StoreAlignedSIMD(a, dupes[0]);
|
||
StoreAlignedSIMD(b, dupes[1]);
|
||
StoreAlignedSIMD(c, dupes[2]);
|
||
StoreAlignedSIMD(d, dupes[3]);
|
||
#endif
|
||
}
|
||
|
||
// Store the FourQuaternions out to four consecutive ordinary quaternions in memory.
|
||
FORCEINLINE void SwizzleAndStoreAligned(QuaternionAligned* qs)
|
||
{
|
||
#if defined( _X360 )
|
||
fltx4 r0 = __vmrghw(x, z);
|
||
fltx4 r1 = __vmrghw(y, w);
|
||
fltx4 r2 = __vmrglw(x, z);
|
||
fltx4 r3 = __vmrglw(y, w);
|
||
|
||
fltx4 rx = __vmrghw(r0, r1);
|
||
fltx4 ry = __vmrglw(r0, r1);
|
||
fltx4 rz = __vmrghw(r2, r3);
|
||
fltx4 rw = __vmrglw(r2, r3);
|
||
|
||
StoreAlignedSIMD(qs, rx);
|
||
StoreAlignedSIMD(++qs, ry);
|
||
StoreAlignedSIMD(++qs, rz);
|
||
StoreAlignedSIMD(++qs, rw);
|
||
#else
|
||
SwizzleAndStoreAligned(qs, qs + 1, qs + 2, qs + 3);
|
||
#endif
|
||
}
|
||
|
||
// Store the FourQuaternions out to four consecutive ordinary quaternions in memory.
|
||
// The mask specifies which of the quaternions are actually written out -- each
|
||
// word in the fltx4 should be all binary ones or zeros. Ones means the corresponding
|
||
// quat will be written.
|
||
FORCEINLINE void SwizzleAndStoreAlignedMasked(QuaternionAligned* RESTRICT qs, const bi32x4& controlMask)
|
||
{
|
||
fltx4 originals[4];
|
||
originals[0] = LoadAlignedSIMD(qs);
|
||
originals[1] = LoadAlignedSIMD(qs + 1);
|
||
originals[2] = LoadAlignedSIMD(qs + 2);
|
||
originals[3] = LoadAlignedSIMD(qs + 3);
|
||
|
||
bi32x4 masks[4] = { SplatXSIMD(controlMask),
|
||
SplatYSIMD(controlMask),
|
||
SplatZSIMD(controlMask),
|
||
SplatWSIMD(controlMask) };
|
||
|
||
#if defined( _X360 )
|
||
fltx4 r0 = __vmrghw(x, z);
|
||
fltx4 r1 = __vmrghw(y, w);
|
||
fltx4 r2 = __vmrglw(x, z);
|
||
fltx4 r3 = __vmrglw(y, w);
|
||
|
||
fltx4 rx = __vmrghw(r0, r1);
|
||
fltx4 ry = __vmrglw(r0, r1);
|
||
fltx4 rz = __vmrghw(r2, r3);
|
||
fltx4 rw = __vmrglw(r2, r3);
|
||
#else
|
||
fltx4 rx = x;
|
||
fltx4 ry = y;
|
||
fltx4 rz = z;
|
||
fltx4 rw = w;
|
||
TransposeSIMD(rx, ry, rz, rw);
|
||
#endif
|
||
|
||
StoreAlignedSIMD(qs + 0, MaskedAssign(masks[0], rx, originals[0]));
|
||
StoreAlignedSIMD(qs + 1, MaskedAssign(masks[1], ry, originals[1]));
|
||
StoreAlignedSIMD(qs + 2, MaskedAssign(masks[2], rz, originals[2]));
|
||
StoreAlignedSIMD(qs + 3, MaskedAssign(masks[3], rw, originals[3]));
|
||
}
|
||
};
|
||
|
||
|
||
|
||
FORCEINLINE FourQuaternions FourQuaternions::Conjugate() const
|
||
{
|
||
return FourQuaternions(NegSIMD(x), NegSIMD(y), NegSIMD(z), w);
|
||
}
|
||
|
||
|
||
|
||
|
||
FORCEINLINE const fltx4 Dot(const FourQuaternions& a, const FourQuaternions& b)
|
||
{
|
||
return
|
||
MaddSIMD(a.x, b.x,
|
||
MaddSIMD(a.y, b.y,
|
||
MaddSIMD(a.z, b.z, MulSIMD(a.w, b.w))
|
||
)
|
||
);
|
||
}
|
||
|
||
|
||
FORCEINLINE const FourQuaternions Madd(const FourQuaternions& a, const fltx4& scale, const FourQuaternions& c)
|
||
{
|
||
FourQuaternions ret;
|
||
ret.x = MaddSIMD(a.x, scale, c.x);
|
||
ret.y = MaddSIMD(a.y, scale, c.y);
|
||
ret.z = MaddSIMD(a.z, scale, c.z);
|
||
ret.w = MaddSIMD(a.w, scale, c.w);
|
||
return ret;
|
||
}
|
||
|
||
FORCEINLINE const FourQuaternions Mul(const FourQuaternions& a, const fltx4& scale)
|
||
{
|
||
FourQuaternions ret;
|
||
ret.x = MulSIMD(a.x, scale);
|
||
ret.y = MulSIMD(a.y, scale);
|
||
ret.z = MulSIMD(a.z, scale);
|
||
ret.w = MulSIMD(a.w, scale);
|
||
return ret;
|
||
}
|
||
|
||
FORCEINLINE const FourQuaternions Add(const FourQuaternions& a, const FourQuaternions& b)
|
||
{
|
||
FourQuaternions ret;
|
||
ret.x = AddSIMD(a.x, b.x);
|
||
ret.y = AddSIMD(a.y, b.y);
|
||
ret.z = AddSIMD(a.z, b.z);
|
||
ret.w = AddSIMD(a.w, b.w);
|
||
return ret;
|
||
}
|
||
|
||
FORCEINLINE const FourQuaternions Sub(const FourQuaternions& a, const FourQuaternions& b)
|
||
{
|
||
FourQuaternions ret;
|
||
ret.x = SubSIMD(a.x, b.x);
|
||
ret.y = SubSIMD(a.y, b.y);
|
||
ret.z = SubSIMD(a.z, b.z);
|
||
ret.w = SubSIMD(a.w, b.w);
|
||
return ret;
|
||
}
|
||
|
||
FORCEINLINE const FourQuaternions Neg(const FourQuaternions& q)
|
||
{
|
||
FourQuaternions ret;
|
||
ret.x = NegSIMD(q.x);
|
||
ret.y = NegSIMD(q.y);
|
||
ret.z = NegSIMD(q.z);
|
||
ret.w = NegSIMD(q.w);
|
||
return ret;
|
||
}
|
||
|
||
FORCEINLINE const FourQuaternions MaskedAssign(const bi32x4& mask, const FourQuaternions& a, const FourQuaternions& b)
|
||
{
|
||
FourQuaternions ret;
|
||
ret.x = MaskedAssign(mask, a.x, b.x);
|
||
ret.y = MaskedAssign(mask, a.y, b.y);
|
||
ret.z = MaskedAssign(mask, a.z, b.z);
|
||
ret.w = MaskedAssign(mask, a.w, b.w);
|
||
return ret;
|
||
}
|
||
|
||
#ifdef DIFFERENT_NATIVE_VECTOR_TYPES
|
||
FORCEINLINE const FourQuaternions MaskedAssign(const fltx4& mask, const FourQuaternions& a, const FourQuaternions& b)
|
||
{
|
||
return MaskedAssign((bi32x4)mask, a, b);
|
||
}
|
||
#endif
|
||
|
||
|
||
FORCEINLINE FourQuaternions QuaternionAlign(const FourQuaternions& p, const FourQuaternions& q)
|
||
{
|
||
// decide if one of the quaternions is backwards
|
||
bi32x4 cmp = CmpLtSIMD(Dot(p, q), Four_Zeros);
|
||
return MaskedAssign(cmp, Neg(q), q);
|
||
}
|
||
|
||
|
||
FORCEINLINE const FourQuaternions QuaternionNormalize(const FourQuaternions& q)
|
||
{
|
||
fltx4 radius = Dot(q, q);
|
||
bi32x4 mask = CmpEqSIMD(radius, Four_Zeros); // all ones iff radius = 0
|
||
fltx4 invRadius = ReciprocalSqrtSIMD(radius);
|
||
|
||
FourQuaternions ret = MaskedAssign(mask, q, Mul(q, invRadius));
|
||
return ret;
|
||
}
|
||
|
||
|
||
#if !defined(__SPU__)
|
||
FORCEINLINE FourQuaternions& FourQuaternions::FromAxisAndAngles(const fltx4& axis,
|
||
const float& angle0, const float& angle1, const float& angle2, const float& angle3)
|
||
{
|
||
return FromAxisAndAngles(axis, LoadGatherSIMD(angle0, angle1, angle2, angle3));
|
||
}
|
||
|
||
FORCEINLINE FourQuaternions& FourQuaternions::FromAxisAndAngles(const fltx4& axis,
|
||
const fltx4& angles)
|
||
{
|
||
// compute the half theta
|
||
fltx4 theta = MulSIMD(angles, Four_PointFives);
|
||
// compute the sine and cosine of each angle simultaneously
|
||
fltx4 vsines; fltx4 vcoses;
|
||
SinCosSIMD(vsines, vcoses, theta);
|
||
// now the sines and coses vectors contain the results for four angles.
|
||
// for each of the angles, splat them out and then swizzle together so
|
||
// as to get a < cos, sin, sin, sin > coefficient vector
|
||
|
||
x = MulSIMD(vsines, SplatXSIMD(axis)); // sin(t0) * x, sin(t1) * x, etc
|
||
y = MulSIMD(vsines, SplatYSIMD(axis));
|
||
z = MulSIMD(vsines, SplatZSIMD(axis));
|
||
w = vcoses;
|
||
|
||
|
||
return *this;
|
||
}
|
||
#endif
|
||
|
||
|
||
/// this = this * q;
|
||
FORCEINLINE FourQuaternions FourQuaternions::Mul(FourQuaternions const& q) const
|
||
{
|
||
// W = w1w2 - x1x2 - y1y2 - z1z2
|
||
FourQuaternions ret;
|
||
fltx4 signMask = LoadAlignedSIMD((float*)g_SIMD_signmask);
|
||
// as we do the multiplication, also do a dot product, so we know whether
|
||
// one of the quats is backwards and if we therefore have to negate at the end
|
||
fltx4 dotProduct = MulSIMD(w, q.w);
|
||
|
||
ret.w = MulSIMD(w, q.w); // W = w1w2
|
||
ret.x = MulSIMD(w, q.x); // X = w1x2
|
||
ret.y = MulSIMD(w, q.y); // Y = w1y2
|
||
ret.z = MulSIMD(w, q.z); // Z = w1z2
|
||
|
||
dotProduct = MaddSIMD(x, q.x, dotProduct);
|
||
ret.w = MsubSIMD(x, q.x, ret.w); // W = w1w2 - x1x2
|
||
ret.x = MaddSIMD(x, q.w, ret.x); // X = w1x2 + x1w2
|
||
ret.y = MsubSIMD(x, q.z, ret.y); // Y = w1y2 - x1z2
|
||
ret.z = MaddSIMD(x, q.y, ret.z); // Z = w1z2 + x1y2
|
||
|
||
dotProduct = MaddSIMD(y, q.y, dotProduct);
|
||
ret.w = MsubSIMD(y, q.y, ret.w); // W = w1w2 - x1x2 - y1y2
|
||
ret.x = MaddSIMD(y, q.z, ret.x); // X = w1x2 + x1w2 + y1z2
|
||
ret.y = MaddSIMD(y, q.w, ret.y); // Y = w1y2 - x1z2 + y1w2
|
||
ret.z = MsubSIMD(y, q.x, ret.z); // Z = w1z2 + x1y2 - y1x2
|
||
|
||
dotProduct = MaddSIMD(z, q.z, dotProduct);
|
||
ret.w = MsubSIMD(z, q.z, ret.w); // W = w1w2 - x1x2 - y1y2 - z1z2
|
||
ret.x = MsubSIMD(z, q.y, ret.x); // X = w1x2 + x1w2 + y1z2 - z1y2
|
||
ret.y = MaddSIMD(z, q.x, ret.y); // Y = w1y2 - x1z2 + y1w2 + z1x2
|
||
ret.z = MaddSIMD(z, q.w, ret.z); // Z = w1z2 + x1y2 - y1x2 + z1w2
|
||
|
||
fltx4 Zero = Four_Zeros;
|
||
bi32x4 control = CmpLtSIMD(dotProduct, Four_Zeros);
|
||
signMask = MaskedAssign(control, signMask, Zero); // negate quats where q1.q2 < 0
|
||
ret.w = XorSIMD(signMask, ret.w);
|
||
ret.x = XorSIMD(signMask, ret.x);
|
||
ret.y = XorSIMD(signMask, ret.y);
|
||
ret.z = XorSIMD(signMask, ret.z);
|
||
|
||
return ret;
|
||
}
|
||
|
||
|
||
FORCEINLINE void FourQuaternions::RotateFourVectors(FourVectors* RESTRICT vecs) const RESTRICT
|
||
{
|
||
fltx4 tmpX, tmpY, tmpZ, tmpW;
|
||
fltx4 outX, outY, outZ;
|
||
|
||
tmpX = SubSIMD(MaddSIMD(w, vecs->x, MulSIMD(y, vecs->z)),
|
||
MulSIMD(z, vecs->y));
|
||
|
||
tmpY = SubSIMD(MaddSIMD(w, vecs->y, MulSIMD(z, vecs->x)),
|
||
MulSIMD(x, vecs->z));
|
||
|
||
tmpZ = SubSIMD(MaddSIMD(w, vecs->z, MulSIMD(x, vecs->y)),
|
||
MulSIMD(y, vecs->x));
|
||
|
||
tmpW = AddSIMD(MaddSIMD(x, vecs->x, MulSIMD(y, vecs->y)),
|
||
MulSIMD(z, vecs->z));
|
||
|
||
|
||
outX = AddSIMD(SubSIMD(MaddSIMD(tmpW, x, MulSIMD(tmpX, w)),
|
||
MulSIMD(tmpY, z)),
|
||
MulSIMD(tmpZ, y));
|
||
|
||
outY = AddSIMD(SubSIMD(MaddSIMD(tmpW, y, MulSIMD(tmpY, w)),
|
||
MulSIMD(tmpZ, x)),
|
||
MulSIMD(tmpX, z));
|
||
|
||
outZ = AddSIMD(SubSIMD(MaddSIMD(tmpW, z, MulSIMD(tmpZ, w)),
|
||
MulSIMD(tmpX, y)),
|
||
MulSIMD(tmpY, x));
|
||
|
||
// although apparently redundant, assigning the results to intermediate local variables
|
||
// seems to improve code scheduling slightly in SN.
|
||
vecs->x = outX;
|
||
vecs->y = outY;
|
||
vecs->z = outZ;
|
||
}
|
||
|
||
|
||
/*
|
||
|
||
void QuaternionScale( const Quaternion &p, float t, Quaternion &q )
|
||
{
|
||
Assert( s_bMathlibInitialized );
|
||
|
||
|
||
float r;
|
||
|
||
// FIXME: nick, this isn't overly sensitive to accuracy, and it may be faster to
|
||
// use the cos part (w) of the quaternion (sin(omega)*N,cos(omega)) to figure the new scale.
|
||
float sinom = sqrt( DotProduct( &p.x, &p.x ) );
|
||
sinom = min( sinom, 1.f );
|
||
|
||
float sinsom = sin( asin( sinom ) * t );
|
||
|
||
t = sinsom / (sinom + FLT_EPSILON);
|
||
VectorScale( &p.x, t, &q.x );
|
||
|
||
// rescale rotation
|
||
r = 1.0f - sinsom * sinsom;
|
||
|
||
// Assert( r >= 0 );
|
||
if (r < 0.0f)
|
||
r = 0.0f;
|
||
r = sqrt( r );
|
||
|
||
// keep sign of rotation
|
||
if (p.w < 0)
|
||
q.w = -r;
|
||
else
|
||
q.w = r;
|
||
|
||
Assert( q.IsValid() );
|
||
|
||
return;
|
||
}
|
||
|
||
*/
|
||
|
||
FORCEINLINE FourQuaternions FourQuaternions::ScaleAngle(const fltx4& scale) const
|
||
{
|
||
FourQuaternions ret;
|
||
static const fltx4 OneMinusEpsilon = { 1.0f - 0.000001f, 1.0f - 0.000001f, 1.0f - 0.000001f, 1.0f - 0.000001f };
|
||
const fltx4 Zero = Four_Zeros;
|
||
fltx4 signMask = LoadAlignedSIMD((float*)g_SIMD_signmask);
|
||
// work out if there are any tiny scales or angles, which are unstable
|
||
bi32x4 tinyAngles = CmpGtSIMD(w, OneMinusEpsilon);
|
||
bi32x4 negativeRotations = CmpLtSIMD(w, Zero); // if any w's are <0, we will need to negate later down
|
||
|
||
// figure out the theta
|
||
fltx4 angles = ArcCosSIMD(w);
|
||
|
||
// test also if w > -1
|
||
fltx4 negativeWs = XorSIMD(signMask, w);
|
||
tinyAngles = OrSIMD(CmpGtSIMD(negativeWs, OneMinusEpsilon), tinyAngles);
|
||
|
||
// meanwhile start working on computing the dot product of the
|
||
// vector component, and trust in the scheduler to interleave them
|
||
fltx4 vLenSq = MulSIMD(x, x);
|
||
vLenSq = MaddSIMD(y, y, vLenSq);
|
||
vLenSq = MaddSIMD(z, z, vLenSq);
|
||
|
||
// scale the angles
|
||
angles = MulSIMD(angles, scale);
|
||
|
||
// clear out the sign mask where w>=0
|
||
signMask = MaskedAssign(negativeRotations, signMask, Zero);
|
||
|
||
// work out the new w component and vector length
|
||
fltx4 vLenRecip = ReciprocalSqrtSIMD(vLenSq); // interleave with Cos to hide latencies
|
||
fltx4 sine;
|
||
SinCosSIMD(sine, ret.w, angles);
|
||
ret.x = MulSIMD(x, vLenRecip); // renormalize so the vector length + w = 1
|
||
ret.y = MulSIMD(y, vLenRecip); // renormalize so the vector length + w = 1
|
||
ret.z = MulSIMD(z, vLenRecip); // renormalize so the vector length + w = 1
|
||
ret.x = MulSIMD(ret.x, sine);
|
||
ret.y = MulSIMD(ret.y, sine);
|
||
ret.z = MulSIMD(ret.z, sine);
|
||
|
||
// negate where necessary
|
||
ret.x = XorSIMD(ret.x, signMask);
|
||
ret.y = XorSIMD(ret.y, signMask);
|
||
ret.z = XorSIMD(ret.z, signMask);
|
||
ret.w = XorSIMD(ret.w, signMask);
|
||
|
||
// finally, toss results from where cos(theta) is close to 1 -- these are non rotations.
|
||
ret.x = MaskedAssign(tinyAngles, x, ret.x);
|
||
ret.y = MaskedAssign(tinyAngles, y, ret.y);
|
||
ret.z = MaskedAssign(tinyAngles, z, ret.z);
|
||
ret.w = MaskedAssign(tinyAngles, w, ret.w);
|
||
|
||
return ret;
|
||
}
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: return = this * ( s * q )
|
||
// In other words, for a quaternion representing a rotation of angle theta, return
|
||
// one of angle s*theta
|
||
// s is four floats in a fltx4 -- one for each quaternion
|
||
//-----------------------------------------------------------------------------
|
||
|
||
FORCEINLINE FourQuaternions FourQuaternions::MulAc(const fltx4& s, const FourQuaternions& q) const
|
||
{
|
||
/*
|
||
void QuaternionMA( const Quaternion &p, float s, const Quaternion &q, Quaternion &qt )
|
||
{
|
||
Quaternion p1, q1;
|
||
|
||
QuaternionScale( q, s, q1 );
|
||
QuaternionMult( p, q1, p1 );
|
||
QuaternionNormalize( p1 );
|
||
qt[0] = p1[0];
|
||
qt[1] = p1[1];
|
||
qt[2] = p1[2];
|
||
qt[3] = p1[3];
|
||
}
|
||
*/
|
||
|
||
return Mul(q.ScaleAngle(s));
|
||
}
|
||
|
||
|
||
FORCEINLINE FourQuaternions FourQuaternions::ScaleMul(const fltx4& s, const FourQuaternions& q) const
|
||
{
|
||
return ScaleAngle(s).Mul(q);
|
||
}
|
||
|
||
|
||
FORCEINLINE FourQuaternions FourQuaternions::Slerp(const FourQuaternions& originalto, const fltx4& t)
|
||
{
|
||
FourQuaternions ret;
|
||
static const fltx4 OneMinusEpsilon = { 1.0f - 0.000001f, 1.0f - 0.000001f, 1.0f - 0.000001f, 1.0f - 0.000001f };
|
||
|
||
// align if necessary.
|
||
|
||
// actually, before we even do that, start by computing the dot product of
|
||
// the quaternions. it has lots of dependent ops and we can sneak it into
|
||
// the pipeline bubbles as we figure out alignment. Of course we don't know
|
||
// yet if we need to realign, so compute them both -- there's plenty of
|
||
// space in the bubbles. They're roomy, those bubbles.
|
||
fltx4 cosineOmega;
|
||
#if 0 // Maybe I don't need to do alignment separately, using the xb360 technique...
|
||
FourQuaternions to;
|
||
{
|
||
fltx4 diffs[4], sums[4], originalToNeg[4];
|
||
fltx4 dotIfAligned, dotIfNotAligned;
|
||
|
||
// compute negations of the TO quaternion.
|
||
originalToNeg[0] = NegSIMD(originalto.x);
|
||
originalToNeg[1] = NegSIMD(originalto.y);
|
||
originalToNeg[2] = NegSIMD(originalto.z);
|
||
originalToNeg[3] = NegSIMD(originalto.w);
|
||
|
||
dotIfAligned = MulSIMD(x, originalto.x);
|
||
dotIfNotAligned = MulSIMD(x, originalToNeg[0]);
|
||
|
||
diffs[0] = SubSIMD(x, originalto.x);
|
||
diffs[1] = SubSIMD(y, originalto.y);
|
||
diffs[2] = SubSIMD(z, originalto.z);
|
||
diffs[3] = SubSIMD(w, originalto.w);
|
||
|
||
sums[0] = AddSIMD(x, originalto.x);
|
||
sums[1] = AddSIMD(y, originalto.y);
|
||
sums[2] = AddSIMD(z, originalto.z);
|
||
sums[3] = AddSIMD(w, originalto.w);
|
||
|
||
dotIfAligned = MaddSIMD(y, originalto.y, dotIfAligned);
|
||
dotIfNotAligned = MaddSIMD(y, originalToNeg[1], dotIfNotAligned);
|
||
|
||
fltx4 diffsDot, sumsDot;
|
||
|
||
diffsDot = MulSIMD(diffs[0], diffs[0]); // x^2
|
||
sumsDot = MulSIMD(sums[0], sums[0]); // x^2
|
||
// do some work on the dot products while letting the multiplies cook
|
||
dotIfAligned = MaddSIMD(z, originalto.z, dotIfAligned);
|
||
dotIfNotAligned = MaddSIMD(z, originalToNeg[2], dotIfNotAligned);
|
||
|
||
diffsDot = MaddSIMD(diffs[1], diffs[1], diffsDot); // x^2 + y^2
|
||
sumsDot = MaddSIMD(sums[1], sums[1], sumsDot);
|
||
diffsDot = MaddSIMD(diffs[2], diffs[2], diffsDot); // x^2 + y^2 + z^2
|
||
sumsDot = MaddSIMD(sums[2], sums[2], sumsDot);
|
||
diffsDot = MaddSIMD(diffs[3], diffs[3], diffsDot); // x^2 + y^2 + z^2 + w^2
|
||
sumsDot = MaddSIMD(sums[3], sums[3], sumsDot);
|
||
// do some work on the dot products while letting the multiplies cook
|
||
dotIfAligned = MaddSIMD(w, originalto.w, dotIfAligned);
|
||
dotIfNotAligned = MaddSIMD(w, originalToNeg[3], dotIfNotAligned);
|
||
|
||
// are the differences greater than the sums?
|
||
// if so, we need to negate that quaternion
|
||
fltx4 mask = CmpGtSIMD(diffsDot, sumsDot); // 1 for diffs>0 and 0 elsewhere
|
||
to.x = MaskedAssign(mask, originalToNeg[0], originalto.x);
|
||
to.y = MaskedAssign(mask, originalToNeg[1], originalto.y);
|
||
to.z = MaskedAssign(mask, originalToNeg[2], originalto.z);
|
||
to.w = MaskedAssign(mask, originalToNeg[3], originalto.w);
|
||
|
||
cosineOmega = MaskedAssign(mask, dotIfNotAligned, dotIfAligned);
|
||
}
|
||
|
||
// right, now to is aligned to be the short way round, and we computed
|
||
// the dot product while we were figuring all that out.
|
||
#else
|
||
const FourQuaternions& to = originalto;
|
||
cosineOmega = MulSIMD(x, to.x);
|
||
cosineOmega = MaddSIMD(y, to.y, cosineOmega);
|
||
cosineOmega = MaddSIMD(z, to.z, cosineOmega);
|
||
cosineOmega = MaddSIMD(w, to.w, cosineOmega);
|
||
#endif
|
||
|
||
fltx4 Zero = Four_Zeros;
|
||
bi32x4 cosOmegaLessThanZero = CmpLtSIMD(cosineOmega, Zero);
|
||
// fltx4 shouldNegate = MaskedAssign(cosOmegaLessThanZero, Four_NegativeOnes , Four_Ones );
|
||
fltx4 signMask = LoadAlignedSIMD((float*)g_SIMD_signmask); // contains a one in the sign bit -- xor against a number to negate it
|
||
fltx4 sinOmega = Four_Ones;
|
||
|
||
// negate cosineOmega where necessary
|
||
cosineOmega = MaskedAssign(cosOmegaLessThanZero, XorSIMD(cosineOmega, signMask), cosineOmega);
|
||
fltx4 oneMinusT = SubSIMD(Four_Ones, t);
|
||
bi32x4 bCosOmegaLessThanOne = CmpLtSIMD(cosineOmega, OneMinusEpsilon); // we'll use this to mask out null slerps
|
||
|
||
// figure out the sin component of the diff quaternion.
|
||
// since sin^2(t) + cos^2(t) = 1...
|
||
sinOmega = MsubSIMD(cosineOmega, cosineOmega, sinOmega); // = 1 - cos^2(t) = sin^2(t)
|
||
fltx4 invSinOmega = ReciprocalSqrtSIMD(sinOmega); // 1/sin(t)
|
||
sinOmega = MulSIMD(sinOmega, invSinOmega); // = sin^2(t) / sin(t) = sin(t)
|
||
|
||
// use the arctangent technique to work out omega from tan^-1(sin/cos)
|
||
fltx4 omega = ArcTan2SIMD(sinOmega, cosineOmega);
|
||
|
||
// alpha = sin(omega * (1-T))/sin(omega)
|
||
// beta = sin(omega * T)/sin(omega)
|
||
fltx4 alpha = MulSIMD(omega, oneMinusT); // w(1-T)
|
||
fltx4 beta = MulSIMD(omega, t); // w(T)
|
||
signMask = MaskedAssign(cosOmegaLessThanZero, signMask, Zero);
|
||
|
||
alpha = SinSIMD(alpha); // sin(w(1-T))
|
||
beta = SinSIMD(beta); // sin(wT)
|
||
|
||
alpha = MulSIMD(alpha, invSinOmega);
|
||
beta = MulSIMD(beta, invSinOmega);
|
||
|
||
// depending on whether the dot product was less than zero, negate beta, or not
|
||
beta = XorSIMD(beta, signMask);
|
||
|
||
// mask out singularities (where omega = 1)
|
||
alpha = MaskedAssign(bCosOmegaLessThanOne, alpha, oneMinusT);
|
||
beta = MaskedAssign(bCosOmegaLessThanOne, beta, t);
|
||
|
||
ret.x = MulSIMD(x, alpha);
|
||
ret.y = MulSIMD(y, alpha);
|
||
ret.z = MulSIMD(z, alpha);
|
||
ret.w = MulSIMD(w, alpha);
|
||
|
||
ret.x = MaddSIMD(to.x, beta, ret.x);
|
||
ret.y = MaddSIMD(to.y, beta, ret.y);
|
||
ret.z = MaddSIMD(to.z, beta, ret.z);
|
||
ret.w = MaddSIMD(to.w, beta, ret.w);
|
||
|
||
return ret;
|
||
}
|
||
|
||
|
||
|
||
FORCEINLINE FourQuaternions FourQuaternions::SlerpNoAlign(const FourQuaternions& originalto, const fltx4& t)
|
||
{
|
||
FourQuaternions ret;
|
||
static const fltx4 OneMinusEpsilon = { 1.0f - 0.000001f, 1.0f - 0.000001f, 1.0f - 0.000001f, 1.0f - 0.000001f };
|
||
|
||
// align if necessary.
|
||
|
||
// actually, before we even do that, start by computing the dot product of
|
||
// the quaternions. it has lots of dependent ops and we can sneak it into
|
||
// the pipeline bubbles as we figure out alignment. Of course we don't know
|
||
// yet if we need to realign, so compute them both -- there's plenty of
|
||
// space in the bubbles. They're roomy, those bubbles.
|
||
fltx4 cosineOmega;
|
||
|
||
const FourQuaternions& to = originalto;
|
||
cosineOmega = MulSIMD(x, to.x);
|
||
cosineOmega = MaddSIMD(y, to.y, cosineOmega);
|
||
cosineOmega = MaddSIMD(z, to.z, cosineOmega);
|
||
cosineOmega = MaddSIMD(w, to.w, cosineOmega);
|
||
|
||
fltx4 sinOmega = Four_Ones;
|
||
|
||
fltx4 oneMinusT = SubSIMD(Four_Ones, t);
|
||
bi32x4 bCosOmegaLessThanOne = CmpLtSIMD(cosineOmega, OneMinusEpsilon); // we'll use this to mask out null slerps
|
||
|
||
// figure out the sin component of the diff quaternion.
|
||
// since sin^2(t) + cos^2(t) = 1...
|
||
sinOmega = MsubSIMD(cosineOmega, cosineOmega, sinOmega); // = 1 - cos^2(t) = sin^2(t)
|
||
fltx4 invSinOmega = ReciprocalSqrtSIMD(sinOmega); // 1/sin(t)
|
||
sinOmega = MulSIMD(sinOmega, invSinOmega); // = sin^2(t) / sin(t) = sin(t)
|
||
|
||
// use the arctangent technique to work out omega from tan^-1(sin/cos)
|
||
fltx4 omega = ArcTan2SIMD(sinOmega, cosineOmega);
|
||
|
||
// alpha = sin(omega * (1-T))/sin(omega)
|
||
// beta = sin(omega * T)/sin(omega)
|
||
fltx4 alpha = MulSIMD(omega, oneMinusT); // w(1-T)
|
||
fltx4 beta = MulSIMD(omega, t); // w(T)
|
||
alpha = SinSIMD(alpha); // sin(w(1-T))
|
||
beta = SinSIMD(beta); // sin(wT)
|
||
alpha = MulSIMD(alpha, invSinOmega);
|
||
beta = MulSIMD(beta, invSinOmega);
|
||
|
||
// mask out singularities (where omega = 1)
|
||
alpha = MaskedAssign(bCosOmegaLessThanOne, alpha, oneMinusT);
|
||
beta = MaskedAssign(bCosOmegaLessThanOne, beta, t);
|
||
|
||
ret.x = MulSIMD(x, alpha);
|
||
ret.y = MulSIMD(y, alpha);
|
||
ret.z = MulSIMD(z, alpha);
|
||
ret.w = MulSIMD(w, alpha);
|
||
|
||
ret.x = MaddSIMD(to.x, beta, ret.x);
|
||
ret.y = MaddSIMD(to.y, beta, ret.y);
|
||
ret.z = MaddSIMD(to.z, beta, ret.z);
|
||
ret.w = MaddSIMD(to.w, beta, ret.w);
|
||
|
||
return ret;
|
||
}
|
||
|
||
/***** removed because one of the SWIG permutations doesn't include ssequaternion.h, causing a missing symbol on this function:
|
||
inline void FourVectors::RotateBy( const FourQuaternions &quats )
|
||
{
|
||
quats.RotateFourVectors( this );
|
||
}
|
||
*/
|
||
|
||
|
||
#endif // SSEQUATMATH_H
|
||
|
||
|