Roket_PhysicsJointUniversal::Roket_PhysicsJointUniversal(
		NewtonWorld* worldd, 
		irr::core::vector3df point, 
		irr::core::vector3df pin0, 
		irr::core::vector3df pin1, 
		Roket_PhysicsBody* child, 
		Roket_PhysicsBody* parent,
		RPhysicsJoint* rjoint)
	{
		world = worldd;
		float pointD[3];
		pointD[0] = point.X;
		pointD[1] = point.Y;
		pointD[2] = point.Z;
		float pointP0[3];
		pointP0[0] = pin0.X;
		pointP0[1] = pin0.Y;
		pointP0[2] = pin0.Z;
		float pointP1[3];
		pointP1[0] = pin1.X;
		pointP1[1] = pin1.Y;
		pointP1[2] = pin1.Z;
		if (parent == NULL)
			joint = NewtonConstraintCreateUniversal(world, &pointD[0], &pointP0[0], &pointP1[0], child->body, NULL);
		else
			joint = NewtonConstraintCreateUniversal(world, &pointD[0], &pointP0[0], &pointP1[0], child->body, parent->body);
		NewtonJointSetUserData(joint, &rjoint);
		NewtonJointSetDestructor(joint, &JointDestructor);
	}
NewtonCustomJoint::NewtonCustomJoint (int maxDOF, NewtonBody* body0, NewtonBody* body1)
{
	m_joint = NULL;
	m_body0 = body0;
	m_body1 = body1;
	m_maxDox = maxDOF;
	m_world	= NewtonBodyGetWorld (body0);
	m_joint = NewtonConstraintCreateUserJoint (m_world, maxDOF, SubmitConstrainst, m_body0, m_body1); 

	NewtonJointSetUserData (m_joint, this);
	NewtonJointSetDestructor (m_joint, Destructor);
}
NewtonCustomJoint::~NewtonCustomJoint()
{
	//_ASSERTE (m_joint);
	
	//if the joint has user data it means the application is destroy the joint
	if (NewtonJointGetUserData (m_joint)) {
		// set the joint call to NULL to prevent infinite recursion 
		NewtonJointSetDestructor (m_joint, NULL);  

		// destroy this joint
		NewtonDestroyJoint(m_world, m_joint);
	}
}
void  NewtonCustomJoint::Destructor (const NewtonJoint* me)
{
	NewtonCustomJoint* joint;  

	// get the pointer to the joint class
	joint = (NewtonCustomJoint*) NewtonJointGetUserData (me);  

	// set the joint call to NULL to prevent infinite recursion
	NewtonJointSetDestructor (me, NULL);  
	NewtonJointSetUserData (me, NULL);  

	// delete the joint class
	delete joint;
}
void CustomJoint::Init (int maxDOF, NewtonBody* const body0, NewtonBody* const body1)
{
	m_autoDestroy = 0;
	m_joint = NULL;
	m_body0 = body0;
	m_body1 = body1;
	m_maxDof = maxDOF;
	m_world	= NewtonBodyGetWorld (body0);
	m_joint = NewtonConstraintCreateUserJoint (m_world, maxDOF, SubmitConstraints, GetInfo, m_body0, m_body1); 

	NewtonJointSetUserData (m_joint, this);
	NewtonJointSetDestructor (m_joint, Destructor);

	m_userData = NULL;
	m_userDestructor = NULL;
	m_userConstrationCallback = NULL;

}
void dCustomJoint::Init (int maxDOF, NewtonBody* const body0, NewtonBody* const body1)
{
	m_joint = NULL;
	m_body0 = body0;
	m_body1 = body1;
	m_maxDof = maxDOF;
	m_autoDestroy = 0;
	m_stiffness = 1.0f;

	m_world	= body0 ? NewtonBodyGetWorld (body0) : NewtonBodyGetWorld (body1);
	m_joint = NewtonConstraintCreateUserJoint (m_world, maxDOF, SubmitConstraints, m_body0, m_body1); 

	NewtonJointSetUserData (m_joint, this);
	NewtonJointSetDestructor (m_joint, Destructor);

	m_userData = NULL;
	m_userDestructor = NULL;
}
dCustomJoint::dCustomJoint(NewtonInverseDynamics* const invDynSolver, void* const invDynNode)
	:dCustomAlloc()
	,m_localMatrix0(dGetIdentityMatrix())
	,m_localMatrix1(dGetIdentityMatrix())
{
	m_joint = NULL;
	m_body1 = NULL;
	m_maxDof = 6;
	m_autoDestroy = 0;
	m_stiffness = 1.0f;
	m_body0 = NewtonInverseDynamicsGetBody (invDynSolver, invDynNode);
	m_world = NewtonBodyGetWorld(m_body0);
	m_joint = NewtonInverseDynamicsCreateEffector(invDynSolver, invDynNode, SubmitConstraints);

	NewtonJointSetUserData(m_joint, this);
	NewtonJointSetDestructor(m_joint, Destructor);

	m_userData = NULL;
	m_userDestructor = NULL;
}
CustomJoint::~CustomJoint()
{
	//dAssert (m_joint);
	
	//if the joint has user data it means the application is destroying the joint

// if there is a C destructor call it form here
	CustomJoint* const joint = (CustomJoint*) NewtonJointGetUserData (m_joint);  
	if (joint->m_userDestructor) {
		joint->m_userDestructor ((const NewtonUserJoint*) joint);
	}

//	if (NewtonJointGetUserData (m_joint)) {
	if (!m_autoDestroy) {
		// set the joint call to NULL to prevent infinite recursion 
		NewtonJointSetUserData (m_joint, NULL);  
		NewtonJointSetDestructor (m_joint, NULL);  

		// destroy this joint
		NewtonDestroyJoint(m_world, m_joint);
	}
}
	Roket_PhysicsJointCorkscrew::Roket_PhysicsJointCorkscrew(
		NewtonWorld* worldd,
		irr::core::vector3df point, 
		irr::core::vector3df pin, 
		Roket_PhysicsBody* child, 
		Roket_PhysicsBody* parent,
		RPhysicsJoint* rjoint)
	{
		world = worldd;
		float pointD[3];
		pointD[0] = point.X;
		pointD[1] = point.Y;
		pointD[2] = point.Z;
		float pointP[3];
		pointP[0] = pin.X;
		pointP[1] = pin.Y;
		pointP[2] = pin.Z;
		if (parent == NULL)
			joint = NewtonConstraintCreateCorkscrew(world, &pointD[0], &pointP[0], child->body, NULL);
		else
			joint = NewtonConstraintCreateCorkscrew(world, &pointD[0], &pointP[0], child->body, parent->body);
		NewtonJointSetUserData(joint, &rjoint);
		NewtonJointSetDestructor(joint, &JointDestructor);
	}