Tune sampling coordinates

[hubacji1/iamcar.git] / base / rrtbase.cc

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 3d890364669b05206b9377780c02e93e7fa853a6..e049143a1413250af6535fb6a6901e2c756c3bc4 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -889,7 +889,7 @@ RRTNode *RRTBase::sample()
                 );
                 std::normal_distribution<float> hdist(
                         0,
-                        this->samplingInfo_.h / 3
+                        this->samplingInfo_.h
                 );
                 if (!this->samplingInfo_.r) {
                         float dx = BCAR_WIDTH / 2 +
@@ -915,8 +915,16 @@ RRTNode *RRTBase::sample()
                 } 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);
+                        x += dr * cos(
+                                this->samplingInfo_.sh +
+                                this->samplingInfo_.dh * M_PI / 2 +
+                                dh
+                        );
+                        y += dr * sin(
+                                this->samplingInfo_.sh +
+                                this->samplingInfo_.dh * M_PI / 2 +
+                                dh
+                        );
                         h = -M_PI / 2 + dh;
                 }
                 return new RRTNode(