Add drive of slot method

diff --git a/incl/pslot.hh b/incl/pslot.hh
index fc460b1b47311a3dedceaf82e18ae403fb635f2a..c1c53110781edae500c42cba56a29935e150d6de 100644 (file)
--- a/incl/pslot.hh
+++ b/incl/pslot.hh
@@ -130,6 +130,12 @@ public:
         */
        std::vector<BicycleCar> drive_in_slot(BicycleCar c);
 
+       /*! \brief Drive car `c` from slot.
+        *
+        * \param c Starting bicycle car.
+        */
+       std::vector<BicycleCar> drive_of_slot(BicycleCar c);
+
        /*! \brief Steer car `c` into the parking slot `this`.
         *
         * `steer_in_slot` returns the complete path as the list of `Pose`s, not

diff --git a/src/pslot.cc b/src/pslot.cc
index dd2321354c4068d285d87f5c9238100952ea663e..20509e1b378552129744398b9d9c4e56ef9628be 100644 (file)
--- a/src/pslot.cc
+++ b/src/pslot.cc
@@ -255,6 +255,40 @@ ParkingSlot::drive_in_slot(BicycleCar c)
        return std::vector<BicycleCar>();
 }
 
+std::vector<BicycleCar>
+ParkingSlot::drive_of_slot(BicycleCar c)
+{
+       assert(this->parallel());
+       assert(this->right());
+       assert(c.len() < this->len());
+       assert(c.w() < this->w());
+       assert(this->parked(c));
+       std::vector<BicycleCar> path;
+       path.reserve(this->max_cusp_ + 2);
+       path.push_back(c);
+       unsigned int cusp = 0;
+       auto b_len = sizeof(this->border_) / sizeof(this->border_[0]);
+       std::vector<Point> b(this->border_, this->border_ + b_len);
+       while (cusp < this->max_cusp_ + 1) {
+               if (!c.lf().inside_of(b) && !c.rf().inside_of(b)) {
+                       if (cusp < this->max_cusp_) {
+                               this->max_cusp_ = cusp;
+                       }
+                       path.push_back(c);
+                       return path;
+               }
+               c.next();
+               if (this->collide(c)) {
+                       c.sp(c.sp() * -1.0);
+                       c.next();
+                       path.push_back(c);
+                       c.st(c.st() * -1.0);
+                       cusp += 1;
+               }
+       }
+       return std::vector<BicycleCar>();
+}
+
 std::vector<Pose>
 ParkingSlot::steer_in_slot(BicycleCar c)
 {