From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Wed, 19 Jun 2019 07:02:02 +0000 (+0200)
Subject: Rewrite circle with line segments collide method
X-Git-Tag: v0.7.0~7^2~3
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/9980204c550ce082b90d897e7c554a4595fa3239

Rewrite circle with line segments collide method
---

diff --git a/perception/obstacle.cc b/perception/obstacle.cc
index 43ae78e..ad911f6 100644
--- a/perception/obstacle.cc
+++ b/perception/obstacle.cc
@@ -81,47 +81,9 @@ bool CircleObstacle::collide(RRTEdge *e)
 
 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;
 }