From 626467db1432ceb53f6d613f99f54136f278bd89 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 13 Aug 2018 13:40:22 +1000
Subject: [PATCH] AP_Math: add tests for line intersection

---
 .../tests/test_segment_intersection.cpp       | 65 +++++++++++++++++++
 libraries/AP_Math/vector2.h                   | 38 +++++++++++
 2 files changed, 103 insertions(+)
 create mode 100644 libraries/AP_Math/tests/test_segment_intersection.cpp

diff --git a/libraries/AP_Math/tests/test_segment_intersection.cpp b/libraries/AP_Math/tests/test_segment_intersection.cpp
new file mode 100644
index 0000000000..0be0516276
--- /dev/null
+++ b/libraries/AP_Math/tests/test_segment_intersection.cpp
@@ -0,0 +1,65 @@
+#include <AP_gtest.h>
+
+#include <AP_Math/vector2.h>
+
+#define EXPECT_VECTOR2F_EQ(v1, v2)              \
+    do {                                        \
+        EXPECT_FLOAT_EQ(v1[0], v2[0]);          \
+        EXPECT_FLOAT_EQ(v1[1], v2[1]);          \
+    } while (false);
+
+#define TEST_INTERSECT(s1p1x,s1p1y,s1p2x,s1p2y, s2p1x,s2p1y,s2p2x,s2p2y, ix,iy, expected) \
+    do {                                                                \
+        const Vector2f s1p1(s1p1x,s1p1y);                               \
+        const Vector2f s1p2(s1p2x,s1p2y);                               \
+        const Vector2f s2p1(s2p1x,s2p1y);                               \
+        const Vector2f s2p2(s2p2x,s2p2y);                               \
+        Vector2f calculated_intersection;                               \
+        bool result = Vector2f::segment_intersection(s1p1, s1p2, s2p1, s2p2, calculated_intersection); \
+        EXPECT_EQ(expected, result);                                    \
+        if (result == expected && expected) {                           \
+            Vector2f expected_intersection{ix, iy};                     \
+            EXPECT_VECTOR2F_EQ(calculated_intersection, expected_intersection); \
+        }                                                               \
+        result = Vector2f::segment_intersection(s2p1, s2p2, s1p1, s1p2, calculated_intersection); \
+        EXPECT_EQ(expected, result);                                    \
+        if (expected) {                           \
+            Vector2f expected_intersection{ix, iy};                     \
+            EXPECT_VECTOR2F_EQ(calculated_intersection, expected_intersection); \
+            EXPECT_EQ(Vector2f::point_on_segment(calculated_intersection,s1p1, s1p2), true); \
+            EXPECT_EQ(Vector2f::point_on_segment(calculated_intersection,s2p1, s2p2), true); \
+        }                                                               \
+    } while (false);
+
+#define SHOULD_INTERSECT(s1p1x,s1p1y,s1p2x,s1p2y, s2p1x,s2p1y,s2p2x,s2p2y, ix, iy) \
+    TEST_INTERSECT(s1p1x,s1p1y,s1p2x,s1p2y, s2p1x,s2p1y,s2p2x,s2p2y, ix, iy, true)
+
+#define SHOULD_NOT_INTERSECT(s1p1x,s1p1y,s1p2x,s1p2y, s2p1x,s2p1y,s2p2x,s2p2y) \
+    TEST_INTERSECT(s1p1x,s1p1y,s1p2x,s1p2y, s2p1x,s2p1y,s2p2x,s2p2y, 0, 0, false)
+
+TEST(SegmentIntersectionTests, Simple)
+{
+    SHOULD_INTERSECT(0,0,2,2, 2,0,0,2, 1,1);
+    SHOULD_INTERSECT(0,0,5,5, 2,0,0,2, 1,1);
+}
+TEST(SegmentIntersectionTests, Parallel)
+{
+    SHOULD_NOT_INTERSECT(0,0,0,1, 2,0,2,1);
+}
+// the following should probably intersect but don't:
+// TEST(SegmentIntersectionTests, Subsegment)
+// {
+//     SHOULD_INTERSECT(0,0,0,2, 0,0,0,1, 1,1);
+// }
+// TEST(SegmentIntersectionTests, Identical)
+// {
+//     SHOULD_INTERSECT(0,0,0,2, 0,0,0,2, 1,1);
+// }
+TEST(SegmentIntersectionTests, NonIntersecting)
+{
+    SHOULD_NOT_INTERSECT(0,0,0,2, 1,1,1,2);
+    SHOULD_NOT_INTERSECT(0,0,0,1, -2,2,2,2)
+}
+
+
+AP_GTEST_MAIN()
diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h
index 196f4a7301..c8ab575c69 100644
--- a/libraries/AP_Math/vector2.h
+++ b/libraries/AP_Math/vector2.h
@@ -230,6 +230,44 @@ struct Vector2
     // returns true if they intersect and intersection argument is updated with intersection closest to seg_start
     static bool circle_segment_intersection(const Vector2<T>& seg_start, const Vector2<T>& seg_end, const Vector2<T>& circle_center, float radius, Vector2<T>& intersection);
 
+    static bool point_on_segment(const Vector2<T>& point,
+                                 const Vector2<T>& seg_start,
+                                 const Vector2<T>& seg_end) {
+        const float expected_run = seg_end.x-seg_start.x;
+        const float intersection_run = point.x-seg_start.x;
+        // check slopes are identical:
+        if (fabsf(expected_run) < FLT_EPSILON) {
+            if (fabsf(intersection_run) > FLT_EPSILON) {
+                return false;
+            }
+        } else {
+            const float expected_slope = (seg_end.y-seg_start.y)/expected_run;
+            const float intersection_slope = (point.y-seg_start.y)/intersection_run;
+            if (expected_slope != intersection_slope) {
+                return false;
+            }
+        }
+        // check for presence in bounding box
+        if (seg_start.x < seg_end.x) {
+            if (point.x < seg_start.x || point.x > seg_end.x) {
+                return false;
+            }
+        } else {
+            if (point.x < seg_end.x || point.x > seg_start.x) {
+                return false;
+            }
+        }
+        if (seg_start.y < seg_end.y) {
+            if (point.y < seg_start.y || point.y > seg_end.y) {
+                return false;
+            }
+        } else {
+            if (point.y < seg_end.y || point.y > seg_start.y) {
+                return false;
+            }
+        }
+        return true;
+    }
 };
 
 typedef Vector2<int16_t>        Vector2i;