From 1946e0082f07c18ad4c457cc222e29d6033f71a0 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Fri, 30 Jul 2021 15:46:00 +0200
Subject: [PATCH] Add code to prove parallel slot dimensions

---
 CMakeLists.txt             |  3 +++
 src/prove_parallel_slot.cc | 53 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 56 insertions(+)
 create mode 100644 src/prove_parallel_slot.cc

diff --git a/CMakeLists.txt b/CMakeLists.txt
index c5a0d6a..a79e109 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -16,6 +16,9 @@ target_link_libraries(pslot bcar)
 add_executable(compute_pslot_table src/compute_pslot_table.cc)
 target_link_libraries(compute_pslot_table pslot)
 
+add_executable(prove_parallel_slot src/prove_parallel_slot.cc)
+target_link_libraries(prove_parallel_slot pslot)
+
 if (SKIP_UT)
 	return()
 endif()
diff --git a/src/prove_parallel_slot.cc b/src/prove_parallel_slot.cc
new file mode 100644
index 0000000..84f6a30
--- /dev/null
+++ b/src/prove_parallel_slot.cc
@@ -0,0 +1,53 @@
+#include <iostream>
+#include <vector>
+#include "pslot.hh"
+
+#define CAR_CURB_TO_CURB 10.820
+#define CAR_WIDTH 1.625
+#define CAR_WHEELBASE 2.450
+#define CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT 3.105
+#define CAR_LENGTH 3.760
+
+#define SLOT_MAX_WIDTH 2.2
+#define SLOT_STEP_LENGTH 0.01
+#define SLOT_STEP_WIDTH 0.01
+
+#define PARKING_SPEED -0.001
+#define MAX_CUSP 10
+#define DELTA_ANGLE_TO_SLOT 0.0001
+
+int main()
+{
+	std::cout << std::fixed;
+	std::cerr << std::fixed;
+
+	bcar::BicycleCar c;
+	c.ctc(CAR_CURB_TO_CURB);
+	c.w(CAR_WIDTH);
+	c.wb(CAR_WHEELBASE);
+	c.df(CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT);
+	c.len(CAR_LENGTH);
+
+	bcar::Point zp(0.0, 0.0);
+	double zh = 0.0;
+	double w = c.w() + 4.5 * SLOT_STEP_WIDTH;
+
+	while (w < SLOT_MAX_WIDTH) {
+		double len = c.len() + SLOT_STEP_LENGTH;
+		while (true) {
+			bcar::ParkingSlot s(zp, zh, w, len);
+			s.set_parking_speed(PARKING_SPEED);
+			s.set_max_cusp(MAX_CUSP);
+			s.set_delta_angle_to_slot(DELTA_ANGLE_TO_SLOT);
+			auto pr = s.fe(c);
+			if (!(pr.x() == 0.0 && pr.y() == 0.0 && pr.b() == 0.0
+					&& pr.e() == 0.0)) {
+				std::cout << w << " " << len << std::endl;
+				break;
+			}
+			len += SLOT_STEP_LENGTH;
+		}
+		w += SLOT_STEP_WIDTH;
+	}
+	return 0;
+}
-- 
2.39.2