From 36fe9a18a0d3ebcc9469637da6846e8dbfcd9357 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 22 May 2019 21:03:08 +1000
Subject: [PATCH] AP_Math: make Polygon_outside cope with being passed unclosed
 polygons

---
 libraries/AP_Math/polygon.cpp            | 14 +++++++++++++-
 libraries/AP_Math/tests/test_polygon.cpp | 13 +++++++++++++
 2 files changed, 26 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Math/polygon.cpp b/libraries/AP_Math/polygon.cpp
index a5e9fd3261..9cabb2b0f9 100644
--- a/libraries/AP_Math/polygon.cpp
+++ b/libraries/AP_Math/polygon.cpp
@@ -37,9 +37,21 @@
 template <typename T>
 bool Polygon_outside(const Vector2<T> &P, const Vector2<T> *V, unsigned n)
 {
+    const bool complete = Polygon_complete(V, n);
+    if (complete) {
+        // the last point is the same as the first point; treat as if
+        // the last point wasn't passed in
+        n--;
+    }
+
     unsigned i, j;
+    // step through each edge pair-wise looking for crossings:
     bool outside = true;
-    for (i = 0, j = n-1; i < n; j = i++) {
+    for (i=0; i<n; i++) {
+        j = i+1;
+        if (j >= n) {
+            j = 0;
+        }
         if ((V[i].y > P.y) == (V[j].y > P.y)) {
             continue;
         }
diff --git a/libraries/AP_Math/tests/test_polygon.cpp b/libraries/AP_Math/tests/test_polygon.cpp
index fbccce6df1..ed89171c79 100644
--- a/libraries/AP_Math/tests/test_polygon.cpp
+++ b/libraries/AP_Math/tests/test_polygon.cpp
@@ -104,6 +104,19 @@ TEST(Polygon, outside_long)
     }
 }
 
+TEST(Polygon, outside_long_closed_equal_to_unclosed)
+{
+    // uint8_t count = 0;
+    for (const struct PB_long &pb : points_boundaries_long) {
+        // ::fprintf(stderr, "count=%u\n", count++);
+        Vector2l v[4];
+        memcpy(v, pb.boundary, sizeof(pb.boundary));
+        v[3] = v[0]; // close it
+        EXPECT_EQ(Polygon_outside(pb.point, v, 3),
+                  Polygon_outside(pb.point, v, 4));
+    }
+}
+
 
 #define TEST_POLYGON_POINTS(POLYGON, TEST_POINTS)                       \
     do {                                                                \