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); } }