From 7ef11bbd8bac71836b621a48d344884819bb751b Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
Date: Sun, 21 Nov 2021 23:11:42 +0100
Subject: [PATCH] Add compare to zips

---
 CMakeLists.txt         |  3 ++
 src/compare_to_zips.cc | 64 ++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 67 insertions(+)
 create mode 100644 src/compare_to_zips.cc

diff --git a/CMakeLists.txt b/CMakeLists.txt
index f09e0b5..9d5d002 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -26,6 +26,9 @@ target_link_libraries(prove_parallel_slot pslot)
 add_executable(compare_to_vorobieva src/compare_to_vorobieva.cc)
 target_link_libraries(compare_to_vorobieva pslot)
 
+add_executable(compare_to_zips src/compare_to_zips.cc)
+target_link_libraries(compare_to_zips pslot)
+
 if (SKIP_UT)
 	return()
 endif()
diff --git a/src/compare_to_zips.cc b/src/compare_to_zips.cc
new file mode 100644
index 0000000..58fb229
--- /dev/null
+++ b/src/compare_to_zips.cc
@@ -0,0 +1,64 @@
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+/*! \file
+ * \brief Compare with two-phased static optimization [1].
+ *
+ * [1]: Zips, P., Bock, M., and Kugi, A. (2013). A fast mo- tion planning
+ * algorithm for car parking based on static optimization. In 2013 IEEE/RSJ
+ * Interna- tional Conference on Intelligent Robots and Sys- tems, pages
+ * 2392–2397. ISSN: 2153-0866.
+ */
+
+#include <cmath>
+#include <iostream>
+#include <vector>
+#include "pslot.hh"
+
+#define CAR_CURB_TO_CURB 9.0
+#define CAR_WIDTH 1.8
+#define CAR_WHEELBASE 2.7
+#define CAR_DISTANCE_FROM_REAR_AXLE_TO_FRONT 3.7
+#define CAR_LENGTH 4.7
+
+#define SLOT_WIDTH 2.2
+#define SLOT_LENGTH 5.1
+
+#define PARKING_SPEED -0.001
+#define MAX_CUSP 15
+#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;
+
+	int cusp[2] = {-1, -1};
+	bcar::ParkingSlot s(zp, zh, SLOT_WIDTH, SLOT_LENGTH);
+	// In-slot planner
+	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)) {
+		cusp[1] = s.get_max_cusp();
+	}
+	using namespace std;
+	cout << SLOT_WIDTH << " " << SLOT_LENGTH << " ";
+	cout << cusp[1] << " " << pr << endl;
+	return 0;
+}
-- 
2.39.2