From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 11 Jun 2019 14:30:54 +0000 (+0200)
Subject: Use sampling info for setting up the sampling
X-Git-Tag: v0.7.0~12
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/8689fd2366da2ad65e16ed071f82eefe06577842

Use sampling info for setting up the sampling

But comment it for now as the values used in sampling info don't
correspond to needed values.
---

diff --git a/base/main.cc b/base/main.cc
index 614709a..3b1751d 100644
--- a/base/main.cc
+++ b/base/main.cc
@@ -196,8 +196,7 @@ int main()
 #endif
         if (ps.slot().bnodes().size() > 0) {
                 ps.setAll();
-                p.samplingInfo_ = ps.getSamplingInfo();
-                p.useSamplingInfo_ = true;
+                //p.setSamplingInfo(ps.getSamplingInfo());
         }
         if (ps.cusp().size() > 0) {
                 p.goal(ps.getMidd());
diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index ed41b91..9600bb4 100644
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -990,28 +990,19 @@ std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
 }
 
 // RRT Framework
+void RRTBase::setSamplingInfo(SamplingInfo si)
+{
+        this->ndx_ = std::normal_distribution<float>(si.x0, si.x);
+        this->ndy_ = std::normal_distribution<float>(si.y0, si.y);
+        this->ndh_ = std::normal_distribution<float>(si.h0, si.h);
+}
+
 RRTNode *RRTBase::sample()
 {
-        if (this->useSamplingInfo_ && this->nodes().size() % 2 == 0) {
-                float x = static_cast<float>(rand());
-                x /= static_cast<float>(RAND_MAX / this->samplingInfo_.x);
-                x -= this->samplingInfo_.x / 2;
-                x += this->samplingInfo_.x0;
-                float y = static_cast<float>(rand());
-                y /= static_cast<float>(RAND_MAX / this->samplingInfo_.y);
-                y -= this->samplingInfo_.y / 2;
-                y += this->samplingInfo_.y0;
-                float h = static_cast<float>(rand());
-                h /= static_cast<float>(RAND_MAX / this->samplingInfo_.h);
-                h -= this->samplingInfo_.h / 2;
-                h += this->samplingInfo_.h0;
-                return new RRTNode(x, y, h);
-        } else {
-                float x = this->ndx_(this->gen_);
-                float y = this->ndy_(this->gen_);
-                float h = this->ndh_(this->gen_);
-                return new RRTNode(x, y, h);
-        }
+        float x = this->ndx_(this->gen_);
+        float y = this->ndy_(this->gen_);
+        float h = this->ndh_(this->gen_);
+        return new RRTNode(x, y, h);
 }
 
 float RRTBase::cost(RRTNode *init, RRTNode *goal)
diff --git a/incl/rrtbase.h b/incl/rrtbase.h
index f83be79..023a550 100644
--- a/incl/rrtbase.h
+++ b/incl/rrtbase.h
@@ -158,8 +158,7 @@ class RRTBase {
                 std::vector<RRTNode *> findt(RRTNode *n);
 
                 // RRT Framework
-                SamplingInfo samplingInfo_;
-                bool useSamplingInfo_ = false;
+                void setSamplingInfo(SamplingInfo si);
                 RRTNode *sample();
                 float cost(RRTNode *init, RRTNode *goal);
                 RRTNode *nn(RRTNode *rs);