void dCustomKinematicController::Init (NewtonBody* const body, const dMatrix& matrix)
{
	CalculateLocalMatrix(matrix, m_localMatrix0, m_localMatrix1);

	m_autoSleepState = NewtonBodyGetAutoSleep(body) ? true : false;
	NewtonBodySetSleepState(body, 0);

	SetPickMode(1);
	SetLimitRotationVelocity(10.0f);
	SetTargetMatrix(matrix);
	SetMaxLinearFriction(1.0f);
	SetMaxAngularFriction(1.0f);

	// set as soft joint
	SetSolverModel(2);
}
CustomKinematicController::CustomKinematicController(NewtonBody* const body, const dVector& handleInGlobalSpace)
	:CustomJoint(6, body, NULL)
{
	dMatrix matrix;

	// get the initial position and orientation
	NewtonBodyGetMatrix (body, &matrix[0][0]);

	m_autoSlepState = NewtonBodyGetSleepState (body);
	NewtonBodySetAutoSleep (body, 0);

	m_localHandle = matrix.UntransformVector (handleInGlobalSpace);
	matrix.m_posit = handleInGlobalSpace;

	SetPickMode (1);
	SetTargetMatrix (matrix);
	SetMaxLinearFriction(1.0f); 
	SetMaxAngularFriction(1.0f); 
}