FIX 85aee31

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 516504ef3a6fff99e4a81825acdbe469279ef75c..97e4a5f1d354eff8d72141dfdef8b653c5e2f99d 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -95,28 +95,36 @@ RRTBase::RRTBase()
         : root_(new RRTNode())
         , goal_(new RRTNode())
         , gen_(std::random_device{}())
-        , ndx_(HMAX - HMIN, (HMAX - HMIN) / 4)
-        , ndy_(VMAX - VMIN, (VMAX - VMIN) / 4)
-        , ndh_(0, M_PI * 2 / 4)
 {
         this->nodes_.reserve(NOFNODES);
         this->nodes_.push_back(this->root_);
         this->add_iy(this->root_);
         this->add_ixy(this->root_);
+        float hcenter = (this->HMAX - this->HMIN) / 2 + this->HMIN;
+        float hrange = (this->HMAX - this->HMIN) / 2;
+        float vcenter = (this->VMAX - this->VMIN) / 2 + this->VMIN;
+        float vrange = (this->VMAX - this->VMIN) / 2;
+        this->ndx_ = std::normal_distribution<float>(hcenter, hrange);
+        this->ndy_ = std::normal_distribution<float>(vcenter, vrange);
+        this->ndh_ = std::normal_distribution<float>(0, 2 * M_PI);
 }
 
 RRTBase::RRTBase(RRTNode *init, RRTNode *goal)
         : root_(init)
         , goal_(goal)
         , gen_(std::random_device{}())
-        , ndx_(HMIN + (HMAX - HMIN) / 2, (HMAX - HMIN) / 4)
-        , ndy_(VMIN + (VMAX - VMIN) / 2, (VMAX - VMIN) / 4)
-        , ndh_(0, M_PI * 2 / 4)
 {
         this->nodes_.reserve(NOFNODES);
         this->nodes_.push_back(init);
         this->add_iy(init);
         this->add_ixy(init);
+        float hcenter = (this->HMAX - this->HMIN) / 2 + this->HMIN;
+        float hrange = (this->HMAX - this->HMIN) / 2;
+        float vcenter = (this->VMAX - this->VMIN) / 2 + this->VMIN;
+        float vrange = (this->VMAX - this->VMIN) / 2;
+        this->ndx_ = std::normal_distribution<float>(hcenter, hrange);
+        this->ndy_ = std::normal_distribution<float>(vcenter, vrange);
+        this->ndh_ = std::normal_distribution<float>(0, 2 * M_PI);
 }
 
 // getter