java - 坚持一些角度算术

Question

你好，

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

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

我一直认为一定有一个聪明的方法来做到这一点，但它逃避了我。

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

如果 requiredHeading 小于远离 initialHeading 的 turnRate，那么它应该返回 requiredHeading

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

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

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

-弗林克

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

    }

}

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