Update

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

qc_opendrive/base/utils.py

@@ -1546,18 +1546,21 @@ def get_point_xyz_from_road(
     if yaw is None or pitch is None or roll is None:
         return None
 
-    rotation = transforms3d.euler.euler2mat(yaw, pitch, roll, "rzyx")
-    d_point = rotation.dot(np.array([0.0, t, h]))
+    surface_rotation = transforms3d.euler.euler2mat(yaw, pitch, roll, "rzyx")
+    d_point_surface = surface_rotation.dot(np.array([0.0, t, 0.0]))
+
+    height_rotation = transforms3d.euler.euler2mat(yaw, 0.0, roll, "rzyx")
+    d_point_height = height_rotation.dot(np.array([0.0, 0.0, h]))
 
     ref_line_point = get_point_xyz_from_road_reference_line(road, s)
 
     if ref_line_point is None:
         return None
 
     point_xyz = models.Point3D(
-        x=ref_line_point.x + d_point[0],
-        y=ref_line_point.y + d_point[1],
-        z=ref_line_point.z + d_point[2],
+        x=ref_line_point.x + d_point_surface[0] + d_point_height[0],
+        y=ref_line_point.y + d_point_surface[1] + d_point_height[1],
+        z=ref_line_point.z + d_point_surface[2] + d_point_height[2],
     )
 
     return point_xyz

tests/test_utils.py

@@ -49,6 +49,7 @@ def test_get_point_xyz_from_road_invalid_s() -> None:
     [
         ("simple_line.xodr", 30, -10, -20, 30, -10, -20),
         ("simple_line_heading.xodr", 30, 10, 20, -10, 30, 20),
+        ("simple_line_elevation.xodr", 30, -10, -20, 30, -10, 10),
         (
             "Ex_Line-Spiral-Arc.xodr",
             0,