Застрял в некоторой угловой арифметике

Question

Алло,

У меня проблемы с написанием функции:

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

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

Все значения приведены в радианах, заголовки от -PI до + PI, а скорость поворота от -0,5 до + 0,5.

Если requiredHeading меньше, чем turnRate от первоначального заголовка, тогда он должен вернуть требуемый заголовок

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

Есть идеи? Я застреваю, когда заголовки находятся по обе стороны от прямой, например, -3 и + 3.

ОБНОВЛЕНИЕ: Вот некоторые тестовые коды и тестовые данные (см. Мой ответ для моего кода ниже):

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

Я думаю, что мои тестовые данные теперь охватывают все случаи ...

-Frink

Answer 1

Если эта функция вызывается часто, я бы держался подальше от модульного деления, потому что это относительно дорогая операция. В вашем случае обороты могут когда-либо превышать Pi или -Pi менее чем на один оборот, поэтому мы можем использовать сложение или вычитание Pi из значения, чтобы исправить эту ошибку, что является более дешевым вариантом производительности.

//helper function to determine the distance, in radians, between two headings
float AngularDist(float heading1, float heading2) //assumes all headings are between -Pi and Pi
{
   if(heading1*heading2>0.0) //this checks if they have the same sign. If so result will be positive
   {
      return(abs(heading1-heading2));
   }
   else //If the headings don't have the same sign we need to do some extra math to check whether it is shorter to turn clockwise or counterclockwise to reach other angle
   {
      float clockwise=abs(heading1)+abs(heading2);
      float counterclockwise=2.0*Pi-clockwise;
      if(clockwise<counterclockwise)
         return(clockwise);
      else
         return(counterclockwise);
   }
}

float turnToRequestedHeading(float initialHeading, float requiredHeading, float turnRate)
    {
     if((AngularDist(initialHeading,requiredHeading)/turnRate<1.0) //if the turn is too small return required
    {
       return(requiredHeading)
    }
    else
    {
       float positiveTurn=initialHeading+turnRate
       //need to check if variable is greater than Pi
       if(positiveTurn>Pi)
          positiveTurn-=Pi;
       float negativeTurn=initialHeading-turnRate
       //need to check if we variable is less than -Pi
       if(negativeTurn<-Pi)
          negativeTurn+=Pi;

       if(AngularDist(negativeTurn,requiredHeading)<AngularDist(positiveTurn,requiredHeading))
          return(negativeTurn);
       else
          return(positiveTurn);

    }

}

EDIT Увидел, что мой оригинальный код, чтобы проверить, был ли новый курс менее чем в один оборот, неправильно использовал абсолютное значение и исправил его. РЕДАКТИРОВАТЬ 2: комментатор прав, что случай обработки переноса с положительного на отрицательное число был неверным. Исправлено, добавив вспомогательную функцию, которая просто вычисляет расстояние. Я предполагаю, что это будет встроено компилятором, но, возможно, стоит проверить разборку, чтобы убедиться, что это так.

Answer 2

Вот код, который я придумал, который удовлетворяет моим тестовым данным и, кажется, ведет себя должным образом в моем коде.Тем не менее, все еще выглядит слишком сложным ...

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

Не стесняйтесь указывать на любые ошибки!(с примерами значений, пожалуйста!)

-Frink

Answer 3

Используйте оператор% против 2pi. Таким образом, у вас может быть что-то вроде этого как часть вашего метода:

if ( (requiredHeading-initialHeading) % ( 2.0*Math.PI) < turnRate )
    return requiredHeading;

Возможно, вам придется добавить несколько явных приведений, чтобы плавать или удваиваться.