#include "StdAfx.h"

#include "Collision.h"
#include "CollisionData.h"

//
//	LineCheckWithTriangle
//	Algorithm based on "Fast, Minimum Storage Ray/Triangle Intersection"
//
float LineCheckWithTriangle(const NiPoint3& start,const NiPoint3& end, const NiPoint3& v0,const NiPoint3& v1,const NiPoint3& v2)
{
	// This is cute: Use barycentric coordinates to represent the triangle
	// Vo(1-u-v) + V1u + V2v and intersect that with a line Po + Dt
	// This gives us 3 equations + 3 unknowns, which we can solve with
	// Cramer's rule...
	//		E1x u + E2x v - Dx t = Pox - Vox
	// There's a couple of other optimizations, Cramer's rule involves
	// computing the determinant of a matrix which has been constructed
	// by three vectors. It turns out that 
	// det | A B C | = -( A x C ) dot B or -(C x B) dot A
	// which we'll use below..

	NiPoint3 edge1 = v1 - v0;
	NiPoint3 edge2 = v2 - v0;
	NiPoint3 org;

	NiPoint3 delta = end - start;

	// Cull out one-sided stuff

	// FIXME: This is inaccurate, but fast for boxes
	// We want to do a fast separating axis implementation here
	// with a swept triangle along the reverse direction of the ray.

	// Compute some intermediary terms
	NiPoint3 dirCrossEdge2, orgCrossEdge1;
	dirCrossEdge2 = delta.Cross(edge2);

	// Compute the denominator of Cramer's rule:
	//		| -Dx E1x E2x |
	// det	| -Dy E1y E2y | = (D x E2) dot E1
	//		| -Dz E1z E2z |
	float denom = dirCrossEdge2.Dot( edge1 );
	if( NiAbs( denom ) < DELTA )
		return 2.0f;

	denom = 1.0f / denom;

	// Compute u. It's gotta lie in the range of 0 to 1.
	//				   | -Dx orgx E2x |
	// u = denom * det | -Dy orgy E2y | = (D x E2) dot org
	//				   | -Dz orgz E2z |
	org = start - v0;
	float u = dirCrossEdge2.Dot( org ) * denom;
	if ((u < 0.0f) || (u > 1.0f))
		return 2.0f;

	// Compute t and v the same way...
	// In barycentric coords, u + v < 1
	orgCrossEdge1 = org.Cross( edge1 );
	float v = orgCrossEdge1.Dot( end - start ) * denom;
	if ((v < 0.0f) || (v + u > 1.0f))
		return 2.0f;

	float t = orgCrossEdge1.Dot( edge2 ) * denom;
	if(t < 0.0f || t > 1.0f)
		return 2.0f;

	return t;
}

//
//	Separating axis code.
//
static void CheckMinIntersectTime(float& MinIntersectTime, float Time)
{
	if(Time > MinIntersectTime)
	{
		MinIntersectTime = Time;
	}
}

static bool TestSeparatingAxis(
	const NiPoint3& V0,
	const NiPoint3& V1,
	const NiPoint3& V2,
	const NiPoint3& Line,
	float ProjectedStart,
	float ProjectedEnd,
	float ProjectedExtent,
	float& MinIntersectTime,
	float& MaxIntersectTime
	)
{
	float	ProjectedDirection = ProjectedEnd - ProjectedStart;
	float	ProjectedV0 = Line.Dot(V0);
	float	ProjectedV1 = Line.Dot(V1);
	float	ProjectedV2 = Line.Dot(V2);
	float	TriangleMin = NiMin(ProjectedV0, NiMin(ProjectedV1,ProjectedV2)) - ProjectedExtent;
	float	TriangleMax = NiMax(ProjectedV0, NiMax(ProjectedV1,ProjectedV2)) + ProjectedExtent;

	if(ProjectedStart < TriangleMin)
	{
		if(ProjectedDirection < DELTA)
			return false;
		CheckMinIntersectTime(MinIntersectTime,(TriangleMin - ProjectedStart) / ProjectedDirection);
		MaxIntersectTime = NiMin(MaxIntersectTime,(TriangleMax - ProjectedStart) / ProjectedDirection);
	}
	else if(ProjectedStart > TriangleMax)
	{
		if(ProjectedDirection > -DELTA)
			return false;
		CheckMinIntersectTime(MinIntersectTime,(TriangleMax - ProjectedStart) / ProjectedDirection);
		MaxIntersectTime = NiMin(MaxIntersectTime,(TriangleMin - ProjectedStart) / ProjectedDirection);
	}
	else
	{
		if(ProjectedDirection > DELTA)
			MaxIntersectTime = NiMin(MaxIntersectTime,(TriangleMax - ProjectedStart) / ProjectedDirection);
		else if(ProjectedDirection < -DELTA)
			MaxIntersectTime = NiMin(MaxIntersectTime,(TriangleMin - ProjectedStart) / ProjectedDirection);
	}

	return MaxIntersectTime >= MinIntersectTime;
}

static bool TestSeparatingAxis1(
	const NiPoint3& V0,
	const NiPoint3& V1,
	const NiPoint3& V2,
	const NiPoint3& Line,
	const NiPoint3& Start,
	const NiPoint3& End,
	float ProjectedExtent,
	float& MinIntersectTime,
	float& MaxIntersectTime
	)
{
	return TestSeparatingAxis(V0,V1,V2,Line, Line.Dot(Start), Line.Dot(End), ProjectedExtent,MinIntersectTime,MaxIntersectTime);
}

static bool TestSeparatingAxis2(
	const NiPoint3& V0,
	const NiPoint3& V1,
	const NiPoint3& V2,
	const NiPoint3& TriangleEdge,
	const NiPoint3& BoxEdge,
	const NiPoint3& Start,
	const NiPoint3& End,
	const NiPoint3& BoxX,
	const NiPoint3& BoxY,
	const NiPoint3& BoxZ,
	const NiPoint3& BoxExtent,
	float& MinIntersectTime,
	float& MaxIntersectTime
	)
{
	NiPoint3 Line = BoxEdge.Cross(TriangleEdge);

	if(Line.SqrLength() < DELTA)
		return true;

	float ProjectedExtent = BoxExtent.x * NiAbs(Line.Dot(BoxX)) + BoxExtent.y * NiAbs(Line.Dot(BoxY)) + BoxExtent.z * NiAbs(Line.Dot(BoxZ));
	return TestSeparatingAxis(V0,V1,V2,Line, Line.Dot(Start), Line.Dot(End), ProjectedExtent, MinIntersectTime, MaxIntersectTime);
}

float FindSeparatingAxis(
	const NiPoint3& V0,
	const NiPoint3& V1,
	const NiPoint3& V2,
	const NiPoint3& Start,
	const NiPoint3& End,
	const NiPoint3& BoxExtent,
	const NiPoint3& BoxX,
	const NiPoint3& BoxY,
	const NiPoint3& BoxZ
	)
{
	float	MinIntersectTime = -1.0f,
		MaxIntersectTime = 2.0f;

	// Box faces.
	if(!TestSeparatingAxis1(V0,V1,V2,BoxX,Start,End,BoxExtent.x * BoxX.SqrLength(),MinIntersectTime,MaxIntersectTime))
		return 2.0f;

	if(!TestSeparatingAxis1(V0,V1,V2,BoxY,Start,End,BoxExtent.y * BoxY.SqrLength(),MinIntersectTime,MaxIntersectTime))
		return 2.0f;

	if(!TestSeparatingAxis1(V0,V1,V2,BoxZ,Start,End,BoxExtent.z * BoxZ.SqrLength(),MinIntersectTime,MaxIntersectTime))
		return 2.0f;

	// Triangle normal.
	if(!TestSeparatingAxis2(V0,V1,V2,(V2 - V1),(V1 - V0),Start,End,BoxX,BoxY,BoxZ,BoxExtent,MinIntersectTime,MaxIntersectTime))
		return 2.0f;

	// Box faces +/- X x triangle edges.
	if(!TestSeparatingAxis2(V0,V1,V2,(V1 - V0),BoxX,Start,End,BoxX,BoxY,BoxZ,BoxExtent,MinIntersectTime,MaxIntersectTime))
		return 2.0f;

	if(!TestSeparatingAxis2(V0,V1,V2,(V2 - V1),BoxX,Start,End,BoxX,BoxY,BoxZ,BoxExtent,MinIntersectTime,MaxIntersectTime))
		return 2.0f;

	if(!TestSeparatingAxis2(V0,V1,V2,(V0 - V2),BoxX,Start,End,BoxX,BoxY,BoxZ,BoxExtent,MinIntersectTime,MaxIntersectTime))
		return 2.0f;

	// Box faces +/- Y x triangle edges.
	if(!TestSeparatingAxis2(V0,V1,V2,(V1 - V0),BoxY,Start,End,BoxX,BoxY,BoxZ,BoxExtent,MinIntersectTime,MaxIntersectTime))
		return 2.0f;

	if(!TestSeparatingAxis2(V0,V1,V2,(V2 - V1),BoxY,Start,End,BoxX,BoxY,BoxZ,BoxExtent,MinIntersectTime,MaxIntersectTime))
		return 2.0f;
	
	if(!TestSeparatingAxis2(V0,V1,V2,(V0 - V2),BoxY,Start,End,BoxX,BoxY,BoxZ,BoxExtent,MinIntersectTime,MaxIntersectTime))
		return 2.0f;
	
	// Box faces +/- Z x triangle edges.
	if(!TestSeparatingAxis2(V0,V1,V2,(V1 - V0),BoxZ,Start,End,BoxX,BoxY,BoxZ,BoxExtent,MinIntersectTime,MaxIntersectTime))
		return 2.0f;

	if(!TestSeparatingAxis2(V0,V1,V2,(V2 - V1),BoxZ,Start,End,BoxX,BoxY,BoxZ,BoxExtent,MinIntersectTime,MaxIntersectTime))
		return 2.0f;
	
	if(!TestSeparatingAxis2(V0,V1,V2,(V0 - V2),BoxZ,Start,End,BoxX,BoxY,BoxZ,BoxExtent,MinIntersectTime,MaxIntersectTime))
		return 2.0f;

	if(MinIntersectTime >= 0.0f)
	{
		return MinIntersectTime;
	}
	else
		return 0.0f;
}

bool Collide(NiAVObject* pkAVObject, const NiPoint3& start, const NiPoint3& end, float& fTime)
{
	CCollisionData* pkData = GetCollisionData(pkAVObject);
	
	if( pkAVObject->IsNode() )
	{
		if( pkData && !pkData->GetBoundBox().CollideLine(start, end) )
			return false;

		NiNode* pkNode = (NiNode*)pkAVObject;
		const unsigned int uiSize = pkNode->GetArrayCount();

		for (unsigned int ui = 0; ui < uiSize; ++ui)
		{
			NiAVObject* pkChild = pkNode->GetAt(ui);
			if( pkChild )
			{
				if(Collide(pkChild, start, end, fTime))
					return true;
			}
		}
	}
	else
	{
		if(pkData && pkData->Collide(start, end, fTime))
			return true;
	}

	return false;
}

void MultiCollide(NiAVObject* pkAVObject, const NiPoint3& start, const NiPoint3& end, const NiPoint3& extent, ResultList& resultList)
{
	CCollisionData* pkData = GetCollisionData(pkAVObject);
	
	if( pkAVObject->IsNode() )
	{
		if( pkData && !pkData->GetBoundBox().CollideBox(start, end, extent) )
			return;

		NiNode* pkNode = (NiNode*)pkAVObject;
		const unsigned int uiSize = pkNode->GetArrayCount();

		for (unsigned int ui = 0; ui < uiSize; ++ui)
		{
			NiAVObject* pkChild = pkNode->GetAt(ui);
			if( pkChild )
			{
				MultiCollide(pkChild, start, end, extent, resultList); 
			}
		}
	}
	else
	{
		if( pkData )
			pkData->MultiCollide(start, end, extent, resultList);
	}
}

bool BoxCheckModelInstance(NiAVObject* pkAVObject, const CBox& kBox)
{
	CCollisionData* pkData = GetCollisionData(pkAVObject);
	
	if( pkAVObject->IsNode() )
	{
		if( pkData && !pkData->GetBoundBox().IsOverlapped(kBox) )
			return false;

		NiNode* pkNode = (NiNode*)pkAVObject;
		const unsigned int uiSize = pkNode->GetArrayCount();
		for (unsigned int ui = 0; ui < uiSize; ++ui)
		{
			NiAVObject* pkChild = pkNode->GetAt(ui);
			if(pkChild && BoxCheckModelInstance(pkChild, kBox))
				return true;
		}
	}
	else
	{
		if(pkData && pkData->GetBoundBox().IsOverlapped(kBox))
			return true;
	}

	return false;
}

bool IsCollisionObject(NiAVObject* pkAVObject)
{
	const char* pcName = pkAVObject->GetName();
	if( !pcName )
		return false;

	if(pcName[0] != 'c' && pcName[0] != 'C')
		return false;

	if(pcName[1] != 'o' && pcName[1] != 'O')
		return false;

	if(pcName[2] != 'l' && pcName[2] != 'L')
		return false;

	return true;
}

bool CreateCollisionData(NiAVObject* pkAVObject)
{
    CCollisionData* pkData = GetCollisionData(pkAVObject);
	if( pkData )
		return true;

	if( IsCollisionObject(pkAVObject) )
	{
		pkData = NiNew CCollisionData(pkAVObject);
		pkAVObject->SetCollisionObject(pkData);
		pkAVObject->SetAppCulled(true);
	}


	// Create data only for NiTriBasedGeom.
	if(NiIsKindOf(NiTriBasedGeom, pkAVObject) && pkData)
	{
		pkData->CreateWorldVertices();
		pkData->UpdateWorldVertices();

		return true;
	}

	bool bRet = false;
	CCollisionData* pkChildData;
	if(pkAVObject->IsNode())
	{
		NiNode* pkNode = (NiNode*)pkAVObject;
		const unsigned int uiSize = pkNode->GetArrayCount();

		for (unsigned int ui = 0; ui < uiSize; ++ui)
		{
			NiAVObject* pkChild = pkNode->GetAt(ui);
			if( pkChild == NULL )
				continue;

			if(CreateCollisionData(pkChild))
				bRet = true;

			pkChildData = GetCollisionData(pkChild);
			if(pkData && pkChildData)
			{
				pkData->GetBoundBox().Intersect(pkChildData->GetBoundBox());
			}
		}
	}

	return bRet;
}