From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Thu, 22 Jul 2021 15:26:26 +0000 (+0200)
Subject: Add ext14
X-Git-Tag: v0.8.0~1^2~13
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/rrts.git/commitdiff_plain/b2d21183d42c1d310afd818b52f4c7731779f3fb

Add ext14
---

diff --git a/CMakeLists.txt b/CMakeLists.txt
index d9903e0..4d34736 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,6 +17,7 @@ link_libraries(jsoncpp_lib)
 
 add_library(rrts STATIC
 	src/rrts.cc
+	src/rrtext14.cc
 	src/rrtext2.cc
 	src/reeds_shepp.cpp
 )
diff --git a/README.md b/README.md
index 4fdfd97..f914b73 100644
--- a/README.md
+++ b/README.md
@@ -49,6 +49,7 @@ number accomply to file `src/rrtextN.cc` where `N` is:
 11. "goal zone" gz -- Use drivable of libbcar to check if goal found.
 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)
 
 [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 6d8679a..5f5e11b 100644
--- a/incl/rrtext.hh
+++ b/incl/rrtext.hh
@@ -16,6 +16,25 @@
 // ext9
 #define GRID_MAX_HI 60
 
+namespace rrts {
+
+/*! \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
+ */
+class RRTExt14 : public virtual RRTS {
+private:
+	Point circle_c_;
+	double circle_r_ = 0.0;
+	std::uniform_real_distribution<double> udr_;
+	std::uniform_real_distribution<double> udt_;
+	std::uniform_real_distribution<double> udh_;
+	RRTNode sample();
+public:
+	RRTExt14();
+	void reset();
+};
+
 /*! Use Dijkstra-based path optimization, goal zone for interesting nodes. */
 class RRTExt13 : public virtual RRTS {
 	private:
diff --git a/src/rrtext14.cc b/src/rrtext14.cc
new file mode 100644
index 0000000..f833c9d
--- /dev/null
+++ b/src/rrtext14.cc
@@ -0,0 +1,41 @@
+#include "rrtext.hh"
+
+namespace rrts {
+
+RRTNode
+RRTExt14::sample()
+{
+	if (this->circle_r_ == 0.0) {
+		RRTNode& f = this->nodes_.front();
+		RRTNode& g = this->goal_;
+		double dx = g.x() - f.x();
+		double dy = g.y() - f.y();
+		this->circle_r_ = sqrt(dx * dx + dy * dy);
+		this->circle_c_.x((f.x() + g.x()) / 2.0);
+		this->circle_c_.y((f.y() + g.y()) / 2.0);
+		return this->goal_;
+	}
+	double r = this->circle_r_ * sqrt(this->udr_(this->gen_));
+	double theta = this->udt_(this->gen_);
+	RRTNode rs;
+	rs.x(this->circle_c_.x() + r * cos(theta));
+	rs.y(this->circle_c_.y() + r * sin(theta));
+	rs.h(this->udh_(this->gen_));
+	return rs;
+}
+
+RRTExt14::RRTExt14() : RRTS()
+{
+	this->udr_ = std::uniform_real_distribution<double>(0.0, 1.0);
+	this->udt_ = std::uniform_real_distribution<double>(0.0, 2.0 * M_PI);
+	this->udh_ = std::uniform_real_distribution<double>(0.0, 2.0 * M_PI);
+}
+
+void
+RRTExt14::reset()
+{
+	RRTS::reset();
+	this->circle_r_ = 0.0;
+}
+
+} // namespace rrts