diff --git a/RoadLogicSuite/Internal/Geometry/param_poly_3.cpp b/RoadLogicSuite/Internal/Geometry/param_poly_3.cpp
index e29fca58b8fff6dbd716266d6590c2ff6cc79296..80fdb7ec7d374a07d07e9b90b74f0d2b34ced5bb 100644
--- a/RoadLogicSuite/Internal/Geometry/param_poly_3.cpp
+++ b/RoadLogicSuite/Internal/Geometry/param_poly_3.cpp
@@ -149,7 +149,7 @@ Vec2<units::length::meter_t> ParamPoly3::CalcDirectionAt(const units::length::me
auto local_heading = Vec2<units::length::meter_t>{units::length::meter_t(u_poly.CalcDerivativeAt(distance_query)),
units::length::meter_t(v_poly.CalcDerivativeAt(distance_query))};
- return local_heading.Rotate(hdg) / local_heading.Length().value();
+ return local_heading / local_heading.Length().value();
}
units::angle::radian_t ParamPoly3::GetHeadingAt(const units::length::meter_t& s) const
diff --git a/RoadLogicSuite/Internal/Geometry/param_poly_3_test.cpp b/RoadLogicSuite/Internal/Geometry/param_poly_3_test.cpp
index 9fcd94bc82f926e1200f4c81394fae258a44f3e0..6283029e09b65521f941604d72d4672e87b8ed04 100644
--- a/RoadLogicSuite/Internal/Geometry/param_poly_3_test.cpp
+++ b/RoadLogicSuite/Internal/Geometry/param_poly_3_test.cpp
@@ -55,4 +55,22 @@ TEST(ParamPoly3Test,
ASSERT_NEAR(result.value(), 1.570656, kAbsErrorMax);
}
+TEST(ParamPoly3Test,
+ GivenSimpleParametricPolynomialRoad_WhenCalcDirectionAt_ThenCorrectDirectionAtFiftyMetersMustBeReturned)
+{
+ auto param_poly = ParamPoly3(units::length::meter_t(0),
+ Vec2<units::length::meter_t>(units::length::meter_t(0), units::length::meter_t(0)),
+ units::angle::radian_t(0),
+ units::length::meter_t(100),
+ "polynomial_road",
+ types::Poly3(0, 1, 0, 0),
+ types::Poly3(1.5, 1, -3.5, 1),
+ false);
+
+ const auto result = param_poly.CalcDirectionAt(units::length::meter_t(50));
+
+ ASSERT_NEAR(result.x.value(), 0.0001398, kAbsErrorMax);
+ ASSERT_NEAR(result.y.value(), 1.0, kAbsErrorMax);
+}
+
} // namespace road_logic_suite
diff --git a/RoadLogicSuite/Tests/Data/Maps/road_with_param_poly3_geometry.xodr b/RoadLogicSuite/Tests/Data/Maps/road_with_param_poly3_geometry.xodr
new file mode 100644
index 0000000000000000000000000000000000000000..9d23585c4c81aa834987673425b705aaeaa90fe7
--- /dev/null
+++ b/RoadLogicSuite/Tests/Data/Maps/road_with_param_poly3_geometry.xodr
@@ -0,0 +1,36 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes"?>
+<OpenDRIVE>
+ <header revMajor="1" revMinor="4" date="Thu Mar 26 14:00:00 CEST 2024" north="5000" south="-100" east="100" west="-1000">
+ <geoReference></geoReference>
+ </header>
+ <road name="Road0" length="100" id="0" junction="-1">
+ <link></link>
+ <type s="0.0" type="motorway" />
+ <planView>
+ <geometry s="0.0" x="0.0" y="0.0" hdg="0.0" length="100">
+ <paramPoly3 aU="0.000000000000e+00" bU="1.000000000000e+00" cU="-4.666602734948e-09" dU="-2.629787927644e-08" aV="0.000000000000e+00" bV="1.665334536938e-16" cV="-1.987729787588e-04" dV="-1.317158625579e-09" pRange="arcLength">
+ </paramPoly3>
+ </geometry>
+ </planView>
+ <lanes>
+ <laneSection s="0.0">
+ <center>
+ <lane id="0" type="driving" level="true">
+ <link />
+ <roadMark sOffset="0.0" type="solid" weight="standard" color="standard" width="0.13" />
+ </lane>
+ </center>
+ <right>
+ <lane id="-1" type="driving" level="true">
+ <width sOffset="0.0" a="3.5" b="0.0" c="0.0" d="0.0" />
+ <roadMark sOffset="0.0" type="broken" weight="standard" color="standard" width="0.13" />
+ </lane>
+ <lane id="-2" type="driving" level="true">
+ <width sOffset="0.0" a="3.5" b="0.0" c="0.0" d="0.0" />
+ <roadMark sOffset="0.0" type="broken" weight="standard" color="standard" width="0.13" />
+ </lane>
+ </right>
+ </laneSection>
+ </lanes>
+ </road>
+</OpenDRIVE>
diff --git a/RoadLogicSuite/Tests/MantleMapConvertIntegrationTest/BUILD b/RoadLogicSuite/Tests/MantleMapConvertIntegrationTest/BUILD
index 98b4be48966596e4ddfe3af954972e02a5a2ad37..ecfc79c47f2f693cb6e0915326d0a1a503820fab 100644
--- a/RoadLogicSuite/Tests/MantleMapConvertIntegrationTest/BUILD
+++ b/RoadLogicSuite/Tests/MantleMapConvertIntegrationTest/BUILD
@@ -118,3 +118,10 @@ cc_test(
srcs = ["pedestrain_crossing_integration_test.cpp"],
deps = ["//RoadLogicSuite/Tests/MantleMapConvertIntegrationTestFixture:mantle_map_convert_integration_test_fixture"],
)
+
+cc_test(
+ name = "param_poly3_integration_test",
+ timeout = "short",
+ srcs = ["param_poly3_integration_test.cpp"],
+ deps = ["//RoadLogicSuite/Tests/MantleMapConvertIntegrationTestFixture:mantle_map_convert_integration_test_fixture"],
+)
diff --git a/RoadLogicSuite/Tests/MantleMapConvertIntegrationTest/param_poly3_integration_test.cpp b/RoadLogicSuite/Tests/MantleMapConvertIntegrationTest/param_poly3_integration_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..03941fdd0f51895515aee60a009300437d76380e
--- /dev/null
+++ b/RoadLogicSuite/Tests/MantleMapConvertIntegrationTest/param_poly3_integration_test.cpp
@@ -0,0 +1,115 @@
+/*******************************************************************************
+ * Copyright (C) 2024, ANSYS, Inc.
+ *
+ * This program and the accompanying materials are made
+ * available under the terms of the Eclipse Public License 2.0
+ * which is available at https://www.eclipse.org/legal/epl-2.0/
+ *
+ * SPDX-License-Identifier: EPL-2.0
+ *******************************************************************************/
+
+#include "RoadLogicSuite/Tests/MantleMapConvertIntegrationTestFixture/mantle_map_convert_integration_test_fixture.h"
+
+#include <gmock/gmock.h>
+
+namespace road_logic_suite::test
+{
+using map_api::Lane;
+using map_api::LaneBoundary;
+using map_api::Map;
+using ::testing::ElementsAre;
+using units::literals::operator""_m;
+
+///////////////////////////////////////////////////////////
+/// @verbatim
+/// OpenDrive Map
+/// origin = (0.0, 0.0)
+/// |-----------------------| ref line
+/// | x x x (Lane -1) x x x |
+/// |-----------------------|
+/// | x x x (Lane -2) x x x |
+/// |-----------------------|
+/// ^ ^
+/// s(0) (100)
+/// x(0)
+///
+/// IDs:
+/// |LaneGroup: 9 |
+/// |-----------------------------|
+/// |Left LaneBoundary: 1 |
+/// |Lane: 3 |
+/// |Right LaneBoundary: 6 |
+/// |-----------------------------|
+/// |Left LaneBoundary: 6 |
+/// |Lane: 4 |
+/// |Right LaneBoundary:7 |
+/// |-----------------------------|
+///
+/// @endverbatim
+
+TEST_F(MapConvertIntegrationTestFixture,
+ GivenMapWithParametricPolynomialGeometry_WhenParsingMap_ThenConversionLaneAndLaneBoundariesAreCorrect)
+{
+
+ const std::string test_map = GetResolvedMapPathInternal("road_with_param_poly3_geometry.xodr");
+
+ const auto& map = GetMap(test_map);
+
+ ASSERT_EQ(map.lanes.size(), 2);
+ const auto& lane_3 = map.lanes[0];
+ const auto& lane_4 = map.lanes[1];
+
+ ASSERT_EQ(lane_3->left_lane_boundaries.size(), 1);
+ const auto& lane_boundary_1 = lane_3->left_lane_boundaries[0];
+ ASSERT_EQ(lane_3->right_lane_boundaries.size(), 1);
+ const auto& lane_boundary_6 = lane_3->right_lane_boundaries[0];
+ ASSERT_EQ(lane_4->right_lane_boundaries.size(), 1);
+ const auto& lane_boundary_7 = lane_4->right_lane_boundaries[0];
+
+ // Lane 3
+ EXPECT_THAT(lane_3->left_adjacent_lanes, ::testing::IsEmpty());
+ EXPECT_THAT(lane_3->right_adjacent_lanes, ElementsAre(*lane_4));
+ EXPECT_THAT(lane_3->centerline.front(), MantlePositionIsNear(MantlePosition{0.0_m, -1.75_m, 0.0_m}));
+ EXPECT_THAT(lane_3->centerline.back(), MantlePositionIsNear(MantlePosition{99.904_m, -3.73766_m, 0.0_m}));
+ ASSERT_THAT(lane_3->left_lane_boundaries, ElementsAre(lane_boundary_1));
+ ASSERT_THAT(lane_3->right_lane_boundaries, ElementsAre(lane_boundary_6));
+
+ // Lane 4
+ EXPECT_THAT(lane_4->left_adjacent_lanes, ElementsAre(*lane_3));
+ EXPECT_THAT(lane_4->centerline.front(), MantlePositionIsNear(MantlePosition{0.0_m, -5.25_m, 0.0_m}));
+ EXPECT_THAT(lane_4->centerline.back(), MantlePositionIsNear(MantlePosition{99.7647_m, -7.23489_m, 0.0_m}));
+ ASSERT_THAT(lane_4->left_lane_boundaries, ElementsAre(lane_boundary_6));
+ ASSERT_THAT(lane_4->right_lane_boundaries, ElementsAre(lane_boundary_7));
+
+ // Lane boundary 1
+ EXPECT_EQ(lane_boundary_1.get().type, LaneBoundary::Type::kSolidLine);
+ EXPECT_EQ(lane_boundary_1.get().color, LaneBoundary::Color::kStandard);
+ EXPECT_THAT(lane_boundary_1.get().boundary_line.front(),
+ LaneBoundaryIsEqual(
+ LaneBoundary::BoundaryPoint{.position = {0.0_m, 0.0_m, 0.0_m}, .width = 0.13_m, .height = 0.0_m}));
+ EXPECT_THAT(lane_boundary_1.get().boundary_line.back(),
+ LaneBoundaryIsEqual(LaneBoundary::BoundaryPoint{
+ .position = {99.9737_m, -1.98905_m, 0.0_m}, .width = 0.13_m, .height = 0.0_m}));
+
+ // Lane boundary 6
+ EXPECT_EQ(lane_boundary_6.get().type, LaneBoundary::Type::kDashedLine);
+ EXPECT_EQ(lane_boundary_6.get().color, LaneBoundary::Color::kStandard);
+ EXPECT_THAT(lane_boundary_6.get().boundary_line.front(),
+ LaneBoundaryIsEqual(
+ LaneBoundary::BoundaryPoint{.position = {0.0_m, -3.5_m, 0.0_m}, .width = 0.13_m, .height = 0.0_m}));
+ EXPECT_THAT(lane_boundary_6.get().boundary_line.back(),
+ LaneBoundaryIsEqual(LaneBoundary::BoundaryPoint{
+ .position = {99.8344_m, -5.48627_m, 0.0_m}, .width = 0.13_m, .height = 0.0_m}));
+
+ // Lane boundary 7
+ EXPECT_EQ(lane_boundary_7.get().type, LaneBoundary::Type::kDashedLine);
+ EXPECT_EQ(lane_boundary_7.get().color, LaneBoundary::Color::kStandard);
+ EXPECT_THAT(lane_boundary_7.get().boundary_line.front(),
+ LaneBoundaryIsEqual(
+ LaneBoundary::BoundaryPoint{.position = {0.0_m, -7.0_m, 0.0_m}, .width = 0.13_m, .height = 0.0_m}));
+ EXPECT_THAT(lane_boundary_7.get().boundary_line.back(),
+ LaneBoundaryIsEqual(LaneBoundary::BoundaryPoint{
+ .position = {99.6951_m, -8.9835_m, 0.0_m}, .width = 0.13_m, .height = 0.0_m}));
+}
+
+} // namespace road_logic_suite::test