当前位置： 文江博客话题详情

陷入一些角度算术

发布于 2024-10-13 11:06:15 字数 4967 浏览 5 评论 0原文

你好，

我在编写一个函数时遇到了麻烦：

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

我一直在想一定有一种聪明的方法来做到这一点，但它却忽略了我。

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

如果 requiredHeading 小于与 initialHeading 相距的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);

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

-Frink

原文

Hallo,

I'm having trouble 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.

Any ideas? I get stuck when the headings are either side of straight down, e.g. -3 and +3.

UPDATE: Here is some test code and test data (see my answer for my code below):

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

I think my test data covers all cases now...

-Frink

如果你对这篇内容有疑问，欢迎到本站社区发帖提问 参与讨论，获取更多帮助，或者扫码二维码加入 Web 技术交流群。

扫码二维码加入Web技术交流群

发布评论

需要 登录 才能够评论， 你可以免费 注册 一个本站的账号。

评论（3）

So要识趣 2024-10-20 11:06:16

如果这是一个经常被调用的函数，我会远离模除法，因为它是一个相对昂贵的操作。在您的情况下，圈数只能超过 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：评论者是对的，处理从正数到负数的换行的情况是不正确的。通过添加仅计算距离的辅助函数来修复它。我想这将由编译器内联，但可能值得检查反汇编以确保它是内联的。

If this is a function that gets called often I would stay away from modular division because it is a relatively expensive operation. In your case, the turns can only ever exceed Pi or -Pi by less than one revolution so we can use adding or subtracting Pi from the value to correct for this mistake which is a cheaper option performance wise.

//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
Saw that my original code to check if the new heading was less than one turn rate away incorrectly used absolute value and corrected it.
EDIT 2: commenter is right that the case for handling wrapping from a positive to negative number was incorrect. Fixed it by adding a helper function that just calculates distance. I imagine that this will be inlined by the compiler, but it might be worth checking the disassembly to make sure that it is.

回复 原文
蓝海似她心 2024-10-20 11:06:16

对 2pi 使用 % 运算符。因此，您的方法中可能有这样的内容：

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

您可能需要放入一些显式转换为 float 或 double。

Use the % operator against 2pi. So you might have something like this as part of your method:

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

You may need to put in some explicit casts to float or double.

回复 原文
素食主义者 2024-10-20 11:06:16

这是我编写的代码，它满足我的测试数据并且似乎在我的代码中表现正常。但对我来说仍然显得过于复杂......

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

Here's the code I have come up with which satisfies my test data and seems to behave properly in my code. Still looks over-complex to me though...

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

Feel free to point out any errors! (with example values please!)

-Frink

回复 原文
~没有更多了~

关于作者

不奢求什么

暂无简介

0 文章
0 评论
22 人气
发私信

相关话题

更多

热门标签

操作系统 程序设计 IT运维 Linux系统管理 JavaScript 服务器应用 solaris C/C++ PHP Shell BSD Vue.js aix Oracle Python HTML 系统管理 HTML5 CSS 前端
更多

推荐作者

留蓝

文章 0 评论 0

18790681156

文章 0 评论 0

zach7772

文章 0 评论 0

Wini

文章 0 评论 0

ayeshaaroy

文章 0 评论 0

初雪

文章 0 评论 0

更多

友情链接

文江博客
    我们使用 Cookies 和其他技术来定制您的体验包括您的登录状态等。通过阅读我们的 隐私政策 了解更多相关信息。 单击 接受 或继续使用网站，即表示您同意使用 Cookies 和您的相关数据。
    原文