Add cumulative cost function

diff --git a/api/rrts.h b/api/rrts.h
index 2e7f53da46b9fd483edccfca7de43e50a72c6a77..427f6e1cd0968e2d6d4c50764bee53ce203626ae 100644 (file)
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -91,4 +91,10 @@ class RRTS {
                 RRTS();
 };
 
+/*! \brief Compute cumulative cost of RRT node.
+
+\param t RRT node to compute cumulative cost to.
+*/
+double cc(RRTNode &t);
+
 #endif /* RRTS_H */

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;
+}

diff --git a/ut/rrts.t.cc b/ut/rrts.t.cc
index a872cb0a1ab7de696eac0fa4072b6319988fe927..73b161a5a697f1a3caeee43b2eb35b10ce08fa46 100644 (file)
--- a/ut/rrts.t.cc
+++ b/ut/rrts.t.cc
@@ -12,6 +12,7 @@ WVTEST_MAIN("RRT node basic tests")
 WVTEST_MAIN("RRT* basic tests")
 {
         RRTS rrts;
+        WVPASSEQ_DOUBLE(cc(rrts.nodes().front()), 0, 0.00001);
         WVPASSEQ(rrts.nodes().size(), 1);
         rrts.next();
         WVPASSEQ(rrts.nodes().size(), 2);