Add cumulative cost function

[hubacji1/rrts.git] / src / rrts.cc

diff --git a/src/rrts.cc b/src/rrts.cc
index 3968da41fb18f4408473db28069351afb3f423b5..7ded4624d7d3a1e1f6b607ab497a2cfe3e33f93c 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -151,3 +151,14 @@ RRTS::RRTS()
 {
         this->nodes().push_back(RRTNode()); // root
 }
+
+double cc(RRTNode &t)
+{
+        RRTNode *n = &t;
+        double cost = 0;
+        while (n != nullptr) {
+                cost += n->c();
+                n = n->p();
+        }
+        return cost;
+}