
///////////////////////////////////////////////////////////////////////////////
//
// File   : AVP_CalculateSurfaceReorientationNormal_SphereCache.cpp
// Author : Andy Tate
// Date   : 5-Feb-2008
//
// Copyright (C) 2008 Rebellion, All Rights Reserved.
//
///////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
// Include files...
////////////////////////////////////////////////////////////////////////////////

#include "Core/Asura.h"

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

// Asura independent header-files...
#include "Debugging/Asura_DebugLineRender.h"
#include "Dynamics/Asura_Collision_Object.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_Object.h"
#include "Physics/Asura_Physics_RigidBody.h"
#include "Physics/Asura_Physics_RigidBodyDefinition.h"
// Game independent header-files...
#include "Core/AVP_Maths.h"
#include "Core/AVP_Plane.h"
#include "Entity/PhysicalObject/AVP_ClientEntity_PhysicalObject.h"
#include "Entity/PhysicalObject/AVP_ServerEntity_PhysicalObject.h"
#include "Entity/PhysicalObject/Actor/Vertebrates/Alien/AVP_ClientActor_Alien.h"
#include "Mesh/AVP_PolygonFlags.h"

#if !defined(ASURA_NO_DEBUG_OUTPUT)
static bool dbg_bDebugRenderEnvCache	= false;	//	true;	//	
static bool dbg_bDebugRenderQuerySphere	= false;	//	true;	//	
static bool dbg_bDebugRenderTriangles	= false;	//	true;	//	
static bool dbg_bDebugRenderNormals		= false;	//	true;	//	
static bool dbg_bDebugRenderVelocities	= false;	//	true;	//	

static u_int g_uCurrentDebugColour	= 0;
static const u_int g_uMaxDebugRenderColours	= 8;
static const u_int g_auDebugRenderColours[ g_uMaxDebugRenderColours ] =
{
	0x80FFFFFF,
	0x80FFFF20,
	0x80FF20FF,
	0x8020FFFF,
	0x80FF2020,
	0x8020FF20,
	0x802020FF,
	0x80202020,
};

#endif	//!defined(ASURA_NO_DEBUG_OUTPUT)

DEBUGVAR float dbg_fQueryGrowLimit	= 0.25f; // 25 cm

AVP_CalculateSurfaceReorientationNormal_SphereCache::AVP_CalculateSurfaceReorientationNormal_SphereCache(const float fCosLimit, const u_short usPolyFlagsToIgnore)
: PARENT( fCosLimit, 1 )
, m_fQueryRadius( 0.0f )
, m_fClosestDistance( Asura_Maths::LargeFloat )
, m_fClosestDistanceOfBadNormal( Asura_Maths::LargeFloat )
, m_xQueryCentre( Asura_Maths::ZeroAVector3 )
, m_xClosestNormal( Asura_Maths::ZeroAVector3 )
, m_xPrevNormal( Asura_Maths::ZeroAVector3 )
{
	// Ensure that we ignore the polys we're supposed to...
	m_xEnvironmentCache.SetFlagsOfPolysToIgnore(usPolyFlagsToIgnore);
	m_xEnvironmentCache.SetFacesOnly(true);
}

void AVP_CalculateSurfaceReorientationNormal_SphereCache::Update(Asura_Physics_Object& xPhysicsObject,
																 Asura_AVector3_InParam xInputVelocity,
																 Asura_AVector3_InParam xInputNormal)
{
	// Call the parent version first...
	PARENT::Update(xPhysicsObject, xInputVelocity, xInputNormal);

	const Asura_Collision_Object* pxCollObj = xPhysicsObject.GetCollisionObject();
	Asura_Assert( pxCollObj, "No collision object" );
	if ( pxCollObj )
	{
		Asura_Bounding_Box	xBB	= pxCollObj->GetBoundingBox();
		xBB.Grow( dbg_fQueryGrowLimit );
		if ( xPhysicsObject.GetCollisionProxy() == Asura_Physics_Object::COLLISION_PROXY_SPHERE )
		{
			m_fQueryRadius = 0.5f * xBB.CalculateExtents().GetX();
		}
		else
		{
			m_fQueryRadius = xBB.CalculateRadius();
		}

		m_xQueryCentre = Asura_AVector3( xPhysicsObject.GetPosition() + ( xBB.CalculateCentre() * xPhysicsObject.GetOrientation() ) );
	}

	// Update the environment-cache...
	Asura_Bounding_Box xCacheBB = xPhysicsObject.GetBoundingBox();
	m_xEnvironmentCache.Update( xCacheBB, 1.0f );

#if !defined(ASURA_NO_DEBUG_OUTPUT)
	g_uCurrentDebugColour = 0;
	if ( dbg_bDebugRenderEnvCache )
	{
		// Render the OBBs used...
		Asura_Bounding_Box xBigCacheBB = xCacheBB;
		xBigCacheBB.Grow( 1.0f );
		Asura_DebugLineRender::AddBoundingBox( xCacheBB, Asura_Maths::ZeroVector3, Asura_Maths::IdentityMatrix3x3, 0x80FFFFFF );
		Asura_DebugLineRender::AddBoundingBox( xBigCacheBB, Asura_Maths::ZeroVector3, Asura_Maths::IdentityMatrix3x3, 0x80FFFFFF );
	}
	if ( dbg_bDebugRenderQuerySphere )
	{
		Asura_DebugLineRender::AddSphere( Asura_Vector_3( m_xQueryCentre ), m_fQueryRadius, 0x80FF00FF );
	}
#endif	// !defined(ASURA_NO_DEBUG_OUTPUT)

	m_fClosestDistance = m_fQueryRadius;
	m_fClosestDistanceOfBadNormal = Asura_Maths::LargeFloat;
	// Test for both environment surfaces AND objects...
	UpdateNormalForEnvironment( xPhysicsObject, xInputVelocity );
	UpdateNormalForObject( xPhysicsObject, xInputVelocity );
	
	if ( m_fClosestDistance < m_fQueryRadius )
	{
		AddNormal( m_xClosestNormal );
	}

	xPhysicsObject.SetAllSurfacesWalkable( m_fClosestDistanceOfBadNormal > m_fQueryRadius - dbg_fQueryGrowLimit ? !m_bWallWalkingDisabled : false); // walk on everything unless the nearest poly is a bad one

	// Update the velocity...
	if ( m_xDesiredNormal.ApproximatelyEquals( Asura_Maths::ZeroAVector3 ) )
	{
		m_xDesiredNormal = xInputNormal;
		m_xDesiredVelocity	= xInputVelocity;
	}
	else
	{
		m_xDesiredNormal.Normalise();
		Asura_Quat xRot;
		xRot.RotationFromAToB( xInputNormal, m_xDesiredNormal );
		m_xDesiredVelocity = xRot.RotateVector( xInputVelocity );
	}

	if ( !m_xDesiredNormal.ApproximatelyEquals( m_xPrevNormal ) )
	{
		float fDot = m_xDesiredNormal * m_xPrevNormal;
		m_xPrevNormal = m_xDesiredNormal;
		AVP_ClientActor_Alien* pxAlien = AVP_ClientActor_Alien::FindEntity( xPhysicsObject.GetAssociatedEntityGuid() );
		if ( pxAlien )
		{
			pxAlien->SetLookSmoothingTransitionAngle( Asura_Maths::ArcCosine( fDot ) );
		}
	}

#if !defined(ASURA_NO_DEBUG_OUTPUT)
	if ( dbg_bDebugRenderNormals )
	{
		Asura_DebugLineRender::AddArrow( m_xQueryCentre, m_xQueryCentre + xInputNormal, 0x80C00000 );
		Asura_DebugLineRender::AddArrow( m_xQueryCentre, m_xQueryCentre + m_xDesiredNormal, 0xFFFF0000 );
	}
	if ( dbg_bDebugRenderVelocities )
	{
		Asura_DebugLineRender::AddArrow( m_xQueryCentre, m_xQueryCentre + xInputVelocity, 0x80C00000 );
		Asura_DebugLineRender::AddArrow( m_xQueryCentre, m_xQueryCentre + m_xDesiredVelocity, 0xFFFF0000 );
	}
#endif
}

void AVP_CalculateSurfaceReorientationNormal_SphereCache::UpdateNormalForEnvironment(	const Asura_Physics_Object& xPhysicsObject,
																						Asura_AVector3_InParam xInputVelocity )
{
	const Asura_Physics_EnvironmentCache_Face* pxFace = m_xEnvironmentCache.GetFaceList();

	// Sum the normals of all possible "floor" polys...
	while(pxFace != NULL)
	{
		const Asura_Physics_Tri	xTriangle(pxFace->m_uFaceID);
		UpdateNormalForTriangle(xTriangle, xInputVelocity);

		pxFace	= pxFace->m_pxNext;
	}
}

void AVP_CalculateSurfaceReorientationNormal_SphereCache::UpdateNormalForObject(	const Asura_Physics_Object& xPhysicsObject,
																					Asura_AVector3_InParam xInputVelocity )
{
	Asura_Bounding_Box xBB = xPhysicsObject.GetBoundingBox();
	xBB.Grow( dbg_fQueryGrowLimit );
	Asura_Physics_Broadphase_Query xQuery;
	xQuery.MakeQuery( xBB, true, true, false, false );
	for ( const Asura_Physics_Broadphase_Entry* pxEntry = xQuery.GetFirstEntry(); pxEntry; pxEntry = pxEntry->GetNextEntry() )
	{
		Asura_Physics_RigidBodyInstance* pxRigidBodyInstance = pxEntry->GetInstance();
		if(pxRigidBodyInstance != NULL)
		{
			const Asura_AVector3& xInstancePosition = pxRigidBodyInstance->GetRenderingPosition();
			const Asura_AMatrix3x3& xInstanceOri = pxRigidBodyInstance->GetOrientation();
			const Asura_Guid uGuid = pxRigidBodyInstance->GetAssociatedEntityGuid();

			const bool bCanUseForSRO = AVP_ServerEntity_PhysicalObject::CanUseForSurfaceReorientation(uGuid);
			if ( bCanUseForSRO )
			{
				const Asura_Physics_RigidBodyInstance::COLLISION_PROXY eProxy = pxRigidBodyInstance->GetCollisionProxy();
				Asura_Physics_Tri			xTriangle;
				switch ( eProxy )
				{
					case Asura_Physics_RigidBodyInstance::COLLISION_PROXY_POLYGONAL:
					{
						const Asura_Physics_RigidBodyDefinition*	pxRigidBodyDefinition	= pxRigidBodyInstance->GetDefinition();
						Asura_Assert(pxRigidBodyDefinition, "No definition" );
						if(pxRigidBodyDefinition != NULL)
						{
							const Asura_CollisionMesh*	pxCollisionMesh	= pxRigidBodyDefinition->GetCollisionMesh();

							// Do we have a collision-mesh?
							Asura_Assertf( pxCollisionMesh, ("Art/Design bug: No Collision Mesh on polygonal object (%d)", uGuid ) );
							if(pxCollisionMesh)
							{
								Asura_Physics_CollisionMesh_Iterator	xIterator(pxCollisionMesh);

								// Create the triangles which make up this mesh...
								for(xIterator.AccessFirstTri(); xIterator.GetCurrentTri(xTriangle); xIterator.AccessNextTri())
								{
									if ( ( xIterator.GetFlagsOfCurrentTri() & AVP_PolygonFlag_IgnoreForReorientation ) == 0 )
									{
										xTriangle.Transform( xInstanceOri, xInstancePosition );
										UpdateNormalForTriangle(xTriangle, xInputVelocity);
									}
								}
							}
						}
					}
					break;

					case Asura_Physics_RigidBodyInstance::COLLISION_PROXY_OBB:
					default:
					{
						const Asura_OBB xCollOBB( pxRigidBodyInstance->GetHalfExtents(), pxRigidBodyInstance->GetCentreOfMass(), xInstanceOri );
						Asura_Physics_CollisionOBB_Iterator	xIterator ( xCollOBB );

						// Create the triangles which make up this OBB...
						for(xIterator.AccessFirstTri(); xIterator.GetCurrentTri(xTriangle); xIterator.AccessNextTri())
						{
							UpdateNormalForTriangle(xTriangle, xInputVelocity);
						}						
					}
					break;
				}
			}
		}
	}
}

void AVP_CalculateSurfaceReorientationNormal_SphereCache::UpdateNormalForTriangle(	const Asura_Physics_Tri& xTriangle,
																					Asura_AVector3_InParam xInputVelocity )
{
	Asura_AVector3	xClosestPointOnTri;
	Asura_Physics_CCD::FindClosestPointOnTriangle( m_xQueryCentre, xTriangle.GetVertexArray()[ 0 ], xTriangle.GetVertexArray()[ 1 ], xTriangle.GetVertexArray()[ 2 ], xClosestPointOnTri );
	Asura_AVector3 xToEdge = xClosestPointOnTri - m_xQueryCentre;
	const float fDist = xToEdge.NormaliseReturningMagnitude();
#if !defined(ASURA_NO_DEBUG_OUTPUT)
	g_uCurrentDebugColour	= ( g_uCurrentDebugColour + 1 ) % g_uMaxDebugRenderColours;
	u_int	uColour			= g_auDebugRenderColours[ g_uCurrentDebugColour ];
#endif
	if ( IsWithinCosLimits( -xToEdge, xInputVelocity ) )
	{
		if ( fDist < m_fClosestDistance )
		{
			m_fClosestDistance = fDist;
			m_xClosestNormal = -xToEdge;
		}
	}
	else
	{
		if ( fDist < m_fClosestDistanceOfBadNormal )
		{
			m_fClosestDistanceOfBadNormal = fDist;
		}
#if !defined(ASURA_NO_DEBUG_OUTPUT)
		uColour = 0xFFFF0000;
#endif
	}
#if !defined(ASURA_NO_DEBUG_OUTPUT)
	// Decide whether to render any debug data...
	if ( dbg_bDebugRenderTriangles )
	{
		const Asura_AVector3*	pxVertices	= xTriangle.GetVertexArray();

		Asura_DebugLineRender::AddLine( pxVertices[0], pxVertices[1], uColour );
		Asura_DebugLineRender::AddLine( pxVertices[1], pxVertices[2], uColour );
		Asura_DebugLineRender::AddLine( pxVertices[2], pxVertices[0], uColour );
		Asura_DebugLineRender::AddSphere( Asura_Vector_3( xClosestPointOnTri ), 0.05f, uColour, 4, 2 );
	}

	if ( dbg_bDebugRenderNormals )
	{
		Asura_DebugLineRender::AddArrow( xClosestPointOnTri, xClosestPointOnTri - xToEdge, uColour );
	}
#endif
}

/// End of file. ///
