Add RRTNode's update cost method

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 4ae2496b81b5eacd81fa8a6fc7209e58216ac9a5..019d87d8787d69ce8ddd44cafecbd5f27a7523ae 100644 (file)
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -24,6 +24,8 @@ The format is based on [Keep a Changelog][] and this project adheres to
 - OpenGL 2.1 plot using SDL2.
 - `nn4` procedure - set node's heading to heading of currently comparing node.
 - Add optimize path method based on [Islam2012]'s *Path Optimize* procedure.
+- Update cost method of `RRTNode` that updates cumulative costs of all
+  children.
 
 ### Changed
 - Build with Ninja.

diff --git a/base/rrtnode.cc b/base/rrtnode.cc
index ff9d006cca675165b0951124a934e5e54e7f475f..f721cc87b45a4555c92136e28efca615762dfb95 100644 (file)
--- a/base/rrtnode.cc
+++ b/base/rrtnode.cc
@@ -198,6 +198,28 @@ bool RRTNode::comp_ccost(RRTNode *n1, RRTNode *n2)
         return n1->ccost() < n2->ccost();
 }
 
+float RRTNode::update_ccost()
+{
+        std::vector<RRTNode *> s; // DFS stack
+        std::vector<RRTNode *> r; // reset visited_
+        RRTNode *tmp;
+        s.push_back(this);
+        while (s.size() > 0) {
+                tmp = s.back();
+                s.pop_back();
+                if (!tmp->visit()) {
+                        r.push_back(tmp);
+                        for (auto ch: tmp->children()) {
+                                ch->ccost(tmp->ccost() + ch->dcost());
+                                s.push_back(ch);
+                        }
+                }
+        }
+        for (auto n: r)
+                n->visit(false);
+        return tmp->ccost();
+}
+
 bool RRTNode::visit()
 {
         if (this->visited_) {

diff --git a/incl/rrtnode.h b/incl/rrtnode.h
index 8a06bbb0a4284a4d057cd7bf92db7aed8238ed24..1ab68932c69c6504708fa456face7f5983b8d9ff 100644 (file)
--- a/incl/rrtnode.h
+++ b/incl/rrtnode.h
@@ -87,6 +87,7 @@ class RRTNode {
 
                 // other
                 static bool comp_ccost(RRTNode *n1, RRTNode *n2);
+                float update_ccost();
                 bool visit();
 };