Add steer in slot method

diff --git a/incl/pslot.hh b/incl/pslot.hh
index b7d3956cac05d7b9fa986a564ca37a2ddaaf11c8..1562631eb842d74b924909259a084501464a8926 100644 (file)
--- a/incl/pslot.hh
+++ b/incl/pslot.hh
@@ -89,6 +89,15 @@ public:
         */
        std::vector<BicycleCar> drive_in_slot(BicycleCar c, unsigned int& max);
 
+       /*! \brief Steer car `c` into the parking slot `this`.
+        *
+        * `steer_in_slot` returns the complete path as the list of `Pose`s, not
+        * just cusp `BicycleCar`s as `drive_to_slot`.
+        *
+        * \param c Starting bicycle car.
+        */
+       std::vector<Pose> steer_in_slot(BicycleCar c);
+
        /*! \brief Find entry.
         *
         * \param c For which `BicycleCar` should entry be found?

diff --git a/src/pslot.cc b/src/pslot.cc
index a10b17cb74bda952917cafb416299b50f5b3cf53..fb42ab0d6f728d9648e38f909d6cec5c215c85cb 100644 (file)
--- a/src/pslot.cc
+++ b/src/pslot.cc
@@ -174,6 +174,23 @@ ParkingSlot::drive_in_slot(BicycleCar c, unsigned int& max)
        return std::vector<BicycleCar>();
 }
 
+std::vector<Pose>
+ParkingSlot::steer_in_slot(BicycleCar c)
+{
+       std::vector<Pose> path;
+       while (!this->parked(c)) {
+               path.push_back(c);
+               c.next();
+               if (this->collide(c)) {
+                       c.sp(c.sp() * -1.0);
+                       c.next();
+                       c.st(c.st() * -1.0);
+               }
+       }
+       path.push_back(c);
+       return path;
+}
+
 PoseRange
 ParkingSlot::fe(BicycleCar c, unsigned int& max)
 {