AP_Math: add Polygon_closest_distance_line

Also add optimisations

libraries/AP_Math/polygon.cpp

@@ -18,6 +18,8 @@
 
 #include "AP_Math.h"
 
+#pragma GCC optimize("O3")
+
 /*
  *  The point in polygon algorithm is based on:
  *  https://wrf.ecse.rpi.edu//Research/Short_Notes/pnpoly.html
@@ -121,3 +123,75 @@ template bool Polygon_outside<int32_t>(const Vector2l &P, const Vector2l *V, uns
 template bool Polygon_complete<int32_t>(const Vector2l *V, unsigned n);
 template bool Polygon_outside<float>(const Vector2f &P, const Vector2f *V, unsigned n);
 template bool Polygon_complete<float>(const Vector2f *V, unsigned n);
+
+
+/*
+  determine if the polygon of N verticies defined by points V is
+  intersected by a line from point p1 to point p2
+ */
+bool Polygon_intersects(const Vector2f *V, unsigned N, const Vector2f &p1, const Vector2f &p2)
+{
+    for (uint8_t i=0; i<N-1; i++) {
+        Vector2f tmp;
+        const Vector2f &v1 = V[i];
+        const Vector2f &v2 = V[i+1];
+        // optimisations for common cases
+        if (v1.x > p1.x && v2.x > p1.x && v1.x > p2.x && v2.x > p2.x) {
+            continue;
+        }
+        if (v1.y > p1.y && v2.y > p1.y && v1.y > p2.y && v2.y > p2.y) {
+            continue;
+        }
+        if (v1.x < p1.x && v2.x < p1.x && v1.x < p2.x && v2.x < p2.x) {
+            continue;
+        }
+        if (v1.y < p1.y && v2.y < p1.y && v1.y < p2.y && v2.y < p2.y) {
+            continue;
+        }
+        if (Vector2f::segment_intersection(v1,v2,p1,p2,tmp)) {
+            return true;
+        }
+    }
+    return false;
+}
+
+/*
+  return the closest distance that a line from p1 to p2 comes to an
+  edge of closed polygon V, defined by N points
+ */
+float Polygon_closest_distance_line(const Vector2f *V, unsigned N, const Vector2f &p1, const Vector2f &p2)
+{
+    if (Polygon_intersects(V,N,p1,p2)) {
+        return 0;
+    }
+    float closest_sq = FLT_MAX;
+    for (uint8_t i=0; i<N-1; i++) {
+        const Vector2f &v1 = V[i];
+        const Vector2f &v2 = V[i+1];
+
+        float dist_sq = Vector2f::closest_distance_between_lines_squared(v1, v2, p1, p2);
+        if (dist_sq < closest_sq) {
+            closest_sq = dist_sq;
+        }
+    }
+    return sqrtf(closest_sq);
+}
+
+/*
+  return the closest distance that point p comes to an edge of closed
+  polygon V, defined by N points
+ */
+float Polygon_closest_distance_point(const Vector2f *V, unsigned N, const Vector2f &p)
+{
+    float closest_sq = FLT_MAX;
+    for (uint8_t i=0; i<N-1; i++) {
+        const Vector2f &v1 = V[i];
+        const Vector2f &v2 = V[i+1];
+
+        float dist_sq = Vector2f::closest_distance_between_line_and_point_squared(v1, v2, p);
+        if (dist_sq < closest_sq) {
+            closest_sq = dist_sq;
+        }
+    }
+    return sqrtf(closest_sq);
+}

libraries/AP_Math/polygon.h

@@ -24,3 +24,21 @@ bool        Polygon_outside(const Vector2<T> &P, const Vector2<T> *V, unsigned n
 template <typename T>
 bool        Polygon_complete(const Vector2<T> *V, unsigned n);
 
+/*
+  determine if the polygon of N verticies defined by points V is
+  intersected by a line from point p1 to point p2
+ */
+bool Polygon_intersects(const Vector2f *V, unsigned N, const Vector2f &p1, const Vector2f &p2);
+
+
+/*
+  return the closest distance that a line from p1 to p2 comes to an
+  edge of closed polygon V, defined by N points
+ */
+float Polygon_closest_distance_line(const Vector2f *V, unsigned N, const Vector2f &p1, const Vector2f &p2);
+
+/*
+  return the closest distance that a point p comes to an edge of
+  closed polygon V, defined by N points
+ */
+float Polygon_closest_distance_point(const Vector2f *V, unsigned N, const Vector2f &p);