/*
   Test-ZTransform.cpp
Author: Charles Lena <cmlena@762studios.com>
Created: 3/29/2012

Purpose: 

Tests to ensure that the math transform routines function accurately.
*/

#include "ZUnitTest.hpp"
#include <stdio.h>
#include <stdlib.h>
#include <climits>
#include <cfloat>
#include <math.h>
#include <cmath>
#include <SST/SST_Math.h>

inline bool fpcomp(float typed_1, float typed_2, float tol = FLT_EPSILON)
{
		return (std::abs(typed_1 - typed_2) <= tol);
}
inline bool dpcomp(double typed_1, double typed_2, double tol = DBL_EPSILON)
{
		return (std::abs(typed_1 - typed_2) <= tol);
}
static const SST_Vec4f X_AXIS_3D = {{1.f,0.f,0.f,1.f}};
static const SST_Vec4f Y_AXIS_3D = {{0.f,1.f,0.f,1.f}};
static const SST_Vec4f Z_AXIS_3D = {{0.f,0.f,1.f,1.f}};
static const SST_Vec4f UNIT_SQUARE[4] = {
	{{0.f,0.f,1.f}},
	{{1.f,0.f,1.f}},
	{{1.f,1.f,1.f}},
	{{0.f,1.f,1.f}},
};
static const SST_Vec4f UNIT_CUBE[8] = {
	{{0.f,0.f,0.f,1.f}},
	{{1.f,0.f,0.f,1.f}},
	{{1.f,1.f,0.f,1.f}},
	{{0.f,1.f,0.f,1.f}},
	{{0.f,0.f,1.f,1.f}},
	{{1.f,0.f,1.f,1.f}},
	{{1.f,1.f,1.f,1.f}},
	{{0.f,1.f,1.f,1.f}},
};
char msg[4096];

static const char* testSST_Math_Mat33fCreateIdentity();
static const char* testSST_Math_Mat44fCreateIdentity();
static const char* testSST_Math_Mat33fCreateTranslation();
static const char* testSST_Math_Mat44fCreateTranslation();
static const char* testSST_Math_Mat44fCreateEulerX();
static const char* testSST_Math_Mat44fCreateEulerY();
static const char* testSST_Math_Mat44fCreateEulerZ();
static const char* testSST_Math_Mat44fCreateEulerXC();
static const char* testSST_Math_Mat44fCreateEulerYC();
static const char* testSST_Math_Mat44fCreateEulerZC();
static const char* testSST_Math_Mat44fCreateShear();
static const char* testSST_Math_Mat44fCreateFreeTransform();
static const char* testSST_Math_Mat44fConvertQuaternion();
static const char* testSST_Math_Mat44fCreateRotationFromQuaternion();
static const char* testSST_Math_Mat44fCreateScale();
static const char* testSST_Math_Mat44fCreateLookAt();
static const char* testSST_Math_Mat44fCreateLookDir();
static const char* testSST_Math_Mat33fCreateFromMat44f();
static const char* testSST_Math_Mat44fCreatePerspective();
static const char* testSST_Math_Mat44fCreateOrtho();

static const char* testSST_Math_Mat44dFromVec4d();
static const char* testSST_Math_Mat44fFromVec4f();
static const char* testSST_Math_Mat33dFromVec3d();
static const char* testSST_Math_Mat33fFromVec3f();

//List of unit tests
ZUnitTest SST_Math_TransformUnitTests[] =
{
{"testSST_Math_Mat33fCreateIdentity",testSST_Math_Mat33fCreateIdentity},
{"testSST_Math_Mat44fCreateIdentity",testSST_Math_Mat44fCreateIdentity},
{"testSST_Math_Mat33fCreateTranslation",testSST_Math_Mat33fCreateTranslation},
{"testSST_Math_Mat44fCreateTranslation",testSST_Math_Mat44fCreateTranslation},
{"testSST_Math_Mat44fCreateEulerX",testSST_Math_Mat44fCreateEulerX},
{"testSST_Math_Mat44fCreateEulerY",testSST_Math_Mat44fCreateEulerY},
{"testSST_Math_Mat44fCreateEulerZ",testSST_Math_Mat44fCreateEulerZ},
{"testSST_Math_Mat44fCreateEulerXC",testSST_Math_Mat44fCreateEulerXC},
{"testSST_Math_Mat44fCreateEulerYC",testSST_Math_Mat44fCreateEulerYC},
{"testSST_Math_Mat44fCreateEulerZC",testSST_Math_Mat44fCreateEulerZC},
{"testSST_Math_Mat44fCreateScale",testSST_Math_Mat44fCreateScale},
{"testSST_Math_Mat44fCreateShear",testSST_Math_Mat44fCreateShear},
{"testSST_Math_Mat44fCreateFreeTransform",testSST_Math_Mat44fCreateFreeTransform},
{"testSST_Math_Mat44fConvertQuaternion",testSST_Math_Mat44fConvertQuaternion},
{"testSST_Math_Mat44fCreateRotationFromQuaternion",testSST_Math_Mat44fCreateRotationFromQuaternion},
{"testSST_Math_Mat44fCreateLookAt",testSST_Math_Mat44fCreateLookAt},
{"testSST_Math_Mat44fCreateLookDir",testSST_Math_Mat44fCreateLookDir},
{"testSST_Math_Mat33fCreateFromMat44f",testSST_Math_Mat33fCreateFromMat44f},
{"testSST_Math_Mat44fCreatePerspective",testSST_Math_Mat44fCreatePerspective},
{"testSST_Math_Mat44fCreateOrtho",testSST_Math_Mat44fCreateOrtho},
{"testSST_Math_Mat44dFromVec4d", testSST_Math_Mat44dFromVec4d},
{"testSST_Math_Mat44fFromVec4f", testSST_Math_Mat44fFromVec4f},
{"testSST_Math_Mat33dFromVec3d", testSST_Math_Mat33dFromVec3d},
{"testSST_Math_Mat33fFromVec3f", testSST_Math_Mat33fFromVec3f},
};

DECLARE_ZTESTBLOCK(SST_Math_Transform);

// =======================================
/*
void SST_Math_Mat33fCreateIdentity(SST_Mat33f* Mat);
void SST_Math_Mat44fCreateIdentity(SST_Mat44f* Mat);
void SST_Math_Mat33fCreateTranslation(SST_Mat33f* Mat, const float x, const float y);
void SST_Math_Mat44fCreateTranslation(SST_Mat44f* Mat, const float x, const float y, const float z);
void SST_Math_Mat44fCreateEulerX(SST_Mat44f* Mat, const float angle);
void SST_Math_Mat44fCreateEulerY(SST_Mat44f* Mat, const float angle);
void SST_Math_Mat44fCreateEulerZ(SST_Mat44f* Mat, const float angle);
void SST_Math_Mat44fCreateEulerXC(SST_Mat44f* Mat, const float angle);
void SST_Math_Mat44fCreateEulerYC(SST_Mat44f* Mat, const float angle);
void SST_Math_Mat44fCreateEulerZC(SST_Mat44f* Mat, const float angle);
void SST_Math_Mat44fCreateShear(SST_Mat44f* Mat, const float x_in_y, const float x_in_z, const float y_in_x, const float y_in_z, const float z_in_x, const float z_in_y);
void SST_Math_Mat44fCreateFreeTransform(SST_Mat44f* Mat,const float scale_x, const float scale_y, const float scale_z, const float x_in_y, const float x_in_z, const float y_in_x, const float y_in_z, const float z_in_x, const float z_in_y);
void SST_Math_Mat44fConvertQuaternion(SST_Mat44f* Mat,const SST_Vec4f* q);
void SST_Math_Mat44fCreateRotationFromQuaternion(SST_Mat44f* Mat,const SST_Vec4f* q);
void SST_Math_Mat44fCreateScale(SST_Mat44f* Mat, const float scale_x, const float scale_y, const float scale_z);
void SST_Math_Mat44fCreateLookAt(SST_Mat44f* Mat, const SST_Vec3f* eye,const SST_Vec3f* at, const SST_Vec3f* up);
void SST_Math_Mat44fCreateLookDir(SST_Mat44f* Mat, const SST_Vec3f* eye,const SST_Vec3f* dir, const SST_Vec3f* up);
void SST_Math_Mat33fCreateFromMat44f(const SST_Mat44f* Mat44, SST_Mat33f* Mat33);
void SST_Math_Mat44fCreatePerspective(SST_Mat44f* Mat, const float FOV_in_y, const float aspect_in_x, const float zNear, const float zFar);
void SST_Math_Mat44fCreateOrtho(SST_Mat44f* Mat, const float left, const float right, const float bottom, const float top, const float nearVal, const float farVal);
*/
static const char* testSST_Math_Mat33fCreateIdentity(){
	SST_Mat33f A;
	SST_Math_Mat33fCreateIdentity(&A);
	const int N = 3;
	for(int i = 0; i < N; i++)
		for(int j = 0; j < N; j++)
		{
			if(i==j) { TASSERT(A.v[i+N*j] == 1.f, "Diagonal was not 1!"); }
			else     { TASSERT(A.v[i+N*j] == 0.f, "Off-diagonal was not 0!"); }
		}

	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat44fCreateIdentity(){
	SST_Mat44f A;
	SST_Math_Mat44fCreateIdentity(&A);
	const int N = 4;
	for(int i = 0; i < N; i++)
		for(int j = 0; j < N; j++)
		{
			if(i==j) { TASSERT(A.v[i+N*j] == 1.f, "Diagonal was not 1!"); }
			else     { TASSERT(A.v[i+N*j] == 0.f, "Off-diagonal was not 0!"); }
		}
	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat33fCreateTranslation(){
	SST_Mat33f A;
	SST_Vec3f  r,p;
	const float x = (float)rand();
	const float y = (float)rand();
	r.x = 0;
	r.y = 0;
	r.z = 1.f;

	SST_Math_Mat33fCreateTranslation(&A,x,y);
	SST_Math_Mat33fMultiplyVector(&A,&r,&p);
	TASSERT(fpcomp(p.x,x,1.e-7F),"x component of p incorrect!");
	TASSERT(fpcomp(p.y,y,1.e-7F),"y component of p incorrect!");
	TASSERT(fpcomp(p.z,1.f,1.e-7F),"z(inactive) component of p incorrect!");

	SST_Math_Mat33fCreateTranslation(&A,-x,-y);
	SST_Math_Mat33fMultiplyVector(&A,&p,&r);
	TASSERT(fpcomp(r.x,0.f,1.0e-7F),"x component of r incorrect!");
	TASSERT(fpcomp(r.y,0.f,1.0e-7F),"y component of r incorrect!");
	TASSERT(fpcomp(r.z,1.f,1.0e-7F),"z(inactive) component of r incorrect!");

	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat44fCreateTranslation(){
	SST_Mat44f A;
	SST_Vec4f  r,p;
	const float x = (float)rand();
	const float y = (float)rand();
	const float z = (float)rand();
	const float w = (float)rand();
	r.x = 0.f;
	r.y = 0.f;
	r.z = 0.f;
	r.w = 1.f;

	p.w = w;

	SST_Math_Mat44fCreateTranslation(&A,x,y,z);
	SST_Math_Mat44fMultiplyVector(&A,&r,&p);
	TASSERT(fpcomp(p.x,x,1.0e-7F),"x component of p incorrect!");
	TASSERT(fpcomp(p.y,y,1.0e-7F),"y component of p incorrect!");
	TASSERT(fpcomp(p.z,z,1.0e-7F),"z component of p incorrect!");
	TASSERT(fpcomp(p.w,1.f,1.0e-7F),"w(inactive) component of p incorrect!");

	SST_Math_Mat44fCreateTranslation(&A,-x,-y,-z);
	SST_Math_Mat44fMultiplyVector(&A,&p,&r);
	TASSERT(fpcomp(r.x,0.f,1.0e-7F),"x component of r incorrect!");
	TASSERT(fpcomp(r.y,0.f,1.0e-7F),"y component of r incorrect!");
	TASSERT(fpcomp(r.z,0.f,1.0e-7F),"z(inactive) component of r incorrect!");
	TASSERT(fpcomp(r.w,1.f,1.0e-7F),"w(inactive) component of r incorrect!");

	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat44fCreateEulerX(){
	SST_Mat44f R;
	SST_Vec4f out;
	SST_Math_Mat44fCreateEulerX(&R,(float)M_PI_2);
	// Rotate X-axis around X-axis 90 degrees
	SST_Math_Mat44fMultiplyVector(&R,&X_AXIS_3D,&out);
	TASSERT(fpcomp(out.v[0],X_AXIS_3D.x,1.0e-7F),"x-axis - x component of out incorrect!");
	TASSERT(fpcomp(out.v[1],X_AXIS_3D.y,1.0e-7F),"x-axis - y component of out incorrect!");
	TASSERT(fpcomp(out.v[2],X_AXIS_3D.z,1.0e-7F),"x-axis - z component of out incorrect!");
	TASSERT(fpcomp(out.v[3],X_AXIS_3D.w,1.0e-7F),"x-axis - w component of out incorrect!");

	// Rotate Y-axis around X-axis 90 degrees
	SST_Math_Mat44fMultiplyVector(&R,&Y_AXIS_3D,&out);
	TASSERT(fpcomp(out.v[0],0.f,1.0e-7F),"y-axis - x component of out incorrect!");
	TASSERT(fpcomp(out.v[1],0.f,1.0e-7F),"y-axis - y component of out incorrect!");
	TASSERT(fpcomp(out.v[2],1.f,1.0e-7F),"y-axis - z component of out incorrect!");
	TASSERT(fpcomp(out.v[3],1.f,1.0e-7F),"y-axis - w component of out incorrect!");
	
	// Rotate Z-axis around X-axis 90 degrees gives
	SST_Math_Mat44fMultiplyVector(&R,&Z_AXIS_3D,&out);
	TASSERT(fpcomp(out.v[0], 0.f,1.0e-7F),"y-axis - x component of out incorrect!");
	TASSERT(fpcomp(out.v[1],-1.f,1.0e-7F),"y-axis - y component of out incorrect!");
	TASSERT(fpcomp(out.v[2], 0.f,1.0e-7F),"y-axis - z component of out incorrect!");
	TASSERT(fpcomp(out.v[3], 1.f,1.0e-7F),"y-axis - w component of out incorrect!");
	
	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat44fCreateEulerY(){
	SST_Mat44f R;
	SST_Vec4f out;
	SST_Math_Mat44fCreateEulerY(&R,(float)M_PI_2);
	// Rotate Y-axis around Y-axis 90 degrees
	SST_Math_Mat44fMultiplyVector(&R,&Y_AXIS_3D,&out);
	TASSERT(fpcomp(out.v[0],Y_AXIS_3D.x,1.0e-7F),"y-axis - x component of out incorrect!");
	TASSERT(fpcomp(out.v[1],Y_AXIS_3D.y,1.0e-7F),"y-axis - y component of out incorrect!");
	TASSERT(fpcomp(out.v[2],Y_AXIS_3D.z,1.0e-7F),"y-axis - z component of out incorrect!");
	TASSERT(fpcomp(out.v[3],Y_AXIS_3D.w,1.0e-7F),"y-axis - w component of out incorrect!");

	// Rotate X-axis around Y-axis 90 degrees
	SST_Math_Mat44fMultiplyVector(&R,&X_AXIS_3D,&out);
	TASSERT(fpcomp(out.v[0],0.f,1.0e-7F),"x-axis - x component of out incorrect!");
	TASSERT(fpcomp(out.v[1],0.f,1.0e-7F),"x-axis - y component of out incorrect!");
	TASSERT(fpcomp(out.v[2],-1.f,1.0e-7F),"x-axis - z component of out incorrect!");
	TASSERT(fpcomp(out.v[3],1.f,1.0e-7F),"x-axis - w component of out incorrect!");
	
	// Rotate Z-axis around Y-axis 90 degrees gives
	SST_Math_Mat44fMultiplyVector(&R,&Z_AXIS_3D,&out);
	TASSERT(fpcomp(out.v[0], 1.f,1.0e-7F),"z-axis - x component of out incorrect!");
	TASSERT(fpcomp(out.v[1], 0.f,1.0e-7F),"z-axis - y component of out incorrect!");
	TASSERT(fpcomp(out.v[2], 0.f,1.0e-7F),"z-axis - z component of out incorrect!");
	TASSERT(fpcomp(out.v[3], 1.f,1.0e-7F),"z-axis - w component of out incorrect!");
	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat44fCreateEulerZ(){
	SST_Mat44f R;
	SST_Vec4f out;
	SST_Math_Mat44fCreateEulerZ(&R,(float)M_PI_2);
	// Rotate Z-axis around Z-axis 90 degrees
	SST_Math_Mat44fMultiplyVector(&R,&Z_AXIS_3D,&out);
	TASSERT(fpcomp(out.v[0],Z_AXIS_3D.x,1.0e-7F),"z-axis - x component of out incorrect!");
	TASSERT(fpcomp(out.v[1],Z_AXIS_3D.y,1.0e-7F),"z-axis - y component of out incorrect!");
	TASSERT(fpcomp(out.v[2],Z_AXIS_3D.z,1.0e-7F),"z-axis - z component of out incorrect!");
	TASSERT(fpcomp(out.v[3],Z_AXIS_3D.w,1.0e-7F),"z-axis - w component of out incorrect!");

	// Rotate Y-axis around Z-axis 90 degrees
	SST_Math_Mat44fMultiplyVector(&R,&Y_AXIS_3D,&out);
	TASSERT(fpcomp(out.v[0],-1.f,1.0e-7F),"y-axis - x component of out incorrect!");
	TASSERT(fpcomp(out.v[1],0.f,1.0e-7F),"y-axis - y component of out incorrect!");
	TASSERT(fpcomp(out.v[2],0.f,1.0e-7F),"y-axis - z component of out incorrect!");
	TASSERT(fpcomp(out.v[3],1.f,1.0e-7F),"y-axis - w component of out incorrect!");
	
	// Rotate X-axis around Z-axis 90 degrees gives
	SST_Math_Mat44fMultiplyVector(&R,&X_AXIS_3D,&out);
	TASSERT(fpcomp(out.v[0], 0.f,1.0e-7F),"x-axis - x component of out incorrect!");
	TASSERT(fpcomp(out.v[1], 1.f,1.0e-7F),"x-axis - y component of out incorrect!");
	TASSERT(fpcomp(out.v[2], 0.f,1.0e-7F),"x-axis - z component of out incorrect!");
	TASSERT(fpcomp(out.v[3], 1.f,1.0e-7F),"x-axis - w component of out incorrect!");
	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat44fCreateScale(){
	SST_Mat44f A;
	SST_Math_Mat44fCreateScale(&A,.3333333333f,1.f,2.f);
	SST_Vec4f  x,r;
	x.x = 3.f;
	x.y = 1.f;
	x.z = .5f;
	x.w = 1.f;

	SST_Math_Mat44fMultiplyVector(&A,&x,&r);
	
	TASSERT(fpcomp(r.v[0],1.f,1.0e-7F),"x Component incorrect! (should be 1)");
	TASSERT(fpcomp(r.v[1],1.f,1.0e-7F),"y Component incorrect! (should be 1)");
	TASSERT(fpcomp(r.v[2],1.f,1.0e-7F),"z Component incorrect! (should be 1)");
	TASSERT(fpcomp(r.v[3],1.f,1.0e-7F),"w Component incorrect! (should be 1)");
	
	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat44fCreateEulerXC(){
	SST_Mat44f R;
	SST_Vec4f out;
	SST_Math_Mat44fCreateIdentity(&R);
	SST_Math_Mat44fCreateEulerXC(&R,(float)M_PI_2);
	// Rotate X-axis around X-axis 90 degrees
	SST_Math_Mat44fMultiplyVector(&R,&X_AXIS_3D,&out);
	TASSERT(fpcomp(out.v[0],X_AXIS_3D.x,1.0e-7F),"x-axis - x component of out incorrect!");
	TASSERT(fpcomp(out.v[1],X_AXIS_3D.y,1.0e-7F),"x-axis - y component of out incorrect!");
	TASSERT(fpcomp(out.v[2],X_AXIS_3D.z,1.0e-7F),"x-axis - z component of out incorrect!");
	TASSERT(fpcomp(out.v[3],X_AXIS_3D.w,1.0e-7F),"x-axis - w component of out incorrect!");

	// Rotate Y-axis around X-axis 90 degrees
	SST_Math_Mat44fMultiplyVector(&R,&Y_AXIS_3D,&out);
	TASSERT(fpcomp(out.v[0],0.f,1.0e-7F),"y-axis - x component of out incorrect!");
	TASSERT(fpcomp(out.v[1],0.f,1.0e-7F),"y-axis - y component of out incorrect!");
	TASSERT(fpcomp(out.v[2],1.f,1.0e-7F),"y-axis - z component of out incorrect!");
	TASSERT(fpcomp(out.v[3],1.f,1.0e-7F),"y-axis - w component of out incorrect!");
	
	// Rotate Z-axis around X-axis 90 degrees gives
	SST_Math_Mat44fMultiplyVector(&R,&Z_AXIS_3D,&out);
	TASSERT(fpcomp(out.v[0], 0.f,1.0e-7F),"Z-axis - x component of out incorrect!");
	TASSERT(fpcomp(out.v[1],-1.f,1.0e-7F),"z-axis - y component of out incorrect!");
	TASSERT(fpcomp(out.v[2], 0.f,1.0e-7F),"z-axis - z component of out incorrect!");
	TASSERT(fpcomp(out.v[3], 1.f,1.0e-7F),"z-axis - w component of out incorrect!");
	
	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat44fCreateEulerYC(){
	SST_Mat44f R;
	SST_Vec4f out;
	SST_Math_Mat44fCreateIdentity(&R);
	SST_Math_Mat44fCreateEulerYC(&R,(float)M_PI_2);
	// Rotate Y-axis around Y-axis 90 degrees
	SST_Math_Mat44fMultiplyVector(&R,&Y_AXIS_3D,&out);
	TASSERT(fpcomp(out.v[0],Y_AXIS_3D.x,1.0e-7F),"y-axis - x component of out incorrect!");
	TASSERT(fpcomp(out.v[1],Y_AXIS_3D.y,1.0e-7F),"y-axis - y component of out incorrect!");
	TASSERT(fpcomp(out.v[2],Y_AXIS_3D.z,1.0e-7F),"y-axis - z component of out incorrect!");
	TASSERT(fpcomp(out.v[3],Y_AXIS_3D.w,1.0e-7F),"y-axis - w component of out incorrect!");

	// Rotate X-axis around Y-axis 90 degrees
	SST_Math_Mat44fMultiplyVector(&R,&X_AXIS_3D,&out);
	TASSERT(fpcomp(out.v[0],0.f,1.0e-7F),"x-axis - x component of out incorrect!");
	TASSERT(fpcomp(out.v[1],0.f,1.0e-7F),"x-axis - y component of out incorrect!");
	TASSERT(fpcomp(out.v[2],-1.f,1.0e-7F),"x-axis - z component of out incorrect!");
	TASSERT(fpcomp(out.v[3],1.f,1.0e-7F),"x-axis - w component of out incorrect!");
	
	// Rotate Z-axis around Y-axis 90 degrees gives
	SST_Math_Mat44fMultiplyVector(&R,&Z_AXIS_3D,&out);
	TASSERT(fpcomp(out.v[0], 1.f,1.0e-7F),"z-axis - x component of out incorrect!");
	TASSERT(fpcomp(out.v[1], 0.f,1.0e-7F),"z-axis - y component of out incorrect!");
	TASSERT(fpcomp(out.v[2], 0.f,1.0e-7F),"z-axis - z component of out incorrect!");
	TASSERT(fpcomp(out.v[3], 1.f,1.0e-7F),"z-axis - w component of out incorrect!");
	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat44fCreateEulerZC(){
	SST_Mat44f R;
	SST_Vec4f out;
	SST_Math_Mat44fCreateIdentity(&R);
	SST_Math_Mat44fCreateEulerZC(&R,(float)M_PI_2);
	// Rotate Z-axis around Z-axis 90 degrees
	SST_Math_Mat44fMultiplyVector(&R,&Z_AXIS_3D,&out);
	TASSERT(fpcomp(out.v[0],Z_AXIS_3D.x,1.0e-7F),"z-axis - x component of out incorrect!");
	TASSERT(fpcomp(out.v[1],Z_AXIS_3D.y,1.0e-7F),"z-axis - y component of out incorrect!");
	TASSERT(fpcomp(out.v[2],Z_AXIS_3D.z,1.0e-7F),"z-axis - z component of out incorrect!");
	TASSERT(fpcomp(out.v[3],Z_AXIS_3D.w,1.0e-7F),"z-axis - w component of out incorrect!");

	// Rotate Y-axis around Z-axis 90 degrees
	SST_Math_Mat44fMultiplyVector(&R,&Y_AXIS_3D,&out);
	TASSERT(fpcomp(out.v[0],-1.f,1.0e-7F),"y-axis - x component of out incorrect!");
	TASSERT(fpcomp(out.v[1],0.f,1.0e-7F),"y-axis - y component of out incorrect!");
	TASSERT(fpcomp(out.v[2],0.f,1.0e-7F),"y-axis - z component of out incorrect!");
	TASSERT(fpcomp(out.v[3],1.f,1.0e-7F),"y-axis - w component of out incorrect!");
	
	// Rotate X-axis around Z-axis 90 degrees gives
	SST_Math_Mat44fMultiplyVector(&R,&X_AXIS_3D,&out);
	TASSERT(fpcomp(out.v[0], 0.f,1.0e-7F),"x-axis - x component of out incorrect!");
	TASSERT(fpcomp(out.v[1], 1.f,1.0e-7F),"x-axis - y component of out incorrect!");
	TASSERT(fpcomp(out.v[2], 0.f,1.0e-7F),"x-axis - z component of out incorrect!");
	TASSERT(fpcomp(out.v[3], 1.f,1.0e-7F),"x-axis - w component of out incorrect!");
	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat44fCreateShear(){
	SST_Mat44f R;
	SST_Vec4f out[8];
	SST_Math_Mat44fCreateShear(&R,2.f,0.f,0.f,0.f,0.f,0.f);

	for(int i = 0; i < 8; i++)
	{
		SST_Math_Mat44fMultiplyVector(&R,&UNIT_CUBE[i],&out[i]);
		sprintf(msg,"(%5.3f,%5.3f,%5.3f,%5.3f) was not sheared by x+=2y (i=%d)",UNIT_CUBE[i].x,UNIT_CUBE[i].y,UNIT_CUBE[i].z,UNIT_CUBE[i].w,i);
		TASSERT(fpcomp(out[i].x,UNIT_CUBE[i].x+UNIT_CUBE[i].y*2.f,1e-7F),msg);
		TASSERT(fpcomp(out[i].y,UNIT_CUBE[i].y+UNIT_CUBE[i].y*0.f,1e-7F),msg);
		TASSERT(fpcomp(out[i].z,UNIT_CUBE[i].z+UNIT_CUBE[i].y*0.f,1e-7F),msg);
		TASSERT(fpcomp(out[i].w,UNIT_CUBE[i].w+UNIT_CUBE[i].y*0.f,1e-7F),msg);
	}
	/* if != 0, then we're shearing */
	float x_in_y = 2.f;
	float x_in_z = 2.f;
	float y_in_x = 2.f;
	float y_in_z = 0.f;
	float z_in_x = 1.f;
	float z_in_y = 0.f;

	/* these will be used in the free transform */
	float scale_x = .333333333f;
	float scale_y = 1.f;
	float scale_z = 2.f;

	SST_Math_Mat44fCreateFreeTransform(&R,scale_x,scale_y,scale_z,x_in_y,x_in_z,y_in_x,y_in_z,z_in_x,z_in_y);
	for(int i = 0; i < 8; i++)
	{
		SST_Math_Mat44fMultiplyVector(&R,&UNIT_CUBE[i],&out[i]);
		sprintf(msg,"(%5.3f,%5.3f,%5.3f,%5.3f) was not sheared by x = %5.3fx+%5.3fy+%5.3fz (i=%d)",UNIT_CUBE[i].x,UNIT_CUBE[i].y,UNIT_CUBE[i].z,UNIT_CUBE[i].w,scale_x,x_in_y,x_in_z,i);
		TASSERT(fpcomp(out[i].x,UNIT_CUBE[i].x*scale_x+UNIT_CUBE[i].y*x_in_y  + UNIT_CUBE[i].z*x_in_z ,1e-7F),msg);
		sprintf(msg,"(%5.3f,%5.3f,%5.3f,%5.3f) was not sheared by x = %5.3fx+%5.3fy+%5.3fz (i=%d)",UNIT_CUBE[i].x,UNIT_CUBE[i].y,UNIT_CUBE[i].z,UNIT_CUBE[i].w,y_in_x,scale_y,y_in_z,i);
		TASSERT(fpcomp(out[i].y,UNIT_CUBE[i].x*y_in_x +UNIT_CUBE[i].y*scale_y + UNIT_CUBE[i].z*y_in_z ,1e-7F),msg);
		sprintf(msg,"(%5.3f,%5.3f,%5.3f,%5.3f) was not sheared by x = %5.3fx+%5.3fy+%5.3fz (i=%d)",UNIT_CUBE[i].x,UNIT_CUBE[i].y,UNIT_CUBE[i].z,UNIT_CUBE[i].w,z_in_x,z_in_y,scale_z,i);
		TASSERT(fpcomp(out[i].z,UNIT_CUBE[i].x*z_in_x +UNIT_CUBE[i].y*z_in_y  + UNIT_CUBE[i].z*scale_z,1e-7F),msg);
		sprintf(msg,"(%5.3f,%5.3f,%5.3f,%5.3f) was not sheared by w = w (i=%d)",UNIT_CUBE[i].x,UNIT_CUBE[i].y,UNIT_CUBE[i].z,UNIT_CUBE[i].w,i);
		TASSERT(fpcomp(out[i].w,UNIT_CUBE[i].w,1e-7F),msg);
	}
	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat44fCreateFreeTransform(){
		SST_Mat44f R;
	SST_Vec4f out[8];
	/* if != 0, then we're shearing */
	float x_in_y = 2.f;
	float x_in_z = 2.f;
	float y_in_x = 2.f;
	float y_in_z = 0.f;
	float z_in_x = 1.f;
	float z_in_y = 0.f;

	/* these will be used in the free transform */
	float scale_x = 1.f;
	float scale_y = 1.f;
	float scale_z = 1.f;

	SST_Math_Mat44fCreateFreeTransform(&R,scale_x,scale_y,scale_z,x_in_y,x_in_z,y_in_x,y_in_z,z_in_x,z_in_y);
	for(int i = 0; i < 8; i++)
	{
		SST_Math_Mat44fMultiplyVector(&R,&UNIT_CUBE[i],&out[i]);
		sprintf(msg,"(%5.3f,%5.3f,%5.3f,%5.3f) was not sheared by x = %5.3fx+%5.3fy+%5.3fz (i=%d)",UNIT_CUBE[i].x,UNIT_CUBE[i].y,UNIT_CUBE[i].z,UNIT_CUBE[i].w,scale_x,x_in_y,x_in_z,i);
		TASSERT(fpcomp(out[i].x,UNIT_CUBE[i].x*scale_x+UNIT_CUBE[i].y*x_in_y  + UNIT_CUBE[i].z*x_in_z ,1e-7F),msg);
		sprintf(msg,"(%5.3f,%5.3f,%5.3f,%5.3f) was not sheared by x = %5.3fx+%5.3fy+%5.3fz (i=%d)",UNIT_CUBE[i].x,UNIT_CUBE[i].y,UNIT_CUBE[i].z,UNIT_CUBE[i].w,y_in_x,scale_y,y_in_z,i);
		TASSERT(fpcomp(out[i].y,UNIT_CUBE[i].x*y_in_x +UNIT_CUBE[i].y*scale_y + UNIT_CUBE[i].z*y_in_z ,1e-7F),msg);
		sprintf(msg,"(%5.3f,%5.3f,%5.3f,%5.3f) was not sheared by x = %5.3fx+%5.3fy+%5.3fz (i=%d)",UNIT_CUBE[i].x,UNIT_CUBE[i].y,UNIT_CUBE[i].z,UNIT_CUBE[i].w,z_in_x,z_in_y,scale_z,i);
		TASSERT(fpcomp(out[i].z,UNIT_CUBE[i].x*z_in_x +UNIT_CUBE[i].y*z_in_y  + UNIT_CUBE[i].z*scale_z,1e-7F),msg);
		sprintf(msg,"(%5.3f,%5.3f,%5.3f,%5.3f) was not sheared by w = w (i=%d)",UNIT_CUBE[i].x,UNIT_CUBE[i].y,UNIT_CUBE[i].z,UNIT_CUBE[i].w,i);
		TASSERT(fpcomp(out[i].w,UNIT_CUBE[i].w,1e-7F),msg);
	}
	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat44dFromVec4d(){
	SST_Mat44d A;
	SST_Vec4d  tits;
	SST_Vec4d  leet;

	/* TITS! */
	tits.v[0] = 7.0;
	tits.v[1] = 1.0;
	tits.v[2] = 7.0;
	tits.v[3] = 5.0;

	/* LEET! */
	leet.v[0] = 1.0;
	leet.v[1] = 3.0;
	leet.v[2] = 3.0;
	leet.v[3] = 7.0;

	for(int col=0; col < 4; col++){
		/* leet */
		A.v[0+col*4] = leet.x;
		A.v[1+col*4] = leet.y;
		A.v[2+col*4] = leet.z;
		A.v[3+col*4] = leet.w;
		SST_Math_Mat44dFromVec4d(&A,&tits,col);
		TASSERT(A.v[0+col*4] == tits.x, "x component failed in one of the iterations");
		TASSERT(A.v[1+col*4] == tits.y, "y component failed in one of the iterations");
		TASSERT(A.v[2+col*4] == tits.z, "z component failed in one of the iterations");
		TASSERT(A.v[3+col*4] == tits.w, "w component failed in one of the iterations");
	}

	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat44fFromVec4f(){
	SST_Mat44f A;
	SST_Vec4f  tits;
	SST_Vec4f  leet;

	/* TITS! */
	tits.v[0] = 7.0f;
	tits.v[1] = 1.0f;
	tits.v[2] = 7.0f;
	tits.v[3] = 5.0f;

	/* LEET! */
	leet.v[0] = 1.0f;
	leet.v[1] = 3.0f;
	leet.v[2] = 3.0f;
	leet.v[3] = 7.0f;

	for(int col=0; col < 4; col++){
		/* leet */
		A.v[0+col*4] = leet.x;
		A.v[1+col*4] = leet.y;
		A.v[2+col*4] = leet.z;
		A.v[3+col*4] = leet.w;
		SST_Math_Mat44fFromVec4f(&A,&tits,col);
		TASSERT(A.v[0+col*4] == tits.x, "x component failed in one of the iterations");
		TASSERT(A.v[1+col*4] == tits.y, "y component failed in one of the iterations");
		TASSERT(A.v[2+col*4] == tits.z, "z component failed in one of the iterations");
		TASSERT(A.v[3+col*4] == tits.w, "w component failed in one of the iterations");
	}

	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat33dFromVec3d(){
	SST_Mat33d A;
	SST_Vec3d  tits;
	SST_Vec3d  leet;

	/* TITS! */
	tits.v[0] = 7.0;
	tits.v[1] = 1.0;
	tits.v[2] = 7.0;

	/* LEET! */
	leet.v[0] = 1.0;
	leet.v[1] = 3.0;
	leet.v[2] = 3.0;

	for(int col=0; col < 3; col++){
		/* leet */
		A.v[0+col*3] = leet.x;
		A.v[1+col*3] = leet.y;
		A.v[2+col*3] = leet.z;
		SST_Math_Mat33dFromVec3d(&A,&tits,col);
		TASSERT(A.v[0+col*3] == tits.x, "x component failed in one of the iterations");
		TASSERT(A.v[1+col*3] == tits.y, "y component failed in one of the iterations");
		TASSERT(A.v[2+col*3] == tits.z, "z component failed in one of the iterations");
	}


	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat33fFromVec3f(){
	SST_Mat33f A;
	SST_Vec3f  tits;
	SST_Vec3f  leet;

	/* TITS! */
	tits.v[0] = 7.0f;
	tits.v[1] = 1.0f;
	tits.v[2] = 7.0f;

	/* LEET! */
	leet.v[0] = 1.0f;
	leet.v[1] = 3.0f;
	leet.v[2] = 3.0f;

	for(int col=0; col < 3; col++){
		/* leet */
		A.v[0+col*3] = leet.x;
		A.v[1+col*3] = leet.y;
		A.v[2+col*3] = leet.z;
		SST_Math_Mat33fFromVec3f(&A,&tits,col);
		TASSERT(A.v[0+col*3] == tits.x, "x component failed in one of the iterations");
		TASSERT(A.v[1+col*3] == tits.y, "y component failed in one of the iterations");
		TASSERT(A.v[2+col*3] == tits.z, "z component failed in one of the iterations");
	}

	return ZTEST_SUCCESS;
}
#if SST_COMPILER == SST_COMPILER_MSVC
#pragma warning(push)
#pragma warning(disable:4127)
#endif
static const char* testSST_Math_Mat44fConvertQuaternion(){
	TASSERT(false,"Test Not Implemented Yet!");
	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat44fCreateRotationFromQuaternion(){
	TASSERT(false,"Test Not Implemented Yet!");
	return ZTEST_SUCCESS;
}

static const char* testSST_Math_Mat44fCreateLookAt(){
	TASSERT(false,"Test Not Implemented Yet!");
	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat44fCreateLookDir(){
	TASSERT(false,"Test Not Implemented Yet!");
	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat33fCreateFromMat44f(){
	TASSERT(false,"Test Not Implemented Yet!");
	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat44fCreatePerspective(){
	TASSERT(false,"Test Not Implemented Yet!");
	return ZTEST_SUCCESS;
}
static const char* testSST_Math_Mat44fCreateOrtho(){
	TASSERT(false,"Test Not Implemented Yet!");
	return ZTEST_SUCCESS;
}
#if SST_COMPILER == SST_COMPILER_MSVC
#pragma warning(pop)
#endif