Функция обратиться к просила подшипник


Я пишу функцию:

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

Я продолжаю думать, что должен быть умный способ сделать это, но оно ускользает от меня.

Все значения в радианах, заголовки от-Пи до +пи, и turnRate между -0.5 и +0.5.

Если requiredHeading меньше turnRate от initialHeading, то он должен вернуть requiredHeading

В противном случае он должен вернуть initialHeading + или - turnRate, в зависимости от того, приближается к requiredHeading.

Следующий код-это то, что я так далеко, что я вполне доволен, но я что-нибудь упустил, или есть более простой способ сделать это?

// 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;
}

Я написал некоторые тесты код, чтобы проверить его, так как было довольно много различных "особые случаи"

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);


311
1
задан 31 января 2011 в 08:01 Источник Поделиться
Комментарии
2 ответа

Просто летят с верхней части моей головы, псевдокоде следующим образом:
Редактирование - исправление некоторых математика
EDIT2 - есть некоторые 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;
}

1
ответ дан 31 января 2011 в 09:01 Источник Поделиться

Только приблизительное представление. Не проверял.

// 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;

если ваша платформа не обеспечивает математика.IEEEremainder, используйте следующие:

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

или

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

1
ответ дан 3 февраля 2011 в 08:02 Источник Поделиться