Add ClothoidSpline to FollowTrajectoryAction

engine/src/Storyboard/MotionControlAction/FollowTrajectoryAction_impl.cpp

@@ -10,8 +10,11 @@
 
 #include "Storyboard/MotionControlAction/FollowTrajectoryAction_impl.h"
 
+#include <MantleAPI/Common/clothoid_spline.h>
 #include <MantleAPI/Traffic/entity_helper.h>
 
+#include <variant>
+
 #include "Utils/EntityUtils.h"
 
 namespace OpenScenarioEngine::v1_3
@@ -39,8 +42,41 @@ mantle_api::Trajectory ConvertPolyLine(
   return trajectoryRef.value();
 }
 
+mantle_api::Trajectory ConvertClothoidSpline(
+    const std::shared_ptr<mantle_api::IEnvironment>& environment,
+    const std::string& entity,
+    TrajectoryRef trajectoryRef)
+{
+  const auto& refEntity = EntityUtils::GetEntityByName(environment, entity);
+  const auto geometric_center = refEntity.GetProperties()->bounding_box.geometric_center;
+
+  auto& clothoid_spline = std::get<mantle_api::ClothoidSpline>(trajectoryRef->type);
+
+  for (auto& segment : clothoid_spline.segments)
+  {
+    if (segment.position_start.has_value())
+    {
+      segment.position_start.value().position = environment->GetGeometryHelper()->TranslateGlobalPositionLocally(
+          segment.position_start.value().position,
+          segment.position_start.value().orientation,
+          geometric_center);
+    }
+  }
+
+  return trajectoryRef.value();
+}
+
 }  // namespace detail
 
+template <class... Ts>
+struct overloaded : Ts...
+{
+  using Ts::operator()...;
+};
+
+template <class... Ts>
+overloaded(Ts...) -> overloaded<Ts...>;
+
 void FollowTrajectoryAction::SetControlStrategy()
 {
   if (!values.trajectoryRef)
@@ -57,7 +93,23 @@ void FollowTrajectoryAction::SetControlStrategy()
     control_strategy->movement_domain = values.timeReference
                                             ? mantle_api::MovementDomain::kBoth
                                             : mantle_api::MovementDomain::kLateral;
-    control_strategy->trajectory = detail::ConvertPolyLine(mantle.environment, entity, values.trajectoryRef);
+
+    std::visit(
+        overloaded{
+            [&](const mantle_api::PolyLine&)
+            {
+              control_strategy->trajectory = detail::ConvertPolyLine(mantle.environment, entity, values.trajectoryRef);
+            },
+            [&](const mantle_api::ClothoidSpline&)
+            {
+              control_strategy->trajectory = detail::ConvertClothoidSpline(mantle.environment, entity, values.trajectoryRef);
+            },
+            [](auto)
+            {
+              throw std::runtime_error("Unsupported type of Trajectory shape");
+            }},
+        values.trajectoryRef->type);
+
     const auto entity_id = mantle.environment->GetEntityRepository().Get(entity)->get().GetUniqueId();
     mantle.environment->UpdateControlStrategies(entity_id, {control_strategy});
   }
@@ -79,4 +131,4 @@ mantle_api::MovementDomain FollowTrajectoryAction::GetMovementDomain() const
   return mantle_api::MovementDomain::kBoth;
 }
 
-}  // namespace OpenScenarioEngine::v1_3
\ No newline at end of file
+}  // namespace OpenScenarioEngine::v1_3