private void SetJointTorque(RevoluteJoint joint, float torque)
{
float atorque; // abs(torque)
float storque; // sign(torque)
if(torque > 0f) {
atorque = torque;
storque = 1f;
} else if (torque < 0f) {
atorque = -torque;
storque = -1f;
} else {
atorque = 0f;
storque = 0f;
}
//#
//# We have to enumerate various cases because we are emulating friction
//# with active torque and we have to take care not to add energy to
//# the system when a reversal happens inside the timestep.
//#
//# Also, the underlying sim does not let us apply torque directly, but
//# rather we have to factor it into maxMotorTorque and motorSpeed, the
//# latter of which we set to an extreme we don't normally expect to reach.
//#
float speed = joint.JointSpeed;
float fric = __jointFriction;
if(speed*storque >= 0f) //# Pushing in same direction as current motion (and against friction)
{
if(atorque > fric) { //# With enough force to overcome friction:
joint.SetMaxMotorTorque(atorque - fric); //# Remaining torque (after friction)...
joint.MotorSpeed = storque * 100f; //# serves to accelerate current motion
} else { //# Without enough force to overcome friction:
joint.SetMaxMotorTorque(fric - atorque); //# Remaining friction...
joint.MotorSpeed = 0f; //# ...serves to stop the motion (but no more)
}
}
else { //# Pushing against current motion (and with friction)
joint.SetMaxMotorTorque(fric + atorque); //# Forces sum...
joint.MotorSpeed = storque * 100f; //# ...in direction of torque
//# BUG: This case unfortunately adds energy during reversals because friction
//# continues to contribute energy in the same direction after the reversal.
//# Is there a fix for this?
}
}