Use uniform circle sampling

diff --git a/src/rrts.cc b/src/rrts.cc
index 149ff00a5e87b3e5dfb8d4f2fb4e16953cd5e80a..f5babecccdc92da142c7ae6de0b85cdf464b4a07 100644 (file)
--- a/src/rrts.cc
+++ b/src/rrts.cc
@@ -172,12 +172,40 @@ void RRTS::sample()
         double y = 0;
         double h = 0;
         switch (this->sample_dist_type()) {
-        case 1:
+        case 1: // uniform
                 x = this->udx_(this->gen_);
                 y = this->udy_(this->gen_);
                 h = this->udh_(this->gen_);
                 break;
-        default:
+        case 2: // uniform circle
+        {
+                // see https://stackoverflow.com/questions/5837572/generate-a-random-point-within-a-circle-uniformly/50746409#50746409
+                double R = sqrt(
+                        pow(
+                                this->nodes().front().x()
+                                - this->goals().front().x(),
+                                2
+                        )
+                        + pow(
+                                this->nodes().front().y()
+                                - this->goals().front().y(),
+                                2
+                        )
+                );
+                double a = atan2(
+                        this->goals().front().y() - this->nodes().front().y(),
+                        this->goals().front().x() - this->nodes().front().x()
+                );
+                double cx = this->goals().front().x() + R/2 * cos(a);
+                double cy = this->goals().front().y() + R/2 * sin(a);
+                double r = R * sqrt(this->udx_(this->gen_));
+                double theta = this->udy_(this->gen_) * 2 * M_PI;
+                x = cx + r * cos(theta);
+                y = cy + r * sin(theta);
+                h = this->udh_(this->gen_);
+        }
+                break;
+        default: // normal
                 x = this->ndx_(this->gen_);
                 y = this->ndy_(this->gen_);
                 h = this->ndh_(this->gen_);