From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Fri, 5 Apr 2019 10:32:18 +0000 (+0200)
Subject: Tune sampling coordinates
X-Git-Tag: v0.6.0~5^2~1
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/iamcar.git/commitdiff_plain/e84001f0bd4c975869a2f96626be52543c4a4150

Tune sampling coordinates
---

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 3d89036..e049143 100644
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -889,7 +889,7 @@ RRTNode *RRTBase::sample()
                 );
                 std::normal_distribution<float> hdist(
                         0,
-                        this->samplingInfo_.h / 3
+                        this->samplingInfo_.h
                 );
                 if (!this->samplingInfo_.r) {
                         float dx = BCAR_WIDTH / 2 +
@@ -915,8 +915,16 @@ RRTNode *RRTBase::sample()
                 } else {
                         float dr = std::abs(xdist(this->gen_));
                         float dh = hdist(this->gen_);
-                        x += dr * cos(this->samplingInfo_.sh + M_PI / 2 + dh);
-                        y += dr * sin(this->samplingInfo_.sh + M_PI / 2 + dh);
+                        x += dr * cos(
+                                this->samplingInfo_.sh +
+                                this->samplingInfo_.dh * M_PI / 2 +
+                                dh
+                        );
+                        y += dr * sin(
+                                this->samplingInfo_.sh +
+                                this->samplingInfo_.dh * M_PI / 2 +
+                                dh
+                        );
                         h = -M_PI / 2 + dh;
                 }
                 return new RRTNode(
diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 93ea4c7..5121906 100644
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -561,7 +561,7 @@ struct SamplingInfo ParallelSlot::getSamplingInfo()
                         this->slot().bnodes()[1]
                 ) / BCAR_LENGTH);
         } else {
-                si.h = M_PI /2;
+                si.h = M_PI / 2 / 3;
         }
 #else
         si.x = this->slot().bnodes()[3]->x() - this->slot().bnodes()[0]->x();
@@ -582,7 +582,7 @@ struct SamplingInfo ParallelSlot::getSamplingInfo()
         si.r = EDIST(this->slot().bnodes()[0], this->slot().bnodes()[3]) / 2;
         si.r *= 2;
         si.sh = this->slotHeading();
-        si.h = M_PI / 2;
+        si.h = M_PI / 4;
 #endif
         return si;
 }