AP_Math: re-work polygon algorithm for perfect precision

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

libraries/AP_Math/examples/polygon/polygon.pde

@@ -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++) {

libraries/AP_Math/polygon.cpp

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