One way to align it back is to check it's current orientation.
float bodyAngle = body->GetAngle();
Compare it with the desired angle:
float delta = desiredAndle - bodyAngle;
Then use applyTorque
with a tiny amount to slowly increase the speed in that direction
If the angular speed is too high, stop increasing the speed:
if (delta > SMALL_ENOUGH)
{
float angSpeed = body->GetAngularVelocity();
if (sign(speed) != sign(delta) || Math.abs(speed) < MAX_SPEED)
body->ApplyTorque( sign(delta) * EPSILON); // EPSILON is some small value
}
else
{
body->ApplyTorque( -sign(angSpeed) * computeStoppingForce()));
}
Now you need to check how soon you will reach the desired angle and begin to slow down.
Generally, if you are moving as velocity v
and need to stop smoothly after a distance of x
then you are going to travel for t = 2 * (x / v)
time unit, then you need to slow down at a rate of a = v / t
. You can use that to callbrate your stopping motion. v
is angSpeed
. When the delta is small enough, you can have it beging to slow down instead of speeding up.