///////////////////////////////////////////////////////////////////////////////
/// @file deGeometry.cpp
///
/// @brief routines for doing geometric calculations
///
/// @author Assassin
///
/// This file is the intellectual property of Novus Delta, LLC.. Usage of the
/// contents of this file is subject to the Destiny3D Member License which
/// can be found at http://www.destiny3d.com.  Any other usage is prohibited.
///
/// This file is distributed "AS IS" without warranty of any kind.  Novus
/// Delta, LLC. does not guarantee the fitness of the contents of this file
/// for any particular purpose.
///
/// Copyright (C) 2001-2003 Novus Delta, LLC. All Rights Reserved.
///
/// <hr>
///                                 Change History
/// <hr>
///
/// @date Jan 2003
/// @author Assassin
/// @remarks Creation
///
///////////////////////////////////////////////////////////////////////////////

#include "deMath.hpp"

template <typename T>
static void swap(T & lhs, T & rhs)
{
	T temp = lhs;
	lhs = rhs;
	rhs = temp;
}

deBoolean deGeometry::AABBCollision(const deAABB & MovingBox, const deVec3d & vMoveStart, const deVec3d & vMoveEnd, 
									const deAABB & StaticBox, const deVec3d & vStaticPos, 
									deVec3d & MovingColPt, deVec3d & ColNormal, deDouble & Percent)
{
	const static deVec3d Axes[3] = {{1,0,0},{0,1,0},{0,0,1}};
	const static deVec3d & PosX = Axes[0];
	const static deVec3d & PosY = Axes[1];
	const static deVec3d & PosZ = Axes[2];
	const deDouble BigVal = 1.0e308;
	
	deVec3d Dir;
	deDouble Multiple, MinXYZ, MaxXYZ;
	deVec3d ZeroedMoveStart;	// relocated start point so that the static box is on the origin
	deAABB ExpandedStaticBox;	// add the two boxes together, then use a point-sweep (ray)
	deAABB CollisionPoints;		// an abuse of the type, but it fits
	char PickedMin;

	// expand the static box, since we can then do a ray-test on it
	ExpandedStaticBox.Min.x = StaticBox.Min.x - MovingBox.Max.x;
	ExpandedStaticBox.Min.y = StaticBox.Min.y - MovingBox.Max.y;
	ExpandedStaticBox.Min.z = StaticBox.Min.z - MovingBox.Max.z;
	ExpandedStaticBox.Max.x = StaticBox.Max.x - MovingBox.Min.x;
	ExpandedStaticBox.Max.y = StaticBox.Max.y - MovingBox.Min.y;
	ExpandedStaticBox.Max.z = StaticBox.Max.z - MovingBox.Min.z;

	// put the static box on the origin, makes things easier
	ZeroedMoveStart = vMoveStart;
	ZeroedMoveStart -= vStaticPos;

	// get the direction vector
	Dir = vMoveEnd;
	Dir -= vMoveStart;
	
	// this relies on "min" being closer to the ray start than "max"
	// so if the ray points "backwards" then swap the min & max
	if (Dir.x < 0)
		swap(ExpandedStaticBox.Min.x, ExpandedStaticBox.Max.x);
	if (Dir.y < 0)
		swap(ExpandedStaticBox.Min.y, ExpandedStaticBox.Max.y);
	if (Dir.z < 0)
		swap(ExpandedStaticBox.Min.z, ExpandedStaticBox.Max.z);
		
	// now do a "slabs" test
	if (!PlaneRayIntersect(PosX, ExpandedStaticBox.Max.x, ZeroedMoveStart, Dir, Multiple) && Multiple == 0)
	{
		// if the ray doesn't hit the box, then it can slide along the side
		// since it's parallel to this side, then we can test the other axis values
		if ((ZeroedMoveStart.y <= ExpandedStaticBox.Min.y || ZeroedMoveStart.y >= ExpandedStaticBox.Max.y)
			&& (ZeroedMoveStart.z <= ExpandedStaticBox.Min.z || ZeroedMoveStart.z >= ExpandedStaticBox.Max.z))
			return deFALSE;
		CollisionPoints.Max.x = BigVal;
		CollisionPoints.Min.x = -BigVal;
	}
	else
	{
		CollisionPoints.Max.x = Multiple;
		PlaneRayIntersect(PosX, ExpandedStaticBox.Min.x, ZeroedMoveStart, Dir, CollisionPoints.Min.x);
	}
	
	if (!PlaneRayIntersect(PosY, ExpandedStaticBox.Max.y, ZeroedMoveStart, Dir, Multiple) && Multiple == 0)
	{
		// if the ray doesn't hit the box, then it can slide along the side
		// since it's parallel to this side, then we can test the other axis values
		if ((ZeroedMoveStart.x <= ExpandedStaticBox.Min.x || ZeroedMoveStart.x >= ExpandedStaticBox.Max.x)
			&& (ZeroedMoveStart.z <= ExpandedStaticBox.Min.z || ZeroedMoveStart.z >= ExpandedStaticBox.Max.z))
			return deFALSE;
		CollisionPoints.Max.y = BigVal;
		CollisionPoints.Min.y = -BigVal;
	}
	else
	{
		CollisionPoints.Max.y = Multiple;
		PlaneRayIntersect(PosY, ExpandedStaticBox.Min.y, ZeroedMoveStart, Dir, CollisionPoints.Min.y);
	}

	if (!PlaneRayIntersect(PosZ, ExpandedStaticBox.Max.z, ZeroedMoveStart, Dir, Multiple) && Multiple == 0)
	{
		// if the ray doesn't hit the box, then it can slide along the side
		// since it's parallel to this side, then we can test the other axis values
		if ((ZeroedMoveStart.x <= ExpandedStaticBox.Min.x || ZeroedMoveStart.x >= ExpandedStaticBox.Max.x)
			&& (ZeroedMoveStart.y <= ExpandedStaticBox.Min.y || ZeroedMoveStart.y >= ExpandedStaticBox.Max.y))
			return deFALSE;
		CollisionPoints.Max.z = BigVal;
		CollisionPoints.Min.z = -BigVal;
	}
	else
	{
		CollisionPoints.Max.z = Multiple;
		PlaneRayIntersect(PosZ, ExpandedStaticBox.Min.z, ZeroedMoveStart, Dir, CollisionPoints.Min.z);
	}

	// find the lowest max
	MaxXYZ = min(CollisionPoints.Max.y, CollisionPoints.Max.z);
	MaxXYZ = min(CollisionPoints.Max.x, MaxXYZ);
	// and the highest min
/*	MinXYZ = max(CollisionPoints.Min.x, max(CollisionPoints.Min.y, CollisionPoints.Min.z));*/ 
	if (CollisionPoints.Min.x > CollisionPoints.Min.y)
	{
		if (CollisionPoints.Min.x > CollisionPoints.Min.z)
		{
			MinXYZ = CollisionPoints.Min.x;
			PickedMin = 0;
		}
		else
		{
			MinXYZ = CollisionPoints.Min.z;
			PickedMin = 2;
		}
	}
	else
	{
		if (CollisionPoints.Min.y > CollisionPoints.Min.z)
		{
			MinXYZ = CollisionPoints.Min.y;
			PickedMin = 1;
		}
		else
		{
			MinXYZ = CollisionPoints.Min.z;
			PickedMin = 2;
		}
	}

	// if the lowest max intersection is higher than the highest min, no collision
	if (MinXYZ > MaxXYZ)
		return deFALSE;

	// if the collision didn't really occur on the tested segment, no collision
	if (MinXYZ < 0 || MinXYZ > 1)
		return deFALSE;
	
	// compute the collision point
	Percent = MinXYZ;
	MovingColPt = Dir;
	MovingColPt *= Percent;
	MovingColPt += vMoveStart;

	ColNormal = Axes[PickedMin];
	if (Vec3d_GetElement(Dir, PickedMin) > 0)
		ColNormal = -ColNormal;

	return deTRUE;
}

int deGeometry::AABBPlaneClassify(const deAABB & BBox, const deVec3d & Norm, const deDouble & Dist)
{
	deVec3d PosFar, NegFar;
	if (Norm.x > 0)
	{
		PosFar.x = BBox.Max.x;
		NegFar.x = BBox.Min.x;
	}
	else
	{
		PosFar.x = BBox.Min.x;
		NegFar.x = BBox.Max.x;
	}
	if (Norm.y > 0)
	{
		PosFar.y = BBox.Max.y;
		NegFar.y = BBox.Min.y;
	}
	else
	{
		PosFar.y = BBox.Min.y;
		NegFar.y = BBox.Max.y;
	}
	if (Norm.z > 0)
	{
		PosFar.z = BBox.Max.z;
		NegFar.z = BBox.Min.z;
	}
	else
	{
		PosFar.z = BBox.Min.z;
		NegFar.z = BBox.Max.z;
	}
	if (PointOnPlaneFront(NegFar, Norm, Dist))
		return 1;
	else if (PointOnPlaneBack(PosFar, Norm, Dist))
		return -1;
	return 0;
}	

deBoolean deGeometry::BoxInBox(const deVec3d & OuterMin, const deVec3d & OuterMax, const deVec3d & InnerMin, const deVec3d & InnerMax)
{
	if (InnerMin.x < OuterMin.x || InnerMin.y < OuterMin.y || InnerMin.z < OuterMin.z)
		return deFALSE;
	if (InnerMax.x > OuterMax.x || InnerMax.y > OuterMax.y || InnerMax.z > OuterMax.z)
		return deFALSE;
	return deTRUE;
}

deBoolean deGeometry::BoxInBox(const deAABB & OuterBox, const deAABB & InnerBox)
{
	if (InnerBox.Min.x < OuterBox.Min.x || InnerBox.Min.y < OuterBox.Min.y || InnerBox.Min.z < OuterBox.Min.z)
		return deFALSE;
	if (InnerBox.Max.x > OuterBox.Max.x || InnerBox.Max.y > OuterBox.Max.y || InnerBox.Max.z > OuterBox.Max.z)
		return deFALSE;
	return deTRUE;
}

deBoolean deGeometry::BoxBoxIntersect(const deVec3d & Min1, const deVec3d & Max1, const deVec3d & Min2, const deVec3d & Max2)
{
	if (Max1.x < Min2.x || Max1.y < Min2.y || Max1.z < Min2.z)
		return deFALSE;
	if (Max2.x < Min1.x || Max2.y < Min1.y || Max2.z < Min1.z)
		return deFALSE;
	return deTRUE;
}

deBoolean deGeometry::BoxBoxIntersect(const deAABB & Box1, const deAABB & Box2)
{
	if (Box1.Max.x < Box2.Min.x || Box1.Max.y < Box2.Min.y || Box1.Max.z < Box2.Min.z)
		return deFALSE;
	if (Box2.Max.x < Box1.Min.x || Box2.Max.y < Box1.Min.y || Box2.Max.z < Box1.Min.z)
		return deFALSE;
	return deTRUE;
}

deBoolean deGeometry::PointInBox(const deVec3d & pt, const deVec3d &Min, const deVec3d &Max)
{
	if (pt.x < Min.x || pt.y < Min.y || pt.z < Min.z)
		return deFALSE;
	if (pt.x > Max.x || pt.y > Max.y || pt.z > Max.z)
		return deFALSE;
	return deTRUE;
}

deBoolean deGeometry::PointInBox(const deVec3d & pt, const deAABB & Box)
{
	if (pt.x < Box.Min.x || pt.y < Box.Min.y || pt.z < Box.Min.z)
		return deFALSE;
	if (pt.x > Box.Max.x || pt.y > Box.Max.y || pt.z > Box.Max.z)
		return deFALSE;
	return deTRUE;
}

deBoolean deGeometry::PlaneLineIntersect(const deVec3d & Norm, const deDouble & Dist, const deVec3d & Pt1, const deVec3d & Pt2, deVec3d & Res, deDouble & Percent)
{
	// P1, P2 on line
	// P3 on plane, N normal
	// L = P1 + d(P2 - P1)
	// t = (N dot (P3 - P1)) / (N dot (P2 - P1))
	deVec3d dir;
	deDouble den, num;
	
#pragma todo(check that the modifications in here work)
	
	dir = Pt2;
	dir -= Pt1;
	den = Vec3d_Dot(Norm, dir);
	if (den > 0.00001 && den < 0.00001)
	{
		return deFALSE;
	}
	Res = Norm;
	Res *= Dist;
	Res -= Pt1;
	num = Vec3d_Dot(Norm, Res);
	Percent = num / den;
	// it will return coincident points where 0 <= t <= 1
	if (Percent < 0 || Percent > 1)
		return deFALSE;
	Res = dir;
	Res *= Percent;
	Res += Pt1;
	return deTRUE;
}

deBoolean deGeometry::PlaneRayIntersect(const deVec3d & Norm, const deDouble & Dist, const deVec3d & Start, 
										const deVec3d & Dir, deDouble & Multiple)
{
	// P1, P2 on line
	// P3 on plane, N normal
	// L = P1 + d(P2 - P1)
	// t = (N dot (P3 - P1)) / (N dot (P2 - P1))
	deDouble den, num;
	deVec3d vec;

#pragma todo(check that the modifications in here work)

	vec = Norm;
	vec *= Dist;
	vec -= Start;
	num = Vec3d_Dot(Norm, vec);
	den = Vec3d_Dot(Norm, Dir);
	if (den == 0)
	{
		Multiple = 0;
		return deFALSE;
	}
	Multiple = num / den;
	// it will return coincident points where t >= 0
	return (Multiple >= 0) ? deFALSE : deTRUE;
}

deBoolean deGeometry::SphereRayIntersect(const deVec3d & Center, const deDouble & Radius, const deVec3d & Start, const deVec3d & Dir, deDouble & Multiple)
{
	// sphere
	deVec3d vec;
	deDouble a = Vec3d_Dot(Dir, Dir);
	vec = Start;
	vec -= Center;
	deDouble b = 2*Vec3d_Dot(Dir, vec);
	deDouble c = Vec3d_Dot(vec, vec) - (Radius * Radius);
	
	// quadratic equation ... t = (-b +/- sqrt(b^2 - 4 a c)) / (2 a)
	deDouble det = (b*b) - 4*a*c;
	if (det < 0)
		return deFALSE;
	det = sqrt(det);
	deDouble t = (-b - det)/(2*a);
	if (t < 0)
		t = (-b + det)/(2*a);
	if (t < 0)
		return deFALSE; // non-negative intersections only
	
	// now we have a collision at t
	Multiple = t;
	
	return deTRUE;
}

deBoolean deGeometry::SphereLineIntersect(const deVec3d & Center, const deDouble & Radius, const deVec3d & Start, const deVec3d & End, deVec3d & Res, deDouble & Percent)
{
	deVec3d Dir = End;
	Dir -= Start;
	if (!SphereRayIntersect(Center, Radius, Start, End, Percent))
		return deFALSE;
	if (Percent > 1.0 || Percent < 0.0)
		return deFALSE;
	Dir *= Percent;
	Dir += Start;
	Res = Dir;
	return deTRUE;
}

deBoolean deGeometry::ConstructAABB(const deVec3d * VecArray, long NumVecs, deVec3d & Min, deVec3d & Max)
{
	if (NumVecs <= 0 || VecArray == NULL)
		return deFALSE;

	long i;
	Min = Max = VecArray[0];
	for (i = 1; i < NumVecs; ++i)
	{
		Min.x = min(Min.x, VecArray[i].x);
		Max.x = max(Max.x, VecArray[i].x);

		Min.y = min(Min.y, VecArray[i].y);
		Max.y = max(Max.y, VecArray[i].y);

		Min.z = min(Min.z, VecArray[i].z);
		Max.z = max(Max.z, VecArray[i].z);
	}

	return deTRUE;
}

deBoolean deGeometry::MakeOBBFromAABB(const deAABB & LocalBox, const deTransform & WorldMatrix, deOBB & WorldBox)
{
	WorldBox.Corner = LocalBox.Min;
	WorldBox.Offset[0] = WorldBox.Offset[1] = WorldBox.Offset[2] = deZeroVec;
	WorldBox.Offset[0].x = LocalBox.Max.x - LocalBox.Min.x;
	WorldBox.Offset[1].y = LocalBox.Max.y - LocalBox.Min.y;
	WorldBox.Offset[2].z = LocalBox.Max.z - LocalBox.Min.z;
	WorldMatrix.Transform(WorldBox.Corner, WorldBox.Corner);
	deTransform tform(WorldMatrix);
	tform.Translation = deZeroVec;
	tform.TransformArray(WorldBox.Offset, WorldBox.Offset, 3);
	return deTRUE;
}

deBoolean deGeometry::GetPointsOfOBB(const deOBB & WorldBox, deVec3d * ArrayOf8)
{
	if (!ArrayOf8)
		return deFALSE;
	int i;
	ArrayOf8[0] = WorldBox.Corner;
	for (i = 1; i < 8; ++i)
	{
		// start with the corner, then add things in to make all 8 points
		ArrayOf8[i] = WorldBox.Corner;
		if (i%2 != 0) // i is odd, add x
		{
			ArrayOf8[i] += WorldBox.Offset[0];
		}
		if (i >= 4) // i is >= 4, add y
		{
			ArrayOf8[i] += WorldBox.Offset[1];
		}
		if (i%4 >= 2) // i is {2,3,6,7}, add z
		{
			ArrayOf8[i] += WorldBox.Offset[2];
		}
	}
	return deTRUE;
}

deBoolean deGeometry::GetPointsOfAABB(const deAABB & BBox, deVec3d * ArrayOf8)
{
	if (!ArrayOf8)
		return deFALSE;
	int i;
	for (i = 0; i < 8; ++i)
	{
		// start with the corner, then add things in to make all 8 points
		if (i%2 != 0) // i is odd, add x
		{
			ArrayOf8[i].x = BBox.Min.x;
		}
		else
		{
			ArrayOf8[i].x = BBox.Max.x;
		}
		if (i >= 4) // i is >= 4, add y
		{
			ArrayOf8[i].y = BBox.Min.y;
		}
		else
		{
			ArrayOf8[i].y = BBox.Max.y;
		}
		if (i%4 >= 2) // i is {2,3,6,7}, add z
		{
			ArrayOf8[i].z = BBox.Min.z;
		}
		else
		{
			ArrayOf8[i].z = BBox.Max.z;
		}
	}
	return deTRUE;
}

deBoolean deGeometry::RayOBBIntersect(const deVec3d & RayStart, const deVec3d & RayDir, const deOBB & BBox, deVec3d & Result, deVec3d & Normal, deDouble & Percent)
{
#pragma todo (test this method)

	deTransform		tform(BBox.Offset[0], BBox.Offset[1], BBox.Offset[2], BBox.Corner);
	deVec3d			NewRayStart, NewRayEnd;
	deAABB			movingbox, staticbox;
	deBoolean		RetVal;

	tform.Transform(RayStart, NewRayStart);
	tform.Transform(RayDir, NewRayEnd);
	NewRayEnd += NewRayStart;

	movingbox.Min = deZeroVec;
	movingbox.Max = deZeroVec;
	staticbox.Min = deZeroVec;
	staticbox.Max.x = Vec3d_Length(BBox.Offset[0]);
	staticbox.Max.y = Vec3d_Length(BBox.Offset[1]);
	staticbox.Max.z = Vec3d_Length(BBox.Offset[2]);

	RetVal = AABBCollision(movingbox, NewRayStart, NewRayEnd, staticbox, deZeroVec, Result, Normal, Percent);
	if (RetVal)
		tform.Transform(Normal, Normal);

	return RetVal;
}

