AP_Math: added a Polygon_outside() function

this tests if a point is outside of a polygon. This will be used as
part of our geo-fencing support, and also for a new 'fenced mode'

libraries/AP_Math/AP_Math.h

@@ -5,3 +5,4 @@
 #include "vector2.h"
 #include "vector3.h"
 #include "matrix3.h"
+#include "polygon.h"

libraries/AP_Math/examples/polygon/Makefile

@@ -0,0 +1 @@
+include ../../../AP_Common/Arduino.mk

libraries/AP_Math/examples/polygon/polygon.pde

@@ -0,0 +1,89 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+//
+// Unit tests for the AP_Meta_class and AP_Var classes.
+//
+
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Math.h>
+
+FastSerialPort(Serial, 0);
+
+/*
+  this is the boundary of the 2010 outback challenge
+  Note that the last point must be the same as the first for the
+  Polygon_outside() algorithm
+ */
+static const Vector2f OBC_boundary[] = {
+	Vector2f(-26.569564000, 151.837373000),
+	Vector2f(-26.569956000, 151.839405000),
+	Vector2f(-26.576823000, 151.841142000),
+	Vector2f(-26.577308000, 151.840344000),
+	Vector2f(-26.581511000, 151.841950000),
+	Vector2f(-26.578486000, 151.847469000),
+	Vector2f(-26.599489000, 151.852886000),
+	Vector2f(-26.609211000, 151.874742000),
+	Vector2f(-26.645478000, 151.882053000),
+	Vector2f(-26.643572000, 151.830350000),
+	Vector2f(-26.587599000, 151.834405000),
+	Vector2f(-26.569564000, 151.837373000)
+};
+
+static const struct {
+	Vector2f point;
+	bool outside;
+} test_points[] = {
+	{ Vector2f(-26.639887, 151.822281), 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 },
+};
+
+#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))
+
+/*
+  polygon tests
+ */
+void setup(void)
+{
+	unsigned i, count;
+    bool all_passed = true;
+    uint32_t start_time;
+
+	Serial.begin(115200);
+	Serial.println("polygon unit tests\n");
+
+    for (i=0; i<ARRAY_LENGTH(test_points); i++) {
+        bool result;
+        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 ",
+                        result == test_points[i].outside?"PASS":"FAIL");
+        if (result != test_points[i].outside) {
+            all_passed = false;
+        }
+    }
+    Serial.println(all_passed?"TEST PASSED":"TEST FAILED");
+
+    Serial.println("Speed test:");
+    start_time = micros();
+    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));
+            if (result != test_points[i].outside) {
+                all_passed = false;
+            }
+        }
+    }    
+    Serial.printf("%u usec/call\n", (micros() - start_time)/(count*ARRAY_LENGTH(test_points)));
+    Serial.println(all_passed?"ALL TESTS PASSED":"TEST FAILED");                  
+}
+
+void
+loop(void)
+{
+}

libraries/AP_Math/polygon.cpp

@@ -0,0 +1,78 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ * polygon.cpp
+ * Copyright (C) Andrew Tridgell 2011
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#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.
+
+*/
+
+/* 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
+      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)
+{
+	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
+        }
+    }
+    return wn == 0;
+}

libraries/AP_Math/polygon.h

@@ -0,0 +1,21 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ * polygon.h
+ * Copyright (C) Andrew Tridgell 2011
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+bool Polygon_outside(const Vector2f *P, const Vector2f *V, unsigned n);
+