Use normal distribution in sampling

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index d8299d2aaaef9e7d3e4fb83cec3e0674ff376fa0..ed41b9185db661f3cdd4897b8591c81fa9678508 100644 (file)
--- 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 4ece3a7ab98358e2f9f94c6f86ef93c28a4d0815..f83be79cfaa4751a91145969a047bdf1906d1fe1 100644 (file)
--- 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 3d65f84ddd0e89073115294ab77fd7ec1482de5e..4c7c3e743896c43155d9f007450d2662e6aa52e7 100644 (file)
--- 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