
#include "MathMethods.h"

// This module has some code taken from d3dx9math.cpp

#include <math.h>

DVector2::DVector2( float NewX, float NewY ) {
	x = NewX; y = NewY;
}

DVector3::DVector3( float NewX, float NewY, float NewZ ) {
	x = NewX; y = NewY; z = NewZ;
}

DVector4::DVector4( float NewX, float NewY, float NewZ, float NewW ) {
	x = NewX; y = NewY; z = NewZ; w = NewW;
}

DMatrix::DMatrix( float New_11, float New_12, float New_13, float New_14, float New_21, float New_22, float New_23, float New_24, float New_31, float New_32, float New_33, float New_34, float New_41, float New_42, float New_43, float New_44 ) {
	_11 = New_11; _12 = New_12; _13 = New_13; _14 = New_14;
	_21 = New_21; _22 = New_22; _23 = New_23; _24 = New_24;
	_31 = New_31; _32 = New_32; _33 = New_33; _34 = New_34;
	_41 = New_41; _42 = New_42; _43 = New_43; _44 = New_44;
}

DVector3 BulletToDX_V3(btVector3* V) {
	return DVector3(V->getX(),V->getY(),V->getZ());
}

float* ToFloatArray(const DVector2* V) {
	return (float *)V;
}

float* ToFloatArray(const DVector3* V) {
	return (float *)V;
}

float* ToFloatArray(const DVector4* V) {
	return (float *)V;
}

DVector2 AddVec2(DVector2 V1, DVector2 V2) {
	return DVector2(V1.x + V1.x,V1.y + V2.y);
}

DVector2 SubVec2(DVector2 V1, DVector2 V2) {
	return DVector2(V1.x - V1.x,V1.y - V2.y);
}

DVector3 AddVec3(DVector3 V1, DVector3 V2) {
	return DVector3(V1.x + V2.x,V1.y + V2.y,V1.z + V2.z);
}

DVector3 SubVec3(DVector3 V1, DVector3 V2) {
	return DVector3(V1.x - V2.x,V1.y - V2.y,V1.z - V2.z);
}

DVector4 AddVec4(DVector4 V1, DVector4 V2) {
	return DVector4(V1.x + V2.x,V1.y + V2.y,V1.z + V2.z,V1.w + V2.w);
}

DVector4 SubVec4(DVector4 V1, DVector4 V2) {
	return DVector4(V1.x - V2.x,V1.y - V2.y,V1.z - V2.z,V1.w - V2.w);
}

DVector2 ScaleVec2(DVector2 V, float S) {
	return DVector2(V.x * S,V.y * S);
}

DVector3 ScaleVec3(DVector3 V, float S) {
	return DVector3(V.x * S,V.y * S,V.z * S);
}

DVector4 ScaleVec4(DVector4 V, float S) {
	return DVector4(V.x * S,V.y * S,V.z * S,V.w * S);
}

float AbsVec3(DVector3 V) {
	return sqrtf(pow(V.x, 2.0f) + pow(V.y, 2.0f) + pow(V.z, 2.0f));
}

float DistVec3(DVector3 V1, DVector3 V2) {
	return AbsVec3(SubVec3(V1,V2));
}

DVector3 NormalizeVec3(DVector3 V) {
	float Dist;
	Dist = AbsVec3(V);
	if (Dist > 0.00001f || Dist < -0.00001f) {
		return DVector3(V.x / Dist,V.y / Dist,V.z / Dist);
	} else {
		return DVector3(0.0f, 1.0f, 0.0f);
	}
}

DVector3 CrossVec3(DVector3 V1, DVector3 V2) {
	return DVector3((V1.y * V2.z) - (V1.z * V2.y),(V1.z * V2.x) - (V1.x * V2.z),(V1.x * V2.y) - (V1.y * V2.x));
}

// The sum of all element wise multiplications
float DotVec3(DVector3 V1, DVector3 V2) {
	return (V1.x * V2.x) + (V1.y * V2.y) + (V1.z * V2.z);
}

DVector4 Vec3ToVec4(DVector3 V, float W) {
	return DVector4(V.x,V.y,V.z,W);
}

float AbsFloat(float X) {
	if (X > 0.0f) {
		return X;
	} else {
		return -X;
	}
}

float MaxFloat(float A, float B) {
	if (A > B) {
		return A;
	} else {
		return B;
	}
}

float MinFloat(float A, float B) {
	if (A < B) {
		return A;
	} else {
		return B;
	}
}

float ClampFloat(float Min, float X, float Max) {
	if (X < Min) {
		return Min;
	} else if (X > Max) {
		return Max;
	} else {
		return X;
	}
}

int ClampInt(int Min, int X, int Max) {
	if (X < Min) {
		return Min;
	} else if (X > Max) {
		return Max;
	} else {
		return X;
	}
}

// Returns A when Ratio = 0
// Returns (A + B) / 2 when Ratio = 0.5
// Returns B when Ratio = 1
float Lerp(float A, float B, float Ratio) {
	return (A * (1 - Ratio)) + (B * Ratio);
}

// Returns 0 when Value = A
// Returns 0.5 when Value = (A + B) / 2
// Returns 1 when Value = B
float InverseLerp(float A, float B, float Value) {
	float C = B - A;
	if (C == 0.0f) {
		return 0.5f;
	} else {
		return (Value - A) / (B - A);
	}
}

void MakeOrthogonalBtTransform(btTransform* T,DVector3 Pos, DVector3 XAxis, DVector3 YAxis) {
	btMatrix3x3 NewBasis;
	DVector3 XAxis_N;
	DVector3 YAxis_N;
	DVector3 ZAxis_N;
	XAxis_N = NormalizeVec3(XAxis);
	YAxis_N = NormalizeVec3(YAxis);
	ZAxis_N = NormalizeVec3(CrossVec3(XAxis_N,YAxis_N));
	YAxis_N = NormalizeVec3(CrossVec3(ZAxis_N,XAxis_N));
	T->setBasis(btMatrix3x3(
		XAxis_N.x, YAxis_N.x, ZAxis_N.x,
		XAxis_N.y, YAxis_N.y, ZAxis_N.y,
		XAxis_N.z, YAxis_N.z, ZAxis_N.z
	));
	T->setOrigin(btVector3(Pos.x,Pos.y,Pos.z));
}

DMatrix TransposeMatrix(DMatrix M) {
	return DMatrix(
		M._11, M._21, M._31, M._41,
		M._12, M._22, M._32, M._42,
		M._13, M._23, M._33, M._43,
		M._14, M._24, M._34, M._44
	);
}

DVector3 TransformVec3Mat4(DVector3 V, DMatrix M) {
	return DVector3(
		 (V.x * M._11) + (V.y * M._21) + (V.z * M._31) + M._41
		,(V.x * M._12) + (V.y * M._22) + (V.z * M._32) + M._42
		,(V.x * M._13) + (V.y * M._23) + (V.z * M._33) + M._43
	);
}

DVector4 TransformVec4Mat4(DVector4 V, DMatrix M) {
	return DVector4(
		 (V.x * M._11) + (V.y * M._21) + (V.z * M._31) + (V.w * M._41)
		,(V.x * M._12) + (V.y * M._22) + (V.z * M._32) + (V.w * M._42)
		,(V.x * M._13) + (V.y * M._23) + (V.z * M._33) + (V.w * M._43)
		,(V.x * M._14) + (V.y * M._24) + (V.z * M._34) + (V.w * M._44)
	);
}

DMatrix LookAtLH(DVector3 Eye, DVector3 At, DVector3 Up) {
	DVector3 XAxis, YAxis, ZAxis;
    ZAxis = SubVec3(At,Eye);
	ZAxis = NormalizeVec3(ZAxis);
	XAxis = NormalizeVec3(CrossVec3(Up,ZAxis));
	YAxis = CrossVec3(ZAxis,XAxis);
	DMatrix Result;
	Result._11 = XAxis.x;
	Result._21 = XAxis.y;
	Result._31 = XAxis.z;
	Result._41 = -DotVec3(XAxis, Eye);
	Result._12 = YAxis.x;
	Result._22 = YAxis.y;
	Result._32 = YAxis.z;
	Result._42 = -DotVec3(YAxis, Eye);
	Result._13 = ZAxis.x;
	Result._23 = ZAxis.y;
	Result._33 = ZAxis.z;
	Result._43 = -DotVec3(ZAxis, Eye);
	Result._14 = 0.0f;
	Result._24 = 0.0f;
	Result._34 = 0.0f;
	Result._44 = 1.0f;
	return Result;
}

DMatrix OrthoLH(float w, float h, float zn, float zf) {
	DMatrix Result;
	ZeroMemory(&Result, sizeof(DMatrix));
	Result._11 = 2.0f / w;
	Result._22 = 2.0f / h;
	Result._33 = 1.0f / (zf - zn);
	Result._43 = (-zn) / (zf - zn);
	Result._44 = 1.0f;
	return Result;
}

DMatrix PerspectiveFovLH(float fovy, float Aspect, float zn, float zf) {
	DMatrix Result;
	float w = (1.0f / tan(fovy / 2)) / Aspect;
	float h = 1.0f / tan(fovy / 2);
	float Q = zf / (zf - zn);
	ZeroMemory(&Result, sizeof(DMatrix));
	Result._11 = w;
	Result._22 = h;
	Result._33 = Q;
	Result._34 = 1.0f;
	Result._43 = -Q * zn;
	return Result;
}

DMatrix MulMat(DMatrix M1, DMatrix M2) {
	DMatrix Result;
	DVector4 XAxis; DVector4 YAxis; DVector4 ZAxis; DVector4 WAxis;
	XAxis = TransformVec4Mat4(DVector4(M1._11,M1._12,M1._13,M1._14),M2);
	YAxis = TransformVec4Mat4(DVector4(M1._21,M1._22,M1._23,M1._24),M2);
	ZAxis = TransformVec4Mat4(DVector4(M1._31,M1._32,M1._33,M1._34),M2);
	WAxis = TransformVec4Mat4(DVector4(M1._41,M1._42,M1._43,M1._44),M2);
	Result._11 = XAxis.x;
	Result._12 = XAxis.y;
	Result._13 = XAxis.z;
	Result._14 = XAxis.w;
	Result._21 = YAxis.x;
	Result._22 = YAxis.y;
	Result._23 = YAxis.z;
	Result._24 = YAxis.w;
	Result._31 = ZAxis.x;
	Result._32 = ZAxis.y;
	Result._33 = ZAxis.z;
	Result._34 = ZAxis.w;
	Result._41 = WAxis.x;
	Result._42 = WAxis.y;
	Result._43 = WAxis.z;
	Result._44 = WAxis.w;
	return Result;
}

DMatrix IdentityMatrix(void) {
	return DMatrix(
		 1.0f,0.0f,0.0f,0.0f
		,0.0f,1.0f,0.0f,0.0f
		,0.0f,0.0f,1.0f,0.0f
		,0.0f,0.0f,0.0f,1.0f
	);
}
