#include "CPhysicsCorkscrew.h"

vector3df CPhysicsCorkscrew::m_breakForce;
f32 CPhysicsCorkscrew::m_minSlide;
f32 CPhysicsCorkscrew::m_maxSlide;

CPhysicsCorkscrew::CPhysicsCorkscrew()
{

}

CPhysicsCorkscrew::~CPhysicsCorkscrew()
{

}

void CPhysicsCorkscrew::init(NewtonWorld* world, SPhysicsCorkscrewJoint* jnt)
{
	m_world = world;
	
	jnt->pivot *= IrrToNewton;
	jnt->pin *= IrrToNewton;
	m_breakForce = jnt->breakForce;

	vector3df size;
	size = *(vector3df*)NewtonBodyGetUserData(jnt->parent->getBody());

	m_minSlide = (jnt->parent->getNode()->getRelativeTransformation().getTranslation().X - size.X * 0.5f) * IrrToNewton;

	m_maxSlide = (jnt->parent->getNode()->getRelativeTransformation().getTranslation().X + size.X * 0.5f) * IrrToNewton;

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

	NewtonCorkscrewSetUserCallback(m_joint, corkscrewCallback);
}

unsigned CPhysicsCorkscrew::corkscrewCallback(const NewtonJoint* corkscrew, NewtonHingeSliderUpdateDesc* desc)
{
	if (m_breakForce == vector3df(0,0,0))
		return 0;

	vector3df force;
	NewtonCorkscrewGetJointForce(corkscrew, &force.X);

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

	return 0;
}

void CPhysicsCorkscrew::setUserCallback(NewtonCorkscrewCallBack callback)
{
	NewtonCorkscrewSetUserCallback(m_joint, callback);
}


f32 CPhysicsCorkscrew::getHingeAngle()
{
	return NewtonCorkscrewGetJointPosit(m_joint);
}

f32 CPhysicsCorkscrew::getJointVelocity()
{
	return NewtonCorkscrewGetJointVeloc(m_joint);
}

f32 CPhysicsCorkscrew::getJointAngle()
{
	return NewtonCorkscrewGetJointAngle(m_joint);
}

f32 CPhysicsCorkscrew::getJointOmega()
{
	return NewtonCorkscrewGetJointOmega(m_joint);
}

vector3df CPhysicsCorkscrew::getForce()
{
	vector3df force;
	NewtonCorkscrewGetJointForce(m_joint,&force.X);
	return force;
}

