Rotating towards a point in Box2D

Question

I'm using Box2d as my physics engine and I'm trying to solve what would normally be a simple problem but what has become a nightmare for me and preventing me from finishing my app. (6 months work)

I've tried implementing this: (Example of implementation below) http://box2d.org/forum/viewtopic.php?f=18&t=7306 but it's got three problems. 1: When reaching the top angle it stops and goes the other way. (Example in logs) 2: Limited speed, some objects need turn fast. 3: Large objects with moving joints spaz out rocking all over the place.

I have a desired angle and I want my body to rotate at different speeds (depending on turn speed setting as I want large slow turn objects).

Problems with current code. 1: Large object with a moving joint either spin very fast or rock constantly. 2: The rock effect exists on everything.

Edit This is within a game loop, so have access to delta.

Code 1: Box2D attempt:

//Testing speed.
float tempSpeed = MathUtils.PI;

float currentAngle = getBody().getAngle() % MathUtils.PI2;

float nextAngle = getBody().getAngle() + (getBody().getAngularVelocity() * update);
float totalRotation = getDesiredAngle() - nextAngle;

//Assuming this tries to prevent the switch angle problem but limits the speed??
while ( totalRotation < -MathUtils.PI * tempSpeed ) totalRotation += MathUtils.PI2 * tempSpeed;
while ( totalRotation >  MathUtils.PI * tempSpeed ) totalRotation -= MathUtils.PI2 * tempSpeed;

float desiredAngularVelocity = totalRotation;
float change = 1f * tempSpeed; //allow 1 degree rotation per time step
desiredAngularVelocity = Math.min(change, Math.max(-change, desiredAngularVelocity));
float impulse = getBody().getInertia() * desiredAngularVelocity;
getBody().applyAngularImpulse(impulse, true);

//Below shows an example of the top angle being switched.
if(getFocus()){
    Functions.log("currentAngle: " + getBody().getAngle() + " | nextAngle: " + nextAngle + " | totalRotation: " + totalRotation + " | desiredAngularVelocity: " + desiredAngularVelocity);
}
/*
Log: nextAngle: -0.1850999 | totalRotation: 6.400846 | desiredAngularVelocity: 3.1415927
Log: nextAngle: -0.02812685 | totalRotation: 6.2195992 | desiredAngularVelocity: 3.1415927

Example of top angle being reached and switching back.

Log: nextAngle: 0.21522963 | totalRotation: 5.948592 | desiredAngularVelocity: 3.1415927
Log: nextAngle: 0.45278195 | totalRotation: 5.6859627 | desiredAngularVelocity: 3.1415927
 */

Code 2: My warped attempt

//Keep the angle within range.
float currentAngle = getBody().getAngle() % 6.283185f;

//Get the next angle. Use angle in next time step
float nextAngle = currentAngle + (getBody().getAngularVelocity() * update);

float totalRotationAngle = getDesiredAngle() - nextAngle;

//Keep the total rotation within range, so we don't spin all the way around when reaching the other side.

while (totalRotationAngle < -MathUtils.PI) {
    totalRotationAngle += (MathUtils.PI2);
}
while (totalRotationAngle > MathUtils.PI) {
    totalRotationAngle -= (MathUtils.PI2);
}

//Do some other stuff.
float desiredAngularVelocity = totalRotationAngle * getRotationSpeed();

//Inertia is the amount of force required to rotate the object, without it it rotates really fast.
//If we introduce it, we get more realistic results but the horrible rock.
//float torque = (getBody().getInertia() * (desiredAngularVelocity * this.turnSpeed)) * update;

//Need inertia otherwise it can spins things right out the way.

//float turnSpeed = (this.turnSpeed * (4f * (getDesiredAngle())));

float torque = (desiredAngularVelocity * (this.turnSpeed));
torque *= (getBody().getInertia());
torque *= update;

//Apply torque.
getBody().applyTorque(torque, false);

Answer 1

I think you want method 1, not method 2. I assume you a constant turn rate towards the desired heading? In that case, torque, being angular force, is not what you'd want to use here. (You'd need infinite torque to get the initial immediate start, and then infinite torque to stop immediately at the target direction) so the first method is more accurate.

There's a couple places where your variables don't seem to have the right units. It may help to write out (on paper) what units each of your variables is and check the units with dimensional analysis.

The following two lines of code clamp totalRotation/tempSpeed to [-π, π]. Assuming tempSpeed is your maximum angular velocity, this doesn't make much sense to me. I think you should remove the tempSpeed parts, and thus the logic is that "total rotation" needs to be in that range.

//Assuming this tries to prevent the switch angle problem but limits the speed??
while ( totalRotation < -MathUtils.PI * tempSpeed ) totalRotation += MathUtils.PI2 * tempSpeed;
while ( totalRotation >  MathUtils.PI * tempSpeed ) totalRotation -= MathUtils.PI2 * tempSpeed;

// should be:
while (totalRotation < -MathUtils.PI) totalRotation += MathUtils.PI2;
while (.. etc ..)

The following two lines seem wrong to me as well -- your desiredAngularVelocity is angular velocity (unit: 1/time), and not a rotation (radians are unitless), you need to divide by a time variable. Also, the change variable has some you're not allowing 1 degree rotation per timestep, you're allowing 180 degrees rotation per unit time (i.e, second).

float desiredAngularVelocity = totalRotation;    
float change = 1f * tempSpeed; //allow 1 degree rotation per time step

// should be:
float desiredAngularVelocity = totalRotation / timedelta;
float change = tempSpeed * delta; // allow 180 degree rotation per second

With these changes, it should remove the wobbling, which was caused by an incorrect impulse calculation that always overshot the desired value.