#include "CPhysicsPulley.h"

CPhysicsPulley::CPhysicsPulley()
{

}

CPhysicsPulley::~CPhysicsPulley()
{

}

void CPhysicsPulley::init(NewtonWorld* world, SPhysicsPulley* pulley)
{
	m_world = world;

	SPhysicsCustomJoint jnt;
	
	jnt.child = pulley->child;
	jnt.parent = pulley->parent;
	jnt.maxDOF = 1;

	IPhysicsCustom::init(world,&jnt);

	m_gearRatio = pulley->gearRatio;
	m_pPin = pulley->parentPin * IrrToNewton;
	m_cPin = pulley->childPin * IrrToNewton;
	m_child = pulley->child;
	m_parent = pulley->parent;

	vector3df dummyOrient, dummyPos;
	vector3df pivot(100, 200, 40);
	pivot *= IrrToNewton;

	pinAndDirToLocal( pivot, m_pPin, m_mLocalOrient0, m_mLocalPos0, dummyOrient, dummyPos );
	pinAndDirToLocal( pivot, m_cPin, dummyOrient, dummyPos, m_mLocalOrient1, m_mLocalPos1 );	
}

void CPhysicsPulley::submitConstraint()
{
	f32 w0, w1;
	f32 deltat;
	f32 relAccel, relVel;
	vector3df vel0, vel1;

	vector3df globalPos0, globalPos1;
	vector3df globalOrient0, globalOrient1;

	globalOrient0 = m_cPin;
	globalOrient1 = m_pPin;

	vector3df xdir0, xdir1;

	xdir0 = globalOrient0 * vector3df(1,0,0);
	xdir1 = globalOrient1 * vector3df(1,0,0);

	// velocities for both bodies.
	vel0 = m_child->getVelocity();
	vel1 = m_parent->getVelocity();

	// relative angular velocity.
	w0 = vel0.dotProduct( xdir0 );
	w1 = vel1.dotProduct( xdir1 );

	// relative velocity.
	relVel = w0 + (m_gearRatio * w1);

	deltat = NewtonGetTimeStep(m_child->getWorld());
	relAccel = relVel / deltat;

	addGeneralRow(xdir0, vector3df(0,0,0), xdir1, vector3df(0,0,0) );
	setRowAcceleration( relAccel );
}

