From 37f51ce08240dc4c677197ec4a142f97de6d8356 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Thu, 15 Jul 2021 14:13:44 +0200
Subject: [PATCH] Add find entry method

---
 incl/pslot.hh |  4 ++++
 src/pslot.cc  | 60 +++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 64 insertions(+)

diff --git a/incl/pslot.hh b/incl/pslot.hh
index b970cba..c669d23 100644
--- a/incl/pslot.hh
+++ b/incl/pslot.hh
@@ -16,6 +16,7 @@ namespace bcar {
  */
 class ParkingSlot {
 private:
+	double offset_ = 0.001; // to avoid collision during init
 	Point border_[4];
 	Line entry_;
 	Line rear_;
@@ -88,6 +89,9 @@ public:
 	 */
 	std::vector<BicycleCar> drive_in_slot(BicycleCar c, unsigned int& max);
 
+	/*! Find entry. */
+	PoseRange fe();
+
 	friend std::ostream& operator<<(std::ostream& o, ParkingSlot const& s);
 };
 
diff --git a/src/pslot.cc b/src/pslot.cc
index aff8850..07b2cdb 100644
--- a/src/pslot.cc
+++ b/src/pslot.cc
@@ -174,6 +174,66 @@ ParkingSlot::drive_in_slot(BicycleCar c, unsigned int& max)
 	return path;
 }
 
+PoseRange
+ParkingSlot::fe()
+{
+	assert(this->parallel());
+	assert(this->right());
+	BicycleCar c;
+	c.h(this->h());
+	double clen = this->offset_ + this->len() - c.df();
+	double cw = this->offset_ + c.w() / 2.0;
+	c.x(this->lrx() + clen * cos(c.h()) + cw * cos(c.h() + M_PI / 2.0));
+	c.y(this->lry() + clen * sin(c.h()) + cw * sin(c.h() + M_PI / 2.0));
+	c.set_max_steer();
+	c.sp(-0.01);
+	auto const& b3 = this->border_[3];
+	this->curb_.intersects_with(b3, c.len());
+	double max_to_slot;
+	auto const& rr = c.rr();
+	auto const& i1 = this->curb_.in1();
+	auto const& i2 = this->curb_.in2();
+	if (rr.edist(i1) < rr.edist(i2)) {
+		max_to_slot = rr.min_angle_between(b3, i1);
+	} else {
+		max_to_slot = rr.min_angle_between(b3, i2);
+	}
+	std::vector<BicycleCar> starts;
+	double a_to_slot = 0.0;
+	while (a_to_slot < max_to_slot) {
+		a_to_slot += 0.001;
+		c.rotate(b3, 0.001);
+		starts.push_back(c);
+	}
+	std::vector<std::vector<BicycleCar>> entries;
+	unsigned int max_cusp = 10;
+	for (auto s: starts) {
+		auto r = this->drive_in_slot(s, max_cusp);
+		if (r.size() > 0) {
+			entries.push_back(r);
+		}
+	}
+	assert(entries.size() > 0);
+	auto& c1 = entries.front().front();
+	auto& c2 = entries.back().front();
+	double b = std::min(c1.h(), c2.h());
+	double e = std::max(c1.h(), c2.h());
+	clen = c.len();
+	Point b1(c1.x() - clen * cos(c1.h()), c1.y() - clen * sin(c1.h()));
+	Point b2(c2.x() - clen * cos(c2.h()), c2.y() - clen * sin(c2.h()));
+	Point e1(c1.x() + clen * cos(c1.h()), c1.y() + clen * sin(c1.h()));
+	Point e2(c2.x() + clen * cos(c2.h()), c2.y() + clen * sin(c2.h()));
+	Line li1(b1, e1);
+	Line li2(b2, e2);
+	li1.intersects_with(li2);
+	PoseRange pr;
+	pr.x(li1.in1().x());
+	pr.y(li1.in1().y());
+	pr.b(b);
+	pr.e(e);
+	return pr;
+}
+
 std::ostream&
 operator<<(std::ostream& o, ParkingSlot const& s)
 {
-- 
2.39.2