//
// Asura_Physics_Ragdoll.cpp
//
// Created by KJFL - 12 October 2004
//
// Copyright (C) 2004 Rebellion, All Rights Reserved.
//

#include "Core/Asura.h"

#ifndef ASURA_PHYSICS_DISABLE_RAGDOLL

// comment this out to go back to the single-thread version
#define ASURA_USE_MULTITHREADED_RAGDOLL

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

#include "Animation/Asura_Animation.h"

#ifdef ASURA_TOOLS
// TODO - yuk. This is currently needed due to the line in Initialise() below.
#include "PC/FileSystem/Tools_FileSystem.h"
#endif

#include "Physics/Asura_Physics.h" // currently only needed for gravity values
#include "Physics/Asura_Physics_CCD.h"
#include "Physics/Asura_Physics_Solver.h"

// These four are needed for ragdoll - object collisions. 
// Refactoring may reduce the number needed.
#include "Physics/Asura_Physics_Broadphase.h"
#include "Physics/Asura_Physics_CollisionMesh.h"
#include "Physics/Asura_Physics_RigidBody.h"
#include "Physics/Asura_Physics_RigidBodyDefinition.h"

#if defined( ASURA_USE_PRIMITIVE_RENDER )
#include "Rendering/Asura_Primitive_Render.h"
#endif

// TODO - this is only here for hacky impulse tests
#include "Core/Asura_UserInput.h"

// TODO - this can probably go!
Asura_AVector3 evilg_xRagdollForceEffect;

Asura_Physics_Ragdoll_Factory Asura_Physics_Ragdoll_System::s_xFactory;
Asura_Physics_Ragdoll_System::RagdollDescriptionTree Asura_Physics_Ragdoll_System::s_xRagdollDescriptionTree;

// KJFL - 17 Dec 2008 - TODO temporarily allocating some memory to run the solvers.
// This will eventually be part of the Solver class proper, so we won't need to do this.
static const u_int uMAX_NUMBER_OF_CONTACTS = 64;
static Asura_Physics_Solver_SingleBodyContact g_axContacts[uMAX_NUMBER_OF_CONTACTS];

static const float fTOTAL_TIME_IN_CONTACT_BEFORE_SLEEPING = 10.0f;
static const float fTOTAL_TIME_MOTIONLESS_BEFORE_SLEEPING = 0.5f;

#if defined( ASURA_USE_PRIMITIVE_RENDER )
static Asura_Hash_ID g_uDebugRenderCategory;
#endif

#ifdef ASURA_USE_COLLISIONFX
#include "Physics/Asura_Physics_CollisionFX_Report.h"

void Asura_Physics_Ragdoll_CFX_Reporter::CreateReports( Asura_Guid uEntityGuid ) const
{
	if ( m_fVelocityDotNormal > 0.0f )
	{
		// delay the divide until we have to do it...
		Asura_AVector3 xRelVelocity = m_xUnscaledVelocity / Asura_Physics_Ragdoll_System::GetTimeSlice() ;
		Asura_Physics_CollisionFX_Report_System::AddReport( m_xPosition, xRelVelocity, m_xNormal, uEntityGuid, m_uEnvironmentFaceID );
	}
}
#endif

void Asura_Physics_Ragdoll_System::Initialise( void )
{
	// TODO - is there a better place for this?
#ifdef ASURA_TOOLS
	Tools_FileSystem::LoadAsuraFiles("Ragdolls/*.ragdoll");
#endif

#if defined( ASURA_USE_PRIMITIVE_RENDER )
	g_uDebugRenderCategory = Asura_Primitive_Render::RegisterCategory("Physics:Ragdoll");
#endif
}

void Asura_Physics_Ragdoll_System::Reset( void )
{
	s_xFactory.Reset();
	// TODO - KJFL - 12 November 2008 - I've copied the previous system here,
	// and I wonder why the ragdoll description stuff is not reset here too?
	// Should it be Reset when not in Tools builds? (i.e. between levels - 
	// currently it looks like you keep them in memory, causing fragmentation).
}

void Asura_Physics_Ragdoll_System::Shutdown( void )
{
	s_xFactory.Reset();
	s_xRagdollDescriptionTree.Reset();
}

void Asura_Physics_Ragdoll_System::Update( void )
{
#ifndef ASURA_USE_MULTITHREADED_RAGDOLL
	Asura_TrackingHandlePool_Iterator<Asura_Physics_Ragdoll> xIterator = s_xFactory.GetPool()->GetIterator();

	for(xIterator.Begin(); !xIterator.End(); xIterator.Next())
	{
		Asura_Physics_Ragdoll* pxRagdoll = xIterator.GetCurrentPointer();

		if (pxRagdoll->IsAsleep()) continue;

		#ifdef ASURA_USE_COLLISIONFX
			pxRagdoll->ResetCFX();
			#endif

			u_int uNumberOfIterations = 4;
			do
			{
			pxRagdoll->Update();
			if (pxRagdoll->IsAsleep()) break;
		}
		while ( --uNumberOfIterations );

			#ifdef ASURA_USE_COLLISIONFX
			pxRagdoll->ReportCFX();
			#endif
	}
#else

	if (GetNumberOfActiveRagdolls()==0) return;

	Asura_TrackingHandlePool_Iterator<Asura_Physics_Ragdoll> xIterator = s_xFactory.GetPool()->GetIterator();
	
	const u_int uMaxNumberOfIterations = 4;
	u_int uNumberOfIterations = uMaxNumberOfIterations;
		
	do
	{
	
		for(xIterator.Begin(); !xIterator.End(); xIterator.Next())
		{
			Asura_Physics_Ragdoll* pxRagdoll = xIterator.GetCurrentPointer();

			// In the multithreaded version we check for sleepiness before
			// updating, rather than waiting for the solver results
			pxRagdoll->CheckSleepiness();

			if (pxRagdoll->IsAsleep()) continue;

			// first time round, reset the CFX
			#ifdef ASURA_USE_COLLISIONFX
			if ( uNumberOfIterations == uMaxNumberOfIterations)
			{
				pxRagdoll->ResetCFX();
			}
			#endif
		
			pxRagdoll->Update();
				
			// last time round, report any CFX that have been found
			#ifdef ASURA_USE_COLLISIONFX
			if ( uNumberOfIterations == 1 )
			{
				pxRagdoll->ReportCFX();
			}
			#endif
		}

		// Apart from on the last iteration, flush the solver.
		// TODO: we want to remove this, possibly by having barriers/locks
		// on each ragdoll. Ideally we'd do an entire ragdoll in one chunk, 
		// but getting that onto an SPU is the problem.
		if ( uNumberOfIterations != 1 )
		{
			Asura_Physics_Solver::End();
			Asura_Physics_Solver::Begin();
		}
	}
	while ( --uNumberOfIterations );

#endif

}
void Asura_Physics_Ragdoll_System::DebugRender()
{
#if defined( ASURA_USE_PRIMITIVE_RENDER )
	Asura_Primitive_Render::SetRenderParams( 0.0f, g_uDebugRenderCategory );
#endif

	Asura_TrackingHandlePool_Iterator<Asura_Physics_Ragdoll> xIterator = s_xFactory.GetPool()->GetIterator();

	for(xIterator.Begin(); !xIterator.End(); xIterator.Next())
	{
		Asura_Physics_Ragdoll* pxRagdoll = xIterator.GetCurrentPointer();
		pxRagdoll->DebugRender();
	}
}
// TODO - I think this should be Tools_ only, but we'll need to modify calling fns and check all projects
Asura_Physics_Ragdoll_Description* Asura_Physics_Ragdoll_System::GetRagdollDescription(const Asura_String& xName)
{
	return s_xRagdollDescriptionTree.GetDataByHash(xName.GetHash());
}

const Asura_Physics_Ragdoll_Description* Asura_Physics_Ragdoll_System::GetRagdollDescription(const Asura_Hash_ID uHashID)
{
	return s_xRagdollDescriptionTree.GetDataByHash(uHashID);
}

// create new uninitialised (ready for ReadFromChunkStream to be called on it)
Asura_Physics_Ragdoll_Description* Asura_Physics_Ragdoll_System::CreateNewRagdollDescription( const Asura_String& xName )
{
	Asura_Physics_Ragdoll_Description* pxRagdollDescription = s_xRagdollDescriptionTree.GetDataByHash(xName.GetHash());
	if (pxRagdollDescription) return NULL;

	pxRagdollDescription = s_xRagdollDescriptionTree.AddNode(xName.GetHash());
	if (pxRagdollDescription)
	{
		pxRagdollDescription->SetName(xName);
	}
	return pxRagdollDescription;
}

Asura_Handle Asura_Physics_Ragdoll_System::CreateRagdoll( const Asura_Physics_Ragdoll_Description& xRagdollDesc, class Asura_Animation* pxAnim, const Asura_Vector_3& xAnimPos, const Asura_Matrix_3x3& xAnimOri, bool bAnimateInitialPositions/* = false*/ )
{
	if (!pxAnim)
	{
		Asura_Assert( false, "Invalid pxAnim pointer" );
		return ASURA_INVALID_HANDLE;
	}

	if (!pxAnim->GetSkin())
	{
		Asura_Assert( false, "Invalid skin pointer" );
		return ASURA_INVALID_HANDLE;
	}

	if ( xRagdollDesc.GetNumberOfHierarchyNodes() != (u_int)pxAnim->GetSkin()->NumberOfBones )
	{
		Asura_Assert( false, "Number of bones in skin don't match given ragdoll description" );
		return ASURA_INVALID_HANDLE;
	}

	Asura_Handle iHandle = s_xFactory.CreateInstance();

	if (ASURA_INVALID_HANDLE == iHandle)
	{
		return iHandle;
	}

	Asura_Physics_Ragdoll* pxRagdoll = s_xFactory.GetInstancePointer( iHandle );

	pxRagdoll->Initialise( pxAnim, &xRagdollDesc, xAnimPos, xAnimOri );

	return iHandle;
}

void Asura_Physics_Ragdoll_System::DestroyRagdoll( const Asura_Handle iHandle )
{
	Asura_Physics_Ragdoll* pxRagdoll = s_xFactory.GetInstancePointer( iHandle );

	if (!pxRagdoll)
	{
		Asura_Assertf( false, ("Ragdoll not found for given handle: %i", iHandle) );
		return;
	}

	pxRagdoll->Shutdown();

	s_xFactory.DestroyInstance( iHandle );
}

#ifdef ASURA_TOOLS

int Asura_Physics_Ragdoll_System::GetNumRagdollDescriptions( u_int uBoneCountToMatch ) 
{
	if (uBoneCountToMatch)
	{
		int iCount = 0;
		Asura_TemplateHashTree_ConstIterator< Asura_Physics_Ragdoll_Description > xIt( s_xRagdollDescriptionTree );

		for ( ; !xIt.Done(); xIt.Next() )
		{
			const Asura_Physics_Ragdoll_Description* pxDesc = xIt.GetCurrent();
		
			if ( uBoneCountToMatch == pxDesc->GetNumberOfHierarchyNodes() ) ++iCount;
		}

		return iCount;
	}

	return s_xRagdollDescriptionTree.GetNumberOfNodes();
}
	
void Asura_Physics_Ragdoll_System::GetRagdollDescriptionNames( const char** pszNames, u_int uBoneCountToMatch )
{
	const char** pszDescriptionName = pszNames;

	Asura_TemplateHashTree_ConstIterator< Asura_Physics_Ragdoll_Description > xIt( s_xRagdollDescriptionTree );

	for ( ; !xIt.Done(); xIt.Next() )
	{
		const Asura_Physics_Ragdoll_Description* pxDesc = xIt.GetCurrent();

		if (uBoneCountToMatch)
		{
			if ( uBoneCountToMatch != pxDesc->GetNumberOfHierarchyNodes() ) continue;
		}

		*pszDescriptionName = pxDesc->GetName().GetString();
		pszDescriptionName++;
	}
}

#endif//ASURA_TOOLS

u_int Asura_Physics_Ragdoll_System::GetNumberOfActiveRagdolls( void )
{
	u_int uNumberActive = 0;

	// TODO - const here?
	Asura_TrackingHandlePool_Iterator<Asura_Physics_Ragdoll> xIterator = s_xFactory.GetPool()->GetIterator();

	//Asura_TemplateHashTree_Iterator<Asura_Physics_Ragdoll> xIterator = s_xFactory.GetPool()->GetIterator();

	for(xIterator.Begin(); !xIterator.End(); xIterator.Next())
	{
		Asura_Physics_Ragdoll* pxRagdoll = xIterator.GetCurrentPointer();

		if (!pxRagdoll->IsAsleep()) ++uNumberActive;
	}

	return uNumberActive;
}

void Asura_Physics_Ragdoll::Initialise( Asura_Animation* pxAnim, const Asura_Physics_Ragdoll_Description* pxDesc, const Asura_Vector_3& xAnimPos, const Asura_Matrix_3x3& xAnimOri )
{
	m_pxAnim = pxAnim;
	m_pxRagdollDesc = pxDesc;

	m_bReportEnvironmentCollisionFX = false;
	m_uEntityGuid = ASURA_GUID_UNREGISTERED;

	m_usFlagsOfPolysToIgnore = AsuraPolygonFlag_NoCollisions;

	// Prevent any further animations occurring...
	Asura_AnimationController_Group& xRootGroup	= m_pxAnim->GetRootControllerGroup();
	xRootGroup.Deactivate();

	// Extract the node positions and orientations in world space from the anim.
	const u_int uNumberOfNodes = m_pxRagdollDesc->GetNumberOfHierarchyNodes();

	m_pxNodeOrientations = new Asura_Matrix_3x3[ uNumberOfNodes ];
	m_pxNodePositions = new Asura_Vector_3[ uNumberOfNodes ];

	const Asura_Hierarchy_Tween* pxTweenStorage = pxAnim->GetTweenStore();
    if( pxTweenStorage )
	{
		if( !pxTweenStorage->IsPositionDataValid() )
		{
	        // ... so fix up the tween data by doing a quick update.
		    pxAnim->Update( xAnimPos, xAnimOri, ANIMTIME_USEGAMETIME, ASURA_HIERARCHY_RENDER_DONTRENDER | ASURA_HIERARCHY_RENDER_UPDATEBONESALWAYS );
	
		    Asura_Assert( pxTweenStorage->IsPositionDataValid(), "Position data not valid" );
		}
	
		for( u_int i = 0; i < uNumberOfNodes; ++i )
		{
			Asura_Matrix_3x3	xBoneOri;
			Asura_Vector_3		xBonePos;

			pxTweenStorage->GetBoneData(i).GetOrientation(xBoneOri);
			pxTweenStorage->GetBoneData(i).GetPosition(xBonePos);
		
			// Are bone positions & orientations are world relative already?
			if ( pxTweenStorage->IsPositionDataInWorldspace() )
			{
				m_pxNodePositions[i] = xBonePos;
				m_pxNodeOrientations[i] = xBoneOri;
			}
			else
			{
				// Compute the worldspace positions by hand, cascading the transformations bone to bone
				if(i)
				{
					int iParentIndex = pxAnim->GetSkin()->ParentBone[i];
					Asura_Assert( iParentIndex>=0 && iParentIndex<static_cast<int>(i), "Error in hierarchy - bone has parent out of expected index range" );

					m_pxNodePositions[i] = xBonePos * m_pxNodeOrientations[iParentIndex]+m_pxNodePositions[iParentIndex];
					m_pxNodeOrientations[i] =  m_pxNodeOrientations[iParentIndex] * xBoneOri;
				}
				else
				{
					m_pxNodePositions[0] = xBonePos * xAnimOri + xAnimPos;
					m_pxNodeOrientations[0] = xAnimOri * xBoneOri;
				}
			}

		}
	}

	UpdateBoundingBox();

	// Set up the collision objects positions and orientations from those nodes
	u_int uNumberOfCollisionObjects = m_pxRagdollDesc->GetNumberOfCollisionObjects();
	m_pxCollisionObjects = new Asura_Physics_Ragdoll_CollisionObject[ uNumberOfCollisionObjects ];

	for ( u_int u=0; u<uNumberOfCollisionObjects; ++u )
	{
		const Asura_Physics_Ragdoll_Desc_CollisionObject& xDesc = m_pxRagdollDesc->GetCollisionObject(u);
		Asura_Physics_Ragdoll_CollisionObject& xInstance = m_pxCollisionObjects[u];
		xInstance.m_pxDesc = &xDesc;
					
		const u_int uNode = xDesc.GetHierarchyNode();

		Asura_Assert( uNode!=Asura_Physics_Ragdoll_Description::uINVALID_INDEX, "Danger: Invalid index" );
		
		// take relative orientation into world space
		Asura_AMatrix3x3 xMatrix = xDesc.GetRelativeOrientation();
		
		// KJFL - 23 October 2008 - must remember that AMatrix3x3 currently has
		// multiply wrong way round!
		xInstance.m_xBodyState.m_xOrientation = xMatrix*Asura_AMatrix3x3( m_pxNodeOrientations[uNode] );
		xInstance.m_xBodyState.m_xCentreOfMass = Asura_AVector3(m_pxNodePositions[uNode]) + xDesc.GetRelativePosition() * xInstance.GetOrientation(); 
		xInstance.m_xBodyState.m_xPreviousCentreOfMass = xInstance.m_xBodyState.m_xCentreOfMass;

		// TODO - need to take animation velocities
		xInstance.ZeroMomentum();

#if 0
		// handy "force into ground" check
		if ( Asura_UserInput::KeyPressed( ASURA_KEYID_SHIFT ) )
		{
	//		xInstance.m_xLinearVelocity.SetY(0.25f);
			float f = Asura_Random::GetFloat(-0.04f, 0.04f);
			xInstance.m_xLinearVelocity += Asura_AVector3( 1.2f/50.0f+f, -1.0f/50.0f+f, f );
		}
#endif
		xInstance.CalculateWorldInvInertiaTensor();
	}

	WakeUp();
}
void Asura_Physics_Ragdoll::Shutdown( void )
{
	delete [] m_pxNodeOrientations;
	delete [] m_pxNodePositions;
	delete [] m_pxCollisionObjects;
}

void Asura_Physics_Ragdoll::CheckSleepiness( void )
{
	// nothing to do if we're already asleep
	if (IsAsleep()) return;

	// KJFL - 13 February 2009 - stop the ragdoll if it's been in contact for
	// a long time (e.g. 20 secs). Giving impulses to ragdolls resets this timer.
	// Ideally it shouldn't be needed, but it acts a last-resort freeze for 
	// ragdolls falling out of environment, spliced through collision mesh, etc.
	// May want to change drag so it slowly increases with time instead.
	if (m_fTimeInContact > fTOTAL_TIME_IN_CONTACT_BEFORE_SLEEPING)
	{
		Sleep();
		return;
	}

	// Otherwise stop the ragdoll if it's been near-motionless for long enough,
	// which is hopefully the most common form of sleeping.
	float fMaxDisplacementSqd = 0.0f;
	const float fSleepThreshold = 0.002f*0.002f;

	u_int uNumberOfCollisionObjects = m_pxRagdollDesc->GetNumberOfCollisionObjects();
	for ( u_int u=0; u<uNumberOfCollisionObjects; ++u )
	{
		Asura_Physics_Ragdoll_CollisionObject& xInstance = m_pxCollisionObjects[u];
	
		const Asura_AVector3 xDisplacement = xInstance.GetPosition() - xInstance.GetPreviousPosition();

		fMaxDisplacementSqd = Asura_Maths::Max(fMaxDisplacementSqd, xDisplacement.MagnitudeSquared());
	}

	if (fMaxDisplacementSqd < fSleepThreshold)
	{
		m_fTimeMotionless += Asura_Physics_Ragdoll_System::GetTimeSlice();
	
		if ( m_fTimeMotionless> fTOTAL_TIME_MOTIONLESS_BEFORE_SLEEPING )
		{
			Sleep();
		}
	}
	else
	{
		m_fTimeMotionless = 0.0f;
	}
}

void Asura_Physics_Ragdoll::Sleep( void )
{
	m_bAsleep = true;
	
	// Kill any momentum so that if we're woken up we don't suddenly
	// start moving again.
	ZeroMomentum();

	// KJFL - 2 February 2009 - TODO - Throw away the environment cache?
	// Throwing away the cache will make the ragdoll more expensive
	// when it's woken up - versus taking up memory while sleeping
	//m_xEnvironmentCache.Reset();
}

void Asura_Physics_Ragdoll::ZeroMomentum( void )
{
	u_int uNumberOfCollisionObjects = m_pxRagdollDesc->GetNumberOfCollisionObjects();
	for ( u_int u=0; u<uNumberOfCollisionObjects; ++u )
	{
		Asura_Physics_Ragdoll_CollisionObject& xInstance = m_pxCollisionObjects[u];
		xInstance.ZeroMomentum();
	}
}

void Asura_Physics_Ragdoll::Update()
{
	u_int uNumberOfCollisionObjects = m_pxRagdollDesc->GetNumberOfCollisionObjects();
#ifdef ASURA_USE_MULTITHREADED_RAGDOLL
	bool bInContact = false;

	for ( u_int u=0; u<uNumberOfCollisionObjects; ++u )
	{
		Asura_Physics_Ragdoll_CollisionObject& xInstance = m_pxCollisionObjects[u];
		
		// KJFL - 2 February 2009 - might as well throw these contacts away right now,
		// unless we need them for frame-to-frame coherence.
		xInstance.m_xContactClusterList.Reset();

		xInstance.Integrate();
		xInstance.FindEnvironmentContacts( this );
		//xInstance.FindObjectContacts( this );

		bInContact |= (xInstance.m_xContactClusterList.GetFirstCluster()!=0);
	}
	
	Asura_Physics_Solver::SolveRagdoll( this );
	
	if (bInContact)
	{
		m_fTimeInContact += Asura_Physics_Ragdoll_System::GetTimeSlice();
	}
	else
	{
		m_fTimeInContact = 0.0f;
	}

#else
	// KJFL - 17 Dec 2008 - TODO temporarily allocating some memory to run the solvers.
	// This will eventually be part of the Solver class proper, so we won't need to do this.
	u_int uTotalNumberOfContacts = 0;
	u_int uTotalNumberOfJoints = 0;

	Asura_Physics_Solver_Body* pxBodies = new Asura_Physics_Solver_Body[ uNumberOfCollisionObjects ];
	Asura_Physics_Solver_ConeTwistConstraint* pxJoints = new Asura_Physics_Solver_ConeTwistConstraint[ m_pxRagdollDesc->GetNumberOfJoints() ];

	for ( u_int u=0; u<uNumberOfCollisionObjects; ++u )
	{
		Asura_Physics_Ragdoll_CollisionObject& xInstance = m_pxCollisionObjects[u];

		xInstance.Integrate();
		xInstance.FindEnvironmentContacts( this );
		//xInstance.FindObjectContacts( this );

		pxBodies[u].Initialise( &xInstance );

		for ( const Asura_Physics_ContactCluster* pxCluster = xInstance.m_xContactClusterList.GetFirstCluster(); pxCluster; pxCluster = pxCluster->GetNextCluster() )
		{
		// some things are constant over a whole cluster - normal, penetration distance, friction & elasticity values
			const Asura_AVector3& xNormal = pxCluster->GetContactNormal();
			const float fPenetration = pxCluster->GetPlanePenetrationConstant() - xInstance.GetPosition()*xNormal;

			const u_int uNumberOfContacts = pxCluster->GetNumberOfContacts();

			if ( uNumberOfContacts+uTotalNumberOfContacts > uMAX_NUMBER_OF_CONTACTS)
			{
				break;
			}

			for (u_int v=0; v<uNumberOfContacts; ++v)
			{
				const Asura_AVector3 xRel = pxCluster->GetContactPosition(v) - xInstance.GetPosition();

				Asura_Physics_Solver_SingleBodyContact& xContact = g_axContacts[ uTotalNumberOfContacts++ ];
				Asura_Physics_Solver::AddContact( xContact, &pxBodies[u], xRel, xNormal, fPenetration );
			}
		}
		// KJFL - 2 February 2009 - might as well throw these contacts away right now,
		// unless we need them for frame-to-frame coherence.
		xInstance.m_xContactClusterList.Reset();
	}
	
	if (uTotalNumberOfContacts>0)
	{
		m_fTimeInContact += Asura_Physics_Ragdoll_System::GetTimeSlice();
	}
	else
	{
		m_fTimeInContact = 0.0f;
	}

	u_int uNumberOfJoints = m_pxRagdollDesc->GetNumberOfJoints();

	for ( u_int u=0; u<uNumberOfJoints; ++u )
	{
		const Asura_Physics_Ragdoll_Desc_Joint& xJoint =  m_pxRagdollDesc->GetJoint(u);

		if (!xJoint.IsValid()) continue;

		SetupConeTwist( pxJoints[uTotalNumberOfJoints], xJoint, pxBodies );
#ifndef ASURA_USE_MULTITHREADED_RAGDOLL
		pxJoints[uTotalNumberOfJoints].CalculateJacobians();
#endif
		++uTotalNumberOfJoints;
	}
	
	// resolve all joints and contacts
	for (u_int uIterations=0; uIterations<20; ++uIterations)
	{
		for ( u_int u=0; u<uTotalNumberOfJoints; ++u )
		{
			Asura_Physics_Solver_ConeTwistConstraint::Solve( pxJoints[u] );
		}
		for ( u_int u=0; u<uTotalNumberOfContacts; ++u )
		{
			Asura_Physics_Solver_SingleBodyContact::Solve_Clamped( g_axContacts[u] );
		}
	}

	// write back velocities to collision objects
	for ( u_int u=0; u<uNumberOfCollisionObjects; ++u )
	{
		Asura_Physics_Ragdoll_CollisionObject& xInstance = m_pxCollisionObjects[u];
		xInstance.m_xBodyState.m_xLinearVelocity = pxBodies[u].m_xLinearVelocity;
		xInstance.m_xBodyState.m_xAngularVelocity = pxBodies[u].m_xAngularVelocity;

		// KJFL - 12 February 2009 - split separation velocities: doesn't seem like
		// I need to go this far, but I'll leave the code here in just in case.
		/*
		xInstance.m_xPosition += pxBodies[u].m_xSeparationLinearVelocity;

		Asura_AMatrix3x3 xDelta;
		xDelta.Tilde( pxBodies[u].m_xSeparationAngularVelocity );		
		xDelta = xInstance.m_xOrientation*xDelta;

		xInstance.m_xOrientation.Add( xDelta );
		xInstance.m_xOrientation.Orthogonalise();
		*/
	}

	delete [] pxJoints;
	delete [] pxBodies;

	CheckSleepiness();
	
#endif
}

void Asura_Physics_Ragdoll::UpdateAnimation( void )
{
	// Take the ragdoll physics data and update the animation to match it

	// KJFL - 25 November 2008 - in the previous ragdoll system this call
	// to UpdateTimers was added at roughly this point (i.e. before updating
	// the tween store) to update any attachments. It may be slightly in the
	// wrong place - should it be after the tween store is updated? (I'm
	// worried attachments might be a frame out, but we'll see.)
	m_pxAnim->UpdateTimers();

	// fill in the tween store with our data
	Asura_Hierarchy_Tween* pxTween = m_pxAnim->GetTweenStore();
    if( pxTween ) 
    {
		for( u_int u = 0; u < pxTween->GetNumberOfBones(); ++u )
		{
			const Asura_Physics_Ragdoll_Desc_HierarchyNode& xNode = m_pxRagdollDesc->GetHierarchyNode( u );

			u_int uCollisionObject = xNode.GetCollisionObject();

			if ( uCollisionObject != Asura_Physics_Ragdoll_Description::uINVALID_INDEX )
			{
				const Asura_Physics_Ragdoll_CollisionObject& xObject = m_pxCollisionObjects[ uCollisionObject ];

				Asura_AMatrix3x3 xMatrix =  xNode.GetRelativeOrientation() * xObject.GetOrientation();
				xMatrix.ToMat3( m_pxNodeOrientations[u] );

				m_pxNodePositions[u] = xObject.GetPosition() + (xNode.GetRelativePosition() * xMatrix);

			}

			Asura_Hierarchy_TweenStore_BoneData& xBoneData = pxTween->GetBoneData(u);

			xBoneData.SetOrientation(m_pxNodeOrientations[u]);
			xBoneData.SetPosition(m_pxNodePositions[u]);

		}

		UpdateBoundingBox();

        pxTween->SetPositionDataValid(true);
        pxTween->SetPositionDataInWorldspace(true);
        
		m_pxAnim->IncrementNumTimerUpdates();
    }
}


void Asura_Physics_Ragdoll::UpdateBoundingBox( void )
{
	// KJFL - 3 February 2009 - this may be a) a bit too slow and b) smaller than the actual render mesh.
	// TODO - It may be easier just to take the zeroth bone and grow by a set radius.
	m_xBoundingBox.Reset();
	const u_int uNumberOfNodes = m_pxRagdollDesc->GetNumberOfHierarchyNodes();

	for ( u_int u=0; u<uNumberOfNodes; ++u )
	{
		m_xBoundingBox.Set( m_pxNodePositions[u] );
	}
}

void Asura_Physics_Ragdoll::SetFlagsOfPolysToIgnore( const u_short usFlags )
{
	m_usFlagsOfPolysToIgnore = usFlags;
	
	const u_int uNumberOfCollisionObjects = m_pxRagdollDesc->GetNumberOfCollisionObjects();
	
	for ( u_int u=0; u<uNumberOfCollisionObjects; ++u )
	{
		Asura_Physics_Ragdoll_CollisionObject& xInstance = m_pxCollisionObjects[u];

		xInstance.m_xEnvironmentCache.SetFlagsOfPolysToIgnore( usFlags );
	}

}


void Asura_Physics_Ragdoll::DebugRender( void )
{
#if defined( ASURA_USE_PRIMITIVE_RENDER )
	u_int uNumberOfCollisionObjects = m_pxRagdollDesc->GetNumberOfCollisionObjects();
	
	u_int uColour = 0xffff0000;

	if (IsAsleep())
	{
		uColour = 0xffff00ff;
	}

	for ( u_int u=0; u<uNumberOfCollisionObjects; ++u )
	{
		const Asura_Physics_Ragdoll_Desc_CollisionObject& xDesc = m_pxRagdollDesc->GetCollisionObject(u);
		const Asura_Physics_Ragdoll_CollisionObject& xInstance = m_pxCollisionObjects[u];

		Asura_AVector3 xExtents = xDesc.GetCollisionExtents();

		switch (xDesc.GetCollisionProxyType())
		{
			default:
			case Asura_Physics_Ragdoll_Desc_CollisionObject::COLLISION_SPHERE:
			{
				Asura_Primitive_Render::DrawSphereWorld( Asura_Vector_3( xInstance.GetPosition() ), xExtents.GetX(), uColour );
				break;
			}
			case Asura_Physics_Ragdoll_Desc_CollisionObject::COLLISION_CAPSULE:
			{
				Asura_Primitive_Render::DrawCapsuleWorld( xInstance.GetPosition(), xInstance.GetOrientation(), xExtents, uColour );
				break;
			}
		}
	}
#endif // #if defined( ASURA_USE_PRIMITIVE_RENDER )
}

void Asura_Physics_Ragdoll::ApplyImpulseToAll( const Asura_AVector3& xImpulse )
{
	WakeUp();

	Asura_AVector3 xScaledImpulse = xImpulse * Asura_Physics_Ragdoll_System::GetTimeSlice();

	u_int uNumberOfCollisionObjects = m_pxRagdollDesc->GetNumberOfCollisionObjects();
	
	for ( u_int u=0; u<uNumberOfCollisionObjects; ++u )
	{
		Asura_Physics_Ragdoll_CollisionObject& xInstance = m_pxCollisionObjects[u];

		xInstance.ApplyImpulse( xScaledImpulse );
	}

}

void Asura_Physics_Ragdoll::ApplyImpulseToNearbyNodes( const Asura_AVector3& xPosition, const Asura_AVector3& xImpulse )
{
	WakeUp();

	Asura_AVector3 xScaledImpulse = xImpulse * Asura_Physics_Ragdoll_System::GetTimeSlice();

	u_int uNumberOfCollisionObjects = m_pxRagdollDesc->GetNumberOfCollisionObjects();
	
	for ( u_int u=0; u<uNumberOfCollisionObjects; ++u )
	{
		Asura_Physics_Ragdoll_CollisionObject& xInstance = m_pxCollisionObjects[u];
		const Asura_Physics_Ragdoll_Desc_CollisionObject& xDesc = m_pxRagdollDesc->GetCollisionObject(u);
	
		Asura_AVector3 xDisp = xPosition - xInstance.GetPosition();

		if ( xDisp.MagnitudeSquared() < xDesc.GetCollisionExtents().MagnitudeSquared() )
		{
			xInstance.ApplyImpulseAtRelativePosition( xDisp, xScaledImpulse );
		}
	}

}

const Asura_Bounding_Box& Asura_Physics_Ragdoll::GetBoundingBox( void ) const
{
	return m_xBoundingBox;
}
		
void Asura_Physics_Ragdoll::GetRenderingPosition( Asura_Vector_3& xPosition ) const
{
	// return the zeroth bone position
	xPosition = m_pxNodePositions[0];
}

void Asura_Physics_Ragdoll::GetRenderingOrientation( Asura_Matrix_3x3& xOrientation ) const
{
	// return the zeroth bone orientation
	xOrientation = m_pxNodeOrientations[0];
}

static float ApproxAbsArcTan2( float fY, float fX )
{
	float fAbsY = Asura_Maths::Abs( fY );

	float fAngle = Asura_Maths::Pi * 0.25f ;

	// TODO - do I have to cope with both fX=0 & fY=0 ?

	if ( fX >= 0.0f )
	{
		float fRatio = ( fX - fAbsY ) / ( fAbsY + fX );
		fAngle *= (1.0f - fRatio);
	}
	else
	{
		float fRatio = ( fX + fAbsY ) / ( fAbsY - fX );
		fAngle *= (3.0f - fRatio);
	}
#if 01
	float f = (fY*fY + fX*fX) * 100.0f;
	f = f / (f+1.0f);
	fAngle *= f;
#endif
	return fAngle;
}

void Asura_Physics_Ragdoll::SetupConeTwist( Asura_Physics_Solver_ConeTwistConstraint& xConstraint, const Asura_Physics_Ragdoll_Desc_Joint& xJoint, Asura_Physics_Solver_Body* pxBodies )
{
	Asura_Assert( xJoint.IsValid(), "Joint not valid" );

	Asura_Physics_Ragdoll_CollisionObject& xBody1 = m_pxCollisionObjects[xJoint.GetObjectA()];
	Asura_Physics_Ragdoll_CollisionObject& xBody2 = m_pxCollisionObjects[xJoint.GetObjectB()];

	xConstraint.m_pxBodyA = &pxBodies[xJoint.GetObjectA()];
	xConstraint.m_pxBodyB = &pxBodies[xJoint.GetObjectB()];

	// Linear constraint

	const Asura_AVector3 xRel1 = xJoint.GetPositionRelativeToA() * xBody1.GetOrientation();
	const Asura_AVector3 xRel2 = xJoint.GetPositionRelativeToB() * xBody2.GetOrientation();

	Asura_AVector3 xDisplacement = (xRel1 + xBody1.GetPosition()) - (xRel2 + xBody2.GetPosition());

	const float fDistance = xDisplacement.NormaliseReturningMagnitude();

	const float fALLOWED_DISTANCE_THRESHOLD = 0.001f; // a mm

	Asura_AVector3 xTargetVelocity;

	if ( fDistance > fALLOWED_DISTANCE_THRESHOLD )
	{
		xTargetVelocity = xDisplacement * ( fDistance - fALLOWED_DISTANCE_THRESHOLD ) / 1.0f;
	}
	else
	{
		xTargetVelocity.Zero();	
	}

	xConstraint.m_xRelA = xRel1;
	xConstraint.m_xRelB = xRel2;
	xConstraint.m_xTargetVelocity = xTargetVelocity;

	// moved to solver itself
	//xConstraint.CalculateJacobians();

	// Angular constraint - swing & twist

	// Find the joint space relative to each object. As the objects rotate, the joint spaces will
	// become mismatched. When the difference between joint spaces becomes big enough, we need
	// to apply force to reduce the error.
	Asura_AMatrix3x3 xJA = xJoint.GetJointSpaceRelativeToA() * xBody1.GetOrientation();
	Asura_AMatrix3x3 xJB = xJoint.GetJointSpaceRelativeToB() * xBody2.GetOrientation();

	// We measure the error in two ways - 
	//
	// 1) A cone limit: by considering the divergence of the z-axis in the two joint spaces. 
	// 2) A twist limit: by considering the difference in angular rotation around the z-axes.

	// First find the "swing" values to test the cone limit
	
	Asura_AVector3 xJAAxisX = xJA.GetColumn0();
	Asura_AVector3 xJAAxisY = xJA.GetColumn1();
	Asura_AVector3 xJAAxisZ = xJA.GetColumn2();

	Asura_AVector3 xJBAxisX = xJB.GetColumn0();
	//Asura_AVector3 xJBAxisY = xJB.GetColumn1();
	Asura_AVector3 xJBAxisZ = xJB.GetColumn2();

	// Calculate some repeated operations.
	// There may even be an early out based on xJBAxisZ * xJAAxisZ. 
	float fXZ = xJAAxisX * xJBAxisZ;
	float fYZ = xJAAxisY * xJBAxisZ;
	float fZZ = xJAAxisZ * xJBAxisZ;
	
	float fSwingX = ApproxAbsArcTan2( fXZ, fZZ );
	float fSwingY = ApproxAbsArcTan2( fYZ, fZZ );

	float fSwingLimitX = Asura_Maths::Max( Asura_Maths::Pi/128.0f, xJoint.GetSwingLimitX() );
	float fSwingLimitY = Asura_Maths::Max( Asura_Maths::Pi/128.0f, xJoint.GetSwingLimitY() );

	bool bUseCombinedSwing = ( Asura_Maths::Max(fSwingLimitX, fSwingLimitY) < 2.0f*Asura_Maths::Min(fSwingLimitX,fSwingLimitY) );

	if (bUseCombinedSwing)
	{
		xConstraint.m_fSwingErrorY = 0.0f;
		xConstraint.m_bDoSwingY = false;

		// TODO - can precalc this reciprocal squared values
		float fRcpSqdLimitX = 1.0f / ( fSwingLimitX * fSwingLimitX );
		float fRcpSqdLimitY = 1.0f / ( fSwingLimitY * fSwingLimitY );

		float fEllipse = fSwingX*fSwingX*fRcpSqdLimitX + fSwingY*fSwingY*fRcpSqdLimitY;

		if (fEllipse > 1.0f )
		{
			xConstraint.m_fSwingErrorX = Asura_Maths::Min(1.0f,(Asura_Maths::Sqrt( fEllipse)  - 1.0f ))/50.0f;
			xConstraint.m_fSwingAccumulatedImpulseX = 0.0f;
			
			// Find the swing axis for the correction force
			Asura_AVector3 xAxis = xJBAxisZ ^ ( xJAAxisX * ( fXZ ) + xJAAxisY * ( fYZ ) );
			xAxis.Normalise();

			if (fZZ<0.0f) xAxis = -xAxis;

			xConstraint.m_xSwingAxisX = xAxis;
			xConstraint.m_fSwingInvJacobianDiagonalX = Asura_Physics_Solver_ConeTwistConstraint::CalculateAngularJacobian( xAxis, xBody1.GetWorldInvInertiaTensor(), xBody2.GetWorldInvInertiaTensor() );
			xConstraint.m_bDoSwingX = true;
		}
		else
		{
			xConstraint.m_fSwingErrorX = 0.0f;
			xConstraint.m_bDoSwingX = false;
		}
	}
	else
	{
		if ((fSwingX) > fSwingLimitX) 
		{
			xConstraint.m_fSwingErrorX = Asura_Maths::Min(1.0f,(fSwingX) / fSwingLimitX - 1.0f)/50.0f;
			
			float fSign = (fXZ>=0.0f) ? 1.0f : -1.0f;

			xConstraint.m_fSwingAccumulatedImpulseX = 0.0f;
			Asura_AVector3 xAxis = xJBAxisZ ^ ( xJAAxisX * fSign );
			xAxis.Normalise();

			if (fZZ<0.0f) xAxis = -xAxis;

			xConstraint.m_xSwingAxisX = xAxis;
			xConstraint.m_fSwingInvJacobianDiagonalX = Asura_Physics_Solver_ConeTwistConstraint::CalculateAngularJacobian( xAxis, xBody1.GetWorldInvInertiaTensor(), xBody2.GetWorldInvInertiaTensor() );
			xConstraint.m_bDoSwingX = true;
		}
		else
		{
			xConstraint.m_fSwingErrorX = 0.0f;
			xConstraint.m_bDoSwingX = false;
		}

		if ((fSwingY) > fSwingLimitY ) 
		{
			xConstraint.m_fSwingErrorY = Asura_Maths::Min(1.0f,(fSwingY) / fSwingLimitY -1.0f)/50.0f;
				
			float fSign = (fYZ>=0.0f) ? 1.0f : -1.0f;

			xConstraint.m_fSwingAccumulatedImpulseY = 0.0f;
			Asura_AVector3 xAxis = xJBAxisZ ^ ( xJAAxisY * fSign );
			xAxis.Normalise();

			if (fZZ<0.0f) xAxis = -xAxis;

			xConstraint.m_xSwingAxisY = xAxis;
			xConstraint.m_fSwingInvJacobianDiagonalY = Asura_Physics_Solver_ConeTwistConstraint::CalculateAngularJacobian( xAxis, xBody1.GetWorldInvInertiaTensor(), xBody2.GetWorldInvInertiaTensor() );
			xConstraint.m_bDoSwingY = true;
		}
		else
		{
			xConstraint.m_fSwingErrorY = 0.0f;
			xConstraint.m_bDoSwingY = false;
		}
	}


	// Find the "zero-twist" rotation that takes us between joint space z-axes
	Asura_Quat xZeroTwist;
	xZeroTwist.RotationFromAToB( xJBAxisZ, xJAAxisZ );

	// So where would we expect xJBAxisX to be if there wasn't any twist around the z-axis?
	Asura_AVector3 xZeroTwistAxisX = xZeroTwist.RotateVector( xJBAxisX );

	// By comparing where JBAxisX is now with where it should be with zero z-axis twist,
	// we can find our twist angle.
	float fDotXY = xZeroTwistAxisX*xJAAxisY;
	float fTwist = ApproxAbsArcTan2( fDotXY, xZeroTwistAxisX*xJAAxisX );
	
	float fTwistLimit = Asura_Maths::Max( Asura_Maths::Pi/128.0f, xJoint.GetTwistLimit() );

	if ( fTwist >= fTwistLimit )
	{
		xConstraint.m_fTwistError = Asura_Maths::Min(1.001f,(fTwist-fTwistLimit));
		xConstraint.m_fAccumulatedTwistImpulse = 0.0f;

		// Twist axis - make it the average of the z-axes. This will break if the axes
		// are a full 180 degrees apart though. We could use a quat rotation to get half-way
		// if it proves an issue.
		float fSign = Asura_Maths::Sign( fDotXY );
		Asura_AVector3 xAxis = ( xJAAxisZ + xJBAxisZ )*fSign;
		xAxis.Normalise();

		xConstraint.m_xTwistAxis = xAxis;
		xConstraint.m_fTwistInvJacobianDiagonal = Asura_Physics_Solver_ConeTwistConstraint::CalculateAngularJacobian( xAxis, xBody1.GetWorldInvInertiaTensor(), xBody2.GetWorldInvInertiaTensor() );
		xConstraint.m_bDoTwistZ = true;
	}
	else
	{
		xConstraint.m_fTwistError = 0.0f;
		xConstraint.m_bDoTwistZ = false;
	}



}

void Asura_Physics_Ragdoll_CollisionObject::Integrate( void )
{
	// KJFL - 2 December 2008 - TODO
	// Refactor out of method so not recalcing all the time, implement gravity scale per ragdoll, etc
	// May want to make this a Ragdoll class method, and pass in a Collision Object pointer? Issues
	// with private/public status etc.
	Asura_AVector3 xGravity = Asura_Physics_System::GetGravityDirection() * (10.0f * Asura_Physics_Ragdoll_System::GetTimeSlice() * Asura_Physics_Ragdoll_System::GetTimeSlice());
	
	// KJFL - 2 December 2008 - TODO
	// Drag currently constant - may want to vary over life of ragdoll, or have it settable in Tools.
	// Similarly I'm currently using the same value for Linear and Angular drag, which we may want to change.
	const float fDrag = 0.998f;
	
	m_xBodyState.m_xLinearVelocity = (m_xBodyState.m_xLinearVelocity * fDrag) + xGravity;
	m_xBodyState.m_xAngularVelocity *= fDrag;

	m_xBodyState.m_xPreviousCentreOfMass = m_xBodyState.m_xCentreOfMass;
	m_xBodyState.m_xCentreOfMass += m_xBodyState.m_xLinearVelocity;

	Asura_AMatrix3x3 xDelta;
	xDelta.Tilde( m_xBodyState.m_xAngularVelocity );		
	xDelta = m_xBodyState.m_xOrientation*xDelta;

	m_xBodyState.m_xOrientation.Add( xDelta );
	m_xBodyState.m_xOrientation.Orthogonalise();

	// update our inertia tensor		
	CalculateWorldInvInertiaTensor();

	// update broadphase AABB
	{
		Asura_AVector3 xExtents = m_pxDesc->GetCollisionExtents();

		// Could get this more accurate by doing using the "before" and "after" orientations, and then
		// min/maxing the two results.
		float fWorldSpaceExtentX = Asura_Maths::Abs( xExtents.GetX() * m_xBodyState.m_xOrientation.GetColumn0().GetX() )
								 + Asura_Maths::Abs( xExtents.GetY() * m_xBodyState.m_xOrientation.GetColumn1().GetX() )
								 + Asura_Maths::Abs( xExtents.GetZ() * m_xBodyState.m_xOrientation.GetColumn2().GetX() );

		float fWorldSpaceExtentY = Asura_Maths::Abs( xExtents.GetX() * m_xBodyState.m_xOrientation.GetColumn0().GetY() )
								 + Asura_Maths::Abs( xExtents.GetY() * m_xBodyState.m_xOrientation.GetColumn1().GetY() )
								 + Asura_Maths::Abs( xExtents.GetZ() * m_xBodyState.m_xOrientation.GetColumn2().GetY() );

		float fWorldSpaceExtentZ = Asura_Maths::Abs( xExtents.GetX() * m_xBodyState.m_xOrientation.GetColumn0().GetZ() )
								 + Asura_Maths::Abs( xExtents.GetY() * m_xBodyState.m_xOrientation.GetColumn1().GetZ() )
								 + Asura_Maths::Abs( xExtents.GetZ() * m_xBodyState.m_xOrientation.GetColumn2().GetZ() );

	
		// KJFL - 24 September 2008 - we do have vector min and max methods, but since
		// the bounding box members aren't vectorised we'll have little gain in using 
		// them. Something to revisit if/when we have an AABB based on Asura_AVector.
		m_xBroadphaseAABB.MinX = Asura_Maths::Min( m_xBodyState.m_xCentreOfMass.GetX(), m_xBodyState.m_xPreviousCentreOfMass.GetX() ) - fWorldSpaceExtentX;
		m_xBroadphaseAABB.MaxX = Asura_Maths::Max( m_xBodyState.m_xCentreOfMass.GetX(), m_xBodyState.m_xPreviousCentreOfMass.GetX() ) + fWorldSpaceExtentX;
																										
		m_xBroadphaseAABB.MinY = Asura_Maths::Min( m_xBodyState.m_xCentreOfMass.GetY(), m_xBodyState.m_xPreviousCentreOfMass.GetY() ) - fWorldSpaceExtentY;
		m_xBroadphaseAABB.MaxY = Asura_Maths::Max( m_xBodyState.m_xCentreOfMass.GetY(), m_xBodyState.m_xPreviousCentreOfMass.GetY() ) + fWorldSpaceExtentY;
																										
		m_xBroadphaseAABB.MinZ = Asura_Maths::Min( m_xBodyState.m_xCentreOfMass.GetZ(), m_xBodyState.m_xPreviousCentreOfMass.GetZ() ) - fWorldSpaceExtentZ;
		m_xBroadphaseAABB.MaxZ = Asura_Maths::Max( m_xBodyState.m_xCentreOfMass.GetZ(), m_xBodyState.m_xPreviousCentreOfMass.GetZ() ) + fWorldSpaceExtentZ;
	}
}

void Asura_Physics_Ragdoll_CollisionObject::ZeroMomentum( void )
{
	m_xBodyState.m_xLinearVelocity.Zero();
	m_xBodyState.m_xAngularVelocity.Zero();
}

void Asura_Physics_Ragdoll_CollisionObject::CalculateWorldInvInertiaTensor( void )
{
	const Asura_AVector3& xInvInertiaTensorDiagonal = m_pxDesc->GetInvInertiaTensor();

	Asura_AMatrix3x3 xM( xInvInertiaTensorDiagonal.GetX(), 0.0f, 0.0f,
						 0.0f, xInvInertiaTensorDiagonal.GetY(), 0.0f,
						 0.0f, 0.0f, xInvInertiaTensorDiagonal.GetZ() );

	Asura_AMatrix3x3 xTranspose = GetOrientation();
	xTranspose.Transpose();

	m_xBodyState.m_xWorldInvInertiaTensor = (xTranspose*xM)*GetOrientation();
}

Asura_AVector3 Asura_Physics_Ragdoll_CollisionObject::GetSupportPoint( const Asura_AVector3& xAxis ) const 
{
	Asura_AVector3 xExtents = m_pxDesc->GetCollisionExtents();
	
	switch ( m_pxDesc->GetCollisionProxyType() )
	{
		case Asura_Physics_Ragdoll_Desc_CollisionObject::COLLISION_SPHERE:
		{
			Asura_AVector3 xPoint = GetPosition() - xAxis * xExtents.GetX();
			return xPoint;
		}
		default:
		case Asura_Physics_Ragdoll_Desc_CollisionObject::COLLISION_CAPSULE:
		{
			Asura_AVector3 xCapsuleAxis = GetOrientation().GetColumn2();
			float fRadius = xExtents.GetX();
			float fHalfHeight = xExtents.GetZ();
			float fDot = xCapsuleAxis * xAxis;

			fHalfHeight = Asura_Maths::Max( fHalfHeight, fRadius );

			float fLength = (fDot < 0.0f) ? (fRadius-fHalfHeight) : (fHalfHeight-fRadius);

			Asura_AVector3 xOffset = xCapsuleAxis * fLength;

			Asura_AVector3 xPoint = GetPosition() - (xAxis * fRadius + xOffset);

			return xPoint;
		}
	}
}

void Asura_Physics_Ragdoll_CollisionObject::FindEnvironmentContacts( Asura_Physics_Ragdoll* pxParentRagdoll )
{
	m_xContactClusterList.Reset();
	m_xEnvironmentCache.Update( m_xBroadphaseAABB, 0.1f );

	if ( ! m_xEnvironmentCache.GetFaceList() ) return;

	{
		Asura_Physics_MPR_Body xBody1( this );

		#ifdef ASURA_USE_COLLISIONFX
		const bool bDoCollisionFX = pxParentRagdoll->GetReportEnvironmentCollisionFX();
		#endif

		for ( Asura_Physics_EnvironmentCache_Face* pxFace = m_xEnvironmentCache.GetFaceList(); pxFace; pxFace = pxFace->m_pxNext )
		{
			Asura_Physics_Tri xTri( pxFace->m_uFaceID);
			Asura_Physics_MPR_Body xBody2( &xTri );

			Asura_AVector3 xNormal;
			float fSeparation;

			if ( Asura_Physics_CCD::MPR_Test( xBody1, xBody2, xNormal, fSeparation ) )
			{
				Asura_AVector3 axContactPoints[10];
				u_int uNumberOfContactPoints;

				uNumberOfContactPoints = Asura_Physics_CCD::FindContactPoints_PrimitiveVsTri( axContactPoints, &xBody1, xTri, xNormal );

				if (!uNumberOfContactPoints) continue;

				// We always want to push objects out of polygons via the normal direction,
				// even if the quickest route out is the other way.
				Asura_AVector3 xN;
				xTri.GetUnnormalisedNormal( xN );
				
				if (xNormal * xN < 0.0f)
				{
					xNormal = -xNormal;
				}

				float fPenetration = -fSeparation;
				for ( u_int u=0; u<uNumberOfContactPoints; ++u )
				{
					m_xContactClusterList.AddContactPosition( GetPosition(), axContactPoints[u], xNormal, fPenetration );
				}

				#ifdef ASURA_USE_COLLISIONFX
				if (bDoCollisionFX)
				{
					pxParentRagdoll->RegisterCFX( GetLinearVelocity(), axContactPoints[0], xNormal, pxFace->m_uFaceID );
				}
				#endif
			}
		}
	}
}

void Asura_Physics_Ragdoll_CollisionObject::FindObjectContacts( Asura_Physics_Ragdoll* pxParentRagdoll )
{
	// Make a broadphase query to find any static objects
	// TODO - just reuse the same query for the whole ragdoll?
	Asura_Physics_Broadphase_Query xQuery;
	xQuery.MakeQuery( m_xBroadphaseAABB, true, false, false, false );

	const Asura_Physics_Broadphase_Entry* pxEntry = xQuery.GetFirstEntry();

	if (!pxEntry) return;

	Asura_Physics_MPR_Body xBody1( this );

	while ( pxEntry )
	{
		// Get the object for this entry, and move on to the next entry
		// (this means we can use "continue" to skip to the next one safely).
		Asura_Physics_RigidBodyInstance* pxObject = pxEntry->GetInstance();
		pxEntry = pxEntry->GetNextEntry();

		// Polygonal object - we'll need to go through each poly
		if ( pxObject->GetCollisionProxy()==Asura_Physics_RigidBodyInstance::COLLISION_PROXY_POLYGONAL )
		{
			const Asura_Physics_RigidBodyDefinition* pxDefn = pxObject->GetDefinition();
			if ( !pxDefn ) continue;
		
			const Asura_CollisionMesh* pxMesh = pxDefn->GetCollisionMesh();
			if ( !pxMesh ) continue;
			
			const Asura_AMatrix3x3 xOri = pxObject->GetOrientation();
			const Asura_AVector3 xPos = pxObject->GetRenderingPosition();

			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( xOri, xPos );
				Asura_Physics_MPR_Body xBody2( &xTri );

				Asura_AVector3 xNormal;
				float fSeparation;

				if ( Asura_Physics_CCD::MPR_Test( xBody1, xBody2, xNormal, fSeparation ) )
				{
					Asura_AVector3 axContactPoint = xBody1.GetSupportPoint(xNormal);
					
					// We always want to push objects out of polygons via the normal direction,
					// even if the quickest route out is the other way.
					Asura_AVector3 xN;
					xTri.GetUnnormalisedNormal( xN );
					
					if (xNormal * xN < 0.0f)
					{
						xNormal = -xNormal;
					}

					float fPenetration = -fSeparation;
					m_xContactClusterList.AddContactPosition( GetPosition(), axContactPoint, xNormal, fPenetration );
				}
			}

		}
		else // non-polygonal object, which is a lot easier
		{
			Asura_Physics_MPR_Body xBody2( pxObject );

			Asura_AVector3 xNormal;
			float fSeparation;

			if ( Asura_Physics_CCD::MPR_Test( xBody1, xBody2, xNormal, fSeparation ) )
			{
				Asura_AVector3 axContactPoint = xBody1.GetSupportPoint(xNormal);
				float fPenetration = -fSeparation;

				m_xContactClusterList.AddContactPosition( GetPosition(), axContactPoint, xNormal, fPenetration );
			}
		}

	}
}

#endif // #ifndef ASURA_PHYSICS_DISABLE_RAGDOLL
