Function to turn to requested bearing

Question

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
private float turnToRequestedHeading(float initialHeading, float requiredHeading, float turnRate) {
    //DEBUG*/Log.d(this.getClass().getName(), "turnToRequestedHeading(initialHeading="+initialHeading+", requiredHeading="+requiredHeading+", turnRate="+turnRate+"): Started");
    float resultantHeading;
    int   direction = 1;            // clockwise, set anti-clockwise (-1) later if required
    if ((Math.signum(initialHeading) == Math.signum(requiredHeading)) || (Math.signum(initialHeading) == 0) || (Math.signum(requiredHeading) == 0)) {
        // both headings are on the same side of 0 so turn will not pass through the  +/- Pi discontinuity
        if (Math.max(Math.abs(requiredHeading) - Math.abs(initialHeading), Math.abs(initialHeading) - Math.abs(requiredHeading)) < turnRate) {
            // angle to be updated is less than turn rate
            resultantHeading= requiredHeading;
            /*DEBUG*/Log.d(this.getClass().getName(), "turnToRequestedHeading(initialHeading="+initialHeading+", requiredHeading="+requiredHeading+", turnRate="+turnRate+"): Path1");
        } else {
            // angle to be updated is greater than turn rate
            if (initialHeading < requiredHeading) {
                // turn clockwise
                resultantHeading = initialHeading + turnRate;
                /*DEBUG*/Log.d(this.getClass().getName(), "turnToRequestedHeading(initialHeading="+initialHeading+", requiredHeading="+requiredHeading+", turnRate="+turnRate+"): Path2");
            } else {
                // turn anti-clockwise
                resultantHeading = initialHeading - turnRate;
                /*DEBUG*/Log.d(this.getClass().getName(), "turnToRequestedHeading(initialHeading="+initialHeading+", requiredHeading="+requiredHeading+", turnRate="+turnRate+"): Path3");
            }
        }
    } else {
        // headings are on different sides of 0 so turn may pass through the +/- Pi discontinuity
        if (Math.abs(initialHeading) + Math.abs(requiredHeading) < turnRate) {
            // angle to be updated is less than turn rate (around 0)
            resultantHeading= requiredHeading;
            /*DEBUG*/Log.d(this.getClass().getName(), "turnToRequestedHeading(initialHeading="+initialHeading+", requiredHeading="+requiredHeading+", turnRate="+turnRate+"): Path4");
        } else if ((180 - Math.abs(initialHeading)) + (180 - Math.abs(requiredHeading)) < turnRate) {
            // angle to be updated is less than turn rate (around +/- Pi)
            resultantHeading= requiredHeading;
            /*DEBUG*/Log.d(this.getClass().getName(), "turnToRequestedHeading(initialHeading="+initialHeading+", requiredHeading="+requiredHeading+", turnRate="+turnRate+"): Path5");
        } else {
            // angle to be updated is greater than turn rate so calculate direction (previously assumed to be 1)
            if (initialHeading < 0) {
                if (requiredHeading > PIf + initialHeading) direction = -1;
            } else {
                if (requiredHeading > -PIf + initialHeading) direction = -1;
            }
            if ((direction == 1) && (initialHeading > PIf - turnRate)) {
                // angle includes the +/- Pi discontinuity, clockwise
                resultantHeading = -TWO_PIf + turnRate + initialHeading;
                /*DEBUG*/Log.d(this.getClass().getName(), "turnToRequestedHeading(initialHeading="+initialHeading+", requiredHeading="+requiredHeading+", turnRate="+turnRate+"): Path6 snap="+(resultantHeading > requiredHeading));
                if (resultantHeading > requiredHeading) resultantHeading = requiredHeading;
            } else if ((direction == -1) && (initialHeading < -PIf + turnRate)) {
                // angle includes the +/- Pi discontinuity, anti-clockwise
                resultantHeading = TWO_PIf - turnRate + initialHeading;
                /*DEBUG*/Log.d(this.getClass().getName(), "turnToRequestedHeading(initialHeading="+initialHeading+", requiredHeading="+requiredHeading+", turnRate="+turnRate+"): Path7 snap="+(resultantHeading < requiredHeading));
                if (resultantHeading < requiredHeading) resultantHeading = requiredHeading;
            } else {
                // angle does not includes the +/- Pi discontinuity
                resultantHeading = initialHeading + direction * turnRate;
                /*DEBUG*/Log.d(this.getClass().getName(), "turnToRequestedHeading(initialHeading="+initialHeading+", requiredHeading="+requiredHeading+", turnRate="+turnRate+"): Path8 direction="+direction);
            }
        }
    }
    // ensure -PI <= result <= PI
    if (resultantHeading < -PIf) resultantHeading = resultantHeading + TWO_PIf; 
    if (resultantHeading >= PIf)  resultantHeading = resultantHeading - TWO_PIf; 
    //DEBUG*/Log.d(this.getClass().getName(), "turnToRequestedHeading: Returning "+resultantHeading);
    return resultantHeading;
}

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) {
    if (Math.round(turnToRequestedHeading(initialHeading*PIf/180, requiredHeading*PIf/180, turnRate*PIf/180)*180/PIf) != expectedResult) {
        /*DEBUG*/Log.i(this.getClass().getName(), "test(initial="+initialHeading+", required="+requiredHeading+", rate="+turnRate+") Expected "+expectedResult+", Returns "+(Math.round(turnToRequestedHeading(initialHeading*PIf/180, requiredHeading*PIf/180, turnRate*PIf/180)*180/PIf)));
    }
}

/*DEBUG*/Log.i(this.getClass().getName(), "turnToRequestedHeading tests:");
turnToRequestedHeadingTest(   0,   0,  0,   0);
turnToRequestedHeadingTest(   0,   0, 25,   0);
turnToRequestedHeadingTest(  10,  15, 25,  15);
turnToRequestedHeadingTest(  20,  55, 25,  45);
turnToRequestedHeadingTest(  85,  95, 25,  95);
turnToRequestedHeadingTest( 150,-170, 25, 175);
turnToRequestedHeadingTest( 170, 177, 25, 177);
turnToRequestedHeadingTest( 170,-175, 25,-175);
turnToRequestedHeadingTest( 175,-100, 25,-160);
turnToRequestedHeadingTest( 175,   0, 25, 150);
turnToRequestedHeadingTest( 180,   0, 25, 155);
turnToRequestedHeadingTest(-170,-100, 25,-145);
turnToRequestedHeadingTest(-100, -80, 25, -80);
turnToRequestedHeadingTest( -30, -15, 25, -15);
turnToRequestedHeadingTest( -30,  15, 25,  -5);
turnToRequestedHeadingTest( -20,  -5, 25,  -5);
turnToRequestedHeadingTest( -20,   5, 25,   5);
turnToRequestedHeadingTest( -20,  15, 25,   5);
turnToRequestedHeadingTest(  10, 180, 25,  35);
turnToRequestedHeadingTest(  10,-160, 25, -15);
turnToRequestedHeadingTest( 170,   0, 25, 145);
turnToRequestedHeadingTest( 170, -15, 25,-165);
turnToRequestedHeadingTest(-170,   5, 25,-145);
turnToRequestedHeadingTest( -10, 160, 25,  15);
turnToRequestedHeadingTest( -10,-150, 25, -35);
turnToRequestedHeadingTest(  10,-170, 25, -15);
turnToRequestedHeadingTest(   0, 180, 25,  25);
turnToRequestedHeadingTest( -10, -15, 25, -15);
turnToRequestedHeadingTest( -20, -55, 25, -45);
turnToRequestedHeadingTest( -85, -95, 25, -95);
turnToRequestedHeadingTest(-150, 170, 25,-175);
turnToRequestedHeadingTest(-170,-177, 25,-177);
turnToRequestedHeadingTest(-170, 175, 25, 175);
turnToRequestedHeadingTest(-175, 100, 25, 160);
turnToRequestedHeadingTest(-175,   0, 25,-150);
turnToRequestedHeadingTest( 170, 100, 25, 145);
turnToRequestedHeadingTest( 100,  80, 25,  80);
turnToRequestedHeadingTest(  30,  15, 25,  15);
turnToRequestedHeadingTest(  30, -15, 25,   5);
turnToRequestedHeadingTest(  20,   5, 25,   5);
turnToRequestedHeadingTest(  20,  -5, 25,  -5);
turnToRequestedHeadingTest(  20, -15, 25,  -5);
turnToRequestedHeadingTest( -10,-180, 25, -35);
turnToRequestedHeadingTest( -10, 160, 25,  15);
turnToRequestedHeadingTest(-170,   0, 25,-145);
turnToRequestedHeadingTest(-170,  15, 25, 165);
turnToRequestedHeadingTest( 170,  -5, 25, 145);
turnToRequestedHeadingTest(  10,-160, 25, -15);
turnToRequestedHeadingTest(  10, 150, 25,  35);
turnToRequestedHeadingTest( -10, 170, 25,  15);
// More tests
turnToRequestedHeadingTest(   0,  15, 25,  15);
turnToRequestedHeadingTest(   0,  60, 25,  25);
turnToRequestedHeadingTest(   0, -15, 25, -15);
turnToRequestedHeadingTest(   0, -60, 25, -25);
turnToRequestedHeadingTest( 180, 165, 25, 165);
turnToRequestedHeadingTest( 180, 100, 25, 155);
turnToRequestedHeadingTest( 180,-165, 25,-165);
turnToRequestedHeadingTest( 180,-100, 25,-155);
turnToRequestedHeadingTest(-180, 165, 25, 165);
turnToRequestedHeadingTest(-180, 100, 25, 155);
turnToRequestedHeadingTest(-180,-165, 25,-165);
turnToRequestedHeadingTest(-180,-100, 25,-155);
turnToRequestedHeadingTest(  25,   0, 25,   0);
turnToRequestedHeadingTest(  25, -25, 25,   0);
turnToRequestedHeadingTest( -25,   0, 25,   0);
turnToRequestedHeadingTest( -25,  25, 25,   0);
turnToRequestedHeadingTest( 155, 180, 25, 180);
turnToRequestedHeadingTest( 155,-155, 25, 180);
turnToRequestedHeadingTest(-155, 180, 25,-180);
turnToRequestedHeadingTest(-155, 155, 25,-180);
turnToRequestedHeadingTest( 155,-180, 25,-180);
turnToRequestedHeadingTest(-155,-180, 25,-180);

Answer 1

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;
    var na = turnToRequestedHeading(ang1,ang2,turnRate);
    na = na * (180.0/Math.PI);
    if(Math.abs(na- newAngle) > Math.epsilon)
        throw "Failed on:" + initial + "-" + requested + " " + na;
}
function turnToRequestedHeading(ang1,ang2,turnRate)
{
    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;
}

Answer 2

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));
float resultantHeeding = Math.IEEEremainder(initialHeading + headingChange, FUL_CIRCLE);
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);
}