mirror of
https://github.com/Mauler125/r5sdk.git
synced 2025-02-09 19:15:03 +01:00
102 lines
2.6 KiB
C++
102 lines
2.6 KiB
C++
//===== Copyright <20> 1996-2005, Valve Corporation, All rights reserved. ======//
|
||
//
|
||
// Purpose:
|
||
//
|
||
// $NoKeywords: $
|
||
//
|
||
//===========================================================================//
|
||
|
||
#include "core/stdafx.h"
|
||
#include "mathlib/mathlib.h"
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: qt = ( s * p ) * q
|
||
//-----------------------------------------------------------------------------
|
||
void QuaternionSM(float s, const Quaternion& p, const Quaternion& q, Quaternion& qt)
|
||
{
|
||
Quaternion p1, q1;
|
||
|
||
QuaternionScale(p, s, p1);
|
||
QuaternionMult(p1, q, q1);
|
||
QuaternionNormalize(q1);
|
||
qt[0] = q1[0];
|
||
qt[1] = q1[1];
|
||
qt[2] = q1[2];
|
||
qt[3] = q1[3];
|
||
}
|
||
|
||
#if ALLOW_SIMD_QUATERNION_MATH
|
||
FORCEINLINE fltx4 QuaternionSMSIMD(const fltx4& s, const fltx4& p, const fltx4& q)
|
||
{
|
||
fltx4 p1, q1, result;
|
||
p1 = QuaternionScaleSIMD(p, s);
|
||
q1 = QuaternionMultSIMD(p1, q);
|
||
result = QuaternionNormalizeSIMD(q1);
|
||
return result;
|
||
}
|
||
|
||
FORCEINLINE fltx4 QuaternionSMSIMD(float s, const fltx4& p, const fltx4& q)
|
||
{
|
||
return QuaternionSMSIMD(ReplicateX4(s), p, q);
|
||
}
|
||
#endif
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: qt = p * ( s * q )
|
||
//-----------------------------------------------------------------------------
|
||
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];
|
||
}
|
||
|
||
#if ALLOW_SIMD_QUATERNION_MATH
|
||
|
||
FORCEINLINE fltx4 QuaternionMASIMD(const fltx4& p, const fltx4& s, const fltx4& q)
|
||
{
|
||
fltx4 p1, q1, result;
|
||
q1 = QuaternionScaleSIMD(q, s);
|
||
p1 = QuaternionMultSIMD(p, q1);
|
||
result = QuaternionNormalizeSIMD(p1);
|
||
return result;
|
||
}
|
||
|
||
FORCEINLINE fltx4 QuaternionMASIMD(const fltx4& p, float s, const fltx4& q)
|
||
{
|
||
return QuaternionMASIMD(p, ReplicateX4(s), q);
|
||
}
|
||
#endif
|
||
|
||
|
||
//-----------------------------------------------------------------------------
|
||
// Purpose: qt = p + s * q
|
||
//-----------------------------------------------------------------------------
|
||
void QuaternionAccumulate(const Quaternion& p, float s, const Quaternion& q, Quaternion& qt)
|
||
{
|
||
Quaternion q2;
|
||
QuaternionAlign(p, q, q2);
|
||
|
||
qt[0] = p[0] + s * q2[0];
|
||
qt[1] = p[1] + s * q2[1];
|
||
qt[2] = p[2] + s * q2[2];
|
||
qt[3] = p[3] + s * q2[3];
|
||
}
|
||
|
||
#if ALLOW_SIMD_QUATERNION_MATH
|
||
FORCEINLINE fltx4 QuaternionAccumulateSIMD(const fltx4& p, float s, const fltx4& q)
|
||
{
|
||
fltx4 q2, s4, result;
|
||
q2 = QuaternionAlignSIMD(p, q);
|
||
s4 = ReplicateX4(s);
|
||
result = MaddSIMD(s4, q2, p);
|
||
return result;
|
||
}
|
||
#endif
|