From f4bd3cb5a20a168e1fcbdf9e595196803280e870 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 15 Dec 2011 14:42:34 +1100
Subject: [PATCH] AP_Math: added Polygon_complete() function

used to veryify user supplied boundaries
---
 libraries/AP_Math/examples/polygon/polygon.pde | 12 +++++++++++-
 libraries/AP_Math/polygon.cpp                  | 12 ++++++++++++
 libraries/AP_Math/polygon.h                    |  1 +
 3 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Math/examples/polygon/polygon.pde b/libraries/AP_Math/examples/polygon/polygon.pde
index ff3c6432e8..339ec996ea 100644
--- a/libraries/AP_Math/examples/polygon/polygon.pde
+++ b/libraries/AP_Math/examples/polygon/polygon.pde
@@ -55,6 +55,16 @@ void setup(void)
 	Serial.begin(115200);
 	Serial.println("polygon unit tests\n");
 
+    if (!Polygon_complete(OBC_boundary, ARRAY_LENGTH(OBC_boundary))) {
+        Serial.println("OBC boundary is not complete!");
+        all_passed = false;
+    }
+
+    if (Polygon_complete(OBC_boundary, ARRAY_LENGTH(OBC_boundary)-1)) {
+        Serial.println("Polygon_complete test failed");
+        all_passed = false;
+    }
+
     for (i=0; i<ARRAY_LENGTH(test_points); i++) {
         bool result;
         result = Polygon_outside(&test_points[i].point, OBC_boundary, ARRAY_LENGTH(OBC_boundary));
@@ -79,7 +89,7 @@ void setup(void)
             }
         }
     }    
-    Serial.printf("%u usec/call\n", (micros() - start_time)/(count*ARRAY_LENGTH(test_points)));
+    Serial.printf("%u usec/call\n", (unsigned)((micros() - start_time)/(count*ARRAY_LENGTH(test_points))));
     Serial.println(all_passed?"ALL TESTS PASSED":"TEST FAILED");                  
 }
 
diff --git a/libraries/AP_Math/polygon.cpp b/libraries/AP_Math/polygon.cpp
index 790cf22db1..e0eb40ba77 100644
--- a/libraries/AP_Math/polygon.cpp
+++ b/libraries/AP_Math/polygon.cpp
@@ -76,3 +76,15 @@ bool Polygon_outside(const Vector2f *P, const Vector2f *V, unsigned n)
     }
     return wn == 0;
 }
+
+/*
+  check if a polygon is complete. 
+
+  We consider a polygon to be complete if we have at least 4 points,
+  and the first point is the same as the last point. That is the
+  minimum requirement for the Polygon_outside function to work
+ */
+bool Polygon_complete(const Vector2f *V, unsigned n)
+{
+    return (n >= 4 && V[n-1].x == V[0].x && V[n-1].y == V[0].y);
+}
diff --git a/libraries/AP_Math/polygon.h b/libraries/AP_Math/polygon.h
index 1ae785db09..f536ebdfe8 100644
--- a/libraries/AP_Math/polygon.h
+++ b/libraries/AP_Math/polygon.h
@@ -18,4 +18,5 @@
  */
 
 bool Polygon_outside(const Vector2f *P, const Vector2f *V, unsigned n);
+bool Polygon_complete(const Vector2f *V, unsigned n);