From aa99a7810a51f95dae313b8fef5ded984d35bb3c Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 11 Mar 2019 12:40:05 +0100
Subject: [PATCH] Use RRTEdge intersect in segment obstacle

---
 perception/obstacle.cc | 35 +++--------------------------------
 1 file changed, 3 insertions(+), 32 deletions(-)

diff --git a/perception/obstacle.cc b/perception/obstacle.cc
index 61373a8..03a0d8b 100644
--- a/perception/obstacle.cc
+++ b/perception/obstacle.cc
@@ -191,38 +191,9 @@ bool SegmentObstacle::collide(RRTNode *n)
 
 bool SegmentObstacle::collide(RRTEdge *e)
 {
-        // see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
-        float x1 = this->init()->x();
-        float y1 = this->init()->y();
-        float x2 = this->goal()->x();
-        float y2 = this->goal()->y();
-        float x3 = e->init()->x();
-        float y3 = e->init()->y();
-        float x4 = e->goal()->x();
-        float y4 = e->goal()->y();
-        float deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
-        if (deno == 0) {
-                return false; // parallel
-        }
-        //return true; // colliding lines, not line segments
-        //float px = (x1 * y2 - y1 * x2) * (x3 - x4) -
-        //        (x1 - x2) * (x3 * y4 - y3 * x4);
-        //px /= deno;
-        //float py = (x1 * y2 - y1 * x2) * (y3 - y4) -
-        //        (y1 - y2) * (x3 * y4 - y3 * x4);
-        //py /= deno;
-        float s = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
-        s /= deno;
-        if (s < 0 || s > 1) {
-                return false;
-        }
-        float t = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
-        t *= -1;
-        t /= deno;
-        if (t < 0 || t > 1) {
-                return false;
-        }
-        return true;
+        if (this->intersect(e, true))
+                return true;
+        return false;
 }
 
 bool SegmentObstacle::collide(std::vector<RRTEdge *> &edges)
-- 
2.39.2