2
\$\begingroup\$

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);
\$\endgroup\$
1
  • \$\begingroup\$ (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? \$\endgroup\$
    – rwong
    Feb 3 '11 at 8:22
1
\$\begingroup\$

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;
}
\$\endgroup\$
2
  • \$\begingroup\$ 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! \$\endgroup\$ Feb 1 '11 at 14:44
  • \$\begingroup\$ Yeah, I ran your exact test cases through that first function "turnToRequestedHeadingTest", and nothing was thrown, so it should work. :) \$\endgroup\$
    – JerKimball
    Feb 3 '11 at 15:33
1
\$\begingroup\$

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);
}
\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.