Add ext14

diff --git a/CMakeLists.txt b/CMakeLists.txt
index d9903e077dac0e6c1fed1c46a03c7cc9797bca0c..4d34736738fef69270ad1ace1241de456c948e40 100644 (file)
--- 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 4fdfd97c71f7d5fa850b3fcb50883d12c4ed2101..f914b737776f5df324b0516d6a8cbab97140fbaf 100644 (file)
--- 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 6d8679aa77f3ea42d1df4101fc8ac6804c183c5a..5f5e11bc869651a6ba22b9d14bc23f91e360bd4a 100644 (file)
--- 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 (file)
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