Optimal Steps Model

libjupedsim/include/jupedsim/jupedsim.h

@@ -105,7 +105,8 @@ typedef struct JPS_Waypoint {
  */
 typedef enum JPS_ModelType {
     JPS_GeneralizedCentrifugalForceModel,
-    JPS_CollisionFreeSpeedModel
+    JPS_CollisionFreeSpeedModel,
+    JPS_OptimalStepsModel
 } JPS_ModelType;
 
 /**
@@ -297,6 +298,44 @@ JUPEDSIM_API JPS_OperationalModel JPS_CollisionFreeSpeedModelBuilder_Build(
 JUPEDSIM_API void
 JPS_CollisionFreeSpeedModelBuilder_Free(JPS_CollisionFreeSpeedModelBuilder handle);
 
+/**
+ * Opaque type for a Collision Free Speed Model Builder
+ */
+typedef struct JPS_OptimalStepsModelBuilder_t* JPS_OptimalStepsModelBuilder;
+
+/**
+ * Creates a Collision Free Speed Model builder.
+ * @param strength_neighbor_repulsion describes the strength with which neighbors repulse each
+ * other.
+ * @param range_neighbor_repulsion describes the range at hich neighbors repulse each other.
+ * @param strength_geometry_repulsion describes the strength with which neighbors are repules by
+ * geometry.
+ * @param range_geometry_repulsion describes the range at hich neighbors are repulsed by geometry.
+ * @return the builder
+ */
+JUPEDSIM_API JPS_OptimalStepsModelBuilder JPS_OptimalStepsModelBuilder_Create(
+    double strengthNeighborRepulsion,
+    double rangeNeighborRepulsion,
+    double strengthGeometryRepulsion,
+    double rangeGeometryRepulsion);
+
+/**
+ * Creates a JPS_OperationalModel of type Collision Free Speed Model from the
+ * JPS_GeneralizedCentrifugalForceModelBuilder.
+ * @param handle the builder to operate on
+ * @param[out] errorMessage if not NULL: will be set to a JPS_ErrorMessage in case of an error
+ * @return a JPS_GeneralizedCentrifugalForceModel or NULL if an error occured.
+ */
+JUPEDSIM_API JPS_OperationalModel JPS_OptimalStepsModelBuilder_Build(
+    JPS_OptimalStepsModelBuilder handle,
+    JPS_ErrorMessage* errorMessage);
+
+/**
+ * Frees a JPS_OptimalStepsModelBuilder
+ * @param handle to the JPS_OptimalStepsModelBuilder to free.
+ */
+JUPEDSIM_API void JPS_OptimalStepsModelBuilder_Free(JPS_OptimalStepsModelBuilder handle);
+
 /**
  * Opaque type that represents the geometry the simulation acts on.
  * This type is created from JPS_GeometryBuilder.
@@ -734,6 +773,68 @@ JPS_CollisionFreeSpeedModelState_GetRadius(JPS_CollisionFreeSpeedModelState hand
 JUPEDSIM_API void
 JPS_CollisionFreeSpeedModelState_SetRadius(JPS_CollisionFreeSpeedModelState handle, double radius);
 
+/**
+ * Opaque type of Optimal Steps model state
+ */
+typedef struct JPS_OptimalStepsModelState_t* JPS_OptimalStepsModelState;
+
+/**
+ * Read e0 of this agent.
+ * @param handle of the Agent to access.
+ * @return e0 of this agent
+ */
+JUPEDSIM_API JPS_Point JPS_OptimalStepsModelState_GetE0(JPS_OptimalStepsModelState handle);
+
+/**
+ * Write e0 of this agent.
+ * @param handle of the Agent to access.
+ * @param e0 of this agent.
+ */
+JUPEDSIM_API void JPS_OptimalStepsModelState_SetE0(JPS_OptimalStepsModelState handle, JPS_Point e0);
+
+/**
+ * Read tau of this agent.
+ * @param handle of the Agent to access.
+ * @return tau of this agent
+ */
+JUPEDSIM_API double JPS_OptimalStepsModelState_GetTau(JPS_OptimalStepsModelState handle);
+
+/**
+ * Write tau of this agent.
+ * @param handle of the Agent to access.
+ * @param tau of this agent.
+ */
+JUPEDSIM_API void JPS_OptimalStepsModelState_SetTau(JPS_OptimalStepsModelState handle, double tau);
+
+/**
+ * Read v0 of this agent.
+ * @param handle of the Agent to access.
+ * @return v0 of this agent
+ */
+JUPEDSIM_API double JPS_OptimalStepsModelState_GetV0(JPS_OptimalStepsModelState handle);
+
+/**
+ * Write v0 of this agent.
+ * @param handle of the Agent to access.
+ * @param v0 of this agent.
+ */
+JUPEDSIM_API void JPS_OptimalStepsModelState_SetV0(JPS_OptimalStepsModelState handle, double v0);
+
+/**
+ * Read radius of this agent.
+ * @param handle of the Agent to access.
+ * @return radius of this agent
+ */
+JUPEDSIM_API double JPS_OptimalStepsModelState_GetRadius(JPS_OptimalStepsModelState handle);
+
+/**
+ * Write radius of this agent in meters.
+ * @param handle of the Agent to access.
+ * @param radius (m) of this agent.
+ */
+JUPEDSIM_API void
+JPS_OptimalStepsModelState_SetRadius(JPS_OptimalStepsModelState handle, double radius);
+
 /**
  * Identifies the type of stage
  */
@@ -874,6 +975,16 @@ JPS_Agent_GetGeneralizedCentrifugalForceModelState(
 JUPEDSIM_API JPS_CollisionFreeSpeedModelState
 JPS_Agent_GetCollisionFreeSpeedModelState(JPS_Agent handle, JPS_ErrorMessage* errorMessage);
 
+/**
+ * Access Optimal Steps model state.
+ * Precondition: Agent needs to use Optimal Steps model
+ * @param handle of the agent to access.
+ * @param[out] errorMessage if not NULL: will be set to a JPS_ErrorMessage in case of an error.
+ * @return state or NULL on error
+ */
+JUPEDSIM_API JPS_OptimalStepsModelState
+JPS_Agent_GetOptimalModelState(JPS_Agent handle, JPS_ErrorMessage* errorMessage);
+
 /**
  * Opaque type of an iterator over agents
  */
@@ -985,6 +1096,35 @@ typedef struct JPS_CollisionFreeSpeedModelAgentParameters {
     double radius = 0.2;
 } JPS_CollisionFreeSpeedModelAgentParameters;
 
+/**
+ * Describes parameters of an Agent in GeneralizedCentrifugalForceModel
+ */
+typedef struct JPS_OptimalStepsModelAgentParameters {
+    /**
+     * Position of the agent.
+     * The position needs to inside the accessible area.
+     */
+    JPS_Point position{0, 0};
+    /**
+     * Defines the journey this agent will take use
+     */
+    JPS_JourneyId journeyId = 0;
+    /**
+     * Defines the current stage of its journey
+     */
+    JPS_StageId stageId = 0;
+
+    // TODO add doc
+    /**
+     *@param v0 of the agents using this profile(desired speed) double radius;
+     */
+    double v0 = 1.2;
+    /**
+     *@param radius of the agent in 'meters'
+     */
+    double radius = 0.2;
+} JPS_OptimalStepsModelAgentParameters;
+
 /**
  * Opaque type of an iterator over agent ids
  */
@@ -1157,6 +1297,22 @@ JUPEDSIM_API JPS_AgentId JPS_Simulation_AddCollisionFreeSpeedModelAgent(
     JPS_CollisionFreeSpeedModelAgentParameters parameters,
     JPS_ErrorMessage* errorMessage);
 
+/**
+ * Adds a new agent to the simulation.
+ * This can be called at any time, i.e. agents can be added at any iteration.
+ * NOTE: Currently there is no checking done to ensure the agent can be placed at the desired
+ * location.
+ * @param handle to the simulation to act on
+ * @param parameters describing the new agent.
+ * @param[out] errorMessage if not NULL. Will contain address of JPS_ErrorMessage in case of an
+ * error.
+ * @return id of the new agent or 0 if the agent could not be added due to an error.
+ */
+JUPEDSIM_API JPS_AgentId JPS_Simulation_AddOptimalStepsModelAgent(
+    JPS_Simulation handle,
+    JPS_OptimalStepsModelAgentParameters parameters,
+    JPS_ErrorMessage* errorMessage);
+
 /**
  * Marks an agent from the simuation for removal.
  * The agent will be removed at the start of the next simulation iteration, before the interaction

libjupedsim/src/jupedsim.cpp

@@ -22,6 +22,7 @@
 #include <Logger.hpp>
 #include <OperationalModel.hpp>
 #include <OperationalModelType.hpp>
+#include <OptimalStepsModelBuilder.hpp>
 #include <Point.hpp>
 #include <RoutingEngine.hpp>
 #include <Simulation.hpp>
@@ -215,6 +216,49 @@ JUPEDSIM_API void JPS_CollisionFreeSpeedModelBuilder_Free(JPS_CollisionFreeSpeed
     delete reinterpret_cast<CollisionFreeSpeedModelBuilder*>(handle);
 }
 
+////////////////////////////////////////////////////////////////////////////////////////////////////
+/// Collision Free Speed Model Builder
+////////////////////////////////////////////////////////////////////////////////////////////////////
+JUPEDSIM_API JPS_OptimalStepsModelBuilder JPS_OptimalStepsModelBuilder_Create(
+    double strengthNeighborRepulsion,
+    double rangeNeighborRepulsion,
+    double strengthGeometryRepulsion,
+    double rangeGeometryRepulsion)
+{
+    return reinterpret_cast<JPS_OptimalStepsModelBuilder>(new OptimalStepsModelBuilder(
+        strengthNeighborRepulsion,
+        rangeNeighborRepulsion,
+        strengthGeometryRepulsion,
+        rangeGeometryRepulsion));
+}
+
+JUPEDSIM_API JPS_OperationalModel JPS_OptimalStepsModelBuilder_Build(
+    JPS_OptimalStepsModelBuilder handle,
+    JPS_ErrorMessage* errorMessage)
+{
+    assert(handle != nullptr);
+    auto builder = reinterpret_cast<OptimalStepsModelBuilder*>(handle);
+    JPS_OperationalModel result{};
+    try {
+        result = reinterpret_cast<JPS_OperationalModel>(new OptimalStepsModel(builder->Build()));
+    } catch(const std::exception& ex) {
+        if(errorMessage) {
+            *errorMessage = reinterpret_cast<JPS_ErrorMessage>(new JPS_ErrorMessage_t{ex.what()});
+        }
+    } catch(...) {
+        if(errorMessage) {
+            *errorMessage = reinterpret_cast<JPS_ErrorMessage>(
+                new JPS_ErrorMessage_t{"Unknown internal error."});
+        }
+    }
+    return result;
+}
+
+JUPEDSIM_API void JPS_OptimalStepsModelBuilder_Free(JPS_OptimalStepsModelBuilder handle)
+{
+    delete reinterpret_cast<OptimalStepsModelBuilder*>(handle);
+}
+
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 /// GeometryBuilder
 ////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -548,6 +592,36 @@ void JPS_CollisionFreeSpeedModelState_SetRadius(
     auto state = reinterpret_cast<CollisionFreeSpeedModelData*>(handle);
     state->radius = radius;
 }
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+/// OptimalStepsModelState
+////////////////////////////////////////////////////////////////////////////////////////////////////
+double JPS_OptimalStepsModelState_GetV0(JPS_OptimalStepsModelState handle)
+{
+    assert(handle);
+    const auto state = reinterpret_cast<const OptimalStepsModelData*>(handle);
+    return state->v0;
+}
+
+void JPS_OptimalStepsModelState_SetV0(JPS_OptimalStepsModelState handle, double v0)
+{
+    assert(handle);
+    auto state = reinterpret_cast<OptimalStepsModelData*>(handle);
+    state->v0 = v0;
+}
+double JPS_OptimalStepsModelState_GetRadius(JPS_OptimalStepsModelState handle)
+{
+    assert(handle);
+    const auto state = reinterpret_cast<const OptimalStepsModelData*>(handle);
+    return state->radius;
+}
+
+void JPS_OptimalStepsModelState_SetRadius(JPS_OptimalStepsModelState handle, double radius)
+{
+    assert(handle);
+    auto state = reinterpret_cast<OptimalStepsModelData*>(handle);
+    state->radius = radius;
+}
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 /// NotifiableQueueProxy
 ////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -816,6 +890,26 @@ JPS_Agent_GetCollisionFreeSpeedModelState(JPS_Agent handle, JPS_ErrorMessage* er
     return nullptr;
 }
 
+JPS_OptimalStepsModelState
+JPS_Agent_GetOptimalStepsModelState(JPS_Agent handle, JPS_ErrorMessage* errorMessage)
+{
+    assert(handle);
+    const auto agent = reinterpret_cast<GenericAgent*>(handle);
+    try {
+        auto& model = std::get<OptimalStepsModelData>(agent->model);
+        return reinterpret_cast<JPS_OptimalStepsModelState>(&model);
+    } catch(const std::exception& ex) {
+        if(errorMessage) {
+            *errorMessage = reinterpret_cast<JPS_ErrorMessage>(new JPS_ErrorMessage_t{ex.what()});
+        }
+    } catch(...) {
+        if(errorMessage) {
+            *errorMessage = reinterpret_cast<JPS_ErrorMessage>(
+                new JPS_ErrorMessage_t{"Unknown internal error."});
+        }
+    }
+    return nullptr;
+}
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 /// AgentIterator
 ////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -1208,6 +1302,39 @@ JPS_AgentId JPS_Simulation_AddCollisionFreeSpeedModelAgent(
     return result.getID();
 }
 
+JPS_AgentId JPS_Simulation_AddOptimalStepsModelAgent(
+    JPS_Simulation handle,
+    JPS_OptimalStepsModelAgentParameters parameters,
+    JPS_ErrorMessage* errorMessage)
+{
+    assert(handle);
+    auto result = GenericAgent::ID::Invalid;
+    auto simulation = reinterpret_cast<Simulation*>(handle);
+    try {
+        if(simulation->ModelType() != OperationalModelType::OPTIMAL_STEPS) {
+            throw std::runtime_error("Simulation is not configured to use Optimal Steps Model");
+        }
+        GenericAgent agent(
+            GenericAgent::ID::Invalid,
+            Journey::ID(parameters.journeyId),
+            BaseStage::ID(parameters.stageId),
+            intoPoint(parameters.position),
+            {},
+            OptimalStepsModelData(parameters.v0, parameters.radius));
+        result = simulation->AddAgent(std::move(agent));
+    } catch(const std::exception& ex) {
+        if(errorMessage) {
+            *errorMessage = reinterpret_cast<JPS_ErrorMessage>(new JPS_ErrorMessage_t{ex.what()});
+        }
+    } catch(...) {
+        if(errorMessage) {
+            *errorMessage = reinterpret_cast<JPS_ErrorMessage>(
+                new JPS_ErrorMessage_t{"Unknown internal error."});
+        }
+    }
+    return result.getID();
+}
+
 bool JPS_Simulation_MarkAgentForRemoval(
     JPS_Simulation handle,
     JPS_AgentId agentId,
@@ -1359,6 +1486,8 @@ JPS_ModelType JPS_Simulation_ModelType(JPS_Simulation handle)
             return JPS_CollisionFreeSpeedModel;
         case OperationalModelType::GENERALIZED_CENTRIFUGAL_FORCE:
             return JPS_GeneralizedCentrifugalForceModel;
+        case OperationalModelType::OPTIMAL_STEPS:
+            return JPS_OptimalStepsModel;
     }
     UNREACHABLE();
 }

libsimulator/CMakeLists.txt

@@ -36,13 +36,21 @@ add_library(simulator STATIC
     src/Mathematics.cpp
     src/Mathematics.hpp
     src/NeighborhoodSearch.hpp
+    src/OperationalDecisionSystem.cpp
     src/OperationalDecisionSystem.hpp
     src/OperationalModel.hpp
     src/OperationalModelUpdate.hpp
+    src/OptimalStepsModel.cpp
+    src/OptimalStepsModel.hpp
+    src/OptimalStepsModelBuilder.cpp
+    src/OptimalStepsModelBuilder.hpp
+    src/OptimalStepsModelData.hpp
+    src/OptimalStepsModelUpdate.hpp
     src/Point.cpp
     src/Point.hpp
     src/Polygon.cpp
     src/Polygon.hpp
+    src/RNG.hpp
     src/RoutingEngine.cpp
     src/RoutingEngine.hpp
     src/Simulation.cpp