From cc746fcec871de71fe795f9cd7764702023d6ef4 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 22 Jul 2019 11:15:43 +0200
Subject: [PATCH] Add line segment intersection skeleton, ut

---
 api/psp.h   | 27 +++++++++++++++++++++++++++
 src/psp.cc  | 10 ++++++++++
 ut/psp.t.cc |  6 ++++++
 3 files changed, 43 insertions(+)

diff --git a/api/psp.h b/api/psp.h
index 086a363..65d5747 100644
--- a/api/psp.h
+++ b/api/psp.h
@@ -1,6 +1,8 @@
 #ifndef PSP_H
 #define PSP_H
 
+#include <tuple>
+
 #include "bcar.h"
 #include "pslot.h"
 
@@ -36,4 +38,29 @@ class PSPlanner {
                 PSPlanner();
 };
 
+/*! \brief Return intersection of two line segments.
+
+The output is tuple `std::tuple<bool, double, double>`, where the first
+value is true when there is an intersection and false otherwise. The
+second and third parameters in the return tuple are coordinates of the
+intersection.
+
+\see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
+
+\param x1 First line segment first `x` coordinate.
+\param y1 First line segment first `y` coordinate.
+\param x2 First line segment second `x` coordinate.
+\param y2 First line segment second `y` coordinate.
+\param x3 Second line segment first `x` coordinate.
+\param y3 Second line segment first `y` coordinate.
+\param x4 Second line segment second `x` coordinate.
+\param y4 Second line segment second `y` coordinate.
+*/
+std::tuple<bool, double, double> intersect(
+        double x1, double y1,
+        double x2, double y2,
+        double x3, double y3,
+        double x4, double y4
+);
+
 #endif /* PSP_H */
diff --git a/src/psp.cc b/src/psp.cc
index 6d90ce4..367b1f0 100644
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -17,3 +17,13 @@ void PSPlanner::fer()
 PSPlanner::PSPlanner()
 {
 }
+
+std::tuple<bool, double, double> intersect(
+        double x1, double y1,
+        double x2, double y2,
+        double x3, double y3,
+        double x4, double y4
+)
+{
+        return std::make_tuple(false, 0, 0);
+}
diff --git a/ut/psp.t.cc b/ut/psp.t.cc
index 6ee1331..277e118 100644
--- a/ut/psp.t.cc
+++ b/ut/psp.t.cc
@@ -23,6 +23,12 @@ WVTEST_MAIN("parking slot planner basic test")
         WVPASSEQ_DOUBLE(psp.ps().heading(), psp.gc().h(), 0.00001);
 
         // collide
+        auto tmpi1 = intersect(1, 1, 3, 3, 1, 3, 3, 1);
+        WVPASS(std::get<0>(tmpi1));
+        WVPASSEQ_DOUBLE(std::get<1>(tmpi1), 2, 0.00001);
+        WVPASSEQ_DOUBLE(std::get<2>(tmpi1), 2, 0.00001);
+        auto tmpi2 = intersect(1, 1, 1, 3, 3, 1, 3, 3);
+        WVPASS(!std::get<0>(tmpi2));
         PSPlanner tpsp;
         tpsp.ps().border(2, 3, 4, 3, 4, 8, 2, 8);
         tpsp.gc() = BicycleCar(psp.gc());
-- 
2.39.2