From 4c3704aaa91eed7227c023a07318815e0e9b625a Mon Sep 17 00:00:00 2001
From: daohu527 <daohu527@gmail.com>
Date: Tue, 10 Sep 2024 23:31:10 +0800
Subject: [PATCH] feat: add parking space

---
 imap/lib/common.py            | 30 ++++++++++++++++++++++++++++++
 imap/lib/convertor.py         | 14 ++++++++++----
 imap/lib/opendrive/lanes.py   | 13 +------------
 imap/lib/opendrive/objects.py | 11 +++++++++++
 setup.py                      |  2 +-
 5 files changed, 53 insertions(+), 17 deletions(-)

diff --git a/imap/lib/common.py b/imap/lib/common.py
index 6d65e7c..91813f0 100644
--- a/imap/lib/common.py
+++ b/imap/lib/common.py
@@ -106,6 +106,17 @@ def __str__(self):
                                                                         self.y, self.z, self.s, self.yaw)
 
 
+def binary_search(arr, val):
+    left, right = 0, len(arr) - 1
+    while left <= right:
+        mid = math.floor((left + right)/2)
+        if arr[mid] <= val:
+            left = mid + 1
+        else:
+            right = mid - 1
+    return left - 1
+
+
 def shift_t(point3d, offset):
     npoint = copy.deepcopy(point3d)
     npoint.shift_t(offset)
@@ -134,6 +145,25 @@ def calc_length(points):
     return length
 
 
+def get_rotated_rectangle_points(center, hdg, height, width):
+    cx, cy = center
+    half_height, half_width = height / 2, width / 2
+
+    # Rectangle's 4 corners relative to the center
+    corners = [
+        (-half_width, -half_height),  # bottom-left
+        (half_width, -half_height),   # bottom-right
+        (half_width, half_height),    # top-right
+        (-half_width, half_height)    # top-left
+    ]
+
+    # Rotate each corner and translate by the center
+    return [
+        (cx + x * math.cos(hdg) - y * math.sin(hdg),
+         cy + x * math.sin(hdg) + y * math.cos(hdg))
+        for x, y in corners
+    ]
+
 if __name__ == '__main__':
     vec_x = Vector3d(0.9201668879354276, -0.3915263699257437, 0)
     vec_z = Vector3d(0, 0, 1)
diff --git a/imap/lib/convertor.py b/imap/lib/convertor.py
index db26bfc..3578105 100644
--- a/imap/lib/convertor.py
+++ b/imap/lib/convertor.py
@@ -550,10 +550,16 @@ def convert_signals(self, xodr_road, pb_last_section):
                 for pb_lane in pb_last_section:
                     self.construct_signal_overlap(pb_lane, pb_signal)
 
-    def convert_objects(self):
-        # xodr_road.reference_line
-        # pb_parking_space = self.pb_map.parking_space.add()
-        pass
+    def convert_objects(self, xodr_road):
+        reference_line = xodr_road.reference_line
+        for obj in xodr_road.objects.objects:
+            pb_parking_space = self.pb_map.parking_space.add()
+            # Not sure if this is correct, need to pay attention to the units
+            pb_parking_space.heading = obj.hdg
+            for corner in obj.process_corners(reference_line):
+                point = pb_parking_space.polygon.point.add()
+                point.x, point.y = corner
+
 
     def convert_roads(self):
         for _, xodr_road in self.xodr_map.roads.items():
diff --git a/imap/lib/opendrive/lanes.py b/imap/lib/opendrive/lanes.py
index fda2711..46f9ba8 100644
--- a/imap/lib/opendrive/lanes.py
+++ b/imap/lib/opendrive/lanes.py
@@ -19,23 +19,12 @@
 
 import imap.global_var as global_var
 
-from imap.lib.common import shift_t, calc_length
+from imap.lib.common import shift_t, calc_length, binary_search
 from imap.lib.draw import draw_line
 
 from imap.lib.opendrive.common import convert_speed
 
 
-def binary_search(arr, val):
-    left, right = 0, len(arr) - 1
-    while left <= right:
-        mid = math.floor((left + right)/2)
-        if arr[mid] <= val:
-            left = mid + 1
-        else:
-            right = mid - 1
-    return left - 1
-
-
 def is_adjacent(road_marks) -> bool:
     if not road_marks:
         return True
diff --git a/imap/lib/opendrive/objects.py b/imap/lib/opendrive/objects.py
index ea2fb08..7d5c46f 100644
--- a/imap/lib/opendrive/objects.py
+++ b/imap/lib/opendrive/objects.py
@@ -17,6 +17,7 @@
 import abc
 from enum import Enum
 
+from imap.lib.common import shift_t, binary_search
 
 class ObjectType(Enum):
     BARRIER = "barrier"
@@ -83,6 +84,16 @@ class ParkingSpace(Object):
     def __init__(self) -> None:
         pass
 
+    def process_corners(self, reference_line):
+        # Find the point closest to s, considering adding interpolation
+        idx = binary_search([point3d.s for point3d in reference_line], self.s)
+        reference_point3d = reference_line[idx]
+        inertial_point3d = shift_t(point3d, self.t * self.direction)
+        center = [inertial_point3d.x, inertial_point3d.y]
+        corners = get_rotated_rectangle_points(center, self.hdg, self.height, self.width)
+        return corners
+
+
 class Pole(Object):
     def __init__(self) -> None:
         pass
diff --git a/setup.py b/setup.py
index 062195c..8bd2a35 100644
--- a/setup.py
+++ b/setup.py
@@ -5,7 +5,7 @@
 
 setuptools.setup(
     name="imap_box",
-    version="0.1.9",
+    version="0.1.10",
     author="daohu527",
     author_email="daohu527@gmail.com",
     description="High-resolution map visualization and conversion tool",