
///////////////////////////////////////////////////////////////////////////////
//
// File   : Asura_Animation_IK_Link.cpp
// Author : Will Plano
// Date   : 14/09/2007
//
// Copyright (C) 2007 Rebellion, All Rights Reserved.
//
///////////////////////////////////////////////////////////////////////////////

#include "Core/Asura.h" 

#ifdef ASURA_USE_ANIMATION_IK_SOLVER

#include "Core/Asura_Maths.h"

#include "Animation/IK/Asura_Animation_IK_Link.h"

const Asura_AVector3 Asura_Animation_IK_Link::s_xNullVector( Asura_Maths::LargeFloat, Asura_Maths::LargeFloat, Asura_Maths::LargeFloat );

//////////////////////////////////////////////////////////////////////////
// class Asura_Animation_IK_Link
//////////////////////////////////////////////////////////////////////////

Asura_Animation_IK_Link::Asura_Animation_IK_Link( int iBoneID /* = -1 */, int iTwistBoneID /* = -1 */ )
: m_xPos( s_xNullVector )
, m_xAnimPos( s_xNullVector )
, m_xLastPos( s_xNullVector )
, m_xLastAnimPos( s_xNullVector )
, m_xVel( Asura_Maths::ZeroAVector3 )
, m_xBendAxis( Asura_AVector3( 0.0f, 1.0f, 0.0f ) )
, m_xRollAxis( Asura_AVector3( 1.0f, 0.0f, 0.0f ) )
, m_xQuat( Asura_Maths::IdentityQuat )
, m_xAnimQuat( Asura_Maths::IdentityQuat )
, m_xLastQuat( Asura_Maths::IdentityQuat )
, m_xLastAnimQuat( Asura_Maths::IdentityQuat )
, m_iBoneID( iBoneID )
, m_iTwistBoneID( iTwistBoneID )
, m_fLength( 0.0f )
, m_xMinAngles( Asura_Euler_3( -Asura_Maths::LargeFloat, -Asura_Maths::LargeFloat, -Asura_Maths::LargeFloat ) )
, m_xMaxAngles( Asura_Euler_3(  Asura_Maths::LargeFloat,  Asura_Maths::LargeFloat,  Asura_Maths::LargeFloat ) )
, m_pxPrevLink( NULL )
, m_pxNextLink( NULL )
{ }

void Asura_Animation_IK_Link::SetPosition( const Asura_AVector3& xNewPos, bool bUpdateChain /* = true */ )
{
	if ( bUpdateChain && m_pxPrevLink != NULL )
	{
		Asura_AVector3 xOldDir = m_xPos - m_pxPrevLink->m_xPos;
		xOldDir.Normalise();
		Asura_AVector3 xNewDir = xNewPos - m_pxPrevLink->m_xPos;
		xNewDir.Normalise();
		Asura_Quat xDeltaOri;
		xDeltaOri.RotationFromAToB( xOldDir, xNewDir );
		m_pxPrevLink->IncQuat( xDeltaOri );
	}
	else
	{
		m_xPos = xNewPos;
	}
}

void Asura_Animation_IK_Link::SetQuat( const Asura_Quat& xNewQuat, bool bUpdateChain /* = true */ )
{
	if ( bUpdateChain )
	{
		Asura_Quat xInvQuat = m_xQuat;
		xInvQuat.Invert();
		IncQuat( xNewQuat * xInvQuat );
	}
	else
	{
		m_xQuat = xNewQuat;
	}
}

void Asura_Animation_IK_Link::IncQuat( const Asura_Quat& xIncQuat, bool bUpdateChain /* = true */ )
{
	m_xQuat = xIncQuat * m_xQuat;
	m_xQuat.Normalise();
	if ( bUpdateChain )
	{
		for ( Asura_Animation_IK_Link* pxLink = m_pxNextLink; pxLink; pxLink = pxLink->m_pxNextLink )
		{
			Asura_AVector3 xDir = pxLink->m_xPos - m_xPos;
			xDir = xIncQuat.RotateVector( xDir);
			pxLink->m_xPos = m_xPos + xDir;
			pxLink->m_xQuat = xIncQuat * pxLink->m_xQuat;
		}
	}
}

bool Asura_Animation_IK_Link::SatisfyAngularConstraints()
{
	bool bRet = false;
	Asura_Euler_3 xEuler;
	m_xQuat.ToEuler( xEuler );

	if ( xEuler.x < m_xMinAngles.x )
	{
		xEuler.x = m_xMinAngles.x;
		bRet = true;
	}
	else if ( xEuler.x > m_xMaxAngles.x )
	{
		xEuler.x = m_xMaxAngles.x;
		bRet = true;
	}

	if ( xEuler.y < m_xMinAngles.y )
	{
		xEuler.y = m_xMinAngles.y;
		bRet = true;
	}
	else if ( xEuler.y > m_xMaxAngles.y )
	{
		xEuler.y = m_xMaxAngles.y;
		bRet = true;
	}	

	if ( xEuler.z < m_xMinAngles.z )
	{
		xEuler.z = m_xMinAngles.z;
		bRet = true;
	}
	else if ( xEuler.z > m_xMaxAngles.z )
	{
		xEuler.z = m_xMaxAngles.z;
		bRet = true;
	}

	if ( bRet )
	{
		Asura_Quat xQuat;
		xQuat.FromEuler( xEuler );
		SetQuat( xQuat );
	}

	return bRet;
}

#endif // ASURA_USE_ANIMATION_IK_SOLVER
