polygon: improve the speed and precision of the polygon algorithm

now takes 156 usec per test, with a 11 point boundary

libraries/AP_Math/examples/polygon/polygon.pde

@@ -47,6 +47,86 @@ static const struct {
 
 #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
  */
@@ -83,6 +163,8 @@ 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

@@ -40,19 +40,25 @@ bool Polygon_outside(const Vector2l &P, const Vector2l *V, unsigned n)
     unsigned i, j;
     bool outside = true;
     for (i = 0, j = n-1; i < n; j = i++) {
-	    if ((V[i].y > P.y) == (V[j].y > P.y)) {
-		    continue;
-	    }
-	    float dx1, dx2, dy1, dy2;
-	    // we convert the deltas to floating point numbers to
-	    // prevent integer overflow while maintaining maximum precision
-	    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;
-	    if ( dx1 < dx2 * dy1 / dy2 ) {
-		    outside = !outside;
-	    }
+        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
+        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;
+        if (dy2 < 0) {
+            if ( dx1 * dy2 > dx2 * dy1 ) {
+                outside = !outside;
+            }
+        } else {
+            if ( dx1 * dy2 < dx2 * dy1 ) {
+                outside = !outside;
+            }
+        }            
     }
     return outside;
 }