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);
}
dCustomRackAndPinion::dCustomRackAndPinion(dFloat gearRatio, const dVector& rotationalPin, const dVector& linearPin, NewtonBody* rotationalBody, NewtonBody* linearBody)
:dCustomJoint(2, rotationalBody, linearBody)
{
m_gearRatio = gearRatio;
dMatrix dommyMatrix;
dMatrix pinAndPivot0 (dGrammSchmidt (rotationalPin));
CalculateLocalMatrix (pinAndPivot0, m_localMatrix0, dommyMatrix);
m_localMatrix0.m_posit = dVector (0.0f, 0.0f, 0.0f, 1.0f);
// calculate the local matrix for body body1
dMatrix pinAndPivot1 (dGrammSchmidt(linearPin));
CalculateLocalMatrix (pinAndPivot1, dommyMatrix, m_localMatrix1);
m_localMatrix1.m_posit = dVector (0.0f, 0.0f, 0.0f, 1.0f);
// set as kinematoc loop
SetSolverModel(1);
}
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
dCustomPulley::dCustomPulley(dFloat gearRatio, const dVector& childPin, const dVector& parentPin, NewtonBody* const child, NewtonBody* const parent)
:dCustomJoint(1, child, parent)
{
m_gearRatio = gearRatio;
// calculate the two local matrix of the pivot point
dMatrix dommyMatrix;
// calculate the local matrix for body body0
dMatrix pinAndPivot0 (dGrammSchmidt (childPin));
CalculateLocalMatrix (pinAndPivot0, m_localMatrix0, dommyMatrix);
m_localMatrix0.m_posit = dVector (0.0f, 0.0f, 0.0f, 1.0f);
// calculate the local matrix for body body1
dMatrix pinAndPivot1 (dGrammSchmidt (parentPin));
CalculateLocalMatrix (pinAndPivot1, dommyMatrix, m_localMatrix1);
m_localMatrix1.m_posit = dVector (0.0f, 0.0f, 0.0f, 1.0f);
// set as kinematoc loop
SetSolverModel(1);
}
dCustomJoint::dCustomJoint (NewtonBody* const body0, NewtonBody* const body1, NewtonDeserializeCallback callback, void* const userData)
:dCustomAlloc()
,m_localMatrix0(dGetIdentityMatrix())
,m_localMatrix1(dGetIdentityMatrix())
,m_userData(NULL)
,m_body0(body0)
,m_body1(body1)
,m_joint(NULL)
,m_world(NULL)
,m_userDestructor(NULL)
,m_stiffness(1.0f)
,m_maxDof(0)
,m_autoDestroy(0)
{
int solverModel;
callback (userData, &m_localMatrix0, sizeof (m_localMatrix0));
callback (userData, &m_localMatrix1, sizeof (m_localMatrix1));
callback (userData, &m_maxDof, sizeof (m_maxDof));
callback (userData, &m_stiffness, sizeof (m_stiffness));
callback(userData, &solverModel, sizeof(solverModel));
Init (m_maxDof, body0, body1);
SetSolverModel(solverModel);
}
