1

你好，

我在编写函数时遇到问题：

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

我一直在想一定有一个聪明的方法来做到这一点，但它逃脱了我。

所有值都以弧度为单位，标题在 -PI 和 +PI 之间，turnRate 在 -0.5 和 +0.5 之间。

如果 requiredHeading 小于远离初始Heading 的 turnRate 那么它应该返回 requiredHeading

否则它应该返回 initialHeading + 或 - turnRate，以更接近 requiredHeading 为准。

有任何想法吗？当标题是直下的任一侧时，我会卡住，例如 -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);

我认为我的测试数据现在涵盖了所有情况...

-弗林克

4

3 回答 3

2

如果这是一个经常被调用的函数，我会远离模块化除法，因为它是一个相对昂贵的操作。在您的情况下，匝数只能超过 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);

    }

}

编辑看到我的原始代码检查新航向是否小于一个转弯率，错误地使用了绝对值并更正了它。编辑2：评论者是正确的，处理从正数到负数的包装是不正确的。通过添加仅计算距离的辅助函数来修复它。我想这将由编译器内联，但可能值得检查反汇编以确保它是。

于 2011-01-26T04:25:02.207 回答
1

对 2pi 使用 % 运算符。所以你可能有这样的东西作为你的方法的一部分：

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

您可能需要进行一些显式的强制转换以浮动或加倍。

于 2011-01-25T00:15:59.970 回答
1

这是我提出的满足我的测试数据并且在我的代码中表现正常的代码。不过对我来说仍然看起来过于复杂......

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

欢迎指出任何错误！（请提供示例值！）

-弗林克

于 2011-01-26T21:37:47.927 回答