From 3ddfc6664aa9a4d02e2a1db9d234d5430ef3dff3 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 15 Dec 2011 23:33:28 +1100
Subject: [PATCH] AP_Math: better polygon algorithm

this one seems to do better with single precision floating point
---
 .../AP_Math/examples/polygon/polygon.pde      |  8 ++-
 libraries/AP_Math/polygon.cpp                 | 56 ++++---------------
 libraries/AP_Math/polygon.h                   |  2 +-
 3 files changed, 17 insertions(+), 49 deletions(-)

diff --git a/libraries/AP_Math/examples/polygon/polygon.pde b/libraries/AP_Math/examples/polygon/polygon.pde
index 339ec996ea..d892055a9c 100644
--- a/libraries/AP_Math/examples/polygon/polygon.pde
+++ b/libraries/AP_Math/examples/polygon/polygon.pde
@@ -33,12 +33,14 @@ static const struct {
 	Vector2f point;
 	bool outside;
 } test_points[] = {
-	{ Vector2f(-26.639887, 151.822281), true },
+	{ Vector2f(-26.639887, 151.822000), true },
 	{ Vector2f(-26.641870, 151.870926), false },
 	{ Vector2f(-35.0,      149.0),      true },
 	{ Vector2f(0, 0),                   true },
 	{ Vector2f(-26.576815, 151.840825), false },
 	{ Vector2f(-26.577406, 151.840586), true },
+	{ Vector2f(-26.643563, 151.830344), true },
+	{ Vector2f(-26.643565, 151.831354), false },
 };
 
 #define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))
@@ -67,7 +69,7 @@ void setup(void)
 
     for (i=0; i<ARRAY_LENGTH(test_points); i++) {
         bool result;
-        result = Polygon_outside(&test_points[i].point, OBC_boundary, ARRAY_LENGTH(OBC_boundary));
+        result = Polygon_outside(test_points[i].point, OBC_boundary, ARRAY_LENGTH(OBC_boundary));
         Serial.printf_P(PSTR("%10f,%10f  %s  %s\n"),
                         test_points[i].point.x, test_points[i].point.y,
                         result?"OUTSIDE":"INSIDE ",
@@ -83,7 +85,7 @@ void setup(void)
     for (count=0; count<1000; count++) {
         for (i=0; i<ARRAY_LENGTH(test_points); i++) {
             bool result;
-            result = Polygon_outside(&test_points[i].point, OBC_boundary, ARRAY_LENGTH(OBC_boundary));
+            result = Polygon_outside(test_points[i].point, OBC_boundary, ARRAY_LENGTH(OBC_boundary));
             if (result != test_points[i].outside) {
                 all_passed = false;
             }
diff --git a/libraries/AP_Math/polygon.cpp b/libraries/AP_Math/polygon.cpp
index e0eb40ba77..59416948db 100644
--- a/libraries/AP_Math/polygon.cpp
+++ b/libraries/AP_Math/polygon.cpp
@@ -20,61 +20,27 @@
 #include "AP_Math.h"
 
 /*
-  NOTE: the winding number crossing algorithm is based on 
-  the code from
-  http://www.softsurfer.com/Archive/algorithm_0103/algorithm_0103.htm
-
-  which has the following copyright notice:
-
-// Copyright 2001, softSurfer (www.softsurfer.com)
-// This code may be freely used and modified for any purpose
-// providing that this copyright notice is included with it.
-// SoftSurfer makes no warranty for this code, and cannot be held
-// liable for any real or imagined damage resulting from its use.
-// Users of this code must verify correctness for their application.
-
+  The point in polygon algorithm is based on:
+  http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
 */
 
-/* isLeft(): tests if a point is Left|On|Right of an infinite line.
-    Input:  three points P0, P1, and P2
-    Return: >0 for P2 left of the line through P0 and P1
-            =0 for P2 on the line
-            <0 for P2 right of the line
-    See: the January 2001 Algorithm "Area of 2D and 3D Triangles and Polygons"
-*/
-static int isLeft(const Vector2f *P0, const Vector2f *P1, const Vector2f *P2)
-{
-    float ret = ( (P1->x - P0->x) * (P2->y - P0->y)
-                  - (P2->x - P0->x) * (P1->y - P0->y) );
-    if (ret > 0) return 1;
-    if (ret < 0) return -1;
-    return 0;
-}
 
 /*
- Polygon_outside(): winding number test for a point in a polygon
+  Polygon_outside(): test for a point in a polygon
       Input:   P = a point,
                V[] = vertex points of a polygon V[n+1] with V[n]=V[0]
       Return:  true if P is outside the polygon
 */
-bool Polygon_outside(const Vector2f *P, const Vector2f *V, unsigned n)
+bool Polygon_outside(const Vector2f &P, const Vector2f *V, unsigned n)
 {
-	int    wn = 0;    // the winding number counter
-
-    // loop through all edges of the polygon
-    for (unsigned i=0; i<n; i++) {   // edge from V[i] to V[i+1]
-        if (V[i].y <= P->y) {         // start y <= P.y
-            if (V[i+1].y > P->y)      // an upward crossing
-                if (isLeft(&V[i], &V[i+1], P) > 0)  // P left of edge
-                    ++wn;            // have a valid up intersect
-        }
-        else {                       // start y > P.y (no test needed)
-            if (V[i+1].y <= P->y)     // a downward crossing
-                if (isLeft(&V[i], &V[i+1], P) < 0)  // P right of edge
-                    --wn;            // have a valid down intersect
-        }
+    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)) &&
+             (P.x < (V[j].x - V[i].x) * (P.y - V[i].y) / (V[j].y - V[i].y) + V[i].x) )
+            outside = !outside;
     }
-    return wn == 0;
+    return outside;
 }
 
 /*
diff --git a/libraries/AP_Math/polygon.h b/libraries/AP_Math/polygon.h
index f536ebdfe8..56370e86f6 100644
--- a/libraries/AP_Math/polygon.h
+++ b/libraries/AP_Math/polygon.h
@@ -17,6 +17,6 @@
  * with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-bool Polygon_outside(const Vector2f *P, const Vector2f *V, unsigned n);
+bool Polygon_outside(const Vector2f &P, const Vector2f *V, unsigned n);
 bool Polygon_complete(const Vector2f *V, unsigned n);