Add ext15

diff --git a/CMakeLists.txt b/CMakeLists.txt
index f981dfe2e19aa6dfae6803e3834f8296cbe9c1aa..af8fa2197110282449e7338f3d8da7e93b34ce3c 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,6 +17,7 @@ link_libraries(jsoncpp_lib)
 
 add_library(rrts STATIC
        src/rrts.cc
+       src/rrtext15.cc
        src/rrtext14.cc
        src/rrtext13.cc
        src/rrtext10.cc

diff --git a/README.md b/README.md
index f914b737776f5df324b0516d6a8cbab97140fbaf..13e581d140bfd632440701d1d3627eab09c3e6e2 100644 (file)
--- a/README.md
+++ b/README.md
@@ -50,6 +50,7 @@ number accomply to file `src/rrtextN.cc` where `N` is:
 12. "steer" -- Use random control input for `steer1`, use R&S for `steer2`.
 13. "path optimization" -- Dijkstra algorithm, goal zone for interesting nodes.
 14. "sampling" -- uniform sampling in circle between init, goal (rad = edist)
+15. "log" -- log goal cc each iteration.
 
 [cute c2]: https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
 [K-d tree]: https://en.wikipedia.org/wiki/K-d_tree

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 3469c08edcd48e05a8c997db00690ea2f54d5160..79c5ad3d0a889165aea30541fccca62bb46f8028 100644 (file)
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -18,6 +18,15 @@
 
 namespace rrts {
 
+class RRTExt15 : public virtual RRTS {
+private:
+       std::vector<double> log_path_cost_;
+public:
+       Json::Value json() const;
+       void json(Json::Value jvi);
+       bool next();
+};
+
 /*! \brief Random sampling in the circuit between root and goal.
  *
  * \see https://stackoverflow.com/questions/5837572/generate-a-random-point-within-a-circle-uniformly/50746409#50746409

diff --git a/src/rrtext15.cc b/src/rrtext15.cc
new file mode 100644 (file)
index 0000000..c4b8fd8
--- /dev/null
+++ b/src/rrtext15.cc
@@ -0,0 +1,40 @@
+#include "rrtext.hh"
+
+namespace rrts {
+
+Json::Value
+RRTExt15::json() const
+{
+       auto jvo = RRTS::json();
+       unsigned int i = 0;
+       for (auto c: this->log_path_cost_) {
+               jvo["log_path_cost"][i++] = c;
+       }
+       return jvo;
+}
+
+void
+RRTExt15::json(Json::Value jvi)
+{
+       RRTS::json(jvi);
+}
+
+bool
+RRTExt15::next()
+{
+       bool c = RRTS::next();
+       if (this->log_path_cost_.size() == 0) {
+               this->log_path_cost_.push_back(this->goal_.cc());
+       } else {
+               double lc = this->log_path_cost_.back();
+               double gc = this->goal_.cc();
+               if (gc > 0.0 && (lc == 0.0 || gc < lc)) {
+                       this->log_path_cost_.push_back(gc);
+               } else {
+                       this->log_path_cost_.push_back(lc);
+               }
+       }
+       return c;
+}
+
+} // namespace rrts