
///////////////////////////////////////////////////////////////////////////////
//
// File   : Asura_Animation_IK_Chain.cpp
// Author : Will Plano
// Date   : 08/08/2007
//
// Copyright (C) 2007 Rebellion, All Rights Reserved.
//
///////////////////////////////////////////////////////////////////////////////

#include "Core/Asura.h" 

#ifdef ASURA_USE_ANIMATION_IK_SOLVER

#include "Core/Asura_LOS.h"
#include "Core/Asura_Maths.h"

#include "Animation/IK/Asura_Animation_IK_Chain.h"

#include "Animation/Asura_Animation.h"
#include "Animation/IK/Asura_Animation_IK_Link.h"
#include "Animation/IK/Asura_Animation_IK_Solver.h"
#include "Animation/Asura_Hierarchy_Tween.h"
#include "Debugging/Asura_DebugLineRender.h"
#include "Interface/Asura_CommandConsole.h"
#include "Physics/Asura_Physics.h"
#include "Physics/Asura_Physics_Environment.h"

DVSVAR bool  g_bRenderDebugInfo = false;
DVSVAR u_int g_uNumItr = 5;
DVSVAR float g_fDefaultMoveLimit = 0.5f;
DVSVAR float g_fDefaultPosSmoothingSpeed = 4.0f;					// end point can move (relative to animation) up to 2m per second
DVSVAR float g_fDefaultOriSmoothingSpeed = Asura_Maths::Deg180;	// end point can move (relative to animation) up to 180 degrees per second

#if defined(ASURA_SUPPORT_COMMAND_CONSOLE) && defined(ASURA_DEBUG_VARIABLE_SYSTEM)
void Asura_Animation_IK_Chain::AddConsoleVars()
{
#if !defined(ASURA_NO_DEBUG_OUTPUT)
	Asura_CommandConsole::AddVar( L"IK.RenderChainDebugInfo", &g_bRenderDebugInfo, Asura_CommandConsole::REMOVE_AT_SHUTDOWN );
#endif // ASURA_NO_DEBUG_OUTPUT
	Asura_CommandConsole::AddVar( L"IK.NumItr", &g_uNumItr, 0, 100, Asura_CommandConsole::REMOVE_AT_SHUTDOWN );
	Asura_CommandConsole::AddVar( L"IK.DefaultMoveLimit", &g_fDefaultMoveLimit, 0.0f, FLT_MAX, Asura_CommandConsole::REMOVE_AT_SHUTDOWN );
	Asura_CommandConsole::AddVar( L"IK.DefaultPosSmoothingSpeed", &g_fDefaultPosSmoothingSpeed, 0.0f, FLT_MAX, Asura_CommandConsole::REMOVE_AT_SHUTDOWN );
	Asura_CommandConsole::AddVar( L"IK.DefaultAngleSmoothingSpeed", &g_fDefaultOriSmoothingSpeed, 0.0f, FLT_MAX, Asura_CommandConsole::REMOVE_AT_SHUTDOWN );
}
#endif

//////////////////////////////////////////////////////////////////////////
// class Asura_Animation_IK_Chain
//////////////////////////////////////////////////////////////////////////

Asura_Animation_IK_Chain::Asura_Animation_IK_Chain( const u_int uNumBones )
: m_xElbowHintDir( Asura_Maths::ZeroAVector3 )
, m_xTargetPosition( Asura_Animation_IK_Link::s_xNullVector )
, m_xAnimPosition( Asura_Maths::ZeroAVector3 )
, m_xTargetOrientationDelta( Asura_Maths::IdentityQuat )
, m_xChainOri( Asura_Maths::IdentityQuat )
, m_xAnimOrientation( Asura_Maths::IdentityQuat )
, m_xAnimInvOrientation( Asura_Maths::IdentityQuat )
, m_uFrameStamp( 0 )
, m_uLerpFrameStamp( 0 )
, m_uNumLinks( 0 )
, m_uIKChainFlags( 0 )
, m_fLength( 0.0f )
, m_fLerp( 1.0f )
, m_fTargetLerp( 1.0f )
, m_fDistanceFade( 1.0f )
, m_fDistanceFadeStart( ASURA_IK_FADE_NEAR_RANGE )
, m_fDistanceFadeEnd( ASURA_IK_FADE_FAR_RANGE )
, m_fGroundOffset( 0.0f )
, m_fExtendLimit( 0.05f )
, m_fMoveLimit( g_fDefaultMoveLimit )
, m_fPosSmoothingSpeed( g_fDefaultPosSmoothingSpeed )
, m_fOriSmoothingSpeed( g_fDefaultOriSmoothingSpeed )
, m_fBoneCollisionRadius( 0.05f )
, m_pxFirstLink( NULL )
, m_pxLastLink( NULL )
, m_pxNextChain( NULL )
, m_pxFirstChildChain( NULL )
{
	m_xEnvironmentCache.SetFlagsOfPolysToIgnore( AsuraPolygonFlag_IgnoreForIK );
	m_xEnvironmentCache.SetFacesOnly( true );

	if ( uNumBones > 0 )
	{
		Asura_Animation_IK_Link* pxLink = m_pxFirstLink = m_pxLastLink = new Asura_Animation_IK_Link;
		++m_uNumLinks;
		for ( u_int u = 1; u != uNumBones; ++u )
		{
			m_pxLastLink = new Asura_Animation_IK_Link;
			pxLink->SetNextLink( m_pxLastLink );
			m_pxLastLink->SetPrevLink( pxLink );
			pxLink = m_pxLastLink;
			++m_uNumLinks;
		}
	}
}

Asura_Animation_IK_Chain::Asura_Animation_IK_Chain( const int* piBoneIDs, const int* piTwistBoneIDs )
: m_xElbowHintDir( Asura_Maths::ZeroAVector3 )
, m_xTargetPosition( Asura_Animation_IK_Link::s_xNullVector )
, m_xAnimPosition( Asura_Maths::ZeroAVector3 )
, m_xTargetOrientationDelta( Asura_Maths::IdentityQuat )
, m_xChainOri( Asura_Maths::IdentityQuat )
, m_xAnimOrientation( Asura_Maths::IdentityQuat )
, m_xAnimInvOrientation( Asura_Maths::IdentityQuat )
, m_uFrameStamp( 0 )
, m_uLerpFrameStamp( 0 )
, m_uNumLinks( 0 )
, m_uIKChainFlags( 0 )
, m_fLerp( 1.0f )
, m_fTargetLerp( 1.0f )
, m_fGroundOffset( 0.0f )
, m_fExtendLimit( 0.05f )
, m_fMoveLimit( g_fDefaultMoveLimit )
, m_fPosSmoothingSpeed( g_fDefaultPosSmoothingSpeed )
, m_fOriSmoothingSpeed( g_fDefaultOriSmoothingSpeed )
, m_fBoneCollisionRadius( 0.05f )
, m_pxFirstLink( NULL )
, m_pxLastLink( NULL )
, m_pxNextChain( NULL )
, m_pxFirstChildChain( NULL )
{
	m_xEnvironmentCache.SetFlagsOfPolysToIgnore( AsuraPolygonFlag_IgnoreForIK );
	m_xEnvironmentCache.SetFacesOnly( true );

	SetBonesArrays( piBoneIDs, piTwistBoneIDs );
}

Asura_Animation_IK_Chain::~Asura_Animation_IK_Chain()
{
	Shutdown();
}


void Asura_Animation_IK_Chain::Shutdown()
{
	m_xElbowHintDir = Asura_Maths::ZeroAVector3;
	m_xTargetPosition = Asura_Animation_IK_Link::s_xNullVector;
	m_xAnimPosition = Asura_Maths::ZeroAVector3;
	m_xTargetOrientationDelta = Asura_Maths::IdentityQuat;
	m_xChainOri = Asura_Maths::IdentityQuat;
	m_xAnimOrientation = Asura_Maths::IdentityQuat;
	m_xAnimInvOrientation = Asura_Maths::IdentityQuat;
	m_uFrameStamp = 0;
	m_uLerpFrameStamp = 0;
	m_uNumLinks = 0;
	m_uIKChainFlags = 0;
	m_fLerp = 1.0f;
	m_fTargetLerp = 1.0f;
	m_fGroundOffset = 0.0f;
	m_fExtendLimit = 0.05f;
	m_fMoveLimit = g_fDefaultMoveLimit;
	m_fPosSmoothingSpeed = g_fDefaultPosSmoothingSpeed;
	m_fOriSmoothingSpeed = g_fDefaultOriSmoothingSpeed;
	m_fBoneCollisionRadius = 0.05f;
	
	m_xEnvironmentCache.Reset();
	m_xEnvironmentCache.SetFlagsOfPolysToIgnore( AsuraPolygonFlag_IgnoreForIK );
	m_xEnvironmentCache.SetFacesOnly( true );

	Asura_Animation_IK_Link* pxLink = m_pxFirstLink;
	while ( pxLink )
	{
		Asura_Animation_IK_Link* pxNextLink = pxLink->GetNextLink();
		delete pxLink;
		pxLink = pxNextLink;
	}
	m_uNumLinks = 0;
	m_pxFirstLink = NULL;
	m_pxLastLink = NULL;

	Asura_Animation_IK_Chain* pxChild = m_pxFirstChildChain;
	while ( pxChild )
	{
		Asura_Animation_IK_Chain* pxNext = pxChild->m_pxNextChain;
		pxChild->Shutdown();
		pxChild = pxNext;
	}
	m_pxFirstChildChain = NULL;

	m_pxNextChain = NULL;
}

void Asura_Animation_IK_Chain::SetBonesArrays( const int* piBonesArray, const int* piTwistBonesArray /* = NULL */ )
{
	if ( m_uNumLinks == 0 )
	{
		Asura_Assert( piBonesArray && *piBonesArray != -1, "No array of bone IDs specified, not filling chain." );
		if ( piBonesArray && *piBonesArray != -1 )
		{
			const int* piItr = piBonesArray;
			const int* piItrTwist = piTwistBonesArray;
			Asura_Animation_IK_Link* pxLink = m_pxFirstLink = m_pxLastLink = new Asura_Animation_IK_Link( *piItr, piTwistBonesArray ? *piItrTwist : -1 );
			++m_uNumLinks;
			++piItr;
			++piItrTwist;
			while ( *piItr != -1 )
			{
				m_pxLastLink = new Asura_Animation_IK_Link( *piItr, piTwistBonesArray ? *piItrTwist : -1 );
				pxLink->SetNextLink( m_pxLastLink );
				m_pxLastLink->SetPrevLink( pxLink );
				pxLink = m_pxLastLink;
				++m_uNumLinks;
				++piItr;
				++piItrTwist;
			}
		}
	}
	else
	{
		const int* piItr = piBonesArray;
		const int* piItrTwist = piTwistBonesArray;
		Asura_Animation_IK_Link* pxItrLink = m_pxFirstLink;
		while ( *piItr != -1 && pxItrLink )
		{
			pxItrLink->SetBoneID( *piItr );
			if ( piTwistBonesArray )
			{
				pxItrLink->SetTwistBoneID( *piItrTwist );
			}
			++piItr;
			++piItrTwist;
			pxItrLink = pxItrLink->GetNextLink();
		}
		Asura_Assert( !pxItrLink, "Not enough data in array to fill chain." );
		Asura_Assert( *piItr == -1, "Not enough space in chain for data in array." );
	}
}

bool Asura_Animation_IK_Chain::GetBoneDataFromAnim( const Asura_Animation& xAnim )
{
	if ( !xAnim.GetTweenStore()->IsPositionDataValid() )
	{
		Disable( true );
		return false;
	}

	Enable();

	m_xAnimPosition = xAnim.GetPosition();
	m_fLength = 0.0f;
	Asura_AVector3 xLastPos = Asura_Maths::ZeroAVector3;

	m_xAnimInvOrientation = m_xAnimOrientation = xAnim.GetOrientQuat();
	m_xAnimInvOrientation.Invert();

	for ( Asura_Animation_IK_Link* pxLink = m_pxFirstLink; pxLink; pxLink = pxLink->GetNextLink() )
	{
		Asura_Vector_3	xTempPosition;
		Asura_Quat		xQuat;

		xAnim.GetTweenStore()->GetBoneData( pxLink->GetBoneID() ).GetOrientation( xQuat );
		xAnim.GetTweenStore()->GetBoneData( pxLink->GetBoneID() ).GetPosition( xTempPosition );

		Asura_AVector3 xPos( xTempPosition );
		Asura_Assert( xPos.MagnitudeSquared() > 0.0001f, "Link too short for IK" );

		if ( !xAnim.GetTweenStore()->IsPositionDataInWorldspace() )
		{
			if ( pxLink->GetPrevLink() && pxLink->GetPrevLink()->GetBoneID() == xAnim.GetSkin()->ParentBone[ pxLink->GetBoneID() ] )
			{
				xPos = pxLink->GetPrevLink()->GetAnimQuat().RotateVector( xPos);
				xPos += pxLink->GetPrevLink()->GetAnimPosition();
				xQuat = pxLink->GetPrevLink()->GetAnimQuat() * xQuat;
			}
			else
			{
				Asura_Matrix_3x3 xOri;
				xAnim.GetBoneLocation( pxLink->GetBoneID(), &xTempPosition, &xOri, true );
				xPos = Asura_AVector3( xTempPosition );
				xQuat.FromMatrix( xOri );
			}
		}

#ifndef ASURA_NO_DEBUG_OUTPUT
			if ( g_bRenderDebugInfo )
			{
				Asura_DebugLineRender::AddLine( xPos - Asura_AVector3( 0.0f, 0.0f, 0.03f ), xPos + Asura_AVector3( 0.0f, 0.0f, 0.03f ), 0xFF808080 );
				Asura_DebugLineRender::AddLine( xPos - Asura_AVector3( 0.0f, 0.03f, 0.0f ), xPos + Asura_AVector3( 0.0f, 0.03f, 0.0f ), 0xFF808080 );
				Asura_DebugLineRender::AddLine( xPos - Asura_AVector3( 0.03f, 0.0f, 0.0f ), xPos + Asura_AVector3( 0.03f, 0.0f, 0.0f ), 0xFF808080 );
				if ( pxLink->GetPrevLink() )
				{
					Asura_DebugLineRender::AddLine( xPos, pxLink->GetPrevLink()->GetAnimPosition(), 0xFF808080 );
				}
			}
#endif

		pxLink->SetAnimPosition( xPos );
		pxLink->SetAnimQuat( xQuat );
		pxLink->SetPosition( xPos, false );
		pxLink->SetQuat( xQuat, false );

		if ( pxLink == m_pxFirstLink )
		{
			Asura_Matrix_3x3 xOri;
			xAnim.GetBoneLocation( xAnim.GetSkin()->ParentBone[ pxLink->GetBoneID() ], NULL, &xOri, true );
			m_xChainOri.FromMatrix( xOri );
		}
		else
		{
			float fLength = ( xPos - xLastPos ).Magnitude();
			Asura_Assert( fLength > 0.0001f, "Link too short for IK" );
			pxLink->GetPrevLink()->SetLength( fLength );
			m_fLength += fLength;

			if ( pxLink == m_pxLastLink )
			{
				Asura_AVector3 xEndPosOS = pxLink->GetPosition() - m_xAnimPosition;
				xEndPosOS = m_xAnimInvOrientation.RotateVector( xEndPosOS);
				m_fGroundOffset = -xEndPosOS.GetY();
			}
		}

		xLastPos = xPos;
	}

	bool bOK = true;
	for ( Asura_Animation_IK_Chain* pxChain = m_pxFirstChildChain; pxChain; pxChain = pxChain->m_pxNextChain )
	{
		if ( !pxChain->GetBoneDataFromAnim( xAnim ) )
		{
			bOK = false;
		}
	}
	return bOK;
}

bool Asura_Animation_IK_Chain::PushBoneDataToAnim( Asura_Animation& xAnim, const float fSolverLerp /* = 1.0f */ )
{
	if ( IsActive() )
	{
		for ( Asura_Animation_IK_Link* pxLink = m_pxFirstLink; pxLink; pxLink = pxLink->GetNextLink() )
		{
#ifndef ASURA_NO_DEBUG_OUTPUT
            {
			    Asura_AVector3 xPos = pxLink->GetPosition();
			    if ( g_bRenderDebugInfo )
			    {
				    Asura_DebugLineRender::AddLine( xPos - Asura_AVector3( 0.0f, 0.0f, 0.03f ), xPos + Asura_AVector3( 0.0f, 0.0f, 0.03f ), 0xFF800080 );
				    Asura_DebugLineRender::AddLine( xPos - Asura_AVector3( 0.0f, 0.03f, 0.0f ), xPos + Asura_AVector3( 0.0f, 0.03f, 0.0f ), 0xFF800080 );
				    Asura_DebugLineRender::AddLine( xPos - Asura_AVector3( 0.03f, 0.0f, 0.0f ), xPos + Asura_AVector3( 0.03f, 0.0f, 0.0f ), 0xFF800080 );

				    if ( pxLink->GetPrevLink() )
				    {
					    Asura_DebugLineRender::AddLine( xPos, pxLink->GetPrevLink()->GetPosition(), 0xFF800080 );
				    }
			    }
            }
#endif

			Asura_Quat xBoneQuat;
			xAnim.GetTweenStore()->GetBoneData( pxLink->GetBoneID() ).GetOrientation( xBoneQuat );
			Asura_Quat xInvOldQuat = xBoneQuat;
			xInvOldQuat.Invert();
			Asura_Quat xNewQuat;
			if ( !xAnim.GetTweenStore()->IsPositionDataInWorldspace() )
			{
				if ( pxLink->GetPrevLink() )
				{
					xNewQuat = pxLink->GetPrevLink()->GetQuat();
				}
				else
				{
					xNewQuat = m_xChainOri;
				}
				xNewQuat.Invert();
				xNewQuat = xNewQuat * pxLink->GetQuat();
			}
			else
			{
				xNewQuat = pxLink->GetQuat();
			}
			xBoneQuat.Slerp( xNewQuat, GetCurrentLerp() * fSolverLerp );
			xAnim.GetTweenStore()->GetBoneData( pxLink->GetBoneID() ).SetOrientation( xBoneQuat );
			int iTwistBoneID = pxLink->GetTwistBoneID();
			if ( iTwistBoneID != -1 )
			{
				Asura_Quat xTwistBoneQuat;
				xAnim.GetTweenStore()->GetBoneData( iTwistBoneID ).GetOrientation( xTwistBoneQuat );
				xTwistBoneQuat = xBoneQuat * xInvOldQuat * xTwistBoneQuat;
				xAnim.GetTweenStore()->GetBoneData( iTwistBoneID ).SetOrientation( xTwistBoneQuat );
			}

#ifndef ASURA_NO_DEBUG_OUTPUT
			if ( g_bRenderDebugInfo )
			{
				Asura_Vector_3 xPos;
				Asura_Vector_3 xTwistPos;
				Asura_Matrix_3x3 xOri;
				Asura_Matrix_3x3 xTwistOri;
				if ( xAnim.GetBoneLocation( pxLink->GetBoneID(), &xPos, &xOri, true ) )
				{
						//Asura_DebugLineRender::AddAxis( xPos, xOri, 0.4f );
				}
				if ( pxLink->GetTwistBoneID() != -1 && xAnim.GetBoneLocation( pxLink->GetTwistBoneID(), &xTwistPos, &xTwistOri, true ) )
				{
					//Asura_DebugLineRender::AddAxis( xTwistPos, xTwistOri, 0.3f );
				}
			}
#endif
		}

		bool bOK = true;
		for ( Asura_Animation_IK_Chain* pxChain = m_pxFirstChildChain; pxChain; pxChain = pxChain->m_pxNextChain )
		{
			if ( !pxChain->PushBoneDataToAnim( xAnim, fSolverLerp ) )
			{
				bOK = false;
			}
		}
		return bOK;
	}
	else
	{
		return false;
	}
}

const Asura_AVector3& Asura_Animation_IK_Chain::GetBasePosition() const
{
	return m_pxFirstLink ? m_pxFirstLink->GetPosition() : Asura_Maths::ZeroAVector3;
}

const Asura_AVector3& Asura_Animation_IK_Chain::GetEndPosition() const
{
	return m_pxLastLink->GetPosition();
}

void Asura_Animation_IK_Chain::SetTargetLerp( float fLerp, bool bNow /* = false */ )
{
	Asura_Assert ( fLerp >= 0.0f && fLerp <= 1.0f, "Invalid weight for lerp" );
	UpdateLerpTimer();
	m_fTargetLerp = fLerp;
	if ( bNow )
	{
		m_fLerp = fLerp;
	}
}

void Asura_Animation_IK_Chain::MoveEverything( const Asura_AVector3& xPosDelta, const Asura_Quat& xRotQuat /* = Asura_Maths::IdentityQuat */, const Asura_AVector3& xRotCentre /* = Asura_Maths::ZeroAVector3 */ )
{
	if ( xRotQuat.ApproximatelyEquals( Asura_Maths::IdentityQuat, 0.01f ) )
	{
		m_xAnimPosition += xPosDelta;
		for ( Asura_Animation_IK_Link* pxLink = m_pxFirstLink; pxLink; pxLink = pxLink->GetNextLink() )
		{
			pxLink->SetPosition( pxLink->GetPosition() + xPosDelta, false );
			pxLink->SetAnimPosition( pxLink->GetAnimPosition() + xPosDelta );
		}
	}
	else
	{
		m_xChainOri = xRotQuat * m_xChainOri;
		m_xAnimPosition += xPosDelta - xRotCentre;
		m_xAnimPosition = xRotQuat.RotateVector( m_xAnimPosition);
		m_xAnimPosition += xRotCentre;
		for ( Asura_Animation_IK_Link* pxLink = m_pxFirstLink; pxLink; pxLink = pxLink->GetNextLink() )
		{
			Asura_AVector3 xNewPos( pxLink->GetPosition() + xPosDelta - xRotCentre );
			xNewPos = xRotQuat.RotateVector( xNewPos);
			xNewPos += xRotCentre;
			pxLink->SetPosition( xNewPos, false );
			pxLink->SetQuat( xRotQuat * pxLink->GetQuat(), false );

			xNewPos = pxLink->GetAnimPosition() + xPosDelta - xRotCentre;
			xNewPos = xRotQuat.RotateVector( xNewPos);
			xNewPos += xRotCentre;
			pxLink->SetAnimPosition( xNewPos );
			pxLink->SetAnimQuat( xRotQuat * pxLink->GetAnimQuat() );
		}
	}
	SetNeedsTargetUpdate(); // Make sure the target gets recalculated if we move the chain.

	for ( Asura_Animation_IK_Chain* pxChain = m_pxFirstChildChain; pxChain; pxChain = pxChain->m_pxNextChain )
	{
		pxChain->MoveEverything( xPosDelta, xRotQuat, xRotCentre );
	}
}

void Asura_Animation_IK_Chain::SetAxesFromRestPose( Asura_Hierarchy_Bone_RestFrame* pxRestPose )
{
	Asura_Assert( pxRestPose != NULL, "No rest pose" );
	if ( pxRestPose != NULL )
	{
		Asura_AVector3	xBendAxis;
		Asura_AVector3	xRollAxis;
		Asura_AVector3	xPrevRollAxis;
		Asura_AVector3	xRotRollAxis;
		Asura_Quat		xBendQuat;
		float			fBendAngle;
		for ( Asura_Animation_IK_Link* pxLink = m_pxFirstLink; pxLink; pxLink = pxLink->GetNextLink() )
		{
			if ( pxLink->GetNextLink() )
			{
				xRollAxis = pxRestPose[ pxLink->GetNextLink()->GetBoneID() ].Position;
				xRollAxis.Normalise();
				if ( pxLink->GetPrevLink() )
				{
					xRotRollAxis = pxRestPose[ pxLink->GetBoneID() ].Quat.RotateVector( xRollAxis);
					xBendQuat.RotationFromAToB( pxLink->GetPrevLink()->GetRollAxis(), xRotRollAxis );
					fBendAngle = xBendQuat.ToRotation( xBendAxis );
					xBendAxis.Normalise(); // This doesn't come back normalized enough for my ( or Asura_Quat::FromRotation's ) liking.
					if ( fBendAngle < -0.001f )
					{
						xBendAxis *= -1.0f;
					}
					else if ( fBendAngle < 0.001f )
					{
						Asura_Assertf( false, ( "This bone (ID: %d) is not bent enough relative to the parent bone to calculate the bend axis.", pxLink->GetBoneID() ) );
						xBendAxis = Asura_Vector_3( 0.0f, 1.0f, 0.0f );
					}
				}
				else
				{
					xBendAxis = Asura_Vector_3( 0.0f, 1.0f, 0.0f );
				}
			}
			else
			{
				xRollAxis = Asura_AVector3( 1.0f, 0.0f, 0.0f ); // default to this
				xBendAxis = Asura_Vector_3( 0.0f, 1.0f, 0.0f );
			}
		
			pxLink->SetAxes( xBendAxis, xRollAxis );
		}
	}
}

bool Asura_Animation_IK_Chain::GetTarget( Asura_AVector3* pxPos /* = NULL */, Asura_Quat* pxQuatDelta /* = NULL */, const bool bForceUpdate /* = false */ )
{
	m_fDistanceFade = Asura_Maths::ClampToRange( 1.0f - ( ( GetBasePosition() - Asura_AVector3( *Asura_Camera::GetPosition() ) ).Magnitude() - m_fDistanceFadeStart ) / ( m_fDistanceFadeEnd - m_fDistanceFadeStart ), 0.0f, 1.0f );

	if ( m_fDistanceFade <= 0.0f )
	{
		return false;
	}

	if ( m_uFrameStamp != Asura_Timers::GetFrameNumber() || bForceUpdate )
	{
		m_bTargetOK = CalculateTarget();
		m_uFrameStamp = Asura_Timers::GetFrameNumber();
	}
	if ( pxPos )
	{
		*pxPos = m_xTargetPosition;
	}
	if ( pxQuatDelta )
	{
		*pxQuatDelta = m_xTargetOrientationDelta;
	}
	return m_bTargetOK;
}

bool Asura_Animation_IK_Chain::Solve()
{
	if ( IsActive() )
	{
		if ( m_uNumLinks >= 3 )
		{
			bool bOK = true;
			if ( !SolveInitFunction() )
			{
				bOK = false;
			}

			bool bDoneSomething = true;
			u_int uItr = 0;
			while ( bDoneSomething && uItr < g_uNumItr )
			{
				++uItr;
				bDoneSomething = false;
				if ( !SolveChain() )
				{
					bOK = false;
				}
				if ( SolveLoopFunction() )
				{
					bDoneSomething = true;
				}
				if ( CollideNodes() )
				{
					bDoneSomething = true;
				}
				if ( SatisfyAngularConstraints() )
				{
					bDoneSomething = true;
				}
				if ( SatisfyLinearConstraints() )
				{
					bDoneSomething = true;
				}
			}

			SolveChildChains();
			if ( !SolveEndFunction() )
			{
				bOK = false;
			}
			SatisfyLinearConstraints();

#ifndef ASURA_NO_DEBUG_OUTPUT
			if ( g_bRenderDebugInfo )
			{
				Asura_DebugLineRender::AddSphere( Asura_Vector_3( m_xTargetPosition ), 0.03f, bOK ? 0xFF00FF00 : 0xFFFF0000, 4, 2 );
			}
#endif
			return bOK;
		}
		else
		{
			Asura_Assert( false, "Don't be silly, you can't do IK with less than 3 nodes" );
			return false;
		}
	}
	else
	{
		for ( Asura_Animation_IK_Link* pxLink = m_pxFirstLink; pxLink; pxLink = pxLink->GetNextLink() )
		{
			pxLink->SetPosition( Asura_Animation_IK_Link::s_xNullVector );
			pxLink->SetLastPosition( Asura_Animation_IK_Link::s_xNullVector );
		}
		return false;
	}
}


void Asura_Animation_IK_Chain::AddChildChain( Asura_Animation_IK_Chain& xNewChildChain )
{
	if ( !m_pxFirstChildChain )
	{
		m_pxFirstChildChain = &xNewChildChain;
	}
	else
	{
		Asura_Animation_IK_Chain* pxChain = m_pxFirstChildChain;
		if ( pxChain == &xNewChildChain )
		{
			Asura_Assert( false, "Chain already added" );
			return;
		}
		while ( pxChain->m_pxNextChain )
		{ 
			if ( pxChain->m_pxNextChain == &xNewChildChain )
			{
				Asura_Assert( false, "Chain already added" );
				return;
			}
			pxChain = pxChain->m_pxNextChain;
		}
		pxChain->m_pxNextChain = &xNewChildChain;
	}
}

void Asura_Animation_IK_Chain::RemoveChildChain( Asura_Animation_IK_Chain& xChildChainToRemove )
{
	if ( &xChildChainToRemove == m_pxFirstChildChain )
	{
		m_pxFirstChildChain = m_pxFirstChildChain->m_pxNextChain;
	}
	else
	{
		Asura_Animation_IK_Chain* pxChain = m_pxFirstChildChain;
		while ( pxChain && pxChain->m_pxNextChain != &xChildChainToRemove )
		{ 
			pxChain = pxChain->m_pxNextChain;
		}
		Asura_Assert( pxChain, "Chain not found in list" );
		if ( pxChain )
		{
			pxChain->m_pxNextChain = xChildChainToRemove.m_pxNextChain;
		}
	}
	xChildChainToRemove.SetNextChain( NULL );
}

int Asura_Animation_IK_Chain::GetFirstLinkBoneID() const
{
	return m_pxFirstLink ? m_pxFirstLink->GetBoneID() : -1;
}

bool Asura_Animation_IK_Chain::SolveChildChains()
{
	if ( !m_pxFirstChildChain )
	{
		return true;
	}
	bool bOK = true;
	Asura_AVector3 xPosDelta( m_pxLastLink->GetPosition() - m_pxLastLink->GetAnimPosition() );
	Asura_Quat xOriDelta( m_pxLastLink->GetAnimQuat() );
	xOriDelta.Invert();
	xOriDelta = m_pxLastLink->GetQuat() * xOriDelta;
	for ( Asura_Animation_IK_Chain* pxChain = m_pxFirstChildChain; pxChain; pxChain = pxChain->m_pxNextChain )
	{
		pxChain->MoveEverything( xPosDelta, xOriDelta, m_pxLastLink->GetPosition() );
		if ( !pxChain->Solve() )
		{
			bOK = false;
		}
	}
	return bOK;
}

bool Asura_Animation_IK_Chain::SolveChain()
{
	if ( m_uNumLinks == 3 )
	{
		bool bOK = true;

		Asura_Animation_IK_Link* pxMiddleLink = m_pxFirstLink->GetNextLink();

		Asura_AVector3 xBasePos = m_pxFirstLink->GetPosition();
		Asura_AVector3 xTargetPos;
		Asura_Quat xTargetOriDelta;
		if ( !GetTarget( &xTargetPos, &xTargetOriDelta ) )
		{
			bOK = false;
			Disable();
		}

		if ( !xTargetPos.ApproximatelyEquals( m_pxLastLink->GetPosition(), 1.0e-10f ) )
		{
			Asura_AVector3 xDir = xTargetPos - m_pxLastLink->GetAnimPosition();
			float fLength = xDir.NormaliseReturningMagnitude();
			fLength = Asura_Maths::ClampToRange( fLength, -m_fMoveLimit, m_fMoveLimit );
			xTargetPos = ( xDir * fLength ) + m_pxLastLink->GetAnimPosition();

			xDir = xTargetPos - xBasePos;

#ifndef ASURA_NO_DEBUG_OUTPUT
			if ( g_bRenderDebugInfo )
			{
				Asura_DebugLineRender::AddLine( xBasePos, xTargetPos, 0xFF0000FF );
			}
#endif

			fLength = xDir.NormaliseReturningMagnitude();

			float fLinkLen1 = pxMiddleLink->GetLength();
			float fLinkLen2 = m_pxFirstLink->GetLength();

			if ( fLength == 0 )
			{
				return false;
			}

			float fMaxLengthLimit = fLinkLen1 + fLinkLen2 - m_fExtendLimit;
			float fMaxLengthSq = Asura_Maths::Max( ( m_pxLastLink->GetAnimPosition() - xBasePos ).MagnitudeSquared(), fMaxLengthLimit * fMaxLengthLimit );

			if ( (fLength * fLength) > fMaxLengthSq )
			{
				bOK = false;
				fLength = Asura_Maths::Sqrt( fMaxLengthSq );
#ifndef ASURA_NO_DEBUG_OUTPUT
				if ( g_bRenderDebugInfo )
				{
					Asura_DebugLineRender::AddSphere( Asura_Vector_3( xBasePos + ( xDir * fLength ) ), 0.03f, 0xFF800000, 4, 2 );
				}
#endif
			}

			Asura_AVector3 xElbowAnimVec = pxMiddleLink->GetPosition() - m_pxFirstLink->GetPosition();
			xElbowAnimVec.Normalise();

			Asura_AVector3 xElbowHintDir;
			if ( m_xElbowHintDir != Asura_Maths::ZeroAVector3 )
			{
				// if a hint direction has been given for the elbow, use it...
				xElbowHintDir = m_xAnimOrientation.RotateVector( m_xElbowHintDir);
			}
			else
			{
				// ...otherwise try to work out where best to put the elbow.
				Asura_AVector3 xEndAnimVec = m_pxLastLink->GetPosition() - m_pxFirstLink->GetPosition();
				xEndAnimVec.Normalise();
				Asura_AVector3 xElbowPllelVec = xEndAnimVec * ( xEndAnimVec * xElbowAnimVec );
				xElbowHintDir = xElbowAnimVec - xElbowPllelVec;
				xElbowHintDir.Normalise(); // So this should be a unit vector in the direction of the elbow perpendicular to the vector from the base to the end...
#ifndef ASURA_NO_DEBUG_OUTPUT
				if ( g_bRenderDebugInfo )
				{
					// ...it looks like it is :)
					Asura_DebugLineRender::AddLine( pxMiddleLink->GetPosition(), pxMiddleLink->GetPosition() + ( xElbowHintDir * 0.05f ), 0xFFFF00FF );
				}
#endif
				Asura_AVector3 xAxis = xEndAnimVec ^ xDir;
				if ( xAxis.NormaliseReturningMagnitude() == 0.0f )
				{
					float fCosAng = Asura_Maths::ClampToRange( xDir * xEndAnimVec, -1.0f, 1.0f );
					if ( fCosAng < 0.0f )
					{
						xElbowHintDir *= -1.0f;
					}
				}
				else
				{
					float fCosAng = Asura_Maths::ClampToRange( xDir * xEndAnimVec, -1.0f, 1.0f );
					float fRot = Asura_Maths::ArcCosine( fCosAng );
					Asura_Quat xRot;
					xRot.FromRotation( fRot, xAxis );
					// Now rotate the vector by the change in the chain orientation.
					xElbowHintDir = xRot.RotateVector( xElbowHintDir);
				}
			}
#ifndef ASURA_NO_DEBUG_OUTPUT
			if ( g_bRenderDebugInfo )
			{
				Asura_DebugLineRender::AddLine( pxMiddleLink->GetPosition(), pxMiddleLink->GetPosition() + ( xElbowHintDir * 0.05f ), 0xFFFFFF00 );
			}
#endif

#if 0
			// kink
			Asura_AVector3 xKinkAxis = xDir ^ xElbowHintDir;

			float fCosKinkAngle = ( fLinkLen2 * fLinkLen2 + fLength * fLength - fLinkLen1 * fLinkLen1 ) / ( 2.0f * fLinkLen2 * fLength );
			if ( fCosKinkAngle > 1.0f )
			{
				fCosKinkAngle = 1.0f;
			}
			if ( fCosKinkAngle < -1.0f )
			{
				fCosKinkAngle = -1.0f;
			}
			float fKinkAngle = Asura_Maths::ArcCosine( fCosKinkAngle );
			Asura_Quat xKinkQuat;
			xKinkQuat.FromRotation( fKinkAngle, xKinkAxis );
			Asura_Quat xBaseQuat = xKinkQuat;

			// yaw
			float fYawAngle = Asura_Maths::ArcTangent( xDir.GetX(), xDir.GetZ() );
			float fSinHalfYawAngle;
			float fCosHalfYawAngle;
			Asura_Maths::SinCos( 0.5f * ( fYawAngle ), fSinHalfYawAngle, fCosHalfYawAngle );
			Asura_Quat xYawQuat( 0.0f, -fSinHalfYawAngle, 0.0f, fCosHalfYawAngle );
			xBaseQuat = xBaseQuat * xYawQuat;
			//Asura_Quat xBaseQuat = xYawQuat;
			
			// pitch 
			float fPitchAngle = Asura_Maths::ArcSine( xDir.GetY() );
			float fSinHalfPitchAngle;
			float fCosHalfPitchAngle;
			Asura_Maths::SinCos( 0.5f * ( fPitchAngle ), fSinHalfPitchAngle, fCosHalfPitchAngle );
			Asura_Quat xPitchQuat( 0.0f, 0.0f, fSinHalfPitchAngle, fCosHalfPitchAngle );
			xBaseQuat = xBaseQuat * xPitchQuat;

// 			Asura_Quat xBaseQuat;
// 			xBaseQuat.RotationFromAToB( m_pxFirstLink->GetRollAxis(), xElbowAnimVec );
// 			Asura_Euler_3 xBaseEuler;
// 			xBaseQuat.ToEuler( xBaseEuler );
// 			xBaseEuler.z = 0.0f;
// 			xBaseQuat.FromEuler( xBaseEuler );

			// Fix stupid rigs
			//xBaseQuat = xBaseQuat * xStupidQuat;

// 			Asura_AVector3 xRollVector;
//	 		xStupidQuat.RotateVector( pxMiddleLink->GetBendAxis(), xRollVector );
			
			// Fix stupid rigs
			Asura_Quat xStupidQuat;
			xStupidQuat.RotationFromAToB( m_pxFirstLink->GetRollAxis(), Asura_AVector3( 1.0f, 0.0f, 0.0f ) );

			Asura_Quat xStupidQuat2;
			Asura_AVector3 xNewBend( pxMiddleLink->GetBendAxis() );
			xNewBend = xStupidQuat.RotateVector( xNewBend);
			xStupidQuat2.RotationFromAToB( xNewBend, Asura_AVector3( 0.0f, 1.0f, 0.0f ) );

			Asura_Quat xStupidQuat3;
			xStupidQuat3.RotationFromAToB( pxMiddleLink->GetRollAxis(), Asura_AVector3( 1.0f, 0.0f, 0.0f ) );

			Asura_Quat xStupidQuat4;
			Asura_AVector3 xNewBend2( pxMiddleLink->GetBendAxis() );
			xNewBend2 = xStupidQuat3.RotateVector( xNewBend2);
			xStupidQuat4.RotationFromAToB( xNewBend2, Asura_AVector3( 0.0f, 1.0f, 0.0f ) );

			// roll
			xYawQuat.Invert();
			xPitchQuat.Invert();
			xElbowHintDir = xYawQuat.RotateVector( xElbowHintDir);
			xElbowHintDir = xPitchQuat.RotateVector( xElbowHintDir);
			//xStupidQuat.RotateVector( xElbowHintDir, xElbowHintDir );
			xElbowHintDir = xStupidQuat4.RotateVector( xElbowHintDir);
			//xStupidQuat.Invert();
			//xStupidQuat.RotateVector( xElbowHintDir, xElbowHintDir );

			const float fRollAngle = Asura_Maths::ArcTangent( -xElbowHintDir.GetY(), xElbowHintDir.GetZ() );
			float fSinHalfRollAngle;
			float fCosHalfRollAngle;
			Asura_Maths::SinCos( 0.5f * ( fRollAngle ), fSinHalfRollAngle, fCosHalfRollAngle );
			Asura_Quat xRollQuat( fSinHalfRollAngle, 0.0f, 0.0f, fCosHalfRollAngle );
			xBaseQuat = xBaseQuat * xRollQuat;

			xBaseQuat = xBaseQuat * xStupidQuat2;
			xBaseQuat = xBaseQuat * xStupidQuat;
#else

			Asura_Quat xBaseQuat;
			xBaseQuat.RotationFromAToB( m_pxFirstLink->GetRollAxis(), xDir );

			float fCosKinkAngle = Asura_Maths::ClampToRange( ( fLinkLen2 * fLinkLen2 + fLength * fLength - fLinkLen1 * fLinkLen1 ) / ( 2.0f * fLinkLen2 * fLength ), -1.0f, 1.0f );
			float fKinkAngle = Asura_Maths::ArcCosine( fCosKinkAngle );
			Asura_Quat xKinkQuat;
			xKinkQuat.FromRotation( -fKinkAngle, pxMiddleLink->GetBendAxis() );


// 			Asura_Matrix_3x3 xRollOri;
// 			Asura_Maths::OrientationFromDirection( xRollOri, Asura_Vector_3( xElbowAnimVec ) );
// 
// 			Asura_AVector3 xBendVec = pxMiddleLink->GetBendAxis();
// 			xBendVec = xBaseQuat.RotateVector( xBendVec);
// #ifndef ASURA_NO_DEBUG_OUTPUT
// 			if ( g_bRenderDebugInfo )
// 			{
// 				Asura_DebugLineRender::AddLine( pxMiddleLink->GetPosition(), pxMiddleLink->GetPosition() + xBendVec * 0.3f, 0xFFFF0000 );
// 			}
// #endif
// 			xBendVec = xRollOri.ApplyTranspose( Asura_Vector_3( xBendVec ) );
// #ifndef ASURA_NO_DEBUG_OUTPUT
// 			if ( g_bRenderDebugInfo )
// 			{
// 				Asura_DebugLineRender::AddLine( pxMiddleLink->GetPosition(), pxMiddleLink->GetPosition() + xBendVec * 0.3f, 0xFF00FF00 );
// 			}
// #endif
// 			xElbowHintDir = xRollOri.ApplyTranspose( Asura_Vector_3( xElbowHintDir ) );
// #ifndef ASURA_NO_DEBUG_OUTPUT
// 			if ( g_bRenderDebugInfo )
// 			{
// 				Asura_DebugLineRender::AddLine( pxMiddleLink->GetPosition(), pxMiddleLink->GetPosition() + xElbowHintDir * 0.3f, 0xFF0000FF );
// 			}
// #endif
// 			const float fRollAngle = Asura_Maths::ArcTangent( xElbowHintDir.GetX(), -xElbowHintDir.GetY() ) - Asura_Maths::ArcTangent( xBendVec.GetX(), -xBendVec.GetY() ) - Asura_Maths::Deg90;

//////////////////////////////////////////////////////////////////////////
// Try 2
//////////////////////////////////////////////////////////////////////////

// 			Asura_Quat xInvQuat( xBaseQuat );
// 			xInvQuat.Invert();
// 			xInvQuat.RotateVector( xElbowHintDir, xElbowHintDir );
// 			
// 			Asura_Matrix_3x3 xRollOri;
// 			Asura_Maths::OrientationFromDirection( xRollOri, Asura_Vector_3( m_pxFirstLink->GetRollAxis() ) );
// 			Asura_AMatrix3x3 xRollOriAlign( xRollOri );
// 			
// 			xElbowHintDir = xRollOriAlign.ApplyTranspose( xElbowHintDir );
// #ifndef ASURA_NO_DEBUG_OUTPUT
// 			if ( g_bRenderDebugInfo )
// 			{
// 				Asura_Vector_3 xPoo( xElbowHintDir );
// 				xBaseQuat.RotateVector( xPoo, xPoo );
// 				Asura_DebugLineRender::AddLine( Asura_Vector_3( pxMiddleLink->GetPosition() ), Asura_Vector_3( pxMiddleLink->GetPosition() ) + xPoo * 0.3f, 0xFFFF0000 );
// 			}
// #endif
// 			Asura_AVector3 xBendVec = m_pxFirstLink->GetRollAxis() ^ pxMiddleLink->GetBendAxis();
// #ifndef ASURA_NO_DEBUG_OUTPUT
// 			if ( g_bRenderDebugInfo )
// 			{
// 				Asura_Vector_3 xPoo( xBendVec );
// 				xBaseQuat.RotateVector( xPoo, xPoo );
// 				Asura_DebugLineRender::AddLine( Asura_Vector_3( pxMiddleLink->GetPosition() ), Asura_Vector_3( pxMiddleLink->GetPosition() ) + xPoo * 0.3f, 0xFF00FF00 );
// 			}
// #endif
// 			xBendVec = xRollOriAlign.ApplyTranspose( xBendVec );
// #ifndef ASURA_NO_DEBUG_OUTPUT
// 			if ( g_bRenderDebugInfo )
// 			{
// 				Asura_Vector_3 xPoo( xBendVec );
// 				xBaseQuat.RotateVector( xPoo, xPoo );
// 				Asura_DebugLineRender::AddLine( Asura_Vector_3( pxMiddleLink->GetPosition() ), Asura_Vector_3( pxMiddleLink->GetPosition() ) + xPoo * 0.3f, 0xFF0000FF );
// 			}
// #endif
// 			const float fRollAngle = Asura_Maths::ArcTangent( xElbowHintDir.GetX(), -xElbowHintDir.GetY() ) - Asura_Maths::ArcTangent( xBendVec.GetX(), -xBendVec.GetY() );
//			Asura_Quat xRollQuat;
// 			xRollQuat.FromRotation( fRollAngle, xElbowAnimVec );

			Asura_AVector3 xElbowDir = m_pxFirstLink->GetRollAxis() ^ pxMiddleLink->GetBendAxis();
			xElbowDir = xBaseQuat.RotateVector( xElbowDir);
#ifndef ASURA_NO_DEBUG_OUTPUT
			if ( g_bRenderDebugInfo )
			{
				Asura_DebugLineRender::AddLine( pxMiddleLink->GetPosition(), pxMiddleLink->GetPosition() + xElbowDir * 0.3f, 0xFFFF0000 );
			}
#endif
			Asura_AVector3 xElbowHintPerpDir = xElbowHintDir - xDir * ( xDir * xElbowHintDir );
			xElbowHintPerpDir.Normalise(); // So this should be a unit vector in the direction of the elbow perpendicular to the vector of the arm...
#ifndef ASURA_NO_DEBUG_OUTPUT
			if ( g_bRenderDebugInfo )
			{
				Asura_DebugLineRender::AddLine( pxMiddleLink->GetPosition(), pxMiddleLink->GetPosition() + xElbowHintPerpDir * 0.3f, 0xFF00FF00 );
			}
#endif
			Asura_Quat xRollQuat;
			xRollQuat.RotationFromAToB( xElbowDir, xElbowHintPerpDir );
			Asura_AVector3 xRollAxis;
			float fRollAngle = xRollQuat.ToRotation( xRollAxis );
			if ( xRollAxis * xDir < 0.0f )
			{
				fRollAngle *= -1.0f;
			}
			xRollQuat.FromRotation( fRollAngle, m_pxFirstLink->GetRollAxis() );

			xBaseQuat = xBaseQuat * xRollQuat;

			xBaseQuat = xBaseQuat * xKinkQuat;

#endif

#ifndef ASURA_NO_DEBUG_OUTPUT
			if ( g_bRenderDebugInfo )
			{
//				Asura_DebugLineRender::AddAxis( Asura_Vector_3( m_pxFirstLink->GetPosition() ), xBaseQuat, 0.05f );
			}
#endif
			m_pxFirstLink->SetQuat( xBaseQuat );

// 			// Orientation of the middle link
// 			// Use the cosine rule to calculate the angle at the middle link.
			float fCosElbowAngle = Asura_Maths::ClampToRange( -( fLinkLen1 * fLinkLen1 + fLinkLen2 * fLinkLen2 - fLength * fLength ) / ( 2.0f * fLinkLen1 * fLinkLen2 ), -1.0f, 1.0f ); // Minus sign because we want the external angle
			float fElbowAngle = Asura_Maths::ArcCosine( fCosElbowAngle );

			Asura_Quat xFixQuat;
			xFixQuat.RotationFromAToB( pxMiddleLink->GetRollAxis(), m_pxFirstLink->GetRollAxis() );

// 			Asura_Quat xElbowQuat;
// 			Asura_AVector3 xElbowBendDir;
// 			xFixQuat.RotateVector( pxMiddleLink->GetBendAxis(), xElbowBendDir );
// 			xElbowQuat.FromRotation( fElbowAngle, xElbowBendDir );

			Asura_Quat xElbowQuat;
			xElbowQuat.FromRotation( fElbowAngle, pxMiddleLink->GetBendAxis() );

// 			Asura_AVector3 xForeArmDir = m_pxLastLink->GetPosition() - pxMiddleLink->GetPosition();
// 			xForeArmDir.Normalise();
//			xElbowQuat.RotationFromAToB( pxMiddleLink->GetRollAxis(), xForeArmDir );
			
			xElbowQuat = xElbowQuat * xFixQuat;
#ifndef ASURA_NO_DEBUG_OUTPUT
			if ( g_bRenderDebugInfo )
			{
				//Asura_DebugLineRender::AddAxis( Asura_Vector_3( pxMiddleLink->GetPosition() ), xBaseQuat * xElbowQuat, 0.05f );
			}
#endif
			pxMiddleLink->SetQuat( xBaseQuat * xElbowQuat );
		}

		// Orientation for the end effector
		Asura_Quat xEndQuat = xTargetOriDelta * m_pxLastLink->GetAnimQuat();

		m_pxLastLink->SetQuat( xEndQuat );

		return bOK;
	}
	else
	{
		Asura_Assert( false, "Default IK implementation of SolveChain only works for 3 bones!" );
		return false;
	}
}

bool Asura_Animation_IK_Chain::SatisfyLinearConstraints( void )
{
	bool bDoneSomething = false;
	if ( m_bDoJointConstraints )
	{
		for ( Asura_Animation_IK_Link* pxLink = m_pxFirstLink; pxLink && pxLink->GetNextLink(); pxLink = pxLink->GetNextLink() )
		{
			Asura_AVector3 xDisp = pxLink->GetNextLink()->GetPosition() - pxLink->GetPosition(); 

			float fMag = xDisp.NormaliseReturningMagnitude();
			Asura_Assert( fMag > 0.0001f, "IK chain has nodes with same position" );
			float fLength = pxLink->GetLength();
			if ( Asura_Maths::Abs( fLength - fMag ) > 0.0001f )
			{
#ifndef ASURA_NO_DEBUG_OUTPUT
				if ( g_bRenderDebugInfo )
				{
					Asura_DebugLineRender::AddLine( pxLink->GetNextLink()->GetPosition(), pxLink->GetPosition() + ( xDisp * fLength ), 0xFFFFFF00 );
				}
#endif
				pxLink->GetNextLink()->SetPosition( pxLink->GetPosition() + ( xDisp * fLength ) );
				bDoneSomething = true;
			}
		}
	}
	return bDoneSomething;
}

bool Asura_Animation_IK_Chain::SatisfyAngularConstraints( void )
{
	bool bDoneSomething = false;
	if ( m_bDoJointConstraints )
	{
		for ( Asura_Animation_IK_Link* pxLink = m_pxFirstLink; pxLink && pxLink->GetNextLink(); pxLink = pxLink->GetNextLink() )
		{
			if ( pxLink->SatisfyAngularConstraints() )
			{
				bDoneSomething = true;
			}
		}
	}
	return bDoneSomething;
}

bool Asura_Animation_IK_Chain::CollideNodes( void )
{
	// nicked this from ragdoll code to make it go quicker

	bool bDoneSomething = false;
	if ( m_bDoCollisionConstraints )
	{
		// ensure the environment cache is up to date...
		Asura_Bounding_Box xChainBB;
		xChainBB.Reset();
		for ( Asura_Animation_IK_Link* pxLink = m_pxFirstLink; pxLink; pxLink = pxLink->GetNextLink() )
		{
			xChainBB.Set( Asura_Vector_3( pxLink->GetPosition() ) );
		}
		xChainBB.Grow( m_fBoneCollisionRadius );
		m_xEnvironmentCache.Update( xChainBB, 0.1f );

		const Asura_Physics_EnvironmentCache_Face* pxFace = m_xEnvironmentCache.GetFaceList();
		while (pxFace)
		{
			float fDistance;
			bool bFacingTowards;

			const Asura_AVector3& xV0 = Asura_Physics_Environment::GetFaceVertex( pxFace->m_uFaceID, 0 );
			const Asura_AVector3& xV1 = Asura_Physics_Environment::GetFaceVertex( pxFace->m_uFaceID, 1 );
			const Asura_AVector3& xV2 = Asura_Physics_Environment::GetFaceVertex( pxFace->m_uFaceID, 2 );

			Asura_AVector3 xNormal = ((xV1 - xV0)^(xV2 - xV0));
			xNormal.Normalise();

			for ( Asura_Animation_IK_Link* pxLink = ( m_pxFirstLink ? m_pxFirstLink->GetNextLink() : NULL ); pxLink; pxLink = pxLink->GetNextLink() )
			{
				const Asura_AVector3 xEnd = pxLink->GetPosition() - xNormal * m_fBoneCollisionRadius;
				const float fPenetration =  (xV0 - xEnd) * xNormal;

				if ( fPenetration > 0.0f )
				{
					bool bIntersect = Asura_Physics_System::RayIntersectsTri( xEnd, xNormal, xV0, xV1, xV2, bFacingTowards, fDistance );
					if ( bIntersect )
					{
						u_short uFaceFlags = Asura_Physics_Environment::GetFaceFlags( pxFace->m_uFaceID );
						if ( ( uFaceFlags & AsuraPolygonFlag_IgnoreForIK ) == 0 && ( !bFacingTowards || ( uFaceFlags & AsuraPolygonFlag_DoubleSided ) ) )
						{
							Asura_AVector3 xContactPoint = pxLink->GetPosition() + xNormal * (fDistance);
#ifndef ASURA_NO_DEBUG_OUTPUT
							if ( g_bRenderDebugInfo )
							{
								Asura_DebugLineRender::AddLine( pxLink->GetPosition(), xContactPoint, 0xFF00FF00 );
							}
#endif
							pxLink->SetPosition( xContactPoint );
							bDoneSomething = true;
						}
					}
				}
			}
			pxFace = pxFace->m_pxNext;
		}
	}
	return bDoneSomething;
}

void Asura_Animation_IK_Chain::UpdateLerpTimer()
{	
	if ( m_uLerpFrameStamp != Asura_Timers::GetFrameNumber() )
	{
		float fTargetLerp = m_bAnimVeto ? 0.0f : m_fTargetLerp;
		if ( Asura_Maths::Abs( m_fLerp - fTargetLerp ) > 0.0f )
		{

			float fDeltaLerp = fTargetLerp - m_fLerp;
			m_fLerp += Asura_Maths::Sign( fDeltaLerp ) * 10.0f * Asura_Timers::GetGameTimeSlice(); // Update the current lerp value
			if ( fDeltaLerp * ( fTargetLerp - m_fLerp ) < 0.0f )
			{
				// if we've overshot our target, finish up the lerp.
				m_fLerp = fTargetLerp;
			}
		}
		m_uLerpFrameStamp = Asura_Timers::GetFrameNumber();
	}
	Asura_Assert( m_fLerp >= 0.0f && m_fLerp <= 1.0f, "Invalid weight for lerp lerp" );
}

bool Asura_Animation_IK_Chain::SolveInitFunction()
{
	for ( Asura_Animation_IK_Link* pxLink = m_pxFirstLink; pxLink != NULL; pxLink = pxLink->GetNextLink() )
	{
		pxLink->SetPosition( pxLink->GetAnimPosition(), false );
		pxLink->SetQuat( pxLink->GetAnimQuat(), false );
	}
	return true;
}

bool Asura_Animation_IK_Chain::SolveEndFunction()
{
	for ( Asura_Animation_IK_Link* pxLink = ( m_pxFirstLink ? m_pxFirstLink->GetNextLink() : NULL ); pxLink; pxLink = pxLink->GetNextLink() )
	{
		if ( pxLink->GetLastPosition().GetX() < 1.0e+020f )
		{
			if ( !m_bNoPosSmoothing )
			{
				const float fMaxDeltaPos = m_fPosSmoothingSpeed * Asura_Timers::GetGameTimeSlice();
				Asura_AVector3 xNewPos;
				Asura_AVector3 xOldPos = pxLink->GetAnimPosition() + pxLink->GetLastPosition() - pxLink->GetLastAnimPosition();
				Asura_AVector3 xDeltaPos = pxLink->GetPosition() - xOldPos;
				if ( xDeltaPos.MagnitudeSquared() <= fMaxDeltaPos * fMaxDeltaPos)
				{
					xNewPos = pxLink->GetPosition();
				}
				else
				{
					xDeltaPos.Normalise();
					xNewPos = xOldPos + xDeltaPos * fMaxDeltaPos;
					//xNewPos.Lerp( pxLink->GetPosition(), xOldPos, m_fPosSmoothing > 0.0f ? Asura_Maths::ClampToRange( expf( -Asura_Timers::GetGameTimeSlice() / m_fPosSmoothing ), 0.0f, 1.0f ) : 0.0f );
				}
				pxLink->SetPosition( xNewPos );
#ifndef ASURA_NO_DEBUG_OUTPUT
				if ( g_bRenderDebugInfo )
				{
					Asura_DebugLineRender::AddLine( pxLink->GetPosition(), xNewPos, 0xFFFF0000 );
				}
#endif
			}
			if ( !m_bNoOriSmoothing && pxLink == m_pxLastLink )
			{
				const float fMaxDeltaAngle = m_fOriSmoothingSpeed * Asura_Timers::GetGameTimeSlice();
				Asura_Quat xNewQuat;
				Asura_Quat xInvOldAnimQuat = m_pxLastLink->GetLastAnimQuat();
				xInvOldAnimQuat.Invert();
				Asura_Quat xOldQuat = m_pxLastLink->GetAnimQuat() * xInvOldAnimQuat * m_pxLastLink->GetLastQuat();
				Asura_Quat xDeltaOri = xOldQuat;
				xDeltaOri.Invert();
				xDeltaOri = pxLink->GetQuat() * xDeltaOri;
				Asura_AVector3 xAxis;
				float fAngle = xDeltaOri.ToRotation( xAxis );
				if ( fAngle < 0 )
				{
					fAngle *= -1.0f;
					xAxis *= -1.0f;
				}
				if ( fAngle <= fMaxDeltaAngle )
				{
					xNewQuat = pxLink->GetQuat();
				}
				else
				{
					xDeltaOri.FromRotation( fMaxDeltaAngle, xAxis );
					xNewQuat = xDeltaOri * xOldQuat;
					//xNewQuat.Slerp( m_pxLastLink->GetQuat(), xOldQuat, m_fOriSmoothing > 0.0f ? Asura_Maths::ClampToRange( expf( -Asura_Timers::GetGameTimeSlice() / m_fOriSmoothing ), 0.0f, 1.0f ) : 0.0f );
				}
				m_pxLastLink->SetQuat( xNewQuat );
			}
		}
		pxLink->SetLastPosition( pxLink->GetPosition() );
		pxLink->SetLastAnimPosition( pxLink->GetAnimPosition() );
		pxLink->SetLastQuat( pxLink->GetQuat() );
		pxLink->SetLastAnimQuat( pxLink->GetAnimQuat() );
	}

	m_pxFirstLink->SetLastPosition( m_pxFirstLink->GetPosition() );
	m_pxFirstLink->SetLastAnimPosition( m_pxFirstLink->GetAnimPosition() );
	m_pxFirstLink->SetLastQuat( m_pxFirstLink->GetQuat() );
	m_pxFirstLink->SetLastAnimQuat( m_pxFirstLink->GetAnimQuat() );
	

	return true;
}

#endif // ASURA_USE_ANIMATION_IK_SOLVER
