diff --git a/qc_opendrive/base/utils.py b/qc_opendrive/base/utils.py index 6ab84b0..fa5a883 100644 --- a/qc_opendrive/base/utils.py +++ b/qc_opendrive/base/utils.py @@ -1546,8 +1546,11 @@ 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) @@ -1555,9 +1558,9 @@ def get_point_xyz_from_road( 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 diff --git a/tests/test_utils.py b/tests/test_utils.py index 6d04ccb..62aee13 100644 --- a/tests/test_utils.py +++ b/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,