Use normal distribution in sampling

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

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 5768286c8f3c28c665980fdea2c623aed1727da2..ed41b9185db661f3cdd4897b8591c81fa9678508 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -91,10 +91,13 @@ RRTBase::~RRTBase()
         delete this->goal_;
 }
 
-RRTBase::RRTBase():
-        root_(new RRTNode()),
-        goal_(new RRTNode()),
-        gen_(std::random_device{}())
+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_);
@@ -102,10 +105,13 @@ RRTBase::RRTBase():
         this->add_ixy(this->root_);
 }
 
-RRTBase::RRTBase(RRTNode *init, RRTNode *goal):
-        root_(init),
-        goal_(goal),
-        gen_(std::random_device{}())
+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);
@@ -1001,7 +1007,10 @@ RRTNode *RRTBase::sample()
                 h += this->samplingInfo_.h0;
                 return new RRTNode(x, y, h);
         } else {
-                return sa1();
+                float x = this->ndx_(this->gen_);
+                float y = this->ndy_(this->gen_);
+                float h = this->ndh_(this->gen_);
+                return new RRTNode(x, y, h);
         }
 }