Use normal distribution in sampling

[hubacji1/iamcar.git] / base / rrtbase.cc

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index bc031bcd6c6db1ab67664590d8751c67ef2f52ab..ed41b9185db661f3cdd4897b8591c81fa9678508 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -91,10 +91,13 @@ RRTBase::~RRTBase()
         delete this->goal_;
 }
 
-RRTBase::RRTBase():
-        root_(new RRTNode()),
-        goal_(new RRTNode()),
-        gen_(std::random_device{}())
+RRTBase::RRTBase()
+        : root_(new RRTNode())
+        , goal_(new RRTNode())
+        , gen_(std::random_device{}())
+        , ndx_(HMAX - HMIN, (HMAX - HMIN) / 4)
+        , ndy_(VMAX - VMIN, (VMAX - VMIN) / 4)
+        , ndh_(0, M_PI * 2 / 4)
 {
         this->nodes_.reserve(NOFNODES);
         this->nodes_.push_back(this->root_);
@@ -102,10 +105,13 @@ RRTBase::RRTBase():
         this->add_ixy(this->root_);
 }
 
-RRTBase::RRTBase(RRTNode *init, RRTNode *goal):
-        root_(init),
-        goal_(goal),
-        gen_(std::random_device{}())
+RRTBase::RRTBase(RRTNode *init, RRTNode *goal)
+        : root_(init)
+        , goal_(goal)
+        , gen_(std::random_device{}())
+        , ndx_(HMIN + (HMAX - HMIN) / 2, (HMAX - HMIN) / 4)
+        , ndy_(VMIN + (VMAX - VMIN) / 2, (VMAX - VMIN) / 4)
+        , ndh_(0, M_PI * 2 / 4)
 {
         this->nodes_.reserve(NOFNODES);
         this->nodes_.push_back(init);
@@ -139,6 +145,11 @@ std::vector<RRTNode *> &RRTBase::dnodes()
         return this->dnodes_;
 }
 
+std::queue<RRTNode *> &RRTBase::firsts()
+{
+        return this->firsts_;
+}
+
 PolygonObstacle &RRTBase::frame()
 {
         return this->frame_;
@@ -236,6 +247,14 @@ void RRTBase::goal(RRTNode *node)
 void RRTBase::goals(std::vector<RRTNode *> g)
 {
         this->goals_ = g;
+        std::reverse(this->goals_.begin(), this->goals_.end());
+        RRTNode *pn = this->goals_.front();
+        for (auto n: this->goals_) {
+                if (n != pn) {
+                        pn->add_child(n, this->cost(pn ,n));
+                        pn = n;
+                }
+        }
 }
 
 bool RRTBase::logr(RRTNode *root)
@@ -486,6 +505,76 @@ bool RRTBase::goal_found(
                                 return false;
                         }
                         this->goal_found_ = true;
+                        // Update ccost of goal's parents
+                        if (this->goals().size() > 0) {
+                                RRTNode *ch = this->goals().back();
+                                RRTNode *pn = this->goals().back()->parent();
+                                while (pn) {
+                                        pn->ccost(
+                                                ch->ccost()
+                                                - this->cost(pn, ch)
+                                        );
+                                        ch = pn;
+                                        pn = pn->parent();
+                                }
+                        }
+                        return true;
+                }
+        }
+        return false;
+}
+
+bool RRTBase::goal_found(
+        RRTNode *node,
+        RRTNode *goal
+)
+{
+        if (IS_NEAR(node, goal)) {
+                if (this->goal_found_) {
+                        if (
+                                goal->ccost() != -1
+                                && node->ccost() + this->cost(node, goal)
+                                < goal->ccost()
+                        ) {
+                                RRTNode *op; // old parent
+                                float oc; // old cumulative cost
+                                float od; // old direct cost
+                                op = goal->parent();
+                                oc = goal->ccost();
+                                od = goal->dcost();
+                                node->add_child(goal,
+                                                this->cost(node, goal));
+                                if (this->collide(node, goal)) {
+                                        node->children().pop_back();
+                                        goal->parent(op);
+                                        goal->ccost(oc);
+                                        goal->dcost(od);
+                                } else {
+                                        op->rem_child(goal);
+                                        return true;
+                                }
+                        } else {
+                                return false;
+                        }
+                } else {
+                        node->add_child(
+                                goal,
+                                this->cost(node, goal)
+                        );
+                        if (this->collide(node, goal)) {
+                                node->children().pop_back();
+                                goal->remove_parent();
+                                return false;
+                        }
+                        this->goal_found_ = true;
+                        // Update ccost of goal's children
+                        goal->update_ccost();
+                        // Update ccost of goals
+                        for (auto g: this->goals()) {
+                                if (g == goal)
+                                        break;
+                                g->ccost(-1);
+                        }
                         return true;
                 }
         }
@@ -918,7 +1007,10 @@ RRTNode *RRTBase::sample()
                 h += this->samplingInfo_.h0;
                 return new RRTNode(x, y, h);
         } else {
-                return sa1();
+                float x = this->ndx_(this->gen_);
+                float y = this->ndy_(this->gen_);
+                float h = this->ndh_(this->gen_);
+                return new RRTNode(x, y, h);
         }
 }