#include "Stdafx.h"
#include "EtFindCollision.h"
#include "EtTestCollision.h"
#include "EtComputeDist.h"

#ifdef _DEBUG
#define new new(_NORMAL_BLOCK,__FILE__,__LINE__)
#endif

#define MIN_CONTACT_LENGTH_SQ	25.0f

CEtCollisionFinder g_CollisionFinder;
CEtCollisionFinder::CEtCollisionFinder()
{
	m_aCollisionTable[ CT_BOX ][ CT_TRIANGLE ] = ( CollisionFinder )FindCollisionDummy;
	m_aCollisionTable[ CT_BOX ][ CT_SPHERE ] = ( CollisionFinder )FindBoxToSphere;
	m_aCollisionTable[ CT_BOX ][ CT_BOX ] = ( CollisionFinder )FindBoxToBox;
	m_aCollisionTable[ CT_BOX ][ CT_CAPSULE ] = ( CollisionFinder )FindBoxToCapsule;

	m_aCollisionTable[ CT_SPHERE ][ CT_TRIANGLE ] = ( CollisionFinder )FindSphereToTriangle;
	m_aCollisionTable[ CT_SPHERE ][ CT_SPHERE ] = ( CollisionFinder )FindSphereToSphere;
	m_aCollisionTable[ CT_SPHERE ][ CT_BOX ] = ( CollisionFinder )FindSphereToBox;
	m_aCollisionTable[ CT_SPHERE ][ CT_CAPSULE ] = ( CollisionFinder )FindSphereToCapsule;

	m_aCollisionTable[ CT_CAPSULE ][ CT_TRIANGLE ] = ( CollisionFinder )FindCapsuleToTriangle;
	m_aCollisionTable[ CT_CAPSULE ][ CT_SPHERE ] = ( CollisionFinder )FindCapsuleToSphere;
	m_aCollisionTable[ CT_CAPSULE ][ CT_BOX ] = ( CollisionFinder )FindCapsuleToBox;
	m_aCollisionTable[ CT_CAPSULE ][ CT_CAPSULE ] = ( CollisionFinder )FindCapsuleToCapsule;

	m_aCollisionTable[ CT_TRIANGLE ][ CT_TRIANGLE ] = ( CollisionFinder )FindCollisionDummy;
	m_aCollisionTable[ CT_TRIANGLE ][ CT_SPHERE ] = ( CollisionFinder )FindCollisionDummy;
	m_aCollisionTable[ CT_TRIANGLE ][ CT_BOX ] = ( CollisionFinder )FindCollisionDummy;
	m_aCollisionTable[ CT_TRIANGLE ][ CT_CAPSULE ] = ( CollisionFinder )FindCollisionDummy;
}

CEtCollisionFinder::~CEtCollisionFinder()
{
}

bool CEtCollisionFinder::FindCollision( SCollisionPrimitive &Primitive1, SCollisionPrimitive &Primitive2, SCollisionResponse &Response, bool bCalcContactTime )
{
	if( ( Primitive1.Type >= CT_TRIANGLE_LIST ) || ( Primitive2.Type >= CT_TRIANGLE_LIST ) )
	{
		//ASSERT( 0 && "폴리곤 충돌인데 다이나믹 오브젝트로 설정됐다" );
		return false;
	}
	return m_aCollisionTable[ Primitive1.Type ][ Primitive2.Type ]( Primitive1, Primitive2, Response.vMove, Response, bCalcContactTime );
}

bool FindCollisionDummy( SCollisionPrimitive &Primitive1, SCollisionPrimitive &Primitive2, EtVector3 &Move, SCollisionResponse &Response, bool bCalcContactTime )
{
	return false;
}

bool FindBoxToSphere( SCollisionBox &Box, SCollisionSphere &Sphere, EtVector3 &Move, SCollisionResponse &Response, bool bCalcContactTime )
{
	if( !TestBoxToSphere( Box, Sphere ) )
	{
		return false;
	}

	Response.vNormal = Box.vCenter - Sphere.vCenter;
	EtVec3Normalize( &Response.vNormal, &Response.vNormal );

	if( !bCalcContactTime )
	{
		return true;
	}

	if( EtVec3LengthSq( &Move ) > MIN_CONTACT_LENGTH_SQ )
	{
		float fCurTime, fStartTime, fEndTime;
		SCollisionBox Temporary;

		fStartTime = 0.0f;
		fEndTime = 1.0f;
		fCurTime = 0.5f;
		Temporary = Box;
		while( 1 )
		{
			Temporary.vCenter = Box.vCenter - Move * ( 1.0f - fCurTime );
			if( !TestBoxToSphere( Temporary, Sphere ) )
			{
				Response.fContactTime = fCurTime;
				fStartTime = fCurTime;
			}
			else
			{
				fEndTime = fCurTime;
			}
			fCurTime = ( fStartTime + fEndTime ) * 0.5f;
			if( EtVec3LengthSq( &( Move * ( fCurTime - fStartTime ) ) ) < MIN_CONTACT_LENGTH_SQ )
			{
				break;
			}
		}
	}

	return true;
}

bool FindBoxToBox( SCollisionBox &Box1, SCollisionBox &Box2, EtVector3 &Move, SCollisionResponse &Response, bool bCalcContactTime )
{
	if( !TestBoxToBox( Box1, Box2 ) )
	{
		return false;
	}

	FindPointToBox( Box1.vCenter - Move, Box2, Response.vNormal );

	if( !bCalcContactTime )
	{
		return true;
	}

	if( EtVec3LengthSq( &Move ) > MIN_CONTACT_LENGTH_SQ )
	{
		float fCurTime, fStartTime, fEndTime;
		SCollisionBox Temporary;

		fStartTime = 0.0f;
		fEndTime = 1.0f;
		fCurTime = 0.5f;
		Temporary = Box1;
		while( 1 )
		{
			Temporary.vCenter = Box1.vCenter - Move * ( 1.0f - fCurTime );
			if( !TestBoxToBox( Temporary, Box2 ) )
			{
				Response.fContactTime = fCurTime;
				fStartTime = fCurTime;
			}
			else
			{
				fEndTime = fCurTime;
			}
			fCurTime = ( fStartTime + fEndTime ) * 0.5f;
			if( EtVec3LengthSq( &( Move * ( fCurTime - fStartTime ) ) ) < MIN_CONTACT_LENGTH_SQ )
			{
				break;
			}
		}
	}

	return true;
}

bool FindBoxToCapsule( SCollisionBox &Box, SCollisionCapsule &Capsule, EtVector3 &Move, SCollisionResponse &Response, bool bCalcContactTime )
{
	float fSegParam, fBoxParam0, fBoxParam1, fBoxParam2;

	if( !TestBoxToCapsule( Box, Capsule, fSegParam, fBoxParam0, fBoxParam1, fBoxParam2 ) )
	{
		return false;
	}

	EtVector3 SegmentPoint;

	SegmentPoint = Capsule.Segment.vOrigin + fSegParam * Capsule.Segment.vDirection;
	Response.vNormal = Box.vCenter - SegmentPoint;
	EtVec3Normalize( &Response.vNormal, &Response.vNormal );

	if( !bCalcContactTime )
	{
		return true;
	}

	if( EtVec3LengthSq( &Move ) > MIN_CONTACT_LENGTH_SQ )
	{
		float fCurTime, fStartTime, fEndTime;
		SCollisionBox Temporary;

		fStartTime = 0.0f;
		fEndTime = 1.0f;
		fCurTime = 0.5f;
		Temporary = Box;
		while( 1 )
		{
			Temporary.vCenter = Box.vCenter - Move * ( 1.0f - fCurTime );
			if( !TestBoxToCapsule( Temporary, Capsule, fSegParam, fBoxParam0, fBoxParam1, fBoxParam2 ) )
			{
				Response.fContactTime = fCurTime;
				fStartTime = fCurTime;
			}
			else
			{
				fEndTime = fCurTime;
			}
			fCurTime = ( fStartTime + fEndTime ) * 0.5f;
			if( EtVec3LengthSq( &( Move * ( fCurTime - fStartTime ) ) ) < MIN_CONTACT_LENGTH_SQ )
			{
				break;
			}
		}
	}

	return true;
}

bool FindSphereToTriangle( SCollisionSphere &Sphere, SCollisionTriangle &Triangle, EtVector3 &Move, SCollisionResponse &Response, bool bCalcContactTime )
{
	float fTriParam1, fTriParam2;

	if( !TestSphereToTriangle( Sphere, Triangle, fTriParam1, fTriParam2 ) )
	{
		return false;
	}

	Triangle.GetNormal( Response.vNormal );

	if( !bCalcContactTime )
	{
		return true;
	}

	float fMoveDist;

	fMoveDist = EtVec3Length( &Move );
	if( fMoveDist > 0.0f )
	{
		EtVector3 vSphereToTriNormal, vMoveNormal, vTriPoint;
		float fRadiusDiff, fSphereToTriLength, fDot;

		vMoveNormal = Move / fMoveDist;
		vTriPoint = Triangle.vOrigin + Triangle.vEdge1 * fTriParam1 + Triangle.vEdge2 * fTriParam2;
		vSphereToTriNormal = vTriPoint - Sphere.vCenter;
		fSphereToTriLength = EtVec3Length( &vSphereToTriNormal );
		vSphereToTriNormal /= fSphereToTriLength;

		fRadiusDiff = Sphere.fRadius - fSphereToTriLength;
		fDot = EtVec3Dot( &vSphereToTriNormal, &vMoveNormal );
		if( fDot != 0.0f )
		{
			fRadiusDiff /= fDot;
			Response.fContactTime = 1.0f - fRadiusDiff / fMoveDist;
			Response.fContactTime = min( max( Response.fContactTime, 0.0f ), 1.0f );
		}
	}

	return true;
}

bool FindSphereToSphere( SCollisionSphere &Sphere1, SCollisionSphere &Sphere2, EtVector3 &Move, SCollisionResponse &Response, bool bCalcContactTime )
{
	if( !TestSphereToSphere( Sphere1, Sphere2 ) )
	{
		return false;
	}

	Response.vNormal = Sphere1.vCenter - Sphere2.vCenter;
	EtVec3Normalize( &Response.vNormal, &Response.vNormal );

	if( !bCalcContactTime )
	{
		return true;
	}

	if( EtVec3LengthSq( &Move ) > MIN_CONTACT_LENGTH_SQ )
	{
		float fCurTime, fStartTime, fEndTime;
		SCollisionSphere Temporary;

		fStartTime = 0.0f;
		fEndTime = 1.0f;
		fCurTime = 0.5f;
		Temporary = Sphere1;
		while( 1 )
		{
			Temporary.vCenter = Sphere1.vCenter - Move * ( 1.0f - fCurTime );
			if( !TestSphereToSphere( Temporary, Sphere2 ) )
			{
				Response.fContactTime = fCurTime;
				fStartTime = fCurTime;
			}
			else
			{
				fEndTime = fCurTime;
			}
			fCurTime = ( fStartTime + fEndTime ) * 0.5f;
			if( EtVec3LengthSq( &( Move * ( fCurTime - fStartTime ) ) ) < MIN_CONTACT_LENGTH_SQ )
			{
				break;
			}
		}
	}

	return true;
}

bool FindSphereToBox( SCollisionSphere &Sphere, SCollisionBox &Box, EtVector3 &Move, SCollisionResponse &Response, bool bCalcContactTime )
{
	if( !TestBoxToSphere( Box, Sphere ) )
	{
		return false;
	}

	FindPointToBox( Sphere.vCenter - Move, Box, Response.vNormal );

	if( !bCalcContactTime )
	{
		return true;
	}

	if( EtVec3LengthSq( &Move ) > MIN_CONTACT_LENGTH_SQ )
	{
		float fCurTime, fStartTime, fEndTime;
		SCollisionSphere Temporary;

		fStartTime = 0.0f;
		fEndTime = 1.0f;
		fCurTime = 0.5f;
		Temporary = Sphere;
		while( 1 )
		{
			Temporary.vCenter = Sphere.vCenter - Move * ( 1.0f - fCurTime );
			if( !TestBoxToSphere( Box, Temporary ) )
			{
				Response.fContactTime = fCurTime;
				fStartTime = fCurTime;
			}
			else
			{
				fEndTime = fCurTime;
			}
			fCurTime = ( fStartTime + fEndTime ) * 0.5f;
			if( EtVec3LengthSq( &( Move * ( fCurTime - fStartTime ) ) ) < MIN_CONTACT_LENGTH_SQ )
			{
				break;
			}
		}
	}

	return true;
}

bool FindSphereToCapsule( SCollisionSphere &Sphere, SCollisionCapsule &Capsule, EtVector3 &Move, SCollisionResponse &Response, bool bCalcContactTime )
{
	float fSegParam;

	if( !TestSphereToCapsule( Sphere, Capsule, fSegParam ) )
	{
		return false;
	}

	EtVector3 SegmentPoint;

	SegmentPoint = Capsule.Segment.vOrigin + fSegParam * Capsule.Segment.vDirection;
	Response.vNormal = Sphere.vCenter - SegmentPoint;
	EtVec3Normalize( &Response.vNormal, &Response.vNormal );

	if( !bCalcContactTime )
	{
		return true;
	}

	if( EtVec3LengthSq( &Move ) > MIN_CONTACT_LENGTH_SQ )
	{
		float fCurTime, fStartTime, fEndTime;
		SCollisionSphere Temporary;

		fStartTime = 0.0f;
		fEndTime = 1.0f;
		fCurTime = 0.5f;
		Temporary = Sphere;
		while( 1 )
		{
			Temporary.vCenter = Sphere.vCenter - Move * ( 1.0f - fCurTime );
			if( !TestSphereToCapsule( Temporary, Capsule, fSegParam ) )
			{
				Response.fContactTime = fCurTime;
				fStartTime = fCurTime;
			}
			else
			{
				fEndTime = fCurTime;
			}
			fCurTime = ( fStartTime + fEndTime ) * 0.5f;
			if( EtVec3LengthSq( &( Move * ( fCurTime - fStartTime ) ) ) < MIN_CONTACT_LENGTH_SQ )
			{
				break;
			}
		}
	}

	return true;
}

bool FindCapsuleToTriangle( SCollisionCapsule &Capsule, SCollisionTriangle &Triangle, EtVector3 &Move, SCollisionResponse &Response, bool bCalcContactTime )
{
	float fSegParam, fTriParam1, fTriParam2;

	if( !TestCapsuleToTriangle( Capsule, Triangle, fSegParam, fTriParam1, fTriParam2 ) )
	{
		return false;
	}
	Triangle.GetNormal( Response.vNormal );

	if( !bCalcContactTime )
	{
		return true;
	}

	float fMoveDist;

	fMoveDist = EtVec3Length( &Move );
	if( fMoveDist > 0.0f )
	{
		SSegment TestSegment;
		float fCapsuleLength, fContactTime;
		EtVector3 vTriPoint, vMoveNormal;

		vMoveNormal = Move / fMoveDist;
		vTriPoint = Triangle.vOrigin + fTriParam1 * Triangle.vEdge1 + fTriParam2 * Triangle.vEdge2;

		fCapsuleLength = EtVec3Length( &Capsule.Segment.vDirection ) + Capsule.fRadius * 2.0f;
		TestSegment.vOrigin = vTriPoint + vMoveNormal * fCapsuleLength;
		TestSegment.vDirection = -vMoveNormal * fCapsuleLength;
		FindSegmentToCapsule( TestSegment, Capsule, fContactTime );
		Response.fContactTime = min( 1.0f - fCapsuleLength * ( 1.0f - fContactTime ) / fMoveDist, 1.0f );
	}

	return true;
}

bool FindCapsuleToSphere( SCollisionCapsule &Capsule, SCollisionSphere &Sphere, EtVector3 &Move, SCollisionResponse &Response, bool bCalcContactTime )
{
	float fSegParam;

	if( !TestSphereToCapsule( Sphere, Capsule, fSegParam ) )
	{
		return false;
	}

	EtVector3 vSegmentPoint;

	vSegmentPoint = Capsule.Segment.vOrigin + fSegParam * Capsule.Segment.vDirection;
	Response.vNormal = vSegmentPoint - Sphere.vCenter;
	EtVec3Normalize( &Response.vNormal, &Response.vNormal );

	if( !bCalcContactTime )
	{
		return true;
	}

	float fMoveDist;

	fMoveDist = EtVec3Length( &Move );
	if( fMoveDist > 0.0f )
	{
		EtVector3 vCapsuleToSphereNormal, vMoveNormal;
		float fRadiusDiff, fCapsuleToSphereLength, fDot;

		vMoveNormal = Move / fMoveDist;
		vCapsuleToSphereNormal = Sphere.vCenter - vSegmentPoint;
		fCapsuleToSphereLength = EtVec3Length( &vCapsuleToSphereNormal );
		vCapsuleToSphereNormal /= fCapsuleToSphereLength;

		fRadiusDiff = ( Capsule.fRadius + Sphere.fRadius ) - fCapsuleToSphereLength;
		fDot = EtVec3Dot( &vCapsuleToSphereNormal, &vMoveNormal );
		if( fDot != 0.0f )
		{
			fRadiusDiff /= fDot;
			Response.fContactTime = 1.0f - fRadiusDiff / fMoveDist;
			Response.fContactTime = min( max( Response.fContactTime, 0.0f ), 1.0f );
		}
	}

	return true;
}

bool FindCapsuleToBox( SCollisionCapsule &Capsule, SCollisionBox &Box, EtVector3 &Move, SCollisionResponse &Response, bool bCalcContactTime )
{
	float fSegParam, fBoxParam[ 3 ];

	if( !TestBoxToCapsule( Box, Capsule, fSegParam, fBoxParam[ 0 ], fBoxParam[ 1 ], fBoxParam[ 2 ] ) )
	{
		return false;
	}

	int i;
	float fMoveDist;
	EtVector3 vMoveNormal;
	EtVector3 vBoxPoint, vSegmentPoint;

	vSegmentPoint = Capsule.Segment.vOrigin + fSegParam * Capsule.Segment.vDirection;
	vBoxPoint = Box.vCenter + fBoxParam[ 0 ] * Box.vAxis[ 0 ];
	vBoxPoint += fBoxParam[ 1 ] * Box.vAxis[ 1 ];
	vBoxPoint += fBoxParam[ 2 ] * Box.vAxis[ 2 ];

	fMoveDist = EtVec3Length( &Move );
	vMoveNormal = Move / fMoveDist;
	if( fMoveDist > 0.0f )
	{
		float fMaxDot = -FLT_MAX;
		for( i = 0; i < 3; i++ )
		{
			float fDot = EtVec3Dot( &vMoveNormal, Box.vAxis + i );
			float fAbsDot = fabs( fDot );
			EtVector4 vPlane;
			EtVector3 vCurPos, vPlaneNormal;
			if( fDot > 0.0f )
			{
				vPlaneNormal = -Box.vAxis[ i ];
			}
			else
			{
				vPlaneNormal = Box.vAxis[ i ];
			}
			vCurPos = Box.vCenter + vPlaneNormal * Box.fExtent[ i ];
			EtPlaneFromPointNormal( ( EtPlane * )&vPlane, &vCurPos, &vPlaneNormal );
			if( TestPlaneToCapsule( vPlane, Capsule ) )
			{
				EtVector3 *pvTarget = &Response.vNormal;
				if( fAbsDot > fMaxDot )	// 기존에 설정되 있는 노멀이 이동방향과 충돌각이 작으면 기존노멀이 vNormal에 들어가고 현재축이 vExtraNormal에 들어간다.
				{
					if( fMaxDot != -FLT_MAX )
					{
						Response.vExtraNormal = Response.vNormal;
					}
				}
				else
				{
					pvTarget = &Response.vExtraNormal;
				}
				*pvTarget = vPlaneNormal;
				fMaxDot = fAbsDot;
			}
		}
		if( fMaxDot == -FLT_MAX )
		{
			Response.vNormal = EtVector3( 0.0f, 0.0f, 0.0f );
			Response.fContactTime = 0.0;
			// 일단 충돌 아닌걸로 하자.. 클라쪽에서.. 낑기면 못빠져 나올때 있어서.. 
			return false;
		}
	}
	else
	{
		FindPointToBox( vSegmentPoint - Move, Box, Response.vNormal );
	}

	if( !bCalcContactTime )
	{
		return true;
	}

	if( fMoveDist > 0.0f )
	{
		SSegment TestSegment;
		float fCapsuleLength, fContactTime;

		fCapsuleLength = EtVec3Length( &Capsule.Segment.vDirection ) + Capsule.fRadius * 2.0f;
		TestSegment.vOrigin = vBoxPoint + vMoveNormal * fCapsuleLength;
		TestSegment.vDirection = -vMoveNormal * fCapsuleLength;
		FindSegmentToCapsule( TestSegment, Capsule, fContactTime );
		Response.fContactTime = min( 1.0f - fCapsuleLength * ( 1.0f - fContactTime ) / fMoveDist, 1.0f );
	}

	return true;
}

bool FindCapsuleToCapsule( SCollisionCapsule &Capsule1, SCollisionCapsule &Capsule2, EtVector3 &Move, SCollisionResponse &Response, bool bCalcContactTime )
{
	float fSegParam1, fSegParam2;

	if( !TestCapsuleToCapsule( Capsule1, Capsule2, fSegParam1, fSegParam2 ) )
	{
		return false;
	}

	EtVector3 vSegmentPoint1, vSegmentPoint2;

	vSegmentPoint1 = Capsule1.Segment.vOrigin + fSegParam1 * Capsule1.Segment.vDirection;
	vSegmentPoint2 = Capsule2.Segment.vOrigin + fSegParam2 * Capsule2.Segment.vDirection;
	Response.vNormal = vSegmentPoint1 - vSegmentPoint2;
	EtVec3Normalize( &Response.vNormal, &Response.vNormal );

	if( !bCalcContactTime )
	{
		return true;
	}

	float fMoveDist;

	fMoveDist = EtVec3Length( &Move );
	if( fMoveDist > 0.0f )
	{
		EtVector3 vCapsule1ToCapsule2Normal, vMoveNormal;
		float fRadiusDiff, fCapsule1ToCapsule2Length, fDot;

		vMoveNormal = Move / fMoveDist;
		vCapsule1ToCapsule2Normal = vSegmentPoint2 - vSegmentPoint1;
		fCapsule1ToCapsule2Length = EtVec3Length( &vCapsule1ToCapsule2Normal );
		vCapsule1ToCapsule2Normal /= fCapsule1ToCapsule2Length;

		fRadiusDiff = ( Capsule1.fRadius + Capsule2.fRadius ) - fCapsule1ToCapsule2Length;
		fDot = EtVec3Dot( &vCapsule1ToCapsule2Normal, &vMoveNormal );
		if( fDot != 0.0f )
		{
			fRadiusDiff /= fDot;
			Response.fContactTime = 1.0f - fRadiusDiff / fMoveDist;
			Response.fContactTime = min( max( Response.fContactTime, 0.0f ), 1.0f );
		}
	}

	return true;
}

void FindPointToBox( EtVector3 &Point, SCollisionBox &Box, EtVector3 &CollisionNormal )
{
	EtVector3 Direction, TransformDir;

	Direction = Point - Box.vCenter;
	TransformDir.x = EtVec3Dot( &Direction, Box.vAxis ) / Box.fExtent[ 0 ];
	TransformDir.y = EtVec3Dot( &Direction, Box.vAxis + 1 ) / Box.fExtent[ 1 ];
	TransformDir.z = EtVec3Dot( &Direction, Box.vAxis + 2 ) / Box.fExtent[ 2 ];

	if( fabs( TransformDir.x ) > fabs( TransformDir.y ) )
	{
		if( fabs( TransformDir.x ) > fabs( TransformDir.z ) )
		{
			if( TransformDir.x > 0 )
			{
				CollisionNormal = Box.vAxis[ 0 ];
			}
			else
			{
				CollisionNormal = -Box.vAxis[ 0 ];
			}
		}
		else
		{
			if( TransformDir.z > 0 )
			{
				CollisionNormal = Box.vAxis[ 2 ];
			}
			else
			{
				CollisionNormal = -Box.vAxis[ 2 ];
			}
		}
	}
	else
	{
		if( fabs( TransformDir.y ) > fabs( TransformDir.z ) )
		{
			if( TransformDir.y > 0 )
			{
				CollisionNormal = Box.vAxis[ 1 ];
			}
			else
			{
				CollisionNormal = -Box.vAxis[ 1 ];
			}
		}
		else
		{
			if( TransformDir.z > 0 )
			{
				CollisionNormal = Box.vAxis[ 2 ];
			}
			else
			{
				CollisionNormal = -Box.vAxis[ 2 ];
			}
		}
	}
}