Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 08:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: re-work polygon algorithm for perfect precision

Browse Source 
using sign checking and 64 bit integer math only when needed results
in an algorithm that is just as fast as the floating point version,
but has perfect results for any representable lat/lng
This commit is contained in:
Andrew Tridgell 2011-12-19 18:51:13 +11:00
parent 106801a59c
commit 6efa2e53cb
2 changed files with 30 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
libraries/AP_Math/examples/polygon/polygon.pde
View File

@ -43,90 +43,21 @@ static const struct {
{ Vector2l(-266435650, 1518313540), false },
{ Vector2l(-266435690, 1518303530), false },
{ Vector2l(-266435690, 1518303490), true },
{ Vector2l(-265875990, 1518344049), true },
{ Vector2l(-265875990, 1518344051), false },
{ Vector2l(-266454781, 1518820530), true },
{ Vector2l(-266454779, 1518820530), true },
{ Vector2l(-266092109, 1518747420), true },
{ Vector2l(-266092111, 1518747420), false },
{ Vector2l(-266092110, 1518747421), true },
{ Vector2l(-266092110, 1518747419), false },
{ Vector2l(-266092111, 1518747421), true },
{ Vector2l(-266092109, 1518747421), true },
{ Vector2l(-266092111, 1518747419), false },
};
#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))
static float point_distance(Vector2l &p1, Vector2l &p2)
{
float rads = (fabs(p1.x)*1.0e-7) * 0.0174532925;
float lng_scale = cos(rads);
float dlat = (float)(p1.x - p2.x);
float dlong = ((float)(p1.y - p2.y)) * lng_scale;
return sqrt((dlat*dlat) + (dlong*dlong)) * .01113195;
}
/*
test precision of the calculation
*/
static void precision_test(void)
{
Vector2l p1, p2, p;
float r;
int32_t dx, dy;
float worst_precision = 0.0;
const float delta = 0.5;
const float base = 0.9;
Serial.println("Precision test:");
p1 = OBC_boundary[8];
p2 = OBC_boundary[10];
dx = p2.x - p1.x;
dy = p2.y - p1.y;
// first come from the left
for (r=-base; r<0.0; r *= delta) {
p.x = p1.x + r*dx;
p.y = p1.y + r*dy;
if (!Polygon_outside(p, OBC_boundary, ARRAY_LENGTH(OBC_boundary))) {
float precision = point_distance(p, p1);
if (precision > worst_precision) {
worst_precision = precision;
}
}
}
// in the middle
for (r=base; r>0.0; r *= delta) {
p.x = p1.x + r*dx;
p.y = p1.y + r*dy;
if (Polygon_outside(p, OBC_boundary, ARRAY_LENGTH(OBC_boundary))) {
float precision = point_distance(p, p1);
if (precision > worst_precision) {
worst_precision = precision;
}
}
}
// from the left on other side
for (r=-base; r<0.0; r *= delta) {
p.x = p2.x + r*dx;
p.y = p2.y + r*dy;
if (Polygon_outside(p, OBC_boundary, ARRAY_LENGTH(OBC_boundary))) {
float precision = point_distance(p, p2);
if (precision > worst_precision) {
worst_precision = precision;
}
}
}
// from the right
for (r=base; r>0.0; r *= delta) {
p.x = p2.x + r*dx;
p.y = p2.y + r*dy;
if (!Polygon_outside(p, OBC_boundary, ARRAY_LENGTH(OBC_boundary))) {
float precision = point_distance(p, p2);
if (precision > worst_precision) {
worst_precision = precision;
}
}
}
Serial.printf_P(PSTR("worst precision: %f meters\n"), worst_precision);
}
/*
polygon tests
*/
@ -163,8 +94,6 @@ void setup(void)
}
Serial.println(all_passed?"TEST PASSED":"TEST FAILED");
precision_test();
Serial.println("Speed test:");
start_time = micros();
for (count=0; count<1000; count++) {

24
libraries/AP_Math/polygon.cpp
View File

@ -43,19 +43,33 @@ bool Polygon_outside(const Vector2l &P, const Vector2l *V, unsigned n)
if ((V[i].y > P.y) == (V[j].y > P.y)) {
continue;
}
float dx1, dx2, dy1, dy2;
// use floating point to cope with possible integer overflow
// this still results in precision of better than 1m
int32_t dx1, dx2, dy1, dy2;
dx1 = P.x - V[i].x;
dx2 = V[j].x - V[i].x;
dy1 = P.y - V[i].y;
dy2 = V[j].y - V[i].y;
int8_t dx1s, dx2s, dy1s, dy2s, m1, m2;
#define sign(x) ((x)<0?-1:1)
dx1s = sign(dx1);
dx2s = sign(dx2);
dy1s = sign(dy1);
dy2s = sign(dy2);
m1 = dx1s * dy2s;
m2 = dx2s * dy1s;
if (dy2 < 0) {
if ( dx1 * dy2 > dx2 * dy1 ) {
if (m1 > m2) {
outside = !outside;
} else if (m1 < m2) {
continue;
} else if ( dx1 * (int64_t)dy2 > dx2 * (int64_t)dy1 ) {
outside = !outside;
}
} else {
if ( dx1 * dy2 < dx2 * dy1 ) {
if (m1 < m2) {
outside = !outside;
} else if (m1 > m2) {
continue;
} else if ( dx1 * (int64_t)dy2 < dx2 * (int64_t)dy1 ) {
outside = !outside;
}
}