From 965a82a7143f4e43c785371425b3d9dbe55fe3f0 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 13 Jul 2021 13:58:28 +0200
Subject: [PATCH] Use pose instead of bicycle car for drivable

---
 incl/bcar.hh |  4 ++--
 src/bcar.cc  | 44 ++++++++++++++++++++++----------------------
 2 files changed, 24 insertions(+), 24 deletions(-)

diff --git a/incl/bcar.hh b/incl/bcar.hh
index bfb9cda..578e6f9 100644
--- a/incl/bcar.hh
+++ b/incl/bcar.hh
@@ -231,8 +231,8 @@ public:
 	 * \param b The beginning of the heading range.
 	 * \param e The end of the heading range.
 	 */
-	bool drivable(BicycleCar const& bc, double b, double e) const;
-	bool drivable(BicycleCar const& bc) const;
+	bool drivable(Pose const& p, double b, double e) const;
+	bool drivable(Pose const& p) const;
 
 	/*! \brief Return inner radius.
 	 *
diff --git a/src/bcar.cc b/src/bcar.cc
index 4305b7c..3c90942 100644
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -353,21 +353,21 @@ CarMove::st(double st)
 }
 
 bool
-BicycleCar::drivable(BicycleCar const& bc) const
+BicycleCar::drivable(Pose const& p) const
 {
-    return this->drivable(bc, bc.h(), bc.h());
+    return this->drivable(p, p.h(), p.h());
 }
 
 bool
-BicycleCar::drivable(BicycleCar const& bc, double b, double e) const
+BicycleCar::drivable(Pose const& p, double b, double e) const
 {
-	// assert bc.h() == (b + e) / 2.0
-	double a_1 = atan2(bc.y() - this->y(), bc.x() - this->x()) - this->h();
+	// assert p.h() == (b + 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 = bc.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)
@@ -378,12 +378,12 @@ BicycleCar::drivable(BicycleCar const& bc, double b, double e) const
 	} else if (0 < a_1 && a_1 <= M_PI/2) { // left front
 		BicycleCar z(*this); // zone border
 		z.h(e);
-		h_d = bc.h() - this->h();
+		h_d = p.h() - this->h();
 		z.rotate(this->ccl(), h_d);
-		// assert z.h() == bc.h()
-		if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
+		// assert z.h() == p.h()
+		if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 			return true;
-		a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
+		a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 		while (a_2 < -M_PI)
 			a_2 += 2 * M_PI;
 		while (a_2 > +M_PI)
@@ -393,12 +393,12 @@ BicycleCar::drivable(BicycleCar const& bc, double b, double e) const
 	} else if (M_PI/2 < a_1 && a_1 <= M_PI) { // left rear
 		BicycleCar z(*this); // zone border
 		z.h(e);
-		h_d = bc.h() - this->h();
+		h_d = p.h() - this->h();
 		z.rotate(this->ccl(), h_d);
-		// assert z.h() == bc.h()
-		if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
+		// assert z.h() == p.h()
+		if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 			return true;
-		a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
+		a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 		a_2 -= M_PI;
 		while (a_2 < -M_PI)
 			a_2 += 2 * M_PI;
@@ -409,12 +409,12 @@ BicycleCar::drivable(BicycleCar const& bc, double b, double e) const
 	} else if (0 > a_1 && a_1 >= -M_PI/2) { // right front
 		BicycleCar z(*this); // zone border
 		z.h(b);
-		h_d = bc.h() - this->h();
+		h_d = p.h() - this->h();
 		z.rotate(this->ccr(), h_d);
-		// assert z.h() == bc.h()
-		if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
+		// assert z.h() == p.h()
+		if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 			return true;
-		a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
+		a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 		while (a_2 < -M_PI)
 			a_2 += 2 * M_PI;
 		while (a_2 > +M_PI)
@@ -424,12 +424,12 @@ BicycleCar::drivable(BicycleCar const& bc, double b, double e) const
 	} else if (-M_PI/2 > a_1 && a_1 >= -M_PI) { // right rear
 		BicycleCar z(*this); // zone border
 		z.h(b);
-		h_d = bc.h() - this->h();
+		h_d = p.h() - this->h();
 		z.rotate(this->ccr(), h_d);
-		// assert z.h() == bc.h()
-		if (bc.y() == z.y() && bc.x() == z.x()) // bc on zone border
+		// assert z.h() == p.h()
+		if (p.y() == z.y() && p.x() == z.x()) // p on zone border
 			return true;
-		a_2 = atan2(bc.y() - z.y(), bc.x() - z.x());
+		a_2 = atan2(p.y() - z.y(), p.x() - z.x());
 		a_2 -= M_PI;
 		while (a_2 < -M_PI)
 			a_2 += 2 * M_PI;
-- 
2.39.2