From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 24 Jun 2019 16:09:14 +0000 (+0200)
Subject: Add default sampling info setter
X-Git-Tag: v0.7.0~3
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/40bbfddf84eef04947c8366d31ffb77b15f46fe8

Add default sampling info setter
---

diff --git a/base/main.cc b/base/main.cc
index effd757..c327d78 100644
--- a/base/main.cc
+++ b/base/main.cc
@@ -191,6 +191,7 @@ int main()
                         if (tmps > p.VMAX) p.VMAX = tmps;
                 }
         }
+        p.defaultSamplingInfo();
         p.link_obstacles(&co, &so);
         p.ocost(p.root());
         p.ocost(p.goal());
diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 5bb1385..bf366dd 100644
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -1060,6 +1060,17 @@ int RRTBase::YI(RRTNode *n)
 }
 
 // RRT Framework
+void RRTBase::defaultSamplingInfo()
+{
+        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);
+}
+
 void RRTBase::setSamplingInfo(SamplingInfo si)
 {
         this->ndx_ = std::normal_distribution<float>(si.x0, si.x);
diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index 44dc06d..9b17d44 100644
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -181,6 +181,7 @@ class RRTBase {
                 int YI(RRTNode *n);
 
                 // RRT Framework
+                void defaultSamplingInfo();
                 void setSamplingInfo(SamplingInfo si);
                 RRTNode *sample();
                 float cost(RRTNode *init, RRTNode *goal);