added new sanity-check assertions
fixed issue with angles [bad interface] - adjusted other parts accordingly - added corresponding test-cases started working on absolute heading
This commit is contained in:
10
geo/Angle.h
10
geo/Angle.h
@@ -29,6 +29,13 @@ public:
|
||||
return radToDeg(getRAD_2PI(x1,y1,x2,y2));
|
||||
}
|
||||
|
||||
/** ensure the given radians-value is within [0:2pi] */
|
||||
static float makeSafe_2PI(float rad) {
|
||||
while(rad < 0) {rad += 2*PI;}
|
||||
while(rad >= 2*PI) {rad -= 2*PI;}
|
||||
return rad;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* gets the angular difference between
|
||||
@@ -44,13 +51,14 @@ public:
|
||||
|
||||
/**
|
||||
* gets the angular difference between
|
||||
* "angular change from r1 to r2"
|
||||
* - the given radians [0:2PI]
|
||||
* - as a change-in-direction between [-PI:+PI]
|
||||
*/
|
||||
static float getSignedDiffRAD_2PI(const float r1, const float r2) {
|
||||
Assert::isBetween(r1, 0.0f, (float)(2*PI), "r1 out of bounds"); // [0:360] deg
|
||||
Assert::isBetween(r2, 0.0f, (float)(2*PI), "r2 out of bounds"); // [0:360] deg
|
||||
float diff = r1-r2;
|
||||
float diff = r2-r1;
|
||||
if (diff > +PI) {diff = -(2*PI - diff);}
|
||||
else if (diff < -PI) {diff = +(2*PI + diff);}
|
||||
Assert::isBetween(diff, -PI, (float)(+PI), "result out of bounds"); // [-180:+180] deg
|
||||
|
||||
@@ -33,8 +33,11 @@ public:
|
||||
}
|
||||
|
||||
/** signled angular difference [-PI:+PI] */
|
||||
float getSignedDiff(const Heading other) const {
|
||||
return Angle::getSignedDiffRAD_2PI(rad, other.rad);
|
||||
// float getSignedDiff(const Heading other) const {
|
||||
// return Angle::getSignedDiffRAD_2PI(other.rad, rad);
|
||||
// }
|
||||
static float getSignedDiff(const Heading from, const Heading to) {
|
||||
return Angle::getSignedDiffRAD_2PI(from.rad, to.rad);
|
||||
}
|
||||
|
||||
/** update the angle but ensure we stay within [0:2PI] */
|
||||
|
||||
Reference in New Issue
Block a user