From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 11 Jun 2019 13:56:32 +0000 (+0200)
Subject: Use normal distribution in sampling
X-Git-Tag: v0.7.0~14
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/2572a78b37fcedf1ce2659f9eeab89dcd44c5f8b?hp=d287e0fe594fd352808de47d27d6101c539fb50f

Use normal distribution in sampling
---

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index d8299d2..ed41b91 100644
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -95,6 +95,9 @@ 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_);
@@ -106,6 +109,9 @@ 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);
         }
 }
 
diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index 4ece3a7..f83be79 100644
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -80,8 +80,11 @@ class RRTBase {
                 std::vector<float> slog_; // seconds of trajectories
                 std::vector<std::vector<RRTNode *>> tlog_; // trajectories
                 std::vector<RRTNode *> slot_cusp_; // cusp nodes in slot
-
+        protected:
                 std::default_random_engine gen_;
+                std::normal_distribution<float> ndx_;
+                std::normal_distribution<float> ndy_;
+                std::normal_distribution<float> ndh_;
         public:
                 const float GOAL_FOUND_DISTANCE = 0.2;
                 const float GOAL_FOUND_ANGLE = M_PI / 32;
diff --git a/incl/sample.h b/incl/sample.h
index 3d65f84..4c7c3e7 100644
--- a/incl/sample.h
+++ b/incl/sample.h
@@ -20,10 +20,10 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 
 #include "rrtnode.h"
 
-#define HMIN 0 // horizontal
-#define HMAX 26 // horizontal
-#define VMIN -5.3 // vertical
-#define VMAX 5.5 // vertical
+#define HMIN -20 // horizontal
+#define HMAX 20 // horizontal
+#define VMIN -5 // vertical
+#define VMAX 30 // vertical
 
 // OpenGL window size
 #define SCREEN_WIDTH 1000