Update

Signed-off-by: hoangtungdinh <[email protected]>

qc_opendrive/base/utils.py

@@ -1026,18 +1026,11 @@ def get_corresponding_road_geometry_by_s(
     if len(geometries) == 0:
         return None
 
-    previous_geometry = None
-    for geometry in geometries:
-        geometry_s = get_s_from_geometry(geometry)
-        if geometry_s is None:
-            continue
-
-        if geometry_s > s:
-            return previous_geometry
+    geometry_indexes = [get_s_from_geometry(g) for g in geometries]
+    geometry_index = np.searchsorted(geometry_indexes, s, side="right") - 1
+    geometry_index = max(geometry_index, 0)
 
-        previous_geometry = geometry
-
-    return geometries[-1]
+    return geometries[geometry_index]
 
 
 def get_geometry_arc(geometry: etree._Element) -> Union[None, etree._Element]:
@@ -1253,15 +1246,11 @@ def get_corresponding_road_elevation_by_s(
     if len(elevation_list) == 0:
         return ZERO_OFFSET_POLY3
 
-    previous_elevation = ZERO_OFFSET_POLY3
-
-    for elevation in elevation_list:
-        if elevation.s_offset > s:
-            return previous_elevation
+    elevation_indexes = [e.s_offset for e in elevation_list]
+    elevation_index = np.searchsorted(elevation_indexes, s, side="right") - 1
+    elevation_index = max(elevation_index, 0)
 
-        previous_elevation = elevation
-
-    return elevation_list[-1]
+    return elevation_list[elevation_index]
 
 
 def calculate_elevation_value(elevation: models.OffsetPoly3, s: float) -> float:
@@ -1512,15 +1501,11 @@ def get_corresponding_road_superelevation_by_s(
     if len(superelevations) == 0:
         return ZERO_OFFSET_POLY3
 
-    previous_superelevation = ZERO_OFFSET_POLY3
-
-    for superelevation in superelevations:
-        if superelevation.s_offset > s:
-            return previous_superelevation
-
-        previous_superelevation = superelevation
+    superelevation_indexes = [e.s_offset for e in superelevations]
+    superelevation_index = np.searchsorted(superelevation_indexes, s, side="right") - 1
+    superelevation_index = max(superelevation_index, 0)
 
-    return superelevations[-1]
+    return superelevations[superelevation_index]
 
 
 def get_roll_from_road_reference_line(