From 67531a5a81a2817b980f229639d39d471267041a Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Fri, 23 Jul 2021 11:02:23 +0200
Subject: [PATCH] Add ext16

---
 CMakeLists.txt  |  1 +
 incl/rrtext.hh  |  5 +++++
 src/rrtext16.cc | 28 ++++++++++++++++++++++++++++
 3 files changed, 34 insertions(+)
 create mode 100644 src/rrtext16.cc

diff --git a/CMakeLists.txt b/CMakeLists.txt
index af8fa21..882a8ff 100644
--- 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 79c5ad3..d6f3a4f 100644
--- 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
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
-- 
2.39.2