# Function to turn to requested bearing

I've been writing a function:

float turnToRequestedHeading(float initialHeading, float requiredHeading, float turnRate)


I keep thinking there must be a clever way to do it, but it escapes me.

All values are in Radians, Headings between -$$\\pi\$$ and +$$\\pi\$$, and turnRate between -0.5 and +0.5.

If the requiredHeading is less than the turnRate away from the initialHeading then it should return requiredHeading

Otherwise it should return initialHeading + or - turnRate, whichever gets closer to the requiredHeading.

The following code is what I have so far, which I'm quite happy with, but is there anything I've missed, or is there an easier way to do this?

// return the new heading based on the required heading and turn rate
int   direction = 1;            // clockwise, set anti-clockwise (-1) later if required
// both headings are on the same side of 0 so turn will not pass through the  +/- Pi discontinuity
// angle to be updated is less than turn rate
} else {
// angle to be updated is greater than turn rate
// turn clockwise
} else {
// turn anti-clockwise
}
}
} else {
// headings are on different sides of 0 so turn may pass through the +/- Pi discontinuity
// angle to be updated is less than turn rate (around 0)
} else if ((180 - Math.abs(initialHeading)) + (180 - Math.abs(requiredHeading)) < turnRate) {
// angle to be updated is less than turn rate (around +/- Pi)
} else {
// angle to be updated is greater than turn rate so calculate direction (previously assumed to be 1)
} else {
}
if ((direction == 1) && (initialHeading > PIf - turnRate)) {
// angle includes the +/- Pi discontinuity, clockwise
} else if ((direction == -1) && (initialHeading < -PIf + turnRate)) {
// angle includes the +/- Pi discontinuity, anti-clockwise
} else {
// angle does not includes the +/- Pi discontinuity
}
}
}
// ensure -PI <= result <= PI
}


I have written some testing code to check it out as there were quite a few different 'special cases'

private void turnToRequestedHeadingTest(float initialHeading, float requiredHeading, float turnRate, float expectedResult) {
}
}

// More tests

• (1) You can use a code coverage tool to find out if your tests leave any branch of code untested. Then you can add more tests to cover them. This will help cover all the "special cases". (2) Your code contains some constants in degrees (like 360). Are you sure it is handling Radian values correctly? Feb 3 '11 at 8:22

Just winging it off the top of my head, pseudocode follows: EDIT - correcting some math EDIT2 - have some javascript :)

function turnToRequestedHeadingTest(initial, requested, turnRate, newAngle)
{
var ang1 = (Math.PI/180.0) * initial;
var ang2 = (Math.PI/180.0) * requested;
na = na * (180.0/Math.PI);
if(Math.abs(na- newAngle) > Math.epsilon)
throw "Failed on:" + initial + "-" + requested + " " + na;
}
{
if(ang1 == ang2) return ang1;

// pretend there's a vector v1 pointed out from
// the origin along initialHeading of length 1
// v1 = <cos initial, sin initial>
var v1= [Math.cos(ang1), Math.sin(ang1)];

// pretend there's a vector v2 pointed out from
// the origin along requiredHeading of length 1
// v2 = <cos required, sin required>
var v2= [Math.cos(ang2), Math.sin(ang2)];

// angle between v1,v2 = acos(v1 dot v2)
var dang = Math.acos(v1[0]*v2[0], v1[1]*v2[1]);
dang = dang > Math.PI ? Math.PI * 2 - dang : dang;
if(dang < turnRate) return ang2;

// delta angle = acos(cos initial * cos required, sin initial * sin required);
// resulting turn = Math.Min(turnRate, delta angle) * Math.sign(delta angle);
var deltaTurn = (Math.min(turnRate, dang) * Math.sign(ang2-ang1)) + ang1;

// return initial + resulting turn;
return deltaTurn;
}

• Cheers for that. I'm not in a position to test Javascript, but have you tested it using my test data? As this problem is trickier than it looks! Feb 1 '11 at 14:44
• Yeah, I ran your exact test cases through that first function "turnToRequestedHeadingTest", and nothing was thrown, so it should work. :) Feb 3 '11 at 15:33

Just rough idea. Not tested.

// TODO: throw exception if turnRate is negative
// TODO: throw exception if abs(turnRate) exceeds some maximum value.
const float FUL_CIRCLE = 360; // or (2 * Math.PI) for radian
float difference = Math.IEEEremainder(requiredHeading - initialHaeding, FUL_CIRCLE);
float absTurnRate = Math.abs(turnRate);
float headingChange = Math.max(-absTurnRate, Math.min(+absTurnRete, difference));
return resultantHeeding;


if your platform does not provide Math.IEEEremainder, use the following:

double GetRemainder(double dividend, double divisor) {
return dividend - divisor * Math.round(dividend / divisor);
}


or

double GetRemainder(double dividend, double divisor) {
return dividend - divisor * Math.floor(dividend / divisor + 0.5);
}