From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Tue, 20 Jul 2021 16:22:12 +0000 (+0200)
Subject: Find entry for perperdicular parking
X-Git-Tag: v0.6.0~1^2
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hubacji1/bcar.git/commitdiff_plain/574da63277b85193a506d605469b15aba30b80ab

Find entry for perperdicular parking
---

diff --git a/src/pslot.cc b/src/pslot.cc
index 64deb5b..fe6c377 100644
--- 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();