dgFloat32 dgHingeConstraint::CalculateStopAlpha (dgFloat32 angle, const dgJointCallbackParam* param) const
{
dgFloat32 alpha;
dgFloat32 omega;
dgFloat32 penetrationErr;
alpha = dgFloat32 (0.0f);
if (m_angle > angle) {
omega = GetJointOmega ();
if (omega < dgFloat32 (0.0f)) {
omega = dgFloat32 (0.0f);
}
penetrationErr = (angle - m_angle);
alpha = dgFloat32 (100.0f) * penetrationErr - omega * dgFloat32 (1.01f) / param->m_timestep;
} else if (m_angle < angle) {
omega = GetJointOmega ();
if (omega > dgFloat32 (0.0f)) {
omega = dgFloat32 (0.0f);
}
penetrationErr = MIN_JOINT_PIN_LENGTH * (angle - m_angle);
alpha = dgFloat32 (100.0f) * penetrationErr - omega * dgFloat32 (1.01f) / param->m_timestep;
}
return alpha;
}
void dCustomHingeActuator::SubmitConstraintsFreeDof (dFloat timestep, const dMatrix& matrix0, const dMatrix& matrix1)
{
if (m_actuatorFlag) {
dFloat jointangle = GetActuatorAngle();
dFloat relAngle = jointangle - m_angle;
dFloat currentSpeed = GetJointOmega();
dFloat step = dFloat(2.0f) * m_angularRate * timestep;
dFloat desiredSpeed = (dAbs(relAngle) > dAbs(step)) ? -dSign(relAngle) * m_angularRate : -dFloat(0.1f) * relAngle / timestep;
dFloat accel = (desiredSpeed - currentSpeed) / timestep;
NewtonUserJointAddAngularRow(m_joint, relAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration(m_joint, accel);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxForce);
NewtonUserJointSetRowMaximumFriction (m_joint, m_maxForce);
NewtonUserJointSetRowStiffness (m_joint, 1.0f);
} else {
dCustomHinge::SubmitConstraintsFreeDof (timestep, matrix0, matrix1);
}
}
