// 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; 
}