Add ext16

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

diff --git a/incl/rrtext.hh b/incl/rrtext.hh
index 79c5ad3d0a889165aea30541fccca62bb46f8028..d6f3a4f12e7d4d6a0129f1184892fbfebe1a1089 100644 (file)
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -18,6 +18,11 @@
 
 namespace rrts {
 
+class RRTExt16 : public virtual RRTS {
+private:
+       void steer(RRTNode const& f, RRTNode const& t);
+};
+
 class RRTExt15 : public virtual RRTS {
 private:
        std::vector<double> log_path_cost_;

diff --git a/src/rrtext16.cc b/src/rrtext16.cc
new file mode 100644 (file)
index 0000000..fae21d4
--- /dev/null
+++ b/src/rrtext16.cc
@@ -0,0 +1,28 @@
+#include "reeds_shepp.h"
+#include "rrtext.hh"
+
+namespace rrts {
+
+static int
+cb_steer(double q[4], void *w)
+{
+       std::vector<RRTNode>* st = (std::vector<RRTNode>*) w;
+       st->push_back(RRTNode());
+       st->back().x(q[0]);
+       st->back().y(q[1]);
+       st->back().h(q[2]);
+       st->back().sp(q[3]);
+       return 0;
+}
+
+void
+RRTExt16::steer(RRTNode const& f, RRTNode const& t)
+{
+       this->steered_.clear();
+       double q0[] = {f.x(), f.y(), f.h()};
+       double q1[] = {t.x(), t.y(), t.h()};
+       ReedsSheppStateSpace rsss(this->bc_.mtr());
+       rsss.sample(q0, q1, this->eta_, cb_steer, &this->steered_);
+}
+
+} // namespace rrts