Rewrite circle with line segments collide method

diff --git a/perception/obstacle.cc b/perception/obstacle.cc
index 43ae78e0ec06224bf797e2f85769a8466b82b768..ad911f6f72ca11a1db82fab6e469564a60ffbd0a 100644 (file)
--- 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;
 }