#include "CPhysicsHinge.h"

f32 CPhysicsHinge::m_maxAngle;
f32 CPhysicsHinge::m_minAngle;
vector3df CPhysicsHinge::m_breakForce;


CPhysicsHinge::CPhysicsHinge()
{

}

CPhysicsHinge::~CPhysicsHinge()
{

}


void CPhysicsHinge::init(NewtonWorld* nWorld, SPhysicsHingeJoint *jnt)
{
	m_world = nWorld;

	jnt->pivot *= IrrToNewton;
	jnt->pin *= IrrToNewton;

	m_breakForce = jnt->breakForce;
	m_maxAngle = jnt->maxAngle;
	m_minAngle = jnt->minAngle;

	m_joint = NewtonConstraintCreateHinge (m_world, &jnt->pivot.X, &jnt->pin.X, jnt->child->getBody(), jnt->parent->getBody());

	NewtonHingeSetUserCallback (m_joint, hingeCallback);
}

void CPhysicsHinge::setUserCallback(NewtonBallCallBack callback)
{
	NewtonBallSetUserCallback(m_joint, callback);
}

unsigned CPhysicsHinge::hingeCallback(const NewtonJoint* hinge, NewtonHingeSliderUpdateDesc* desc)
{
	float angle;
	angle = NewtonHingeGetJointAngle (hinge);
	if (angle > ((m_maxAngle / 180.0f) * PI)) {
		// if the joint angle is large than the predifine intervale, stop the hinge
		desc->m_accel = NewtonHingeCalculateStopAlpha (hinge, desc, (m_maxAngle / 180.0f) * PI);
		return 1;
	} else if (angle < ((m_minAngle / 180.0f) * PI)) {
		// if the joint angle is large than the predifine intervale, stop the hinge
		desc->m_accel = NewtonHingeCalculateStopAlpha (hinge, desc, (m_minAngle / 180.0f) * PI);
		return 1;
	}

	if (m_breakForce == vector3df(0,0,0))
		return 0;

	vector3df force;
	NewtonHingeGetJointForce(hinge, &force.X);

	if ((force.X > m_breakForce.X) || (force.Y > m_breakForce.Y) || (force.Z > m_breakForce.Z))
		NewtonDestroyJoint(m_world, hinge);

	return 0;
}

f32 CPhysicsHinge::getAngle()
{
	return NewtonHingeGetJointAngle(m_joint);
}

f32 CPhysicsHinge::getOmega()
{
	return NewtonHingeGetJointOmega(m_joint);
}

vector3df CPhysicsHinge::getForce()
{
	vector3df force;
	NewtonHingeGetJointForce(m_joint,&force.X);
	return force;
}