#include "CPhysicsUniversal.h"

/// Break force needed to break the join.
//! This variable is declared static as it is called in
//! static newton callbacks.
vector3df CPhysicsUniversal::m_breakForce;

/// Empty constructor for the joint.
CPhysicsUniversal::CPhysicsUniversal()
{

}

/// The destructor is also empty as the joint destruction
//! is handled by the IPhysicsJoint base class.
CPhysicsUniversal::~CPhysicsUniversal()
{

}

/// This function initializes the universal joint.
void CPhysicsUniversal::init(NewtonWorld* world, SPhysicsUniversalJoint* jnt)
{
	m_world = world;
	
	jnt->pivot *= IrrToNewton;
	jnt->pin *= IrrToNewton;
	jnt->pin2 *= IrrToNewton;
	m_breakForce = jnt->breakForce;

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

	NewtonUniversalSetUserCallback(m_joint, universalCallback);
}

/// This function is used to have a user joint callback.
void CPhysicsUniversal::setUserCallback(NewtonSliderCallBack callback)
{
	NewtonUniversalSetUserCallback(m_joint, callback);
}

unsigned CPhysicsUniversal::universalCallback(const NewtonJoint* universal, NewtonHingeSliderUpdateDesc* desc)
{
	if (m_breakForce == vector3df(0,0,0))
		return 0;

	vector3df force;
	NewtonUniversalGetJointForce(universal, &force.X);

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

	return 0;
}

f32 CPhysicsUniversal::getJointAngle()
{
	return NewtonUniversalGetJointAngle0(m_joint);
}

f32 CPhysicsUniversal::getJointAngle2()
{
	return NewtonUniversalGetJointAngle1(m_joint);
}

f32 CPhysicsUniversal::getJointOmega()
{
	return NewtonUniversalGetJointOmega0(m_joint);
}

f32 CPhysicsUniversal::getJointOmega2()
{
	return NewtonUniversalGetJointOmega1(m_joint);
}

vector3df CPhysicsUniversal::getForce()
{
	vector3df force;
	NewtonUniversalGetJointForce(m_joint, &force.X);
	return force;
}