Find entry for perperdicular parking

diff --git a/src/pslot.cc b/src/pslot.cc
index 64deb5b39cf5a1f4e43bb8de1dd1b54771558ae1..fe6c3773f6b79cf75815159066887e717fc8009d 100644 (file)
--- a/src/pslot.cc
+++ b/src/pslot.cc
@@ -263,7 +263,38 @@ ParkingSlot::steer_in_slot(BicycleCar c)
 PoseRange
 ParkingSlot::fe(BicycleCar c)
 {
-       assert(this->parallel());
+       if (!this->parallel()) {
+               double gd = 0.0;
+               double dd = 0.0;
+               double radi = 0.0;
+               if (this->parking_speed_ < 0) {
+                       gd = c.df();
+                       c.h(this->rear_.h() + M_PI);
+                       c.sp(1.0);
+                       radi = c.iradi();
+               } else {
+                       gd = c.dr();
+                       c.h(this->rear_.h());
+                       c.sp(-1.0);
+                       radi = c.ofradi();
+               }
+               c.x(this->entry_.m().x() + gd * cos(this->rear_.h()));
+               c.y(this->entry_.m().y() + gd * sin(this->rear_.h()));
+               Point cc(0.0, 0.0);
+               if (this->right()) {
+                       cc = c.ccl();
+               } else {
+                       cc = c.ccr();
+               }
+               this->rear_.intersects_with(cc, radi);
+               dd = std::min(this->border_[0].edist(this->rear_.i1()),
+                       this->border_[0].edist(this->rear_.i2()));
+               c.st(0.0);
+               c.sp(c.sp() * dd);
+               c.next();
+               c.sp(this->parking_speed_);
+               return PoseRange(c.x(), c.y(), c.h(), c.h());
+       }
        bool swapped = false;
        if (!this->right()) {
                this->swap_side();