Add polygon obstacle

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 6e0cc4c2a20963eca81d8fdd60eeccf858219288..4709c8e71c446da727e60088fa2a3818d99d3cb1 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -124,6 +124,11 @@ std::vector<RRTNode *> &RRTBase::dnodes()
         return this->dnodes_;
 }
 
+PolygonObstacle &RRTBase::frame()
+{
+        return this->frame_;
+}
+
 std::vector<RRTNode *> &RRTBase::samples()
 {
         return this->samples_;

diff --git a/incl/obstacle.h b/incl/obstacle.h
index 1ccd25f859ec4aaef692c37cab02d3f18369463b..ec44601185823b1940569b355e776b7e0024456d 100644 (file)
--- a/incl/obstacle.h
+++ b/incl/obstacle.h
@@ -39,6 +39,20 @@ class CircleObstacle : public RRTNode, Obstacle {
                 float dist_to(RRTNode *n);
 };
 
+class PolygonObstacle : public Obstacle {
+        private:
+                std::vector<RRTNode *> bnodes_;
+        public:
+                void add_bnode(RRTNode *n);
+                bool collide(RRTNode *n);
+                bool collide(RRTEdge *e);
+                bool collide(std::vector<RRTEdge *> &edges);
+                float dist_to(RRTNode *n);
+
+                // getter
+                std::vector<RRTNode *> &bnodes();
+};
+
 class SegmentObstacle : public RRTEdge, Obstacle {
         public:
                 using RRTEdge::RRTEdge;

diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index 7a5c9323bea93d6c12b8eaf630bf774cd543e084..4a67e9a2bea6247449666ead68a4a4a68d776e5e 100644 (file)
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -60,6 +60,7 @@ class RRTBase {
 
                 std::vector<RRTNode *> nodes_;
                 std::vector<RRTNode *> dnodes_;
+                PolygonObstacle frame_;
                 std::vector<RRTNode *> samples_;
                 std::vector<CircleObstacle> *cobstacles_;
                 std::vector<SegmentObstacle> *sobstacles_;
@@ -86,6 +87,7 @@ class RRTBase {
                 RRTNode *goal();
                 std::vector<RRTNode *> &nodes();
                 std::vector<RRTNode *> &dnodes();
+                PolygonObstacle &frame();
                 std::vector<RRTNode *> &samples();
                 std::vector<RRTNode *> iy_[IYSIZE];
                 Cell ixy_[IXSIZE][IYSIZE];

diff --git a/perception/obstacle.cc b/perception/obstacle.cc
index 2fc92e621656d8174cfaa5ac3bf727a7ce4da4af..3bdec670b39102a6b144df8226392cf475241e68 100644 (file)
--- a/perception/obstacle.cc
+++ b/perception/obstacle.cc
@@ -98,6 +98,53 @@ float CircleObstacle::dist_to(RRTNode *n)
         return (float) (sqrt(xx + yy) - this->r());
 }
 
+void PolygonObstacle::add_bnode(RRTNode *n)
+{
+        this->bnodes_.push_back(n);
+}
+
+bool PolygonObstacle::collide(RRTNode *n)
+{
+        // From substack/point-in-polygon, see
+        // https://github.com/substack/point-in-polygon/blob/master/index.js
+        bool inside = false;
+        unsigned int i;
+        int j;
+        for (i = 0, j = this->bnodes_.size() - 1;
+                        i < this->bnodes_.size();
+                        j = i++) {
+                float xi = this->bnodes_[i]->x();
+                float yi = this->bnodes_[i]->y();
+                float xj = this->bnodes_[j]->x();
+                float yj = this->bnodes_[j]->y();
+                bool intersect = ((yi > n->y()) != (yj > n->y())) &&
+                        (n->x() < (xj - xi) * (n->y() - yi) / (yj - yi) + xi);
+                if (intersect)
+                        inside = !inside;
+        }
+        return inside;
+}
+
+bool PolygonObstacle::collide(RRTEdge *e)
+{
+        return false;
+}
+
+bool PolygonObstacle::collide(std::vector<RRTEdge *> &edges)
+{
+        return false;
+}
+
+float PolygonObstacle::dist_to(RRTNode *n)
+{
+        return 0;
+}
+
+std::vector<RRTNode *> &PolygonObstacle::bnodes()
+{
+        return this->bnodes_;
+}
+
 bool SegmentObstacle::collide(RRTNode *n)
 {
         return false;