/*
   Test-SST_Geo.cpp
Author: Charles Lena <cmlena@762studios.com>
Created: 2/22/2013

Purpose: 

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

#include "ZUnitTest.hpp"
#include <stdio.h>
#include <stdlib.h>
#include <climits>
#include <cfloat>
#include <cmath>
#include <SST/SST_Math.h>
#include <SST/SST_Geo.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 char* testSST_Geo_Intersect();
static const char* testSST_Geo_RayPlaneIntersectionF();
static const char* testSST_Geo_RayPlaneIntersectionD();

//List of unit tests
ZUnitTest SST_Math_GeoUnitTests[] =
{
{"testSST_Geo_Intersect", testSST_Geo_Intersect},
{"testSST_Geo_RayPlaneIntersectionF", testSST_Geo_RayPlaneIntersectionF},
{"testSST_Geo_RayPlaneIntersectionD", testSST_Geo_RayPlaneIntersectionD},
};

DECLARE_ZTESTBLOCK(SST_Math_Geo);

// =======================================
/*
*/
#if SST_COMPILER == SST_COMPILER_MSVC
#pragma warning(push)
#pragma warning(disable:4127)
#endif
static const char* testSST_Geo_Intersect(){
	//
	#define SET_LINE1(ax1,ay1,ax2,ay2) (Line1_Point1).v[0]=ax1; (Line1_Point1).v[1]=ay1; (Line1_Point2).v[0]=ax2; (Line1_Point2).v[1]=ay2
	#define SET_LINE2(ax1,ay1,ax2,ay2) (Line2_Point1).v[0]=ax1; (Line2_Point1).v[1]=ay1; (Line2_Point2).v[0]=ax2; (Line2_Point2).v[1]=ay2
	SST_Vec2f Line1_Point1, Line1_Point2, Line2_Point1, Line2_Point2; // our input test points
	SST_Vec2f overlaps[2]; // will be our output argument

	// Intersection test - perpendicular lines sharing no points
	SET_LINE1(0.f,0.f,0.f,1.f);
	SET_LINE2(1.f,0.f,2.f,0.f);
	TASSERT(SST2DLINESEGINTERSECT_NONE==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Shouldn't intersect");
	
	// Intersection test - perpendicular lines sharing no points - flipped
	SET_LINE2(0.f,0.f,0.f,1.f);
	SET_LINE1(1.f,0.f,2.f,0.f);
	TASSERT(SST2DLINESEGINTERSECT_NONE==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Shouldn't intersect - flipped");

	// SST_Geo_2DSegmentSegmentIntersectionFion test - perpendicular lines sharing internal point
	SET_LINE1(0.f,0.f,1.f,0.f);
	SET_LINE2(.5f,-1.f,.5f,1.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");

	// SST_Geo_2DSegmentSegmentIntersectionFion test - perpendicular lines sharing internal point
	SET_LINE2(0.f,0.f,1.f,0.f);
	SET_LINE1(.5f,-1.f,.5f,1.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect - flipped");

	// SST_Geo_2DSegmentSegmentIntersectionFion test - perpendicular lines sharing internal-bound 1-2
	SET_LINE1(0.f,0.f,1.f,0.f);
	SET_LINE2(.5f,0.f,.5f,1.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect - flipped");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - perpendicular lines sharing internal-bound 1-1
	SET_LINE1(0.f,0.f,1.f,0.f);
	SET_LINE2(.5f,1.f,.5f,0.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect - flipped");	
	
	// SST_Geo_2DSegmentSegmentIntersectionFion test - perpendicular lines sharing internal-bound 2-2
	SET_LINE2(0.f,0.f,1.f,0.f);
	SET_LINE1(.5f,0.f,.5f,1.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect - flipped");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - perpendicular lines sharing bound point 2-1
	SET_LINE2(0.f,0.f,1.f,0.f);
	SET_LINE1(.5f,1.f,.5f,0.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect - flipped");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - perpendicular lines sharing bound point 1-1
	SET_LINE1(0.f,0.f,0.f,1.f);
	SET_LINE2(0.f,0.f,1.f,0.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect - flipped");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - perpendicular lines sharing bound point 1-2
	SET_LINE2(0.f,0.f,0.f,1.f);
	SET_LINE1(0.f,0.f,1.f,0.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect - flipped");	
	
	// SST_Geo_2DSegmentSegmentIntersectionFion test - perpendicular lines sharing bound point 2-1
	SET_LINE1(0.f,0.f,0.f,1.f);
	SET_LINE2(1.f,0.f,0.f,0.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect - flipped");	
		
	// SST_Geo_2DSegmentSegmentIntersectionFion test - perpendicular lines sharing bound point 2-2
	SET_LINE2(0.f,0.f,0.f,1.f);
	SET_LINE1(1.f,0.f,0.f,0.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect - flipped");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - pi/4 oriented lines sharing no points
	SET_LINE1(0.f,0.f,1.f,0.f);
	SET_LINE2(0.f,1.f,2.f,2.f);
	TASSERT(SST2DLINESEGINTERSECT_NONE==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Shouldn't intersect");	
		
	// SST_Geo_2DSegmentSegmentIntersectionFion test - pi/4 oriented lines sharing internal point 
	SET_LINE1(0.f,0.f,1.f,0.f);
	SET_LINE2(-1.f,-1.f,1.f,1.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - pi/4 oriented lines sharing internal point 
	SET_LINE2(0.f,0.f,1.f,0.f);
	SET_LINE1(-1.f,-1.f,1.f,1.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	
		
	// SST_Geo_2DSegmentSegmentIntersectionFion test - pi/4 oriented lines sharing internal point 
	SET_LINE1(0.f,0.f,1.f,0.f);
	SET_LINE2( 1.f, 1.f,-1.f,-1.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - pi/4 oriented lines sharing internal point 
	SET_LINE1(1.f,0.f,0.f,0.f);
	SET_LINE2( 1.f, 1.f,-1.f,-1.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - pi/4 oriented lines sharing internal point 
	SET_LINE1(1.f,0.f,0.f,0.f);
	SET_LINE2(-1.f,-1.f, 1.f, 1.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - pi/4 oriented lines sharing bound point - 1,2,B,1,1
	SET_LINE1(0.f,0.f,1.f,0.f);
	SET_LINE2(0.f,0.f,1.f,1.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - pi/4 oriented lines sharing bound point- 2,1,B,1,1
	SET_LINE2(0.f,0.f,1.f,0.f);
	SET_LINE1(0.f,0.f,1.f,1.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - pi/4 oriented lines sharing bound point- 1,2,B,2,2
	SET_LINE1(1.f,0.f,0.f,0.f);
	SET_LINE2(1.f,1.f,0.f,0.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	
		
	// SST_Geo_2DSegmentSegmentIntersectionFion test - pi/4 oriented lines sharing bound point- 2,1,B,2,2
	SET_LINE2(1.f,0.f,0.f,0.f);
	SET_LINE1(1.f,1.f,0.f,0.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - pi/4 oriented lines sharing bound point- 1,2,B,1,2
	SET_LINE2(0.f,0.f,1.f,0.f);
	SET_LINE1(1.f,1.f,0.f,0.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - parallel linesegs sharing no points on the line EW
	SET_LINE1(0.f,0.f,1.f,0.f);
	SET_LINE2(-1.f,1.f,0.f,1.f);
	TASSERT(SST2DLINESEGINTERSECT_NONE==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - parallel linesegs sharing no points on the line EW
	SET_LINE1(0.f,0.f,0.f,1.f);
	SET_LINE2(1.f,-1.f,1.f,0.f);
	TASSERT(SST2DLINESEGINTERSECT_NONE==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	
		
	// SST_Geo_2DSegmentSegmentIntersectionFion test - parallel colinear lines sharing no points NS
	SET_LINE1(0.f,0.f,0.f,1.f);
	SET_LINE2(1.f,0.f,1.f,1.f);
	TASSERT(SST2DLINESEGINTERSECT_NONE==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	
		
	// SST_Geo_2DSegmentSegmentIntersectionFion test - parallel colinear lines sharing no points NS
	SET_LINE2(0.f,0.f,0.f,1.f);
	SET_LINE1(1.f,0.f,1.f,1.f);
	TASSERT(SST2DLINESEGINTERSECT_NONE==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	
	
	// SST_Geo_2DSegmentSegmentIntersectionFion test - parallel lines sharing one line inside the other
	SET_LINE1(0.f,0.f,0.f,1.f);
	SET_LINE2(0.f,-.5f,0.f,1.5f);
	TASSERT(SST2DLINESEGINTERSECT_OVERLAP==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - parallel lines sharing one line inside the other
	SET_LINE2(0.f,0.f,0.f,1.f);
	SET_LINE1(0.f,-.5f,0.f,1.5f);
	TASSERT(SST2DLINESEGINTERSECT_OVERLAP==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");

	// SST_Geo_2DSegmentSegmentIntersectionFion test - parallel lines sharing some number of points > 1
	SET_LINE1(0.f,0.f,0.f,1.f);
	SET_LINE2(0.f,.5f,0.f,1.5f);
	TASSERT(SST2DLINESEGINTERSECT_OVERLAP==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - parallel lines sharing some number of points > 1
	SET_LINE2(0.f,0.f,0.f,1.f);
	SET_LINE1(0.f,.5f,0.f,1.5f);
	TASSERT(SST2DLINESEGINTERSECT_OVERLAP==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	
	
	// SST_Geo_2DSegmentSegmentIntersectionFion test - parallel lines sharing exactly 1 point
	SET_LINE1(0.f,0.f,0.f,1.f);
	SET_LINE2(0.f,1.f,0.f,2.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - parallel lines sharing exactly 1 point
	SET_LINE2(0.f,0.f,0.f,1.f);
	SET_LINE1(0.f,1.f,0.f,2.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - parallel lines sharing exactly 1 point
	SET_LINE1(0.f,0.f,0.f,1.f);
	SET_LINE2(0.f,2.f,0.f,1.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - parallel lines sharing exactly 1 point
	SET_LINE1(0.f,1.f,0.f,0.f);
	SET_LINE2(0.f,2.f,0.f,1.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	

	// SST_Geo_2DSegmentSegmentIntersectionFion test - parallel lines sharing exactly 1 point
	SET_LINE1(0.f,1.f,0.f,0.f);
	SET_LINE2(0.f,1.f,0.f,2.f);
	TASSERT(SST2DLINESEGINTERSECT_POINT==SST_Geo_2DSegmentSegmentIntersectionF(&Line1_Point1,&Line1_Point2,&Line2_Point1,&Line2_Point2,overlaps),"Should intersect");	

	#undef SET_LINE1
	#undef SET_LINE2
	return ZTEST_SUCCESS;
}
static const char* testSST_Geo_RayPlaneIntersectionF(){
	/* Triangle ABC */
#define SET_VEC3(a,x,y,z) (a).v[0]=x; (a).v[1]=y; (a).v[2]=z
#define COMPARE_VEC3(vec1,vec2,tol) (( std::abs(vec1.x-vec2.x)+std::abs(vec1.y-vec2.y)+std::abs(vec1.z-vec2.z) ) < tol)
	SST_Vec3f A, B, C;
	SET_VEC3(A,1.0f,0.0f,0.0f);
	SET_VEC3(B,0.0f,1.0f,0.0f);
	SET_VEC3(C,0.0f,0.0f,1.0f);
	/* Origin point for rays */
	SST_Vec3f ray_point;
	SET_VEC3(ray_point,1.0f,1.0f,1.0f);
	SST_Vec3f ray, intersect, answer;
	SET_VEC3(ray,-1.0f,-1.0f,-1.0f);

	bool inTriangle = false;
	inTriangle = SST_Geo_RayPlaneIntersectionF(&ray,&ray_point,&A,&B,&C,&intersect);
	/* Also, should intersect at some point which I don't know! heheheh */
	TASSERT(inTriangle,"Test1 - Should have intersected!");

	SET_VEC3(ray,-1.0f,-1.0f,0.0f);
	SET_VEC3(answer,0.0f,0.0f,1.0f);
	inTriangle = SST_Geo_RayPlaneIntersectionF(&ray,&ray_point,&A,&B,&C,&intersect);
	/* Also, should intersect at some point which I don't know! heheheh */
	TASSERT(COMPARE_VEC3(intersect,answer,1.0e-6),"Test 2 - Incorrect Intersection point!");
	TASSERT(inTriangle,"Test 2 - Should have intersected!");

	SET_VEC3(ray,0.0f,-1.0f,-1.0f);
	SET_VEC3(answer,1.0f,0.0f,0.0f);
	inTriangle = SST_Geo_RayPlaneIntersectionF(&ray,&ray_point,&A,&B,&C,&intersect);
	/* Also, should intersect at some point which I don't know! heheheh */
	TASSERT(COMPARE_VEC3(intersect,answer,1.0e-6),"Test 3 - Incorrect Intersection point!");
	TASSERT(inTriangle,"Test 3 - Should have intersected!");

	SET_VEC3(ray,-1.0f,0.0f,-1.0f);
	SET_VEC3(answer,0.0f,1.0f,0.0f);
	inTriangle = SST_Geo_RayPlaneIntersectionF(&ray,&ray_point,&A,&B,&C,&intersect);
	/* Also, should intersect at some point which I don't know! heheheh */
	TASSERT(COMPARE_VEC3(intersect,answer,1.0e-6),"Test 4 - Incorrect Intersection point!");
	TASSERT(inTriangle,"Test 4 - Should have intersected!");
	
	SET_VEC3(A,  0.0f,0.0f,0.0f);
	SET_VEC3(B,  1.0f,0.0f,0.0f);
	SET_VEC3(C,  0.0f,1.0f,0.0f);
	SET_VEC3(ray_point, 1.0f,1.0f,1.0f);
	SET_VEC3(ray,0.0f,0.0f,-1.0f);
	SET_VEC3(answer,1.0f,1.0f,0.0f);
	inTriangle = SST_Geo_RayPlaneIntersectionF(&ray,&ray_point,&A,&B,&C,&intersect);
	TASSERT(COMPARE_VEC3(intersect,answer,1.0e-6),"Test 4 - Incorrect Plane Intersection point!");
	TASSERT(!inTriangle,"Test 4 - Should have not intersected (with triangle)!");
	return ZTEST_SUCCESS;
}
static const char* testSST_Geo_RayPlaneIntersectionD(){
#define SET_VEC3(a,x,y,z) (a).v[0]=x; (a).v[1]=y; (a).v[2]=z
#define COMPARE_VEC3(vec1,vec2,tol) (( std::abs(vec1.x-vec2.x)+std::abs(vec1.y-vec2.y)+std::abs(vec1.z-vec2.z) ) < tol)
	SST_Vec3d A, B, C;
	SET_VEC3(A,1.0,0.0,0.0);
	SET_VEC3(B,0.0,1.0,0.0);
	SET_VEC3(C,0.0,0.0,1.0);
	/* Origin point for rays */
	SST_Vec3d ray_point;
	SET_VEC3(ray_point,1.0,1.0,1.0);
	SST_Vec3d ray, intersect, answer;
	SET_VEC3(ray,-1.0,-1.0,-1.0);

	bool inTriangle = false;
	inTriangle = SST_Geo_RayPlaneIntersectionD(&ray,&ray_point,&A,&B,&C,&intersect);
	/* Also, should intersect at some point which I don't know! heheheh */
	TASSERT(inTriangle,"Test1 - Should have intersected!");

	SET_VEC3(ray,-1.0,-1.0,0.0);
	SET_VEC3(answer,0.0,0.0,1.0);
	inTriangle = SST_Geo_RayPlaneIntersectionD(&ray,&ray_point,&A,&B,&C,&intersect);
	/* Also, should intersect at some point which I don't know! heheheh */
	TASSERT(COMPARE_VEC3(intersect,answer,1.0e-6),"Test 2 - Incorrect Intersection point!");
	TASSERT(inTriangle,"Test 2 - Should have intersected!");

	SET_VEC3(ray,0.0,-1.0,-1.0);
	SET_VEC3(answer,1.0,0.0,0.0);
	inTriangle = SST_Geo_RayPlaneIntersectionD(&ray,&ray_point,&A,&B,&C,&intersect);
	/* Also, should intersect at some point which I don't know! heheheh */
	TASSERT(COMPARE_VEC3(intersect,answer,1.0e-6),"Test 3 - Incorrect Intersection point!");
	TASSERT(inTriangle,"Test 3 - Should have intersected!");

	SET_VEC3(ray,-1.0,0.0,-1.0);
	SET_VEC3(answer,0.0,1.0,0.0);
	inTriangle = SST_Geo_RayPlaneIntersectionD(&ray,&ray_point,&A,&B,&C,&intersect);
	/* Also, should intersect at some point which I don't know! heheheh */
	TASSERT(COMPARE_VEC3(intersect,answer,1.0e-6),"Test 4 - Incorrect Intersection point!");
	TASSERT(inTriangle,"Test 4 - Should have intersected!");
	
	SET_VEC3(A,  0.0,0.0,0.0);
	SET_VEC3(B,  1.0,0.0,0.0);
	SET_VEC3(C,  0.0,1.0,0.0);
	SET_VEC3(ray_point, 1.0,1.0,1.0);
	SET_VEC3(ray,0.0,0.0,-1.0);
	SET_VEC3(answer,1.0,1.0,0.0);
	inTriangle = SST_Geo_RayPlaneIntersectionD(&ray,&ray_point,&A,&B,&C,&intersect);
	TASSERT(COMPARE_VEC3(intersect,answer,1.0e-6),"Test 4 - Incorrect Plane Intersection point!");
	TASSERT(!inTriangle,"Test 4 - Should have not intersected (with triangle)!");

	return ZTEST_SUCCESS;
}
#if SST_COMPILER == SST_COMPILER_MSVC
#pragma warning(pop)
#endif