Rewrite sampling

[hubacji1/iamcar.git] / decision_control / slotplanner.cc

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;
 }