Add PolygonObstacle frame method

diff --git a/incl/obstacle.h b/incl/obstacle.h
index ec44601185823b1940569b355e776b7e0024456d..0a7f2c6e0a0d859e2ee07bdf21ae8066d57cd0fa 100644 (file)
--- a/incl/obstacle.h
+++ b/incl/obstacle.h
@@ -48,6 +48,7 @@ class PolygonObstacle : public Obstacle {
                 bool collide(RRTEdge *e);
                 bool collide(std::vector<RRTEdge *> &edges);
                 float dist_to(RRTNode *n);
+                std::vector<RRTEdge *> frame();
 
                 // getter
                 std::vector<RRTNode *> &bnodes();

diff --git a/perception/obstacle.cc b/perception/obstacle.cc
index 3bdec670b39102a6b144df8226392cf475241e68..033840e665165c4886a29a0493318ec08a95adf4 100644 (file)
--- a/perception/obstacle.cc
+++ b/perception/obstacle.cc
@@ -140,6 +140,28 @@ float PolygonObstacle::dist_to(RRTNode *n)
         return 0;
 }
 
+std::vector<RRTEdge *> PolygonObstacle::frame()
+{
+        std::vector<RRTEdge *> frame;
+        unsigned int i;
+        int j;
+        for (i = 1, j = 0; i < this->bnodes_.size(); j = i++) {
+                frame.push_back(new RRTEdge(
+                        new RRTNode(
+                                this->bnodes_[j]->x(),
+                                this->bnodes_[j]->y(),
+                                this->bnodes_[j]->h()
+                        ),
+                        new RRTNode(
+                                this->bnodes_[i]->x(),
+                                this->bnodes_[i]->y(),
+                                this->bnodes_[i]->h()
+                        )
+                ));
+        }
+        return frame;
+}
+
 std::vector<RRTNode *> &PolygonObstacle::bnodes()
 {
         return this->bnodes_;