Draft: added distance dynamic dimension in speed action

added distance dynamic dimension in speed action

src/Storyboard/Actions/SpeedAction.cpp

@@ -86,18 +86,10 @@ void SpeedAction::StartAction()
         {
             // TODO: Add CUBIC once it's clear how the shape should look like and how the coefficients can be calculated
             auto dynamics_dimension = speed_action_dynamics->GetDynamicsDimension();
-            if (dynamics_dimension == NET_ASAM_OPENSCENARIO::v1_1::DynamicsDimension::TIME ||
-                dynamics_dimension == NET_ASAM_OPENSCENARIO::v1_1::DynamicsDimension::RATE)
-            {
-                for (auto actor : actors_)
-                {
-                    SetLinearVelocitySplineControlStrategy(actor, target_speed, speed_action_dynamics.get());
-                }
-            }
-            else
+
+            for (auto actor : actors_)
             {
-                std::cout << "Dynamics dimension " << dynamics_dimension.GetLiteral()
-                          << " not yet implemented for shapes LINEAR in SpeedAction" << std::endl;
+                SetLinearVelocitySplineControlStrategy(actor, target_speed, speed_action_dynamics.get());
             }
         }
         else
@@ -120,12 +112,12 @@ bool SpeedAction::HasControlStrategyGoalBeenReached(const std::string& actor)
                NET_ASAM_OPENSCENARIO::v1_1::DynamicsShape::STEP ||
            (speed_action_data_->GetSpeedActionDynamics()->GetDynamicsShape() ==
                 NET_ASAM_OPENSCENARIO::v1_1::DynamicsShape::LINEAR &&
-            environment_->HasControlStrategyGoalBeenReached(
-                entity.value().get().GetUniqueId(),
-                mantle_api::ControlStrategyType::kFollowVelocitySpline));
+            environment_->HasControlStrategyGoalBeenReached(entity.value().get().GetUniqueId(),
+                                                            mantle_api::ControlStrategyType::kFollowVelocitySpline));
 }
 
-units::velocity::meters_per_second_t SpeedAction::GetTargetSpeed(NET_ASAM_OPENSCENARIO::v1_1::ISpeedActionTarget* speed_action_target)
+units::velocity::meters_per_second_t SpeedAction::GetTargetSpeed(
+    NET_ASAM_OPENSCENARIO::v1_1::ISpeedActionTarget* speed_action_target)
 {
     units::velocity::meters_per_second_t target_speed{0};
     auto absolute_target_speed = speed_action_target->GetAbsoluteTargetSpeed();
@@ -141,9 +133,9 @@ units::velocity::meters_per_second_t SpeedAction::GetTargetSpeed(NET_ASAM_OPENSC
 
         if (!environment_->GetEntityRepository().Get(relative_entity_name).has_value())
         {
-            throw std::runtime_error(
-                "Relative Entity with name \"" + relative_entity_name + "\" of SpeedAction  does not exist. Please adjust the "
-                "scenario.");
+            throw std::runtime_error("Relative Entity with name \"" + relative_entity_name +
+                                     "\" of SpeedAction  does not exist. Please adjust the "
+                                     "scenario.");
         }
 
         auto target_base = environment_->GetEntityRepository().Get(relative_entity_name).value().get().GetVelocity();
@@ -179,31 +171,45 @@ void SpeedAction::SetLinearVelocitySplineControlStrategy(
         return;
     }
 
-    const auto &entity = environment_->GetEntityRepository().Get(actor).value().get();
+    const auto& entity = environment_->GetEntityRepository().Get(actor).value().get();
 
     mantle_api::SplineSection<units::velocity::meters_per_second> spline_section;
     spline_section.start_time = mantle_api::Time(0);
     if (transition_dynamics->GetDynamicsDimension() == NET_ASAM_OPENSCENARIO::v1_1::DynamicsDimension::TIME)
     {
         spline_section.end_time = units::time::second_t(transition_dynamics->GetValue());
-        std::get<2>(spline_section.polynomial) = (target_speed - entity.GetVelocity().Length()) / units::time::second_t(transition_dynamics->GetValue());
+        std::get<2>(spline_section.polynomial) =
+            (target_speed - entity.GetVelocity().Length()) / units::time::second_t(transition_dynamics->GetValue());
     }
     else if (transition_dynamics->GetDynamicsDimension() == NET_ASAM_OPENSCENARIO::v1_1::DynamicsDimension::RATE)
     {
-        spline_section.end_time =
-            units::math::abs(target_speed - entity.GetVelocity().Length()) /
-            units::acceleration::meters_per_second_squared_t(transition_dynamics->GetValue());
+        spline_section.end_time = units::math::abs(target_speed - entity.GetVelocity().Length()) /
+                                  units::acceleration::meters_per_second_squared_t(transition_dynamics->GetValue());
         std::get<2>(spline_section.polynomial) =
             units::math::copysign(units::acceleration::meters_per_second_squared_t(transition_dynamics->GetValue()),
                                   target_speed - entity.GetVelocity().Length());
     }
+    else if (transition_dynamics->GetDynamicsDimension() == NET_ASAM_OPENSCENARIO::v1_1::DynamicsDimension::DISTANCE)
+    {
+        auto start_speed = entity.GetVelocity().Length().value();
+        // Distance has to be converted to time, since spline is expected over time, not distance
+        //  distance = time(v_init + v_delta/2) = time(v_init + v_end)/2 => time = 2*distance/(v_init + v_end)
+        spline_section.end_time =
+            units::time::second_t(2 * transition_dynamics->GetValue() / (target_speed.value() - start_speed));
+        // v_end = v_init + acceleration*time, where time = 2*distance/(v_init + v_end) then acceleration = (v_end -
+        // v_init) * (v_end + v_init) / 2* distance
+        auto rate = units::acceleration::meters_per_second_squared_t((target_speed.value() - start_speed) *
+                                                                     (target_speed.value() + start_speed) / 2 *
+                                                                     transition_dynamics->GetValue());
+        std::get<2>(spline_section.polynomial) = units::math::copysign(
+            (units::acceleration::meters_per_second_squared_t(rate)), target_speed - entity.GetVelocity().Length());
+    }
     std::get<3>(spline_section.polynomial) = entity.GetVelocity().Length();
 
     auto control_strategy = std::make_shared<mantle_api::FollowVelocitySplineControlStrategy>();
     control_strategy->default_value = target_speed;
     control_strategy->velocity_splines.push_back(spline_section);
-    environment_->UpdateControlStrategies(entity.GetUniqueId(),
-                                          {control_strategy});
+    environment_->UpdateControlStrategies(entity.GetUniqueId(), {control_strategy});
 }
 
 }  // namespace OPENSCENARIO