//
// Asura_Physics_Constraint_SharedPoint.cpp
//
// Created by KJFL - 18 July 2007
//
// Copyright (C) 2007 Rebellion, All Rights Reserved.
//

#ifdef ASURA_PHYSICS_SUPPORT_CONSTRAINTS

#include "Core/Asura.h"

// This file's header.
#include "Physics/Constraints/Asura_Physics_Constraint_SharedPoint.h"
#include "Physics/Asura_Physics_RigidBody.h"

//#include "Debugging/Asura_DebugText.h"

Asura_Physics_Constraint_SharedPoint::Asura_Physics_Constraint_SharedPoint( Asura_Physics_RigidBodyInstance* pxBody1, const Asura_AVector3& xPositionOnBody1, Asura_Physics_RigidBodyInstance* pxBody2, const Asura_AVector3& xPositionOnBody2  )
: Asura_Physics_Constraint( pxBody1, pxBody2 )
, m_xPositionOnBody1( xPositionOnBody1 )
, m_xPositionOnBody2( xPositionOnBody2 )
{
}

void Asura_Physics_Constraint_SharedPoint::ResolveConstraint( void )
{
	// must always check the body pointers are valid first
	if ( !m_pxBody1 || !m_pxBody2 ) return;

	const Asura_AVector3 xRel1 = m_xPositionOnBody1 * m_pxBody1->GetOrientation();
	const Asura_AVector3 xRel2 = m_xPositionOnBody2 * m_pxBody2->GetOrientation();

	Asura_AVector3 xVelocity1;

	m_pxBody1->CalculateVelocityOfRelativePointOnBody( xRel1, xVelocity1 );

	Asura_AVector3 xVelocity2;

	m_pxBody2->CalculateVelocityOfRelativePointOnBody( xRel2, xVelocity2 );

	Asura_AVector3 xDisplacement = (xRel1 + m_pxBody1->GetCentreOfMass()) - (xRel2 + m_pxBody2->GetCentreOfMass());

	const float fDistance = xDisplacement.NormaliseReturningMagnitude();

	const float fALLOWED_DISTANCE_THRESHOLD = 0.001f; // a mm

	Asura_AVector3 xTargetVelocity;

//	Asura_DebugText::AddTextBlock( 10,10,0xffffffff, 0, "Disp: %f mm", fDistance*1000.0f);

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

	Asura_AVector3 xNormal = xTargetVelocity + (xVelocity1 - xVelocity2);


//	Asura_DebugText::AddTextBlock( 10,11,0xffffffff, 0, "Vel1: %f m/s", (xVelocity1).GetY());
//	Asura_DebugText::AddTextBlock( 10,12,0xffffffff, 0, "Vel2: %f m/s", (xVelocity2).GetY());

	const float fImpulse = xNormal.NormaliseReturningMagnitude();

	if ( fImpulse > 0.0f )
	{
		Asura_AVector3 xDenom1 = xRel1^xNormal;

		xDenom1 = xDenom1 * m_pxBody1->GetInvInertiaTensorDiagonal();

		xDenom1 = xDenom1 ^ xRel1;

		Asura_AVector3 xDenom2 = xRel2^xNormal;

		xDenom2 = xDenom2 * m_pxBody2->GetInvInertiaTensorDiagonal();

		xDenom2 = xDenom2 ^ xRel2;

		const float fDenom = m_pxBody1->GetInvMass() + m_pxBody2->GetInvMass() + (xDenom1 + xDenom2) * xNormal;

		Asura_AVector3 xImpulse = xNormal * ( fImpulse / fDenom );

		if ( m_pxBody1->IsDynamic() )
		{
			m_pxBody1->ApplyImpulseAtRelativePosition( xRel1, -xImpulse );
		}

		if ( m_pxBody2->IsDynamic() )
		{
			m_pxBody2->ApplyImpulseAtRelativePosition( xRel2, +xImpulse );
		}
	}
}

#endif


