716 lines
14 KiB
C++
716 lines
14 KiB
C++
// CCMath.cpp : Defines the entry point for the DLL application.
|
|
//
|
|
|
|
#include "windows.h"
|
|
#include "CCMath.h"
|
|
#include "math.h"
|
|
|
|
#ifdef _MANAGED
|
|
#pragma managed(push, off)
|
|
#endif
|
|
|
|
#ifdef CCMATH_DLL_MODE
|
|
|
|
BOOL APIENTRY DllMain( HMODULE hModule,
|
|
DWORD ul_reason_for_call,
|
|
LPVOID lpReserved
|
|
)
|
|
{
|
|
switch (ul_reason_for_call)
|
|
{
|
|
case DLL_PROCESS_ATTACH:
|
|
case DLL_THREAD_ATTACH:
|
|
case DLL_THREAD_DETACH:
|
|
case DLL_PROCESS_DETACH:
|
|
break;
|
|
}
|
|
return TRUE;
|
|
}
|
|
|
|
#endif
|
|
|
|
#ifdef _MANAGED
|
|
#pragma managed(pop)
|
|
#endif
|
|
|
|
/******************* COULD BE USEFUL **************************
|
|
_inline float FastInvSqrt(float x)
|
|
{
|
|
float xhalf = 0.5f*x;
|
|
int i = *(int*)&x; // get bits for floating value
|
|
i = 0x5f375a86- (i>>1); // gives initial guess y0
|
|
x = *(float*)&i; // convert bits back to float
|
|
x = x*(1.5f-xhalf*x*x); // Newton step, repeating increases accuracy
|
|
return x;
|
|
}
|
|
|
|
_inline float FastSqrt(float number) {
|
|
long i;
|
|
float x, y;
|
|
x = number * 0.5F;
|
|
y = number;
|
|
i = * ( long * ) &y;
|
|
i = 0x5f3759df - ( i >> 1 );
|
|
y = * ( float * ) &i;
|
|
y = y * ( 1.5F - ( x * y * y ) );
|
|
y = y * ( 1.5F - ( x * y * y ) );// 2nd iteration : Can be removed
|
|
return number * y;
|
|
}
|
|
*/
|
|
|
|
|
|
#define SMALL_FOR_CLEAN 0.0001f
|
|
|
|
_inline int CheckNAN(float *a)
|
|
{
|
|
if ((*((int*)a) & 0x7f800000) == 0x7f800000) return 1;
|
|
return 0;
|
|
}
|
|
|
|
|
|
Vector::Vector(void)
|
|
{
|
|
}
|
|
|
|
Vector::Vector(const float _x, const float _y, const float _z)
|
|
{
|
|
x = _x; y = _y; z = _z;
|
|
}
|
|
|
|
Vector Vector::operator+(const Vector &vec2) const
|
|
{
|
|
return Vector(x + vec2.x, y + vec2.y, z + vec2.z);
|
|
}
|
|
Vector Vector::operator-(const Vector &vec2) const
|
|
{
|
|
return Vector(x - vec2.x, y - vec2.y, z - vec2.z);
|
|
}
|
|
Vector Vector::operator&(const Vector &vec2) const
|
|
{
|
|
return Vector(x * vec2.x, y * vec2.y, z * vec2.z);
|
|
}
|
|
Vector Vector::operator*(const float Value) const
|
|
{
|
|
return Vector(x * Value, y * Value, z * Value);
|
|
}
|
|
Vector Vector::operator^(const Vector &B) const
|
|
{
|
|
return Vector(
|
|
(y * B.z) - (z * B.y),
|
|
(z * B.x) - (x * B.z),
|
|
(x * B.y) - (y * B.x));
|
|
}
|
|
|
|
float Vector::operator*(const Vector &vec2) const
|
|
{
|
|
return x * vec2.x + y * vec2.y + z * vec2.z;
|
|
}
|
|
void Vector::operator+=(const Vector &vec2)
|
|
{
|
|
x += vec2.x; y += vec2.y; z += vec2.z;
|
|
}
|
|
void Vector::operator+=(const float Value)
|
|
{
|
|
x += Value; y += Value; z += Value;
|
|
}
|
|
void Vector::operator-=(const Vector &vec2)
|
|
{
|
|
x -= vec2.x; y -= vec2.y; z -= vec2.z;
|
|
}
|
|
void Vector::operator*=(const float Value)
|
|
{
|
|
x *= Value; y *= Value; z *= Value;
|
|
}
|
|
void Vector::operator&=(const Vector &vec2)
|
|
{
|
|
x *= vec2.x; y *= vec2.y; z *= vec2.z;
|
|
}
|
|
int Vector::operator!=(const Vector &vec2)
|
|
{
|
|
return x != vec2.x ? 1 : y != vec2.y ? 1 : z != vec2.z ? 1 : 0;
|
|
}
|
|
|
|
|
|
int Vector::CheckNAN()
|
|
{
|
|
return ::CheckNAN(&x)|::CheckNAN(&y)|::CheckNAN(&z);
|
|
}
|
|
|
|
void Vector::Clean()
|
|
{
|
|
if (fabs(x) < SMALL_FOR_CLEAN) x = 0.0f;
|
|
if (fabs(y) < SMALL_FOR_CLEAN) y = 0.0f;
|
|
if (fabs(z) < SMALL_FOR_CLEAN) z = 0.0f;
|
|
}
|
|
|
|
int Vector::isRightXY(const Vector &A,const Vector &B) const
|
|
{
|
|
Vector D,E;
|
|
float R;
|
|
D = *this - A;
|
|
E = B - A;
|
|
R = (D.y * E.x) - (D.x * E.y)/* - 0.005f*/;
|
|
return *((unsigned int *)&R) >> 31;
|
|
}
|
|
|
|
float Vector::DistanceToLineXY(const Vector &A,const Vector &B) const
|
|
{
|
|
Vector D,E;
|
|
float R;
|
|
D = *this - A;
|
|
E = B - A;
|
|
R = (D.x * E.y) - (D.y * E.x);
|
|
return R;
|
|
}
|
|
|
|
Vector Vector::VectorToLine(const Vector &DirNormalized) const
|
|
{
|
|
Vector D,E;
|
|
E = DirNormalized * (DirNormalized * *this);
|
|
D = E - *this;
|
|
return D;
|
|
}
|
|
|
|
float Vector::SqrNorm() const
|
|
{
|
|
return (x * x + y * y + z * z);
|
|
}
|
|
float Vector::Norm() const
|
|
{
|
|
return sqrtf(x * x + y * y + z * z);
|
|
}
|
|
float Vector::NormXY() const
|
|
{
|
|
return sqrtf(x * x + y * y);
|
|
}
|
|
float Vector::SqrNormXY() const
|
|
{
|
|
return (x * x + y * y);
|
|
}
|
|
void Vector::Normalize()
|
|
{
|
|
float NormV;
|
|
NormV = SqrNorm();
|
|
if (NormV)
|
|
{
|
|
NormV = 1.0f / sqrtf(NormV);
|
|
x *= NormV ;
|
|
y *= NormV ;
|
|
z *= NormV ;
|
|
}
|
|
}
|
|
|
|
void Vector::Max(const Vector &vecB)
|
|
{
|
|
x = x > vecB.x ? x : vecB.x;
|
|
y = y > vecB.y ? y : vecB.y;
|
|
z = z > vecB.z ? z : vecB.z;
|
|
}
|
|
|
|
void Vector::Min(const Vector &vecB)
|
|
{
|
|
x = x < vecB.x ? x : vecB.x;
|
|
y = y < vecB.y ? y : vecB.y;
|
|
z = z < vecB.z ? z : vecB.z;
|
|
}
|
|
int Vector::IsIn2D(const Vector &Min,const Vector &Max)
|
|
{
|
|
int Ret;
|
|
Ret = (x > Min.x) && (x < Max.x) ? 1 : 0;
|
|
Ret &= (y > Min.y) && (y < Max.y) ? 1 : 0;
|
|
return Ret;
|
|
}
|
|
|
|
#define Swap(a,b) {float SWP; SWP = a ; a = b ; b = SWP;}
|
|
|
|
inline Vector MATRIX::operator*(const Vector &V) const
|
|
{
|
|
//Vector Res;
|
|
//Res = (I * V.x) + (J * V.y) + (K * V.z) + T;
|
|
//return Res;
|
|
return Vector( I.x * V.x + J.x * V.y + K.x * V.z + T.x,
|
|
I.y * V.x + J.y * V.y + K.y * V.z + T.y,
|
|
I.z * V.x + J.z * V.y + K.z * V.z + T.z);
|
|
}
|
|
|
|
int MATRIX::CheckNAN()
|
|
{
|
|
return I.CheckNAN()|J.CheckNAN()|K.CheckNAN()|T.CheckNAN();
|
|
}
|
|
void MATRIX::Transp()
|
|
{
|
|
Swap(I.y,J.x);
|
|
Swap(I.z,K.x);
|
|
Swap(J.z,K.y);
|
|
}
|
|
void MATRIX::Clean()
|
|
{
|
|
I.Clean();
|
|
J.Clean();
|
|
K.Clean();
|
|
T.Clean();
|
|
}
|
|
void MATRIX::Inverse(MATRIX &Dst)
|
|
{
|
|
Vector Trans;
|
|
memset(&Dst,0,sizeof(MATRIX ));
|
|
Dst.I = J ^ K;
|
|
Dst.J = I ^ K;
|
|
Dst.K = I ^ J;
|
|
|
|
Dst.I *= 1.0f / (I * Dst.I);
|
|
Dst.J *= 1.0f / (J * Dst.J);
|
|
Dst.K *= 1.0f / (K * Dst.K);
|
|
Dst.Transp();
|
|
|
|
Dst.T = Vector(0,0,0);
|
|
Trans.x = -T.x;
|
|
Trans.y = -T.y;
|
|
Trans.z = -T.z;
|
|
Dst.T = Dst * Trans;
|
|
}
|
|
void MATRIX::Blend(MATRIX &EndL,float Factor)
|
|
{
|
|
Vector ZoomB,ZoomE;
|
|
Quat QB,QE;
|
|
MATRIX End,Ret;
|
|
End = EndL;
|
|
if (Factor > 1.0f) Factor = 1.0f;
|
|
if (Factor < 0.0f) Factor = 0.0f;
|
|
|
|
ZoomB.x = I.Norm();
|
|
ZoomB.y = J.Norm();
|
|
ZoomB.z = K.Norm();
|
|
ZoomE.x = End.I.Norm();
|
|
ZoomE.y = End.J.Norm();
|
|
ZoomE.z = End.K.Norm();
|
|
I.Normalize();
|
|
J.Normalize();
|
|
K.Normalize();
|
|
End.I.Normalize();
|
|
End.J.Normalize();
|
|
End.K.Normalize();
|
|
QB.ComputeFromMatrix(this);
|
|
QE.ComputeFromMatrix(&End);
|
|
QB = (QB * (1.0f - Factor)) + (QE * Factor);
|
|
QB.Normalize();
|
|
|
|
QB.TransforToMatrix(&Ret);
|
|
Ret.T = (T * (1.0f - Factor)) + (End.T * Factor);
|
|
ZoomE = (ZoomB * (1.0f - Factor)) + (ZoomE * Factor);
|
|
Ret.I *= ZoomE.x;
|
|
Ret.J *= ZoomE.y;
|
|
Ret.K *= ZoomE.z;
|
|
*this = Ret;
|
|
}
|
|
void MATRIX::GetScale(Vector &V)
|
|
{
|
|
V.x = I.Norm();
|
|
V.y = J.Norm();
|
|
V.z = K.Norm();
|
|
}
|
|
void MATRIX::SetScale(Vector &V)
|
|
{
|
|
I.Normalize();
|
|
J.Normalize();
|
|
K.Normalize();
|
|
I *= V.x;
|
|
J *= V.y;
|
|
K *= V.z;
|
|
}
|
|
void MATRIX::RotateAround_I(float Alpha)
|
|
{
|
|
float COSA,SINA;
|
|
Vector A,B;
|
|
COSA = cosf(Alpha);
|
|
SINA = sinf(Alpha);
|
|
A = J;
|
|
B = K;
|
|
J = A * COSA + B * SINA;
|
|
K = B * COSA - A * SINA;
|
|
}
|
|
void MATRIX::RotateAround_J(float Alpha)
|
|
{
|
|
float COSA,SINA;
|
|
Vector A,B;
|
|
COSA = cosf(Alpha);
|
|
SINA = sinf(Alpha);
|
|
A = K;
|
|
B = I;
|
|
K = A * COSA + B * SINA;
|
|
I = B * COSA - A * SINA;
|
|
}
|
|
void MATRIX::RotateAround_K(float Alpha)
|
|
{
|
|
float COSA,SINA;
|
|
Vector A,B;
|
|
COSA = cosf(Alpha);
|
|
SINA = sinf(Alpha);
|
|
A = I;
|
|
B = J;
|
|
I = A * COSA + B * SINA;
|
|
J = B * COSA - A * SINA;
|
|
}
|
|
void MATRIX::RotateAround_X(float Alpha)
|
|
{
|
|
MATRIX II;
|
|
Vector SaavetTRans;
|
|
II.Identity();
|
|
II.RotateAround_I(Alpha);
|
|
SaavetTRans = T;
|
|
*this *= II;
|
|
T = SaavetTRans;
|
|
}
|
|
void MATRIX::RotateAround_Y(float Alpha)
|
|
{
|
|
MATRIX II;
|
|
Vector SaavetTRans;
|
|
II.Identity();
|
|
II.RotateAround_J(Alpha);
|
|
SaavetTRans = T;
|
|
*this *= II;
|
|
T = SaavetTRans;
|
|
}
|
|
void MATRIX::RotateAround_Z(float Alpha)
|
|
{
|
|
MATRIX II;
|
|
Vector SaavetTRans;
|
|
II.Identity();
|
|
II.RotateAround_K(Alpha);
|
|
SaavetTRans = T;
|
|
*this *= II;
|
|
T = SaavetTRans;
|
|
}
|
|
void MATRIX::RotateAround(int Axis, float Alpha)
|
|
{
|
|
switch(Axis)
|
|
{
|
|
case 0:RotateAround_K(Alpha);break;
|
|
case 1:RotateAround_J(Alpha);break;
|
|
case 2:RotateAround_I(Alpha);break;
|
|
}
|
|
}
|
|
void MATRIX::operator*=(MATRIX &M)
|
|
{
|
|
MATRIX Save;
|
|
Save = *this;
|
|
I.x = Save.I.x * M.I.x + Save.I.y * M.J.x + Save.I.z * M.K.x ;
|
|
I.y = Save.I.x * M.I.y + Save.I.y * M.J.y + Save.I.z * M.K.y ;
|
|
I.z = Save.I.x * M.I.z + Save.I.y * M.J.z + Save.I.z * M.K.z ;
|
|
|
|
J.x = Save.J.x * M.I.x + Save.J.y * M.J.x + Save.J.z * M.K.x ;
|
|
J.y = Save.J.x * M.I.y + Save.J.y * M.J.y + Save.J.z * M.K.y ;
|
|
J.z = Save.J.x * M.I.z + Save.J.y * M.J.z + Save.J.z * M.K.z ;
|
|
|
|
K.x = Save.K.x * M.I.x + Save.K.y * M.J.x + Save.K.z * M.K.x ;
|
|
K.y = Save.K.x * M.I.y + Save.K.y * M.J.y + Save.K.z * M.K.y ;
|
|
K.z = Save.K.x * M.I.z + Save.K.y * M.J.z + Save.K.z * M.K.z ;
|
|
|
|
T.x = (Save.T.x * M.I.x + Save.T.y * M.J.x + Save.T.z * M.K.x) + M.T.x;
|
|
T.y = (Save.T.x * M.I.y + Save.T.y * M.J.y + Save.T.z * M.K.y) + M.T.y;
|
|
T.z = (Save.T.x * M.I.z + Save.T.y * M.J.z + Save.T.z * M.K.z) + M.T.z;
|
|
|
|
/*
|
|
M.Transp();
|
|
I.x = Save.I * M.I;
|
|
I.y = Save.I * M.J;
|
|
I.z = Save.I * M.K;
|
|
|
|
J.x = Save.J * M.I;
|
|
J.y = Save.J * M.J;
|
|
J.z = Save.J * M.K;
|
|
|
|
K.x = Save.K * M.I;
|
|
K.y = Save.K * M.J;
|
|
K.z = Save.K * M.K;
|
|
|
|
T.x = (Save.T * M.I) + M.T.x;
|
|
T.y = (Save.T * M.J) + M.T.y;
|
|
T.z = (Save.T * M.K) + M.T.z;
|
|
|
|
M.Transp();
|
|
*/
|
|
}
|
|
void MATRIX::operator*=(float Scale)
|
|
{
|
|
I *= Scale;
|
|
J *= Scale;
|
|
K *= Scale;
|
|
}
|
|
void MATRIX::Identity()
|
|
{
|
|
I = Vector(1,0,0);
|
|
J = Vector(0,1,0);
|
|
K = Vector(0,0,1);
|
|
T = Vector(0,0,0);
|
|
}
|
|
MATRIX::MATRIX() {
|
|
Identity();
|
|
}
|
|
|
|
float myMod(float val, float mod)
|
|
{
|
|
mod /= 2;
|
|
while (val < -mod)
|
|
val += mod;
|
|
while (val > mod)
|
|
val -= mod;
|
|
return val;
|
|
}
|
|
|
|
void MATRIX::ToEuler(Vector &eulerVect)
|
|
{
|
|
float C;
|
|
float trX;
|
|
float trY;
|
|
float PI = atan(1.0f) * 4;
|
|
|
|
eulerVect.y = -asin( K.x);
|
|
C = cos( eulerVect.y );
|
|
if ( fabs( C ) > 0.005 )
|
|
{
|
|
trX = K.z / C;
|
|
trY = -K.y / C;
|
|
eulerVect.x = atan2( trY, trX );
|
|
trX = I.x / C;
|
|
trY = -J.x / C;
|
|
eulerVect.z = atan2( trY, trX );
|
|
} else
|
|
{
|
|
eulerVect.x = 0.0f;
|
|
trX = J.y;
|
|
trY = I.y;
|
|
eulerVect.z = atan2( trY, trX );
|
|
}
|
|
eulerVect.x = myMod(eulerVect.x, 2*PI);
|
|
eulerVect.y = myMod(eulerVect.y, 2*PI);
|
|
eulerVect.z = myMod(eulerVect.z, 2*PI);
|
|
}
|
|
void MATRIX::FromEuler(Vector eulerVect)
|
|
{
|
|
float A,B,C,D,E,F,AD,BD;
|
|
|
|
A = cos(eulerVect.x);
|
|
B = sin(eulerVect.x);
|
|
C = cos(eulerVect.y);
|
|
D = sin(eulerVect.y);
|
|
E = cos(eulerVect.z);
|
|
F = sin(eulerVect.z);
|
|
|
|
AD = A * D;
|
|
BD = B * D;
|
|
|
|
I.x = C * E;
|
|
I.y = -C * F;
|
|
I.z = -D;
|
|
J.x = -BD * E + A * F;
|
|
J.y = BD * F + A * E;
|
|
J.z = -B * C;
|
|
K.x = AD * E + B * F;
|
|
K.y = -AD * F + B * E;
|
|
K.z = A * C; }
|
|
|
|
void Quat::ComputeFromMatrix(MATRIX *ActualMat)
|
|
{
|
|
float T, S;
|
|
int iBestColumn;
|
|
MATRIX LocalNormed;
|
|
LocalNormed = *ActualMat;
|
|
LocalNormed.I.Normalize();
|
|
LocalNormed.J.Normalize();
|
|
LocalNormed.K.Normalize();
|
|
if ((LocalNormed.I ^ LocalNormed.J) * LocalNormed.K < 0.0f)
|
|
iBestColumn = 0; // <- never enter here !!! */
|
|
|
|
|
|
/* trace of the matrix */
|
|
T = LocalNormed.I.x + LocalNormed.J.y + LocalNormed.K.z;
|
|
|
|
if(T > 0.0f)
|
|
{
|
|
S = sqrtf(T + 1.0f);
|
|
w = S * 0.5f;
|
|
S = 0.5f / S;
|
|
x = (LocalNormed.J.z - LocalNormed.K.y) * S;
|
|
y = (LocalNormed.K.x - LocalNormed.I.z) * S;
|
|
z = (LocalNormed.I.y - LocalNormed.J.x) * S;
|
|
}
|
|
else
|
|
{
|
|
/* Find the greatest diagonal element */
|
|
if(LocalNormed.I.x >= LocalNormed.J.y)
|
|
{
|
|
if(LocalNormed.I.x >= LocalNormed.K.z)
|
|
iBestColumn = 1;
|
|
else
|
|
iBestColumn = 3;
|
|
}
|
|
else
|
|
{
|
|
if(LocalNormed.J.y >= LocalNormed.K.z)
|
|
iBestColumn = 2;
|
|
else
|
|
iBestColumn = 3;
|
|
}
|
|
|
|
switch(iBestColumn)
|
|
{
|
|
case 1:
|
|
S = sqrtf(1.0f + LocalNormed.I.x - LocalNormed.J.y - LocalNormed.K.z);
|
|
x = S * 0.5f;
|
|
S = 0.5f / S;
|
|
y = (LocalNormed.J.x + LocalNormed.I.y) * S;
|
|
z = (LocalNormed.K.x + LocalNormed.I.z) * S;
|
|
w = (LocalNormed.J.z - LocalNormed.K.y) * S;
|
|
break;
|
|
|
|
case 2:
|
|
S = sqrtf(1.0f - LocalNormed.I.x + LocalNormed.J.y - LocalNormed.K.z);
|
|
y = S * 0.5f;
|
|
S = 0.5f / S;
|
|
z = (LocalNormed.K.y + LocalNormed.J.z) * S;
|
|
x = (LocalNormed.I.y + LocalNormed.J.x) * S;
|
|
w = (LocalNormed.K.x - LocalNormed.I.z) * S;
|
|
break;
|
|
|
|
case 3:
|
|
S = sqrtf(1.0f - LocalNormed.I.x - LocalNormed.J.y + LocalNormed.K.z);
|
|
z = S * 0.5f;
|
|
S = 0.5f / S;
|
|
x = (LocalNormed.I.z + LocalNormed.K.x) * S;
|
|
y = (LocalNormed.J.z + LocalNormed.K.y) * S;
|
|
w = (LocalNormed.I.y - LocalNormed.J.x) * S;
|
|
break;
|
|
}
|
|
}
|
|
Normalize();
|
|
}
|
|
|
|
void Quat::TransforToMatrix(MATRIX *ActualMat)
|
|
{
|
|
float xx, xy, xz, xw, yy, yz, yw, zz, zw;
|
|
xx = 0.5f - 2.0f * x * x;
|
|
xy = 2.0f * x * y;
|
|
xz = 2.0f * x * z;
|
|
xw = 2.0f * x * w;
|
|
yy = 0.5f - 2.0f * y * y;
|
|
yz = 2.0f * y * z;
|
|
yw = 2.0f * y * w;
|
|
zz = 0.5f - 2.0f * z * z;
|
|
zw = 2.0f * z * w;
|
|
ActualMat->I.x = yy + zz;
|
|
ActualMat->J.x = xy - zw;
|
|
ActualMat->K.x = xz + yw;
|
|
ActualMat->I.y = xy + zw;
|
|
ActualMat->J.y = xx + zz;
|
|
ActualMat->K.y = yz - xw;
|
|
ActualMat->I.z = xz - yw;
|
|
ActualMat->J.z = yz + xw;
|
|
ActualMat->K.z = xx + yy;
|
|
ActualMat->T = Vector(0,0,0);
|
|
}
|
|
|
|
|
|
void Quat::TransforToVector(Vector &V)
|
|
{
|
|
V.x = x;
|
|
V.y = y;
|
|
V.z = z;
|
|
}
|
|
Quat::Quat(void)
|
|
{
|
|
}
|
|
Quat::Quat(float _x,float _y,float _z,float _w)
|
|
{
|
|
x = _x;y = _y;z = _z;w = _w;
|
|
}
|
|
Quat::Quat(Vector V,float _w) {
|
|
x = V.x;y = V.y;z = V.z;w = _w;
|
|
}
|
|
Quat::Quat(Vector V)
|
|
{
|
|
x = V.x;y = V.y;z = V.z;NormalizeW();
|
|
}
|
|
void Quat::ExtractFrom2Edges(Vector *Src,Vector *Dst)
|
|
{
|
|
Vector V1,V2,VCP;
|
|
V1 = *Src;
|
|
V2 = *Dst;
|
|
V1.Normalize();
|
|
V2.Normalize();
|
|
V2 = V1 * 0.5f + V2 * 0.5f;
|
|
V2.Normalize();
|
|
VCP = V1^V2;
|
|
x = VCP.x;
|
|
y = VCP.y;
|
|
z = VCP.z;
|
|
NormalizeW();
|
|
}
|
|
|
|
void Quat::operator*=(const float F)
|
|
{
|
|
x *= F;
|
|
y *= F;
|
|
z *= F;
|
|
w *= F;
|
|
}
|
|
|
|
void Quat::operator+=(const Quat &Q2)
|
|
{
|
|
x += Q2.x;
|
|
y += Q2.y;
|
|
z += Q2.z;
|
|
w += Q2.w;
|
|
}
|
|
|
|
Quat Quat::operator+(const Quat &Q2)
|
|
{
|
|
Quat Res;
|
|
Res = *this;
|
|
Res.x += Q2.x;
|
|
Res.y += Q2.y;
|
|
Res.z += Q2.z;
|
|
Res.w += Q2.w;
|
|
return Res;
|
|
}
|
|
|
|
|
|
void Quat::Normalize()
|
|
{
|
|
float OoN = 1.0f / sqrtf(x*x + y*y + z*z + w*w);
|
|
x *= OoN;
|
|
y *= OoN;
|
|
z *= OoN;
|
|
w *= OoN;
|
|
}
|
|
void Quat::NormalizeW()
|
|
{
|
|
w = sqrtf(1.0f - x*x - y*y - z*z);
|
|
}
|
|
Quat Quat::operator*(const Quat &Q2) const
|
|
{
|
|
Quat Res;
|
|
Res.x = (w * Q2.x) + (Q2.w * x) + (y * Q2.z) - (z * Q2.y);
|
|
Res.y = (w * Q2.y) + (Q2.w * y) + (z * Q2.x) - (x * Q2.z);
|
|
Res.z = (w * Q2.z) + (Q2.w * z) + (x * Q2.y) - (y * Q2.x);
|
|
Res.w = (w * Q2.w) - (x * Q2.x + y * Q2.y + z * Q2.z);
|
|
return Res;
|
|
}
|
|
Quat Quat::operator*(const float F) const
|
|
{
|
|
Quat Res;
|
|
Res = *this;
|
|
Res *= F;
|
|
//Res.NormalizeW();
|
|
return Res;
|
|
}
|
|
void Quat::operator*=(const Quat &Q2)
|
|
{
|
|
Quat Res;
|
|
Res.x = (w * Q2.x) + (Q2.w * x) + (y * Q2.z) - (z * Q2.y);
|
|
Res.y = (w * Q2.y) + (Q2.w * y) + (z * Q2.x) - (x * Q2.z);
|
|
Res.z = (w * Q2.z) + (Q2.w * z) + (x * Q2.y) - (y * Q2.x);
|
|
Res.w = (w * Q2.w) - (x * Q2.x + y * Q2.y + z * Q2.z);
|
|
*this = Res;
|
|
}
|
|
|