Use RRTEdge intersect in segment obstacle

diff --git a/perception/obstacle.cc b/perception/obstacle.cc
index 61373a8125bc54311b572414702f90b97d50b807..03a0d8b3984ecb9dce40771f7cf27a885b3ccd8c 100644 (file)
--- 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)