Add Social Force Model (#1317)

Add the Social Force Model described in Helbing, D., Farkas, I. & Vicsek, T. Simulating dynamical features of escape panic. Nature 407, 487–490 (2000). https://doi.org/10.1038/35035023.

libjupedsim/CMakeLists.txt

@@ -18,6 +18,7 @@ add_library(jupedsim_obj OBJECT
     src/logging.cpp
     src/operational_model.cpp
     src/simulation.cpp
+    src/social_force_model.cpp
     src/stage.cpp
     src/routing.cpp
 )

libjupedsim/include/jupedsim/agent.h

@@ -7,6 +7,7 @@
 #include "error.h"
 #include "export.h"
 #include "generalized_centrifugal_force_model.h"
+#include "social_force_model.h"
 #include "types.h"
 
 #include <stdbool.h> /*NOLINT(modernize-deprecated-headers)*/
@@ -95,6 +96,16 @@ JPS_Agent_GetGeneralizedCentrifugalForceModelState(
 JUPEDSIM_API JPS_CollisionFreeSpeedModelState
 JPS_Agent_GetCollisionFreeSpeedModelState(JPS_Agent handle, JPS_ErrorMessage* errorMessage);
 
+/**
+ * Access Social Force model state.
+ * Precondition: Agent needs to use Social Force 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_SocialForceModelState
+JPS_Agent_GetSocialForceModelState(JPS_Agent handle, JPS_ErrorMessage* errorMessage);
+
 /**
  * Access Collision Free Speed model v2 state.
  * Precondition: Agent needs to use Collision Free Speed model v2

libjupedsim/include/jupedsim/jupedsim.h

@@ -15,6 +15,7 @@
 #include "operational_model.h"
 #include "routing.h"
 #include "simulation.h"
+#include "social_force_model.h"
 #include "stage.h"
 #include "transition.h"
 #include "types.h"

libjupedsim/include/jupedsim/simulation.h

@@ -11,6 +11,7 @@
 #include "geometry.h"
 #include "journey.h"
 #include "operational_model.h"
+#include "social_force_model.h"
 #include "stage.h"
 #include "types.h"
 
@@ -187,6 +188,22 @@ JUPEDSIM_API JPS_AgentId JPS_Simulation_AddCollisionFreeSpeedModelv2Agent(
     JPS_CollisionFreeSpeedModelv2AgentParameters 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_AddSocialForceModelAgent(
+    JPS_Simulation handle,
+    JPS_SocialForceModelAgentParameters 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/include/jupedsim/social_force_model.h

@@ -0,0 +1,229 @@
+// Copyright © 2012-2024 Forschungszentrum Jülich GmbH
+// SPDX-License-Identifier: LGPL-3.0-or-later
+#pragma once
+
+#include "error.h"
+#include "export.h"
+#include "operational_model.h"
+#include "types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Opaque type for a SocialForceModel Builder
+ */
+typedef struct JPS_SocialForceModelBuilder_t* JPS_SocialForceModelBuilder;
+
+/**
+ * Creates a SocialForceModel builder.
+ * @param bodyForce describes the strength with which an agent is influenced
+ * by pushing forces from obstacles and neighbors in its direct proximity.
+ * @param friction describes the strength with which an agent is influenced
+ * by frictional forces from obstacles and neighbors in its direct proximity.
+ * */
+JUPEDSIM_API JPS_SocialForceModelBuilder
+JPS_SocialForceModelBuilder_Create(double bodyForce, double friction);
+
+/**
+ * Creates a JPS_OperationalModel of type SocialForceModel Model from the
+ * JPS_SocialForceModelBuilder.
+ * @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_SocialForceModel or NULL if an error occured.
+ */
+JUPEDSIM_API JPS_OperationalModel JPS_SocialForceModelBuilder_Build(
+    JPS_SocialForceModelBuilder handle,
+    JPS_ErrorMessage* errorMessage);
+
+/**
+ * Frees a JPS_SocialForceModelBuilder
+ * @param handle to the JPS_SocialForceModelBuilder to free.
+ */
+JUPEDSIM_API void JPS_SocialForceModelBuilder_Free(JPS_SocialForceModelBuilder handle);
+
+/**
+ * Opaque type of Social Force model state
+ */
+typedef struct JPS_SocialForceModelState_t* JPS_SocialForceModelState;
+
+/**
+ * Read Velocity of this agent.
+ * @param handle of the Agent to access.
+ * @return Velocity of this agent.
+ */
+JUPEDSIM_API JPS_Point JPS_SocialForceModelState_GetVelocity(JPS_SocialForceModelState handle);
+
+/**
+ * Write Velocity of this agent.
+ * @param handle of the Agent to access.
+ * @param velocity Velocity of this agent.
+ */
+JUPEDSIM_API void
+JPS_SocialForceModelState_SetVelocity(JPS_SocialForceModelState handle, JPS_Point velocity);
+
+/**
+ * Read mass of this agent.
+ * @param handle of the Agent to access.
+ * @return mass in kg of this agent
+ */
+JUPEDSIM_API double JPS_SocialForceModelState_GetMass(JPS_SocialForceModelState handle);
+
+/**
+ * Write mass of this agent.
+ * @param handle of the Agent to access.
+ * @param mass in kg of this agent.
+ */
+JUPEDSIM_API void JPS_SocialForceModelState_SetMass(JPS_SocialForceModelState handle, double mass);
+
+/**
+ * Read desired Speed of this agent.
+ * @param handle of the Agent to access.
+ * @return desired Speed in m/s of this agent
+ */
+JUPEDSIM_API double JPS_SocialForceModelState_GetDesiredSpeed(JPS_SocialForceModelState handle);
+
+/**
+ * Write desired Speed of this agent.
+ * @param handle of the Agent to access.
+ * @param desiredSpeed in m/s of this agent.
+ */
+JUPEDSIM_API void
+JPS_SocialForceModelState_SetDesiredSpeed(JPS_SocialForceModelState handle, double desiredSpeed);
+
+/**
+ * Read reaction Time of this agent.
+ * @param handle of the Agent to access.
+ * @return reaction Time in s of this agent
+ */
+JUPEDSIM_API double JPS_SocialForceModelState_GetReactionTime(JPS_SocialForceModelState handle);
+
+/**
+ * Write reaction Time of this agent.
+ * @param handle of the Agent to access.
+ * @param reactionTime in s of this agent.
+ */
+JUPEDSIM_API void
+JPS_SocialForceModelState_SetReactionTime(JPS_SocialForceModelState handle, double reactionTime);
+
+/**
+ * Read agent Scale of this agent.
+ * @param handle of the Agent to access.
+ * @return agent Scale of this agent
+ */
+JUPEDSIM_API double JPS_SocialForceModelState_GetAgentScale(JPS_SocialForceModelState handle);
+
+/**
+ * Write agent Scale of this agent.
+ * @param handle of the Agent to access.
+ * @param agentScale of this agent.
+ */
+JUPEDSIM_API void
+JPS_SocialForceModelState_SetAgentScale(JPS_SocialForceModelState handle, double agentScale);
+
+/**
+ * Read obstacle Scale of this agent.
+ * @param handle of the Agent to access.
+ * @return obstacle Scale of this agent
+ */
+JUPEDSIM_API double JPS_SocialForceModelState_GetObstacleScale(JPS_SocialForceModelState handle);
+
+/**
+ * Write obstacle Scale of this agent.
+ * @param handle of the Agent to access.
+ * @param obstacleScale of this agent.
+ */
+JUPEDSIM_API void
+JPS_SocialForceModelState_SetObstacleScale(JPS_SocialForceModelState handle, double obstacleScale);
+
+/**
+ * Read force Distance of this agent.
+ * @param handle of the Agent to access.
+ * @return force Distance of this agent
+ */
+JUPEDSIM_API double JPS_SocialForceModelState_GetForceDistance(JPS_SocialForceModelState handle);
+
+/**
+ * Write force Distance of this agent.
+ * @param handle of the Agent to access.
+ * @param forceDistance of this agent.
+ */
+JUPEDSIM_API void
+JPS_SocialForceModelState_SetForceDistance(JPS_SocialForceModelState handle, double forceDistance);
+
+/**
+ * Read radius of this agent.
+ * @param handle of the Agent to access.
+ * @return radius in m of this agent
+ */
+JUPEDSIM_API double JPS_SocialForceModelState_GetRadius(JPS_SocialForceModelState handle);
+
+/**
+ * Write radius of this agent.
+ * @param handle of the Agent to access.
+ * @param radius in m of this agent.
+ */
+JUPEDSIM_API void
+JPS_SocialForceModelState_SetRadius(JPS_SocialForceModelState handle, double radius);
+
+/**
+ * Describes parameters of an Agent in SocialForceModel
+ */
+typedef struct JPS_SocialForceModelAgentParameters {
+    /**
+     * Position of the agent.
+     * The position needs to inside the accessible area.
+     */
+    JPS_Point position{0, 0};
+    /*
+     * Orientation vector of the agent.
+     * The orientation vector will internally be normalized.
+     */
+    JPS_Point orientation{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;
+    /**
+     * Initial velocity of the Agent
+     */
+    JPS_Point velocity = {0, 0};
+    /**
+     * Mass of the agent
+     */
+    double mass = 80;
+    /**
+     * desired Speed of the agent
+     */
+    double desiredSpeed = 0.8;
+    /**
+     * reaction Time of the agent
+     */
+    double reactionTime = 0.5;
+    /**
+     * agent Scale of the agent
+     */
+    double agentScale = 2000;
+    /**
+     * obstacle Scale of the agent
+     */
+    double obstacleScale = 2000;
+    /**
+     * force Distance of the agent
+     */
+    double forceDistance = 0.08;
+    /**
+     * radius of the agent
+     */
+    double radius = 0.3;
+
+} JPS_SocialForceModelAgentParameters;
+
+#ifdef __cplusplus
+}
+#endif

libjupedsim/include/jupedsim/types.h

@@ -60,7 +60,8 @@ typedef struct JPS_Waypoint {
 typedef enum JPS_ModelType {
     JPS_GeneralizedCentrifugalForceModel,
     JPS_CollisionFreeSpeedModel,
-    JPS_CollisionFreeSpeedModelv2
+    JPS_CollisionFreeSpeedModelv2,
+    JPS_SocialForceModel
 } JPS_ModelType;
 
 /**

libjupedsim/src/agent.cpp

@@ -90,6 +90,8 @@ JPS_ModelType JPS_Agent_GetModelType(JPS_Agent handle)
             return JPS_CollisionFreeSpeedModel;
         case 2:
             return JPS_CollisionFreeSpeedModelv2;
+        case 3:
+            return JPS_SocialForceModel;
     }
     UNREACHABLE();
 }
@@ -157,6 +159,27 @@ JPS_Agent_GetCollisionFreeSpeedModelv2State(JPS_Agent handle, JPS_ErrorMessage*
     return nullptr;
 }
 
+JPS_SocialForceModelState
+JPS_Agent_GetSocialForceModelState(JPS_Agent handle, JPS_ErrorMessage* errorMessage)
+{
+    assert(handle);
+    const auto agent = reinterpret_cast<GenericAgent*>(handle);
+    try {
+        auto& model = std::get<SocialForceModelData>(agent->model);
+        return reinterpret_cast<JPS_SocialForceModelState>(&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
 ////////////////////////////////////////////////////////////////////////////////////////////////////