喂，
我在编写函数时遇到问题：
float turnToRequestedHeading(float initialHeading, float requiredHeading, float turnRate)
我一直认为必须有一个聪明的方法去做，但它逃脱了我。
所有值均为Radians，-PI和+ PI之间的标题，以及-0.5和+0.5之间的turnRate。
如果requiredHeading小于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
答案 0 :(得分：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);
}
}
EDIT 看到我的原始代码，检查新标题是否小于一个转弯率不正确使用绝对值并更正它。 编辑2：评论者是正确的，处理从正数到负数的包装的情况是不正确的。通过添加一个只计算距离的辅助函数来修复它。我想这将由编译器内联，但是可能值得检查反汇编以确保它是。
答案 1 :(得分：1)
对2pi使用％运算符。所以你可能会在这个方法中使用这样的东西：
if ( (requiredHeading-initialHeading) % ( 2.0*Math.PI) < turnRate )
return requiredHeading;
你可能需要输入一些显式的强制转换来浮动或加倍。
答案 2 :(得分：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;
}
随意指出任何错误！ （请举例说明值！）
-Frink