From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Tue, 20 Jul 2021 10:40:54 +0000 (+0200)
Subject: Refactor pose range
X-Git-Tag: v0.6.0~3^2~4
X-Git-Url: https://rtime.felk.cvut.cz/gitweb/hubacji1/bcar.git/commitdiff_plain/a544fef692a66f61d0659ad8af14d858afefadbc?hp=e30226d3f50ec045f70f8aedf5c5c42254bd218b

Refactor pose range
---

diff --git a/incl/bcar.hh b/incl/bcar.hh
index 5370934..22e57d4 100644
--- a/incl/bcar.hh
+++ b/incl/bcar.hh
@@ -156,21 +156,21 @@ public:
 
 class PoseRange : public virtual Pose {
 private:
-	double e_ = 0.0;
-	using Pose::h;
+	Pose bp_;
+	Pose ep_;
+	void set_xyh();
 public:
+	PoseRange(Pose bp, Pose ep);
+
+	Pose bp() const;
+	Pose ep() const;
+
 	/*! Get heading's begin in the interval [-pi, +pi] radians. */
 	double b() const;
 
-	/*! Set heading's begin in radians. It's recomputed to [-pi, +pi]. */
-	void b(double b);
-
 	/*! Get heading's end in the interval [-pi, +pi] radians. */
 	double e() const;
 
-	/*! Set heading's end in radians. It's recomputed to [-pi, +pi]. */
-	void e(double e);
-
 	void rotate(Point const& c, double const angl);
 
 	void reflect(Line const& li);
diff --git a/src/bcar.cc b/src/bcar.cc
index 93f017e..d64f27f 100644
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -309,49 +309,73 @@ operator<<(std::ostream& out, Pose const& p)
 	return out;
 }
 
-double
-PoseRange::b() const
+void
+PoseRange::set_xyh()
+{
+	double clen = 10.0;
+	double bpbx = this->bp_.x() - clen * cos(this->bp_.h());
+	double bpby = this->bp_.y() - clen * sin(this->bp_.h());
+	double bpfx = this->bp_.x() + clen * cos(this->bp_.h());
+	double bpfy = this->bp_.y() + clen * sin(this->bp_.h());
+	Line li1(Point(bpbx, bpby), Point(bpfx, bpfy));
+	double epbx = this->ep_.x() - clen * cos(this->ep_.h());
+	double epby = this->ep_.y() - clen * sin(this->ep_.h());
+	double epfx = this->ep_.x() + clen * cos(this->ep_.h());
+	double epfy = this->ep_.y() + clen * sin(this->ep_.h());
+	Line li2(Point(epbx, epby), Point(epfx, epfy));
+	li1.intersects_with(li2);
+	this->x(li1.i1().x());
+	this->y(li1.i1().y());
+	this->h((this->b() + this->e()) / 2.0);
+}
+
+PoseRange::PoseRange(Pose bp, Pose ep) : bp_(bp), ep_(ep)
+{
+	if (this->bp_ == this->ep_) {
+		this->set_pose(this->ep_);
+	} else {
+		this->set_xyh();
+	}
+}
+
+Pose
+PoseRange::bp() const
 {
-	return this->h();
+	return this->bp_;
 }
 
-void
-PoseRange::b(double b)
+Pose
+PoseRange::ep() const
 {
-	this->h(b);
+	return this->ep_;
 }
 
 double
-PoseRange::e() const
+PoseRange::b() const
 {
-	return this->e_;
+	return std::min(this->bp_.h(), this->ep_.h());
 }
 
-void
-PoseRange::e(double e)
+double
+PoseRange::e() const
 {
-	while (e < -M_PI) {
-		e += 2 * M_PI;
-	}
-	while (e > +M_PI) {
-		e -= 2 * M_PI;
-	}
-	this->e_ = e;
+	return std::max(this->bp_.h(), this->ep_.h());
 }
 
 void
 PoseRange::rotate(Point const& c, double const angl)
 {
-	Pose::rotate(c, angl);
-	this->e(this->e() + angl);
+	this->bp_.rotate(c, angl);
+	this->ep_.rotate(c, angl);
+	this->set_xyh();
 }
 
 void
 PoseRange::reflect(Line const& li)
 {
-	Pose::reflect(li);
-	double dh = li.h() - this->e();
-	this->e(this->e() + 2.0 * dh);
+	this->bp_.reflect(li);
+	this->ep_.reflect(li);
+	this->set_xyh();
 }
 
 std::ostream&
@@ -497,24 +521,18 @@ CarMove::st(double st)
 bool
 BicycleCar::drivable(Pose const& p) const
 {
-	PoseRange pr;
-	pr.x(p.x());
-	pr.y(p.y());
-	pr.b(p.h());
-	pr.e(p.h());
-	return this->drivable(pr);
+	return this->drivable(PoseRange(p, p));
 }
 
 bool
 BicycleCar::drivable(PoseRange const& p) const
 {
-	double h = (p.b() + p.e()) / 2.0;
 	double a_1 = atan2(p.y() - this->y(), p.x() - this->x()) - this->h();
 	while (a_1 < -M_PI)
 		a_1 += 2 * M_PI;
 	while (a_1 > +M_PI)
 		a_1 -= 2 * M_PI;
-	double h_d = h - this->h();
+	double h_d = p.h() - this->h();
 	while (h_d < -M_PI)
 		h_d += 2 * M_PI;
 	while (h_d > +M_PI)
@@ -525,9 +543,9 @@ BicycleCar::drivable(PoseRange const& p) const
 	} else if (0 < a_1 && a_1 <= M_PI/2) { // left front
 		BicycleCar z(*this); // zone border
 		z.h(p.e());
-		h_d = h - this->h();
+		h_d = p.h() - this->h();
 		z.rotate(this->ccl(), h_d);
-		// assert z.h() == h
+		// assert z.h() == p.h()
 		if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 			return true;
 		a_2 = atan2(p.y() - z.y(), p.x() - z.x());
@@ -540,9 +558,9 @@ BicycleCar::drivable(PoseRange const& p) const
 	} else if (M_PI/2 < a_1 && a_1 <= M_PI) { // left rear
 		BicycleCar z(*this); // zone border
 		z.h(p.e());
-		h_d = h - this->h();
+		h_d = p.h() - this->h();
 		z.rotate(this->ccl(), h_d);
-		// assert z.h() == h
+		// assert z.h() == p.h()
 		if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 			return true;
 		a_2 = atan2(p.y() - z.y(), p.x() - z.x());
@@ -556,9 +574,9 @@ BicycleCar::drivable(PoseRange const& p) const
 	} else if (0 > a_1 && a_1 >= -M_PI/2) { // right front
 		BicycleCar z(*this); // zone border
 		z.h(p.b());
-		h_d = h - this->h();
+		h_d = p.h() - this->h();
 		z.rotate(this->ccr(), h_d);
-		// assert z.h() == h
+		// assert z.h() == p.h()
 		if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 			return true;
 		a_2 = atan2(p.y() - z.y(), p.x() - z.x());
@@ -571,9 +589,9 @@ BicycleCar::drivable(PoseRange const& p) const
 	} else if (-M_PI/2 > a_1 && a_1 >= -M_PI) { // right rear
 		BicycleCar z(*this); // zone border
 		z.h(p.b());
-		h_d = h - this->h();
+		h_d = p.h() - this->h();
 		z.rotate(this->ccr(), h_d);
-		// assert z.h() == h
+		// assert z.h() == p.h()
 		if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 			return true;
 		a_2 = atan2(p.y() - z.y(), p.x() - z.x());
diff --git a/src/pslot.cc b/src/pslot.cc
index 5f25efd..6966377 100644
--- a/src/pslot.cc
+++ b/src/pslot.cc
@@ -251,34 +251,15 @@ ParkingSlot::fe(BicycleCar c)
 		}
 	}
 	if (entries.size() == 0) {
-		return PoseRange();
+		return PoseRange(Pose(0.0, 0.0, 0.0), Pose(0.0, 0.0, 0.0));
 	}
 	if (entries.size() == 1) {
-		PoseRange pr;
-		pr.x(entries.front().front().x());
-		pr.y(entries.front().front().y());
-		pr.b(entries.front().front().h());
-		pr.e(entries.front().front().h());
-		return pr;
+		auto f = entries.front().front();
+		return PoseRange(f, f);
 	}
 	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.i1().x());
-	pr.y(li1.i1().y());
-	pr.b(b);
-	pr.e(e);
-	return pr;
+	return PoseRange(c1, c2);
 }
 
 PoseRange