Tune sampling randomizing

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index b4bf553ac1164342c4028df57a5c48a35ef977d0..0f2461c0265ee0bfb8e141df34761ecaea88dc08 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -884,31 +884,45 @@ RRTNode *RRTBase::sample()
         ) {
                 float x = this->samplingInfo_.x;
                 float y = this->samplingInfo_.y;
+                float h = 0;
                 std::normal_distribution<float> xdist(
                         0,
                         (this->samplingInfo_.r)
                                 ?this->samplingInfo_.r
-                                :BCAR_WIDTH / 3
+                                :BCAR_WIDTH * 2
                 );
-                x += this->samplingInfo_.dx
-                        * (BCAR_WIDTH / 2 + std::abs(xdist(this->gen_)))
-                        * cos(this->samplingInfo_.sh + M_PI / 2);
-                y += this->samplingInfo_.dx
-                        * (BCAR_WIDTH / 2 + std::abs(xdist(this->gen_)))
-                        * sin(this->samplingInfo_.sh + M_PI / 2);
-                std::normal_distribution<float> ydist(
-                        0,
-                        (this->samplingInfo_.r)
-                                ?this->samplingInfo_.r
-                                :BCAR_LENGTH / 3
-                );
-                x += ydist(this->gen_) * cos(this->samplingInfo_.sh);
-                y += ydist(this->gen_) * sin(this->samplingInfo_.sh);
                 std::normal_distribution<float> hdist(
                         0,
                         this->samplingInfo_.h / 3
                 );
-                float h = std::abs(hdist(this->gen_));
+                if (!this->samplingInfo_.r) {
+                        float dx = BCAR_WIDTH / 2 +
+                                std::abs(xdist(this->gen_));
+                        x += dx * cos(
+                                this->samplingInfo_.sh +
+                                this->samplingInfo_.dx * M_PI / 2
+                        );
+                        y += dx * cos(
+                                this->samplingInfo_.sh +
+                                this->samplingInfo_.dx * M_PI / 2
+                        );
+                        std::normal_distribution<float> ydist(
+                                0,
+                                (this->samplingInfo_.r)
+                                        ?this->samplingInfo_.r
+                                        :BCAR_LENGTH / 3
+                        );
+                        float dy = ydist(this->gen_);
+                        x += dy * cos(this->samplingInfo_.sh);
+                        y += dy * sin(this->samplingInfo_.sh);
+                        h = std::abs(hdist(this->gen_));
+                } else {
+                        float dr = std::abs(xdist(this->gen_));
+                        float dh = hdist(this->gen_);
+                        x += dr * cos(this->samplingInfo_.sh + M_PI / 2 + dh);
+                        y += dr * sin(this->samplingInfo_.sh + M_PI / 2 + dh);
+                        h = -M_PI / 2 + dh;
+                }
                 return new RRTNode(
                         x,
                         y,