From 7442ba31d85cb4fcfbe63c08d91572f08114cdb8 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sun, 18 Dec 2011 12:59:50 +1100
Subject: [PATCH] polygon: improve the speed and precision of the polygon
 algorithm

now takes 156 usec per test, with a 11 point boundary
---
 .../AP_Math/examples/polygon/polygon.pde      | 82 +++++++++++++++++++
 libraries/AP_Math/polygon.cpp                 | 32 +++++---
 2 files changed, 101 insertions(+), 13 deletions(-)

diff --git a/libraries/AP_Math/examples/polygon/polygon.pde b/libraries/AP_Math/examples/polygon/polygon.pde
index a438ed1368..cf2352f23b 100644
--- a/libraries/AP_Math/examples/polygon/polygon.pde
+++ b/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++) {
diff --git a/libraries/AP_Math/polygon.cpp b/libraries/AP_Math/polygon.cpp
index f3070a8838..0cc3bdb81f 100644
--- a/libraries/AP_Math/polygon.cpp
+++ b/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;
 }