Add compute entries ut, impl

diff --git a/incl/pslot.hh b/incl/pslot.hh
index 542341288c279c93fb2ddfc21350e8fc6d321156..51fd7c83ee38dfefaeb307c22b3aabc2e23ea86e 100644 (file)
--- a/incl/pslot.hh
+++ b/incl/pslot.hh
@@ -33,6 +33,7 @@ private:
        Line _front;
 public:
        std::vector<std::vector<BicycleCar>> _ispaths;
+       std::vector<BicycleCar> _entries;
        /*! \brief Set parking slot.
 
        \param p Point with `x`, `y` coordinates of entry side's corner.
@@ -159,6 +160,19 @@ public:
         */
        PoseRange fe(BicycleCar c);
 
+       /*! \brief Compute entries from `this->_ispaths`.
+        *
+        * The problem with in-slot paths is that car can't leave the parking
+        * slot with full rate to the right (for right sided parking slot.)
+        *
+        * The idea is to move the first cars of in-slot paths (the one that is
+        * computed as entry to the parking slot) with the full rate to the left
+        * until they can leave the parking slot with full rate to the right.
+        *
+        * The computed cars are stored in `this->_entries`.
+        */
+       void compute_entries();
+
        /*! \brief Recompute zero slot's `PoseRange` entry for `this`.
         *
         * The _zero slot_ is the `ParkingSlot(Point(0.0, 0.0), 0.0, W, L);`.

diff --git a/src/pslot.cc b/src/pslot.cc
index f791e15993bde17e04dafc2f002885a9d787a41b..d6593f2008ae2438f36aa55733231f0dcafe033d 100644 (file)
--- a/src/pslot.cc
+++ b/src/pslot.cc
@@ -406,6 +406,23 @@ ParkingSlot::fe(BicycleCar c)
        return p;
 }
 
+void
+ParkingSlot::compute_entries()
+{
+       assert(this->_ispaths.size() > 0);
+       assert(this->right());
+       this->_entries.clear();
+       for (auto p: this->_ispaths) {
+               this->_entries.push_back(p.front());
+       }
+       for (auto& e: this->_entries) {
+               e.sp(e.sp() * -1.0);
+               while (!this->lf().inside_of(e.ccr(), e.iradi())) {
+                       e.next();
+               }
+       }
+}
+
 PoseRange
 ParkingSlot::recompute_entry(PoseRange p)
 {

diff --git a/ut/pslot.t.cc b/ut/pslot.t.cc
index ba0154488931f80e24de5cacb691b81c32f338ec..84005ade19a2330307b351da9b56c27c60a45824 100644 (file)
--- a/ut/pslot.t.cc
+++ b/ut/pslot.t.cc
@@ -40,3 +40,14 @@ WVTEST_MAIN("pslot basic geometry")
        WVPASS(!ps.parallel());
        WVPASSEQ_DOUBLE(ps.h(), -M_PI / 4.0, 0.00001);
 }
+
+WVTEST_MAIN("entries")
+{
+       ParkingSlot s(Point(0, 0), 0, 2.5, 6);
+       s.fe(BicycleCar());
+       s.compute_entries();
+       WVPASS(s._entries.size() > 0);
+       for (auto& e: s._entries) {
+               WVPASS(s.lf().inside_of(e.ccr(), e.iradi()));
+       }
+}