Rewrite circle with line segments collide method

[hubacji1/iamcar.git] / perception / obstacle.cc

diff --git a/perception/obstacle.cc b/perception/obstacle.cc
index 033840e665165c4886a29a0493318ec08a95adf4..ad911f6f72ca11a1db82fab6e469564a60ffbd0a 100644 (file)
--- a/perception/obstacle.cc
+++ b/perception/obstacle.cc
@@ -38,54 +38,52 @@ bool CircleObstacle::collide(RRTNode *n)
 
 bool CircleObstacle::collide(RRTEdge *e)
 {
-        std::vector<RRTEdge *> edges;
-        edges.push_back(e);
-        return this->collide(edges);
+        // see http://doswa.com/2009/07/13/circle-segment-intersectioncollision.html
+        // Find the closest point on seq.
+        float seg_v[] = {
+                e->goal()->x() - e->init()->x(),
+                e->goal()->y() - e->init()->y()
+        };
+        float pt_v[] = {
+                this->x() - e->init()->x(),
+                this->y() - e->init()->y()
+        };
+        float seg_vl = sqrt(pow(seg_v[0], 2) + pow(seg_v[1], 2));
+        // seg_vl must be > 0 otherwise it is invalid segment length.
+        if (seg_vl <= 0)
+                return false;
+        float seg_v_unit[] = {seg_v[0] / seg_vl, seg_v[1] / seg_vl};
+        float proj = pt_v[0]*seg_v_unit[0] + pt_v[1]*seg_v_unit[1];
+        float closest[] = {0, 0};
+        if (proj <= 0) {
+                closest[0] = e->init()->x();
+                closest[1] = e->init()->y();
+        } else if (proj >= seg_vl) {
+                closest[0] = e->goal()->x();
+                closest[1] = e->goal()->y();
+        } else {
+                float proj_v[] = {seg_v_unit[0] * proj, seg_v_unit[1] * proj};
+                closest[0] = proj_v[0] + e->init()->x();
+                closest[1] = proj_v[1] + e->init()->y();
+        }
+        // Find the segment circle.
+        float dist_v[] = {this->x() - closest[0], this->y() - closest[1]};
+        float dist = sqrt(pow(dist_v[0], 2) + pow(dist_v[1], 2));
+        if (dist <= this->r())
+                return true;
+        return false;
+        // Offset computation.
+        // float offset[] = {
+        //        dist_v[0] / dist * (BCAR_TURNING_RADIUS - dist),
+        //        dist_v[1] / dist * (BCAR_TURNING_RADIUS - dist)
+        // };
 }
 
 bool CircleObstacle::collide(std::vector<RRTEdge *> &edges)
 {
-        std::vector<RRTEdge *> bedges;
-        float radi = this->r() / cos(M_PI / 4); // TODO 4 is square
-        float angl = 2 * M_PI / 4;
-        float x1;
-        float y1;
-        float x2;
-        float y2;
-        int i;
-        for (i = 0; i < 4; i++) {
-                x1 = radi * cos(i * angl);
-                y1 = radi * sin(i * angl);
-                x2 = radi * cos((i + 1) * angl);
-                y2 = radi * sin((i + 1) * angl);
-                x1 += this->x();
-                y1 += this->y();
-                x2 += this->x();
-                y2 += this->y();
-                bedges.push_back(new RRTEdge(
-                                        new RRTNode(x1, y1, 0),
-                                        new RRTNode(x2, y2, 0)));
-        }
-        for (auto &be: bedges) {
-                for (auto &e: edges) {
-                        if (SegmentObstacle(
-                                                be->init(),
-                                                be->goal())
-                                        .collide(e)) {
-                                for (auto e: bedges) {
-                                        delete e->init();
-                                        delete e->goal();
-                                        delete e;
-                                }
-                                return true;
-                        }
-                }
-        }
-        for (auto e: bedges) {
-                delete e->init();
-                delete e->goal();
-                delete e;
-        }
+        for (auto e: edges)
+                if (this->collide(e))
+                        return true;
         return false;
 }
 
@@ -127,11 +125,28 @@ bool PolygonObstacle::collide(RRTNode *n)
 
 bool PolygonObstacle::collide(RRTEdge *e)
 {
+        for (auto &f: this->frame()) {
+                if (SegmentObstacle(f->init(), f->goal()).collide(e))
+                        return true;
+        }
         return false;
 }
 
 bool PolygonObstacle::collide(std::vector<RRTEdge *> &edges)
 {
+        for (auto &e: edges) {
+                if (this->collide(e))
+                        return true;
+        }
+        return false;
+}
+
+bool PolygonObstacle::collide(std::vector<RRTEdge *> edges)
+{
+        for (auto &e: edges) {
+                if (this->collide(e))
+                        return true;
+        }
         return false;
 }
 
@@ -174,38 +189,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)