Add uniform int distribution sampling

diff --git a/api/rrts.h b/api/rrts.h
index cd44b09b210b5e22dc62ca0bd0bffff9fa6685d1..32fc3fb9338eb373111240b14c2e9d36a8ddc2c9 100644 (file)
--- a/api/rrts.h
+++ b/api/rrts.h
@@ -191,6 +191,7 @@ class RRTS {
                         std::uniform_real_distribution<double> udx_;
                         std::uniform_real_distribution<double> udy_;
                         std::uniform_real_distribution<double> udh_;
+                        std::uniform_int_distribution<unsigned int> udi_;
                 virtual RRTNode *nn(RRTNode &t);
                 virtual std::vector<RRTNode *> nv(RRTNode &t);
                 void steer(RRTNode &f, RRTNode &t);

diff --git a/src/rrts.cc b/src/rrts.cc
index bca083429b1ed5c5a96ec4ca7ed99c3c7ba32557..3f23f3dd454ddeab2592ee2ea166c45f9424c5b2 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -211,6 +211,18 @@ void RRTS::sample()
                 h = this->udh_(this->gen_);
         }
                 break;
+        case 3: {
+                this->udi_ = std::uniform_int_distribution<unsigned int>(
+                        0,
+                        this->nodes().size() - 1
+                );
+                auto ind = this->udi_(this->gen_);
+                auto n = this->nodes()[ind];
+                x = n.x();
+                y = n.y();
+                h = n.h();
+                break;
+                }
         default: // normal
                 x = this->ndx_(this->gen_);
                 y = this->ndy_(this->gen_);
@@ -472,6 +484,9 @@ void RRTS::set_sample(
         case 2: // uniform circle
                 this->set_sample_uniform_circle();
                 break;
+        case 3: // uniform index of node in nodes
+                this->set_sample_uniform_circle();
+                break;
         default: // normal
                 this->set_sample_normal(x1, x2, y1, y2, h1, h2);
         }