
// Asura_Physics_MovementQuery.cpp
//
// Created by KJFL - 2 February	2007
//
// Copyright (C) 2007 Rebellion, All Rights	Reserved.
//

#include "Core/Asura.h"

// This	file's header.
#include "Physics/Asura_Physics_MovementQuery.h"

#include "Debugging/Asura_Assert.h"

#include "Physics/Asura_Physics_Broadphase.h"
#include "Physics/Asura_Physics_CCD.h"
#include "Physics/Asura_Physics_CollisionMesh.h"
#include "Physics/Asura_Physics_Environment.h"
#include "Physics/Asura_Physics_EnvironmentCache.h"
#include "Physics/Asura_Physics_RigidBody.h"
#include "Physics/Asura_Physics_RigidBodyDefinition.h"

Asura_Physics_MovementQuery::Asura_Physics_MovementQuery( const	Asura_AVector3&	xStartPosition,	const Asura_AVector3& xDisplacement,
														  const	Asura_OBB& xOBB, enum ASURA_PMQ_COLLISION_PROXY	eCollisionProxy/* =	ASURA_PMQCP_OBB	*/,
														  u_short usFlagsToIgnore/*	= 0*/, bool	bStopAtFirstCollision/*	= true */,
														  bool bCollideWithStaticObjects/* = false*/, bool bCollideWithExternalControlObjects/*	= false	*/,
														  bool bCollideWithActorObjects/* =	false */, bool bCollideWithEnvironment/* = true	*/,
														  bool bCollideWithDynamicObjects/* = false */,
														  u_int uCollisionGroups/* = 0xffffffff*/, u_int uCollisionGroupsToCollideWith/* = 0xffffffff*/
)
: m_xStartPosition(	xStartPosition )
, m_xDisplacement( xDisplacement )
, m_xOBB( xOBB )
, m_eCollisionProxy( eCollisionProxy )
, m_iHandleToIgnore( ASURA_INVALID_HANDLE )
, m_usFlagsToIgnore( usFlagsToIgnore )
, m_bStopAtFirstCollision( bStopAtFirstCollision )
, m_bCollideWithStaticObjects( bCollideWithStaticObjects )
, m_bCollideWithExternalControlObjects(	bCollideWithExternalControlObjects )
, m_bCollideWithActorObjects( bCollideWithActorObjects )
, m_bCollideWithEnvironment( bCollideWithEnvironment )
, m_bCollideWithDynamicObjects( bCollideWithDynamicObjects )
, m_uCollisionGroups( uCollisionGroups )
, m_uCollisionGroupsToCollideWith( uCollisionGroupsToCollideWith )
{
}

// Starting	position for test object
void Asura_Physics_MovementQuery::SetStartPosition(	const Asura_AVector3& xStartPosition )
{
	m_xStartPosition = xStartPosition;
}

const Asura_AVector3& Asura_Physics_MovementQuery::GetStartPosition( void )	const
{
	return m_xStartPosition;
}

// Collision OBB for test object
void Asura_Physics_MovementQuery::SetOBB( const	Asura_OBB& xOBB	)
{
	m_xOBB = xOBB;
}

const Asura_OBB& Asura_Physics_MovementQuery::GetOBB( void ) const
{
	return m_xOBB;
}

// Displacement	is the direction and distance you wish the test	object to try to travel
void Asura_Physics_MovementQuery::SetDisplacement( const Asura_AVector3& xDisplacement )
{
	m_xDisplacement	= xDisplacement;
}

const Asura_AVector3& Asura_Physics_MovementQuery::GetDisplacement(	void ) const
{
	return m_xDisplacement;
}

// Should test object Stop at first	collision, or carry	on moving as an	Actor would
void Asura_Physics_MovementQuery::SetStopAtFirstCollision( bool	bStop )
{
	m_bStopAtFirstCollision	= bStop;
}
bool Asura_Physics_MovementQuery::GetStopAtFirstCollision( void	) const
{
	return m_bStopAtFirstCollision;
}

// Flags of	polygons test object should	ignore
void Asura_Physics_MovementQuery::SetFlagsToIgnore(	u_short	usFlags	)
{
	m_usFlagsToIgnore =	usFlags;
}
u_short	Asura_Physics_MovementQuery::GetFlagsToIgnore( void	) const
{
	return m_usFlagsToIgnore;
}

// Handle of physics object	test object	should ignore
void Asura_Physics_MovementQuery::SetHandleToIgnore( Asura_Handle iHandle )
{
	m_iHandleToIgnore =	iHandle;
}

Asura_Handle Asura_Physics_MovementQuery::GetHandleToIgnore( void )	const
{
	return m_iHandleToIgnore;
}

// object collisions: vs STATIC	objects
void Asura_Physics_MovementQuery::SetCollideWithStaticObjects( bool	bCollide )
{
	m_bCollideWithStaticObjects	= bCollide;
}
bool Asura_Physics_MovementQuery::GetCollideWithStaticObjects( void	) const
{
	return m_bCollideWithStaticObjects;
}

// object collisions: vs EXTERNALCONTROL objects
void Asura_Physics_MovementQuery::SetCollideWithExternalControlObjects(	bool bCollide )
{
	m_bCollideWithExternalControlObjects = bCollide;
}
bool Asura_Physics_MovementQuery::GetCollideWithExternalControlObjects(	void ) const
{
	return m_bCollideWithExternalControlObjects;
}

// object collisions: vs ACTOR objects
void Asura_Physics_MovementQuery::SetCollideWithActorObjects( bool bCollide	)
{
	m_bCollideWithActorObjects = bCollide;
}
bool Asura_Physics_MovementQuery::GetCollideWithActorObjects( void ) const
{
	return m_bCollideWithActorObjects;
}

// object collisions: vs environment
void Asura_Physics_MovementQuery::SetCollideWithEnvironment( bool bCollide )
{
	m_bCollideWithEnvironment =	bCollide;
}

bool Asura_Physics_MovementQuery::GetCollideWithEnvironment( void )	const
{
	return m_bCollideWithEnvironment;
}

// object collisions: vs DYNAMIC objects
void Asura_Physics_MovementQuery::SetCollideWithDynamicObjects( bool bCollide )
{
	m_bCollideWithDynamicObjects = bCollide;
}
bool Asura_Physics_MovementQuery::GetCollideWithDynamicObjects( void ) const
{
	return m_bCollideWithDynamicObjects;
}


void Asura_Physics_MovementQuery::SetCollisionGroups( u_int uGroupsBitfield )
{
	m_uCollisionGroups = uGroupsBitfield;
}
u_int Asura_Physics_MovementQuery::GetCollisionGroups( void ) const
{
	return m_uCollisionGroups;
}

void Asura_Physics_MovementQuery::SetCollisionGroupsToCollideWith( u_int uGroupsBitfield )
{
	m_uCollisionGroupsToCollideWith = uGroupsBitfield;
}
u_int Asura_Physics_MovementQuery::GetCollisionGroupsToCollideWIth( void ) const
{
	return m_uCollisionGroupsToCollideWith;
}


////////////////////////////////////////////////////////////////////
// Method to trigger a query, after	which output data may be read //
////////////////////////////////////////////////////////////////////

// TODO	- need these in	a shared place
static const float fAXIALLYALIGNED_DIST_THRESHOLD =	0.005f;
static const float fVERTICAL_BOUNDARY =	0.70710678f; //	= cosine(45degrees)

Asura_Physics_MovementQuery_Results::Asura_Physics_MovementQuery_Results( const	Asura_Physics_MovementQuery& xInput	)
{
	Asura_AVector3 xDirection =	xInput.GetDisplacement();;

	// normalise the direction of travel and calculate how far we should be	going in that direction
	float fDistanceLeftToTravel	= xDirection.NormaliseReturningMagnitude();
	
	Asura_Physics_EnvironmentCache xEnvironmentCache;
	xEnvironmentCache.SetFlagsOfPolysToIgnore( xInput.GetFlagsToIgnore() );

	// we'll update	the	end	position as	we go
	m_xEndPosition = xInput.GetStartPosition();
	m_xAllowedDisplacement.Zero();
	m_xLastCollisionNormal.Zero();

	u_int uIterations =	0;

	const u_int	uMAX_NUMBER_OF_ITERATIONS_FOR_ACTOR_MOVEMENT = 10;

	const bool bEnvironmentCollisions =	xInput.GetCollideWithEnvironment();
	const bool bObjectCollisions	  =	xInput.GetCollideWithStaticObjects() ||	xInput.GetCollideWithExternalControlObjects() || xInput.GetCollideWithActorObjects() || xInput.GetCollideWithDynamicObjects();

	while (	fDistanceLeftToTravel >	0.0f )
	{
		// KJFL	- 27 September 2006	- keep track of	how	many times we resolve our motion against obstructions
		// and at some point give up, e.g. we're at	the	bottom of a	V like pit,	and	non	of the polygons	are
		// flagged as a	walkable surface, so the actor is well and truly stuck.
		if ( ++uIterations > uMAX_NUMBER_OF_ITERATIONS_FOR_ACTOR_MOVEMENT )
		{
			break;
		}
	
		u_int uLastCollisionFaceID = 0xffffffff;
		u_int uLastCollisionFaceFlags =	0;
		bool bStuck = false;

		Asura_AVector3 xVelocity = xDirection *	fDistanceLeftToTravel;

		// find	the	bounding box covering the old and new positions
		// TODO	- some sort	of Asura_AABB to match Asura_OBB which the physics can use?
		Asura_Bounding_Box xBoundingBox;
		xBoundingBox.Reset();
		xBoundingBox.Set( Asura_Vector_3( m_xEndPosition ) );
		xBoundingBox.Set( Asura_Vector_3( m_xEndPosition + xVelocity ) );
		xBoundingBox.Grow( xInput.GetOBB().GetMaximumHalfExtent() );

		float fDistanceAllowed = fDistanceLeftToTravel;

		if ( bEnvironmentCollisions	)
		{
			// test	against	environment	and	see	what's in our way
			xEnvironmentCache.Update( xBoundingBox );

			switch ( xInput.GetCollisionProxy()	)
			{
				default:
				case Asura_Physics_MovementQuery::ASURA_PMQCP_OBB:
				{
					float fTimeToImpact	= 1.0f;
					
					if ( xEnvironmentCache.GetFaceList() )
					{
						for	( Asura_Physics_EnvironmentCache_Face* pxFace =	xEnvironmentCache.GetFaceList(); pxFace; pxFace	= pxFace->m_pxNext )
						{
							Asura_Physics_Tri xTri(	pxFace->m_uFaceID);
							Asura_OBB xMovementOBB(	xInput.GetOBB()	);

							// TODO	- displace method required
							xMovementOBB.SetCentre(	m_xEndPosition + xInput.GetOBB().GetCentre() );
							
							Asura_AVector3 xNormal;
							bool bHitEdge;

							float fHitTime = Asura_Physics_CCD::FindTimeOfIntersection_SweptOBBVsTri( xMovementOBB,	xVelocity, xTri, xNormal, pxFace->m_uLastSeparationPlane, bHitEdge );

							if (fHitTime < -0.1f)
							{
								// We're stuck.	Quit out. 
								return;
							}

							if (fHitTime<fTimeToImpact)
							{
								fTimeToImpact =	fHitTime;
								uLastCollisionFaceID = pxFace->m_uFaceID;
								
								m_xLastCollisionNormal = xNormal;
							}
							if (fTimeToImpact == 0.0f) break;

						}
					}

					if (fTimeToImpact <	1.0f)
					{
						// we've hit something... find how far we're allowed to	travel,	and	subtract a 
						// teeny distance to allow for float inaccuracies.
						fDistanceAllowed = Asura_Maths::Max((fTimeToImpact * fDistanceLeftToTravel)	- fAXIALLYALIGNED_DIST_THRESHOLD, 0.0f);
					}
					else
					{
						fDistanceAllowed = fDistanceLeftToTravel;
					}

					break;
				}
				case Asura_Physics_MovementQuery::ASURA_PMQCP_SPHERE:
				case Asura_Physics_MovementQuery::ASURA_PMQCP_SPHERESTACK:
				{
					Asura_OBB xMovementOBB(	xInput.GetOBB()	);

					// TODO	- displace method required
					xMovementOBB.SetCentre(	m_xEndPosition + xInput.GetOBB().GetCentre() );
				
					bool bSphereStack =	( xInput.GetCollisionProxy() ==	Asura_Physics_MovementQuery::ASURA_PMQCP_SPHERESTACK );

					Asura_Physics_SphereGroup xSphereStack(	xMovementOBB, bSphereStack );
		
					bool bHitEdge;

					fDistanceAllowed = xSphereStack.SweptSpheresVsEnvironment( xDirection, fDistanceLeftToTravel, xEnvironmentCache, m_xLastCollisionNormal, uLastCollisionFaceID, bHitEdge );
				}	
			}
		}

		// test	vs objects
		if ( bObjectCollisions && fDistanceAllowed > 0.0f )
		{	
			Asura_Physics_Broadphase_Query xBroadphaseQuery;

			xBroadphaseQuery.MakeQuery(	xBoundingBox, xInput.GetCollideWithStaticObjects(),	xInput.GetCollideWithExternalControlObjects(), xInput.GetCollideWithActorObjects(), xInput.GetCollideWithDynamicObjects() );

			Asura_OBB xMovementOBB(	xInput.GetOBB()	);

			// TODO	- displace method required
			xMovementOBB.SetCentre(	m_xEndPosition + xInput.GetOBB().GetCentre() );

			Asura_AVector3 xRelativeVelocity = xVelocity;
			
			float fTimeToImpact	= 1.0f;
			Asura_Physics_RigidBodyInstance* pxObstacleHit = 0;

			for	( const	Asura_Physics_Broadphase_Entry*	pxEntry	= xBroadphaseQuery.GetFirstEntry();	pxEntry; pxEntry = pxEntry->GetNextEntry() )
			{
				Asura_Physics_RigidBodyInstance* pxObstacle	= pxEntry->GetInstance();

				// should I collide with his group(s)?
				if ( (xInput.GetCollisionGroupsToCollideWIth() & pxObstacle->GetCollisionGroups()) == 0 ) continue;

				// should he collide with my group(s)?
				if ( (xInput.GetCollisionGroups() & pxObstacle->GetCollisionGroupsToCollideWith()) ==0 ) continue;
		
				if ( pxObstacle->GetHandle() ==	xInput.GetHandleToIgnore() )
				{
					continue;
				}

				switch ( pxObstacle->GetCollisionProxy() )
				{
					default:
					case Asura_Physics_RigidBodyInstance::COLLISION_PROXY_OBB:
					{
						const Asura_OBB	xObstacleOBB( pxObstacle->GetHalfExtents(),	pxObstacle->GetCentreOfMass(), pxObstacle->GetOrientation()	);

						// unlike Actor movement, we want to count any initial overlaps as "can't move" rather than ignoring them.
						if ( Asura_Physics_CCD::FindTimeOfIntersection_SweptOBBVsOBB( xMovementOBB,	xObstacleOBB, xRelativeVelocity, fTimeToImpact,	m_xLastCollisionNormal, false ) )
						{
							uLastCollisionFaceID = 0xffffffff;
							pxObstacleHit =	pxObstacle;
							m_xLastCollisionNormal.Normalise();
						}
						break;
					}
					case Asura_Physics_RigidBodyInstance::COLLISION_PROXY_POLYGONAL:
					{
						if ( !pxObstacle->GetDefinition() )
						{
							break;
						}

						const Asura_CollisionMesh* pxMesh =	pxObstacle->GetDefinition()->GetCollisionMesh();

						if ( !(pxMesh) )
						{
							Asura_Assertf( false, ("Polygonal Proxy	object has no collision	mesh - associated Guid is %d", pxObstacle->GetAssociatedEntityGuid()) );
							break;
						}

						u_short	usFlagsOfHitPoly = 0;

						Asura_Physics_CollisionMesh_Iterator xTriIterator( pxMesh );
						Asura_Physics_Tri xTri;
						for	(xTriIterator.AccessFirstTri();	xTriIterator.GetCurrentTri(	xTri );	xTriIterator.AccessNextTri() )
						{
							// TODO	- rather than calculating the OBB and Tri in world space, perhaps we can use the Tri's local space?
							// we'd	have to	transform something	(i.e. OBB or Tri) so perhaps it	wouldn't save much
							xTri.Transform(	pxObstacle->GetOrientation(), pxObstacle->GetRenderingPosition() );

							u_int uFirstPlane =	0;
							Asura_AVector3 xAxisUsed;
							bool bHitEdge;

							float fHitTime = Asura_Physics_CCD::FindTimeOfIntersection_SweptOBBVsTri( xMovementOBB,	xRelativeVelocity, xTri, xAxisUsed, uFirstPlane,	bHitEdge );

							if ( fHitTime <= 0.0f )	
							{
								fTimeToImpact =	-1.0f;
								bStuck = true;
								break;
							}
							// if you've got an	earlier	impact,	OR if you've found a poly with the same	time of	impact which is	"more in the way"
							else if	( (	fHitTime < fTimeToImpact ) || (	fHitTime ==	fTimeToImpact && xAxisUsed*xVelocity < m_xLastCollisionNormal*xVelocity) )
							{
								// record the new candidate	for	the	first time of impact
								fTimeToImpact =	fHitTime;
								m_xLastCollisionNormal = xAxisUsed;
								usFlagsOfHitPoly = xTriIterator.GetFlagsOfCurrentTri();
								pxObstacleHit =	pxObstacle;
							}
						}

						uLastCollisionFaceFlags	= usFlagsOfHitPoly;

						break;
					}
				}
			}
			if (fTimeToImpact<0.0f)
			{
				fDistanceAllowed = 0.0f;
			}
			else if	(fTimeToImpact<1.0f)
			{
				fDistanceAllowed = Asura_Maths::Max((fTimeToImpact * fDistanceAllowed) - fAXIALLYALIGNED_DIST_THRESHOLD, 0.0f);

				Asura_Assert( pxObstacleHit, "Code bug:	null pointer" );
			}
		}

		if ( bStuck ) { break; }

		// if we can move at all, let's	do so and update the necessary positional data
		if (fDistanceAllowed > 0.0f)
		{
			const Asura_AVector3 xAllowedDisplacement =	xDirection * fDistanceAllowed;

			m_xEndPosition += xAllowedDisplacement;
			m_xAllowedDisplacement += xAllowedDisplacement;

			// subtract	how	far	we've already come from	the	distance we	were expecting to travel.
			fDistanceLeftToTravel -= fDistanceAllowed;
		}
		
		// if we're	flagged	to only	collide	once, we're	done
		if ( xInput.GetStopAtFirstCollision() )
		{
			break;
		}

		// otherwise if	the	distance left to travel	is tiny	then just don't	bother.
		if (fDistanceLeftToTravel <	0.0001f) break;

		const float	fLastCollisionNormalY =	m_xLastCollisionNormal.GetY();
			
		// get the flags for the last face we hit -	if indeed it was a face	(e.g. it could have	been an	edge, or an	object)
		if ( uLastCollisionFaceID != 0xffffffff	)
		{
			uLastCollisionFaceFlags	= Asura_Physics_Environment::GetFaceFlags( uLastCollisionFaceID	);
		}

		// If we're	in contact with	the	floor, and we have no lateral movement (i.e. the character 
		// would like to stand still), don't do	another	iteration (which would make	the	character slide
		// down	a slope, etc.)
		if ( fLastCollisionNormalY < -fVERTICAL_BOUNDARY ||	uLastCollisionFaceFlags	& AsuraPolygonFlag_Physics_WalkableSurface )
		{
			// KJFL	- 28 March 2007	- may want tolerance factor	here as	with Actor movement
			if (xInput.GetDisplacement().GetX() ==	0.0f &&	xInput.GetDisplacement().GetZ()	== 0.0f	) break;
		}
	
		// if the actor	has	hit	a vertical surface,	which is flagged as	walkable, go upwards, e.g. treat it	as stairs
		if ( fLastCollisionNormalY < fVERTICAL_BOUNDARY	&& fLastCollisionNormalY > -fVERTICAL_BOUNDARY )
		{
			if ( uLastCollisionFaceFlags & AsuraPolygonFlag_Physics_WalkableSurface	)
			{
				//xDisp.m_fX = 0.0f;
				xDirection.SetY(-30.0f);
				//xDisp.m_fZ = 0.0f;
				xDirection.Normalise();
				//m_bIsInContactWithFloor =	true;
			}
		}

		// resolve the direction of	travel against this	latest collision
		const float	fDot = -xDirection * m_xLastCollisionNormal;

		// if the direction	of travel is blocked by	the	collision vector...
		if (fDot > 0.0f)
		{
			// give	the	resolution a tiny boost	to avoid sticking against this surface again next frame
			// (we need	to minimise	this value as much as possible really)
			xDirection += m_xLastCollisionNormal * fDot *	1.01f;
		}

		// renormalise the direction of	travel
		const float	fMagnitude = xDirection.NormaliseReturningMagnitude();

		// however,	if the resolved	direction of travel	vector was relatively small
		// prior to	normalisation, this	indicates the object is	heading	pretty
		// much	straight towards an	obstacle - so we should	just stop.
		//
		// If we don't do this objects will	be super slidy - e.g. heading towards a	
		// seemingly perpendicular wall	will slide us left or right	depending on 
		// floating	point inaccuracies.	We may actually	want this for NPCs or something,
		// so a	IsSuperSlidy flag may be appropriate at	some point.
		if (fMagnitude < 0.1f) break;

	}

}
