Rewrite circle with line segments collide method

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

diff --git a/perception/obstacle.cc b/perception/obstacle.cc
index 03a0d8b3984ecb9dce40771f7cf27a885b3ccd8c..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;
 }