Rewrite sampling

diff --git a/base/rrtbase.cc b/base/rrtbase.cc
index 0a2aa99f7aee4654e08c390e7749ca81b748b801..2ac71f95a8000f47692dbeffc42d9b30217ffcc8 100644 (file)
--- a/base/rrtbase.cc
+++ b/base/rrtbase.cc
@@ -874,59 +874,22 @@ std::vector<RRTNode *> RRTBase::findt(RRTNode *n)
 RRTNode *RRTBase::sample()
 {
         if (this->useSamplingInfo_) {
-                float x = this->samplingInfo_.x;
-                float y = this->samplingInfo_.y;
-                float h = 0;
-                std::normal_distribution<float> xdist(
-                        0,
-                        (this->samplingInfo_.r)
-                                ?this->samplingInfo_.r
-                                :BCAR_WIDTH * 2
-                );
-                std::normal_distribution<float> hdist(
-                        0,
-                        this->samplingInfo_.h
-                );
-                std::normal_distribution<float> ydist(
-                        0,
-                        (this->samplingInfo_.r)
-                                ?this->samplingInfo_.h / 2
-                                :BCAR_LENGTH / 3
-                );
-                if (!this->samplingInfo_.r) {
-                        float dx = BCAR_WIDTH / 2 +
-                                std::abs(xdist(this->gen_));
-                        x += dx * cos(
-                                this->samplingInfo_.sh +
-                                this->samplingInfo_.dx * M_PI / 2
-                        );
-                        y += dx * cos(
-                                this->samplingInfo_.sh +
-                                this->samplingInfo_.dx * M_PI / 2
-                        );
-                        float dy = ydist(this->gen_);
-                        x += dy * cos(this->samplingInfo_.sh);
-                        y += dy * sin(this->samplingInfo_.sh);
-                        h = std::abs(hdist(this->gen_));
-                } else {
-                        float dr = std::abs(xdist(this->gen_));
-                        float dh = hdist(this->gen_);
-                        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 = std::abs(ydist(this->gen_));
-                }
+                float sar = static_cast<float>(rand());
+                sar /= static_cast<float>(RAND_MAX / this->samplingInfo_.r);
+                sar += this->samplingInfo_.mr;
+                float sah = static_cast<float>(rand());
+                sah /= static_cast<float>(RAND_MAX / this->samplingInfo_.h);
+                sah *= this->samplingInfo_.dh;
+                sah += this->samplingInfo_.sh;
+                float h = static_cast<float>(rand());
+                h /= static_cast<float>(RAND_MAX / this->samplingInfo_.mh);
+                h += this->samplingInfo_.mmh;
+                h *= this->samplingInfo_.dh;
+                h += this->samplingInfo_.sh;
                 return new RRTNode(
-                        x,
-                        y,
-                        this->samplingInfo_.sh + this->samplingInfo_.dh * h
+                        this->samplingInfo_.x + sar * cos(sah),
+                        this->samplingInfo_.y + sar * sin(sah),
+                        h
                 );
         } else {
                 return sa1();

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 51219061dfbd8f9da58a8f9d0d8543c861b6ac4e..f945bbcb1b93590b880e3eb7bbdad1cdc9270eff 100644 (file)
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -541,48 +541,30 @@ bool ParallelSlot::isInside(BicycleCar *c)
 struct SamplingInfo ParallelSlot::getSamplingInfo()
 {
         struct SamplingInfo si;
-#ifdef USE_SLOTPLANNER
-        BicycleCar *CC = this->getEPC();
-        si.x = this->slot().bnodes()[0]->x();
-        si.y = this->slot().bnodes()[0]->y();
-        if (this->slotSide() == RIGHT) {
-                si.dx = 1;
-                si.dy = 1;
-                si.dh = 1;
-        } else {
-                si.dx = -1;
-                si.dy = -1;
-                si.dh = -1;
-        }
-        si.sh = this->slotHeading();
         if (this->slotType() == PARALLEL) {
-                si.h = this->slotHeading() - acos(EDIST(
-                        this->slot().bnodes()[0],
-                        this->slot().bnodes()[1]
-                ) / BCAR_LENGTH);
+                si.x = this->slot().bnodes().front()->x();
+                si.y = this->slot().bnodes().front()->y();
+                si.mr = BCAR_WIDTH / 2;
+                si.mmh = 0;
         } else {
-                si.h = M_PI / 2 / 3;
+                si.x = this->slot().bnodes().back()->x();
+                si.x -= this->slot().bnodes().front()->x();
+                si.x /= 2;
+                si.x += this->slot().bnodes().front()->x();
+                si.y = this->slot().bnodes().back()->y();
+                si.y -= this->slot().bnodes().front()->y();
+                si.y /= 2;
+                si.y += this->slot().bnodes().front()->y();
+                si.mr = 0;
+                si.mmh = (M_PI - M_PI / 6) / 2;
         }
-#else
-        si.x = this->slot().bnodes()[3]->x() - this->slot().bnodes()[0]->x();
-        si.x /= 2;
-        si.x += this->slot().bnodes()[0]->x();
-        si.y = this->slot().bnodes()[3]->y() - this->slot().bnodes()[0]->y();
-        si.y /= 2;
-        si.y += this->slot().bnodes()[0]->y();
-        if (this->slotSide() == RIGHT) {
-                si.dx = 1;
-                si.dy = 1;
+        si.r = BCAR_LENGTH;
+        si.h = M_PI;
+        si.mh = M_PI / 2;
+        si.sh = this->slotHeading();
+        if (this->slotSide() == RIGHT)
                 si.dh = 1;
-        } else {
-                si.dx = -1;
-                si.dy = -1;
+        else
                 si.dh = -1;
-        }
-        si.r = EDIST(this->slot().bnodes()[0], this->slot().bnodes()[3]) / 2;
-        si.r *= 2;
-        si.sh = this->slotHeading();
-        si.h = M_PI / 4;
-#endif
         return si;
 }

diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index f48e2a264a2573d5a86d49f4f19b06d090c0ec9d..11dc5f1b940147f95e26ddb5df642e5e446ac77f 100644 (file)
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -35,13 +35,14 @@ enum SlotType {
 
 struct SamplingInfo {
         float x;
-        float dx;
         float y;
-        float dy;
-        float r;
-        float sh;
-        float h;
-        float dh;
+        float r; // max radi from [x, y] for random sample
+        float mr; // minimum r added to random r
+        float h; // max angle for random sample
+        float mh; // max angle for heading of random sample
+        float mmh; // minimum mh added to mh
+        float sh; // slot heading
+        float dh; // direction to compute random heading from slot heading
 };
 
 class ParallelSlot {