diff --git a/CMakeLists.txt b/CMakeLists.txt index 432200688..ee21d02b2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -224,7 +224,7 @@ add_subdirectory(python_bindings_jupedsim) # Code formatting ################################################################################ if(UNIX AND WITH_FORMAT) - set(clang-format-version 15) + set(clang-format-version 18) find_program(CLANG_FORMAT NAMES clang-format-${clang-format-version} diff --git a/docs/source/notebooks/index.rst b/docs/source/notebooks/index.rst index b21f21c9b..8f85933bd 100644 --- a/docs/source/notebooks/index.rst +++ b/docs/source/notebooks/index.rst @@ -5,6 +5,7 @@ Notebooks .. toctree:: :maxdepth: 1 + model-comparison corner double-bottleneck journey diff --git a/docs/source/notebooks/model-comparison.ipynb b/docs/source/notebooks/model-comparison.ipynb new file mode 120000 index 000000000..0562bbf2d --- /dev/null +++ b/docs/source/notebooks/model-comparison.ipynb @@ -0,0 +1 @@ +../../../notebooks/model-comparison.ipynb \ No newline at end of file diff --git a/docs/source/pedestrian_models/index.rst b/docs/source/pedestrian_models/index.rst index 1a07db2ed..1e168c910 100644 --- a/docs/source/pedestrian_models/index.rst +++ b/docs/source/pedestrian_models/index.rst @@ -7,6 +7,7 @@ models. Below is a list of all the models that are currently available. Please refer to the links in the respective section for a detailed discussion of the respective model. + ************************** Collision Free Speed Model ************************** @@ -44,6 +45,47 @@ available on `PedestrianDynamics`_. The original publication can be found at https://arxiv.org/abs/1512.05597 +************************** +Anticipation Velocity Model +************************** + +The anticipation velocity model (AVM) is a mathematical approach for pedestrian +dynamics that prevents collisions through anticipatory behavior. The model divides +anticipation into three components: situation perception, future prediction, and +strategy selection. +Building upon the collision-free speed model (CSM), AVM determines agent movement +through exponential repulsion from nearby agents. Unlike CSM, the influence +direction is orthogonal to the agent's desired direction. +Repulsion strength is affected by agents within the perception field - +specifically, those in the union of two half-planes where the agent moves or +intends to move. Agents adjust their speed based on anticipated distance to the +nearest neighbor in their headway, enabling navigation through congested areas +without overlap. +The model incorporates a reaction time factor to adjust the turning process rate +from the current to the new direction. Walls are treated as gliding surfaces: +when an agent is within critical distance of a wall and moving toward it, their +direction is projected parallel to the wall surface. +Movement calculation occurs in two steps: first, combining the desired direction +with neighbor influences, then adjusting for wall interactions when necessary. +Walls do not influence the speed of agents, only their direction. + +The anticipation velocity model takes into account the length of the agent, +which determines the required space for movement, and the maximum achievable +speed of the agent. This simplified and computationally efficient model aims to +mirror real-world pedestrian behaviors while maintaining smooth movement +dynamics. + +The parameters of the anticipation velocity model can be defined per-agent. + +In :class:`~jupedsim.models.AnticipationVelocityModel` neighbor and wall +parameters are per-agent parameters that can be set individually via +:class:`~jupedsim.models.AnticipationVelocityModelAgentParameters` and can be +changed at any time. + + +The original publication can be found at https://doi.org/10.1016/j.trc.2021.103464 + + *********************************** Generalized Centrifugal Force Model *********************************** diff --git a/examples/example7_avm.py b/examples/example7_avm.py new file mode 100644 index 000000000..78409f7f8 --- /dev/null +++ b/examples/example7_avm.py @@ -0,0 +1,72 @@ +#! /usr/bin/env python3 + +# Copyright © 2012-2024 Forschungszentrum Jülich GmbH +# SPDX-License-Identifier: LGPL-3.0-or-later + +import pathlib +import sys + +import jupedsim as jps +from shapely import Polygon + + +def main(): + room1 = Polygon([(0, 0), (10, 0), (10, 10), (0, 10)]) # Room 1 (10m x 10m) + room2 = Polygon([(15, 0), (25, 0), (25, 10), (15, 10)]) + width = 0.8 + num_agents = 10 + corridor = Polygon( + [ + (10, 5 - width / 2), + (15, 5 - width / 2), + (15, 5 + width / 2), + (10, 5 + width / 2), + ] + ) + geometry = room1.union(room2).union(corridor) + exit_polygon = Polygon([(24, 4.5), (25, 4.5), (25, 5.5), (24, 5.5)]) + trajectory_file = "example_7.sqlite" + simulation = jps.Simulation( + model=jps.AnticipationVelocityModel(), + geometry=geometry, + trajectory_writer=jps.SqliteTrajectoryWriter( + output_file=pathlib.Path(trajectory_file) + ), + ) + + exit_id = simulation.add_exit_stage(exit_polygon) + journey = jps.JourneyDescription([exit_id]) + journey_id = simulation.add_journey(journey) + + start_positions = jps.distribute_by_number( + polygon=Polygon([(0, 0), (5, 0), (5, 10), (0, 10)]), + number_of_agents=num_agents, + distance_to_agents=0.5, + distance_to_polygon=0.2, + ) + + for position in start_positions: + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + journey_id=journey_id, + stage_id=exit_id, + position=position, + anticipation_time=0.5, + reaction_time=0.3, + ) + ) + + while simulation.agent_count() > 0 and simulation.iteration_count() < 3000: + try: + simulation.iterate() + except KeyboardInterrupt: + print("CTRL-C Received! Shutting down") + sys.exit(1) + print( + f"Simulation completed after {simulation.iteration_count()} iterations ({simulation.elapsed_time()} s)" + ) + print(f"{trajectory_file = }") + + +if __name__ == "__main__": + main() diff --git a/examples/example8_headon_AVM.py b/examples/example8_headon_AVM.py new file mode 100644 index 000000000..8b4f9055f --- /dev/null +++ b/examples/example8_headon_AVM.py @@ -0,0 +1,62 @@ +#! /usr/bin/env python3 + +# Copyright © 2012-2024 Forschungszentrum Jülich GmbH +# SPDX-License-Identifier: LGPL-3.0-or-later + +import pathlib + +import jupedsim as jps +from shapely import Polygon + + +def main(): + width = 2 + length = 10 + geometry = Polygon([(0, 0), (length, 0), (length, width), (0, width)]) + exit_polygon = Polygon( + [(length - 0.5, 0), (length, 0), (length, width), (length - 0.5, width)] + ) + trajectory_file = "headon_AVM.sqlite" + simulation = jps.Simulation( + model=jps.AnticipationVelocityModel(), + geometry=geometry, + trajectory_writer=jps.SqliteTrajectoryWriter( + output_file=pathlib.Path(trajectory_file) + ), + ) + + exit_id = simulation.add_exit_stage(exit_polygon) + journey = jps.JourneyDescription([exit_id]) + journey_id = simulation.add_journey(journey) + + start_positions = [(3, 0.5 * width), (0.5 * length, 0.5 * width)] + parameters = [ + jps.AnticipationVelocityModelAgentParameters( + journey_id=journey_id, + stage_id=exit_id, + position=start_positions[0], + radius=0.2, + v0=1.2, + ), + jps.AnticipationVelocityModelAgentParameters( + journey_id=journey_id, + stage_id=exit_id, + position=start_positions[1], + radius=0.2, + v0=0.2, + ), + ] + for position, param in zip(start_positions, parameters): + simulation.add_agent(parameters=param) + + while simulation.agent_count() > 0 and simulation.iteration_count() < 1000: + simulation.iterate() + + print( + f"Simulation completed after {simulation.iteration_count()} iterations ({simulation.elapsed_time()} s)" + ) + print(f">> {trajectory_file = }") + + +if __name__ == "__main__": + main() diff --git a/examples/example8_headon_CSM.py b/examples/example8_headon_CSM.py new file mode 100644 index 000000000..30a2c5faf --- /dev/null +++ b/examples/example8_headon_CSM.py @@ -0,0 +1,62 @@ +#! /usr/bin/env python3 + +# Copyright © 2012-2024 Forschungszentrum Jülich GmbH +# SPDX-License-Identifier: LGPL-3.0-or-later + +import pathlib + +import jupedsim as jps +from shapely import Polygon + + +def main(): + width = 2 + length = 10 + geometry = Polygon([(0, 0), (length, 0), (length, width), (0, width)]) + exit_polygon = Polygon( + [(length - 0.5, 0), (length, 0), (length, width), (length - 0.5, width)] + ) + trajectory_file = "headon_CSM.sqlite" + simulation = jps.Simulation( + model=jps.CollisionFreeSpeedModel(), + geometry=geometry, + trajectory_writer=jps.SqliteTrajectoryWriter( + output_file=pathlib.Path(trajectory_file) + ), + ) + + exit_id = simulation.add_exit_stage(exit_polygon) + journey = jps.JourneyDescription([exit_id]) + journey_id = simulation.add_journey(journey) + + start_positions = [(3, 0.5 * width), (0.5 * length, 0.5 * width)] + parameters = [ + jps.CollisionFreeSpeedModelAgentParameters( + journey_id=journey_id, + stage_id=exit_id, + position=start_positions[0], + radius=0.2, + v0=1.2, + ), + jps.CollisionFreeSpeedModelAgentParameters( + journey_id=journey_id, + stage_id=exit_id, + position=start_positions[1], + radius=0.2, + v0=0.2, + ), + ] + for position, param in zip(start_positions, parameters): + simulation.add_agent(parameters=param) + + while simulation.agent_count() > 0 and simulation.iteration_count() < 1000: + simulation.iterate() + + print( + f"Simulation completed after {simulation.iteration_count()} iterations ({simulation.elapsed_time()} s)" + ) + print(f">> {trajectory_file = }") + + +if __name__ == "__main__": + main() diff --git a/libjupedsim/CMakeLists.txt b/libjupedsim/CMakeLists.txt index 2f500307d..541100dbf 100644 --- a/libjupedsim/CMakeLists.txt +++ b/libjupedsim/CMakeLists.txt @@ -11,6 +11,7 @@ add_library(jupedsim_obj OBJECT src/build_info.cpp src/collision_free_speed_model.cpp src/collision_free_speed_model_v2.cpp + src/anticipation_velocity_model.cpp src/error.cpp src/generalized_centrifugal_force_model.cpp src/geometry.cpp @@ -112,6 +113,7 @@ install( ${header_dest}/build_info.h ${header_dest}/collision_free_speed_model.h ${header_dest}/collision_free_speed_model_v2.h + ${header_dest}/anticipation_velocity_model.h ${header_dest}/error.h ${header_dest}/export.h ${header_dest}/generalized_centrifugal_force_model.h diff --git a/libjupedsim/include/jupedsim/agent.h b/libjupedsim/include/jupedsim/agent.h index 605ee0b13..daf30ffb2 100644 --- a/libjupedsim/include/jupedsim/agent.h +++ b/libjupedsim/include/jupedsim/agent.h @@ -2,6 +2,7 @@ /* SPDX-License-Identifier: LGPL-3.0-or-later */ #pragma once +#include "anticipation_velocity_model.h" #include "collision_free_speed_model.h" #include "collision_free_speed_model_v2.h" #include "error.h" @@ -116,6 +117,16 @@ JPS_Agent_GetSocialForceModelState(JPS_Agent handle, JPS_ErrorMessage* errorMess JUPEDSIM_API JPS_CollisionFreeSpeedModelV2State JPS_Agent_GetCollisionFreeSpeedModelV2State(JPS_Agent handle, JPS_ErrorMessage* errorMessage); +/** + * Access Anticipation Velocity Model state. + * Precondition: Agent needs to use Anticipation Velocity 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_AnticipationVelocityModelState +JPS_Agent_GetAnticipationVelocityModelState(JPS_Agent handle, JPS_ErrorMessage* errorMessage); + /** * Opaque type of an iterator over agents */ diff --git a/libjupedsim/include/jupedsim/anticipation_velocity_model.h b/libjupedsim/include/jupedsim/anticipation_velocity_model.h new file mode 100644 index 000000000..778bb0eea --- /dev/null +++ b/libjupedsim/include/jupedsim/anticipation_velocity_model.h @@ -0,0 +1,259 @@ +/* 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 Anticipation Velocity Model Builder. + */ +typedef struct JPS_AnticipationVelocityModelBuilder_t* JPS_AnticipationVelocityModelBuilder; + +/** + * Creates a Anticipation Velocity Model builder. + * @return the builder + */ +JUPEDSIM_API JPS_AnticipationVelocityModelBuilder JPS_AnticipationVelocityModelBuilder_Create(); + +/** + * Creates a JPS_OperationalModel of type Anticipation Velocity Model from the + * JPS_AnticipationVelocityModelBuilder. + * @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_AnticipationVelocityModel or NULL if an error occurred. + */ +JUPEDSIM_API JPS_OperationalModel JPS_AnticipationVelocityModelBuilder_Build( + JPS_AnticipationVelocityModelBuilder handle, + JPS_ErrorMessage* errorMessage); + +/** + * Frees a JPS_AnticipationVelocityModelBuilder + * @param handle to the JPS_AnticipationVelocityModelBuilder to free. + */ +JUPEDSIM_API void +JPS_AnticipationVelocityModelBuilder_Free(JPS_AnticipationVelocityModelBuilder handle); + +/** + * Opaque type of Anticipation Velocity Model state + */ +typedef struct JPS_AnticipationVelocityModelState_t* JPS_AnticipationVelocityModelState; + +/** + * Read strength neighbor repulsion of this agent. + * @param handle of the Agent to access. + * @return strength neighbor repulsion of this agent + */ +JUPEDSIM_API double JPS_AnticipationVelocityModelState_GetStrengthNeighborRepulsion( + JPS_AnticipationVelocityModelState handle); + +/** + * Write strength neighbor repulsion of this agent. + * @param handle of the Agent to access. + * @param strengthNeighborRepulsion of this agent. + */ +JUPEDSIM_API void JPS_AnticipationVelocityModelState_SetStrengthNeighborRepulsion( + JPS_AnticipationVelocityModelState handle, + double strengthNeighborRepulsion); + +/** + * Read range neighbor repulsion of this agent. + * @param handle of the Agent to access. + * @return range neighbor repulsion of this agent + */ +JUPEDSIM_API double JPS_AnticipationVelocityModelState_GetRangeNeighborRepulsion( + JPS_AnticipationVelocityModelState handle); + +/** + * Write range neighbor repulsion of this agent. + * @param handle of the Agent to access. + * @param rangeNeighborRepulsion of this agent. + */ +JUPEDSIM_API void JPS_AnticipationVelocityModelState_SetRangeNeighborRepulsion( + JPS_AnticipationVelocityModelState handle, + double rangeNeighborRepulsion); + +/** + * Read anticipation time of this agent. + * @param handle of the Agent to access. + * @return anticipation time of this agent + */ +JUPEDSIM_API double +JPS_AnticipationVelocityModelState_GetAnticipationTime(JPS_AnticipationVelocityModelState handle); + +/** + * Write anticipation time of this agent. + * @param handle of the Agent to access. + * @param anticipationTime of this agent. + */ +JUPEDSIM_API void JPS_AnticipationVelocityModelState_SetAnticipationTime( + JPS_AnticipationVelocityModelState handle, + double anticipationTime); + +/** + * Read reaction time of this agent. + * @param handle of the Agent to access. + * @return reaction time of this agent + */ +JUPEDSIM_API double +JPS_AnticipationVelocityModelState_GetReactionTime(JPS_AnticipationVelocityModelState handle); + +/** + * Write reaction time of this agent. + * @param handle of the Agent to access. + * @param reactionTime of this agent. + */ +JUPEDSIM_API void JPS_AnticipationVelocityModelState_SetReactionTime( + JPS_AnticipationVelocityModelState handle, + double reactionTime); + +/** + * Write wall buffer distance of this agent. + * @param handle of the Agent to access. + * @param wallBufferDistance of this agent. + */ +JUPEDSIM_API void JPS_AnticipationVelocityModelState_SetWallBufferDistance( + JPS_AnticipationVelocityModelState handle, + double wallBufferDistance); + +/** + * Read wall buffer distance of this agent. + * @param handle of the Agent to access. + * @return wall buffer distance of this agent + */ +JUPEDSIM_API double +JPS_AnticipationVelocityModelState_GetWallBufferDistance(JPS_AnticipationVelocityModelState handle); + +/** + * Read e0 of this agent. + * @param handle of the Agent to access. + * @return e0 of this agent + */ +JUPEDSIM_API JPS_Point +JPS_AnticipationVelocityModelState_GetE0(JPS_AnticipationVelocityModelState handle); + +/** + * Write e0 of this agent. + * @param handle of the Agent to access. + * @param e0 of this agent. + */ +JUPEDSIM_API void +JPS_AnticipationVelocityModelState_SetE0(JPS_AnticipationVelocityModelState handle, JPS_Point e0); + +/** + * Read time gap of this agent. + * @param handle of the Agent to access. + * @return time gap of this agent + */ +JUPEDSIM_API double +JPS_AnticipationVelocityModelState_GetTimeGap(JPS_AnticipationVelocityModelState handle); + +/** + * Write time gap of this agent. + * @param handle of the Agent to access. + * @param time_gap of this agent. + */ +JUPEDSIM_API void JPS_AnticipationVelocityModelState_SetTimeGap( + JPS_AnticipationVelocityModelState handle, + double time_gap); + +/** + * Read v0 of this agent. + * @param handle of the Agent to access. + * @return v0 of this agent + */ +JUPEDSIM_API double +JPS_AnticipationVelocityModelState_GetV0(JPS_AnticipationVelocityModelState handle); + +/** + * Write v0 of this agent. + * @param handle of the Agent to access. + * @param v0 of this agent. + */ +JUPEDSIM_API void +JPS_AnticipationVelocityModelState_SetV0(JPS_AnticipationVelocityModelState handle, double v0); + +/** + * Read radius of this agent. + * @param handle of the Agent to access. + * @return radius of this agent + */ +JUPEDSIM_API double +JPS_AnticipationVelocityModelState_GetRadius(JPS_AnticipationVelocityModelState 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_AnticipationVelocityModelState_SetRadius( + JPS_AnticipationVelocityModelState handle, + double radius); + +/** + * Describes parameters of an Agent in Anticipation Velocity Model + */ +typedef struct JPS_AnticipationVelocityModelAgentParameters { + /** + * 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; + + /** + * @param time_gap of the agents using this profile (T in the OV-function) + */ + double time_gap = 1.06; + /** + * @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.15; + + /** + * Strength of the repulsion from neighbors + */ + double strengthNeighborRepulsion{8.0}; + + /** + * Range of the repulsion from neighbors + */ + double rangeNeighborRepulsion{0.1}; + + /** + * Wall buffer distance to geometry boundaries + */ + double wallBufferDistance{0.1}; + + /** + * Anticipation time in seconds + */ + double anticipationTime{0.5}; + + /** + * Reaction time in seconds + */ + double reactionTime{0.1}; + +} JPS_AnticipationVelocityModelAgentParameters; + +#ifdef __cplusplus +} +#endif diff --git a/libjupedsim/include/jupedsim/jupedsim.h b/libjupedsim/include/jupedsim/jupedsim.h index ba8514e32..44120750f 100644 --- a/libjupedsim/include/jupedsim/jupedsim.h +++ b/libjupedsim/include/jupedsim/jupedsim.h @@ -3,6 +3,7 @@ #pragma once #include "agent.h" +#include "anticipation_velocity_model.h" #include "build_info.h" #include "collision_free_speed_model.h" #include "collision_free_speed_model_v2.h" diff --git a/libjupedsim/include/jupedsim/simulation.h b/libjupedsim/include/jupedsim/simulation.h index 0eaabb21d..c5f12545f 100644 --- a/libjupedsim/include/jupedsim/simulation.h +++ b/libjupedsim/include/jupedsim/simulation.h @@ -3,6 +3,7 @@ #pragma once #include "agent.h" +#include "anticipation_velocity_model.h" #include "collision_free_speed_model.h" #include "collision_free_speed_model_v2.h" #include "error.h" @@ -188,6 +189,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_AddAnticipationVelocityModelAgent( + JPS_Simulation handle, + JPS_AnticipationVelocityModelAgentParameters 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. diff --git a/libjupedsim/include/jupedsim/types.h b/libjupedsim/include/jupedsim/types.h index df82e4371..1520d7145 100644 --- a/libjupedsim/include/jupedsim/types.h +++ b/libjupedsim/include/jupedsim/types.h @@ -61,6 +61,7 @@ typedef enum JPS_ModelType { JPS_GeneralizedCentrifugalForceModel, JPS_CollisionFreeSpeedModel, JPS_CollisionFreeSpeedModelV2, + JPS_AnticipationVelocityModel, JPS_SocialForceModel } JPS_ModelType; diff --git a/libjupedsim/src/agent.cpp b/libjupedsim/src/agent.cpp index d09fc003f..2512a75a0 100644 --- a/libjupedsim/src/agent.cpp +++ b/libjupedsim/src/agent.cpp @@ -91,6 +91,8 @@ JPS_ModelType JPS_Agent_GetModelType(JPS_Agent handle) case 2: return JPS_CollisionFreeSpeedModelV2; case 3: + return JPS_AnticipationVelocityModel; + case 4: return JPS_SocialForceModel; } UNREACHABLE(); @@ -159,6 +161,27 @@ JPS_Agent_GetCollisionFreeSpeedModelV2State(JPS_Agent handle, JPS_ErrorMessage* return nullptr; } +JPS_AnticipationVelocityModelState +JPS_Agent_GetAnticipationVelocityModelState(JPS_Agent handle, JPS_ErrorMessage* errorMessage) +{ + assert(handle); + const auto agent = reinterpret_cast(handle); + try { + auto& model = std::get(agent->model); + return reinterpret_cast(&model); + } catch(const std::exception& ex) { + if(errorMessage) { + *errorMessage = reinterpret_cast(new JPS_ErrorMessage_t{ex.what()}); + } + } catch(...) { + if(errorMessage) { + *errorMessage = reinterpret_cast( + new JPS_ErrorMessage_t{"Unknown internal error."}); + } + } + return nullptr; +} + JPS_SocialForceModelState JPS_Agent_GetSocialForceModelState(JPS_Agent handle, JPS_ErrorMessage* errorMessage) { diff --git a/libjupedsim/src/anticipation_velocity_model.cpp b/libjupedsim/src/anticipation_velocity_model.cpp new file mode 100644 index 000000000..707a128e0 --- /dev/null +++ b/libjupedsim/src/anticipation_velocity_model.cpp @@ -0,0 +1,185 @@ +// Copyright © 2012-2024 Forschungszentrum Jülich GmbH +// SPDX-License-Identifier: LGPL-3.0-or-later +#include "jupedsim/anticipation_velocity_model.h" +#include "jupedsim/error.h" + +#include "Conversion.hpp" +#include "ErrorMessage.hpp" + +#include +#include +#include + +using jupedsim::detail::intoJPS_Point; +using jupedsim::detail::intoPoint; +using jupedsim::detail::intoTuple; + +//////////////////////////////////////////////////////////////////////////////////////////////////// +/// Anticipation Velocity Model Builder +//////////////////////////////////////////////////////////////////////////////////////////////////// +JUPEDSIM_API JPS_AnticipationVelocityModelBuilder JPS_AnticipationVelocityModelBuilder_Create() +{ + return reinterpret_cast( + new AnticipationVelocityModelBuilder()); +} + +JUPEDSIM_API JPS_OperationalModel JPS_AnticipationVelocityModelBuilder_Build( + JPS_AnticipationVelocityModelBuilder handle, + JPS_ErrorMessage* errorMessage) +{ + assert(handle != nullptr); + auto builder = reinterpret_cast(handle); + JPS_OperationalModel result{}; + try { + result = + reinterpret_cast(new AnticipationVelocityModel(builder->Build())); + } catch(const std::exception& ex) { + if(errorMessage) { + *errorMessage = reinterpret_cast(new JPS_ErrorMessage_t{ex.what()}); + } + } catch(...) { + if(errorMessage) { + *errorMessage = reinterpret_cast( + new JPS_ErrorMessage_t{"Unknown internal error."}); + } + } + return result; +} + +JUPEDSIM_API void +JPS_AnticipationVelocityModelBuilder_Free(JPS_AnticipationVelocityModelBuilder handle) +{ + delete reinterpret_cast(handle); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////// +/// AnticipationVelocityModelState +//////////////////////////////////////////////////////////////////////////////////////////////////// +double JPS_AnticipationVelocityModelState_GetStrengthNeighborRepulsion( + JPS_AnticipationVelocityModelState handle) +{ + assert(handle); + const auto state = reinterpret_cast(handle); + return state->strengthNeighborRepulsion; +} + +void JPS_AnticipationVelocityModelState_SetStrengthNeighborRepulsion( + JPS_AnticipationVelocityModelState handle, + double strengthNeighborRepulsion) +{ + assert(handle); + auto state = reinterpret_cast(handle); + state->strengthNeighborRepulsion = strengthNeighborRepulsion; +} + +double JPS_AnticipationVelocityModelState_GetRangeNeighborRepulsion( + JPS_AnticipationVelocityModelState handle) +{ + assert(handle); + const auto state = reinterpret_cast(handle); + return state->rangeNeighborRepulsion; +} + +void JPS_AnticipationVelocityModelState_SetRangeNeighborRepulsion( + JPS_AnticipationVelocityModelState handle, + double rangeNeighborRepulsion) +{ + assert(handle); + auto state = reinterpret_cast(handle); + state->rangeNeighborRepulsion = rangeNeighborRepulsion; +} + +double +JPS_AnticipationVelocityModelState_GetAnticipationTime(JPS_AnticipationVelocityModelState handle) +{ + assert(handle); + const auto state = reinterpret_cast(handle); + return state->anticipationTime; +} + +void JPS_AnticipationVelocityModelState_SetAnticipationTime( + JPS_AnticipationVelocityModelState handle, + double anticipationTime) +{ + assert(handle); + auto state = reinterpret_cast(handle); + state->anticipationTime = anticipationTime; +} + +double JPS_AnticipationVelocityModelState_GetReactionTime(JPS_AnticipationVelocityModelState handle) +{ + assert(handle); + const auto state = reinterpret_cast(handle); + return state->reactionTime; +} + +void JPS_AnticipationVelocityModelState_SetReactionTime( + JPS_AnticipationVelocityModelState handle, + double reactionTime) +{ + assert(handle); + auto state = reinterpret_cast(handle); + state->reactionTime = reactionTime; +} + +double +JPS_AnticipationVelocityModelState_GetWallBufferDistance(JPS_AnticipationVelocityModelState handle) +{ + assert(handle); + const auto state = reinterpret_cast(handle); + return state->wallBufferDistance; +} + +void JPS_AnticipationVelocityModelState_SetWallBufferDistance( + JPS_AnticipationVelocityModelState handle, + double wallBufferDistance) +{ + assert(handle); + auto state = reinterpret_cast(handle); + state->wallBufferDistance = wallBufferDistance; +} + +double JPS_AnticipationVelocityModelState_GetTimeGap(JPS_AnticipationVelocityModelState handle) +{ + assert(handle); + const auto state = reinterpret_cast(handle); + return state->timeGap; +} + +void JPS_AnticipationVelocityModelState_SetTimeGap( + JPS_AnticipationVelocityModelState handle, + double time_gap) +{ + assert(handle); + auto state = reinterpret_cast(handle); + state->timeGap = time_gap; +} + +double JPS_AnticipationVelocityModelState_GetV0(JPS_AnticipationVelocityModelState handle) +{ + assert(handle); + const auto state = reinterpret_cast(handle); + return state->v0; +} + +void JPS_AnticipationVelocityModelState_SetV0(JPS_AnticipationVelocityModelState handle, double v0) +{ + assert(handle); + auto state = reinterpret_cast(handle); + state->v0 = v0; +} +double JPS_AnticipationVelocityModelState_GetRadius(JPS_AnticipationVelocityModelState handle) +{ + assert(handle); + const auto state = reinterpret_cast(handle); + return state->radius; +} + +void JPS_AnticipationVelocityModelState_SetRadius( + JPS_AnticipationVelocityModelState handle, + double radius) +{ + assert(handle); + auto state = reinterpret_cast(handle); + state->radius = radius; +} diff --git a/libjupedsim/src/simulation.cpp b/libjupedsim/src/simulation.cpp index 5c921f2c2..3790a2d10 100644 --- a/libjupedsim/src/simulation.cpp +++ b/libjupedsim/src/simulation.cpp @@ -2,6 +2,7 @@ // SPDX-License-Identifier: LGPL-3.0-or-later #include "jupedsim/simulation.h" +#include "OperationalModelType.hpp" #include "jupedsim/agent.h" #include "jupedsim/error.h" @@ -9,6 +10,7 @@ #include "Conversion.hpp" #include "ErrorMessage.hpp" #include "JourneyDescription.hpp" +#include "jupedsim/types.h" #include #include @@ -281,6 +283,49 @@ JPS_AgentId JPS_Simulation_AddCollisionFreeSpeedModelV2Agent( return result.getID(); } +JPS_AgentId JPS_Simulation_AddAnticipationVelocityModelAgent( + JPS_Simulation handle, + JPS_AnticipationVelocityModelAgentParameters parameters, + JPS_ErrorMessage* errorMessage) +{ + assert(handle); + auto result = GenericAgent::ID::Invalid; + auto simulation = reinterpret_cast(handle); + try { + if(simulation->ModelType() != OperationalModelType::ANTICIPATION_VELOCITY_MODEL) { + throw std::runtime_error( + "Simulation is not configured to use Anticipation Velocity Model."); + } + GenericAgent agent( + GenericAgent::ID::Invalid, + Journey::ID(parameters.journeyId), + BaseStage::ID(parameters.stageId), + intoPoint(parameters.position), + {}, + AnticipationVelocityModelData{ + .strengthNeighborRepulsion = parameters.strengthNeighborRepulsion, + .rangeNeighborRepulsion = parameters.rangeNeighborRepulsion, + .wallBufferDistance = parameters.wallBufferDistance, + .anticipationTime = parameters.anticipationTime, + .reactionTime = parameters.reactionTime, + .velocity = {}, + .timeGap = parameters.time_gap, + .v0 = parameters.v0, + .radius = parameters.radius}); + result = simulation->AddAgent(std::move(agent)); + } catch(const std::exception& ex) { + if(errorMessage) { + *errorMessage = reinterpret_cast(new JPS_ErrorMessage_t{ex.what()}); + } + } catch(...) { + if(errorMessage) { + *errorMessage = reinterpret_cast( + new JPS_ErrorMessage_t{"Unknown internal error."}); + } + } + return result.getID(); +} + JPS_AgentId JPS_Simulation_AddSocialForceModelAgent( JPS_Simulation handle, JPS_SocialForceModelAgentParameters parameters, @@ -475,6 +520,8 @@ JPS_ModelType JPS_Simulation_ModelType(JPS_Simulation handle) return JPS_GeneralizedCentrifugalForceModel; case OperationalModelType::COLLISION_FREE_SPEED_V2: return JPS_CollisionFreeSpeedModelV2; + case OperationalModelType::ANTICIPATION_VELOCITY_MODEL: + return JPS_AnticipationVelocityModel; case OperationalModelType::SOCIAL_FORCE: return JPS_SocialForceModel; } diff --git a/libjupedsim/test/TestJupedsim.cpp b/libjupedsim/test/TestJupedsim.cpp index 81424673e..3e9a913ad 100644 --- a/libjupedsim/test/TestJupedsim.cpp +++ b/libjupedsim/test/TestJupedsim.cpp @@ -2,6 +2,7 @@ // SPDX-License-Identifier: LGPL-3.0-or-later #include "AgentIterator.hpp" #include "Point.hpp" +#include "jupedsim/anticipation_velocity_model.h" #include "gtest/gtest.h" #include #include @@ -44,6 +45,33 @@ TEST(OperationalModel, DefaultsOfCollisionFreeSpeedModelAgentParameters) ASSERT_DOUBLE_EQ(agentParameters.radius, 0.2); } +TEST(OperationalModel, CanConstructAnticipationVelocityModel) +{ + JPS_ErrorMessage errorMsg{}; + auto builder = JPS_AnticipationVelocityModelBuilder_Create(); + auto model = JPS_AnticipationVelocityModelBuilder_Build(builder, &errorMsg); + EXPECT_NE(model, nullptr); + EXPECT_EQ(errorMsg, nullptr); + JPS_AnticipationVelocityModelBuilder_Free(builder); + JPS_OperationalModel_Free(model); +} + +TEST(OperationalModel, DefaultsOfAnticipationVelocityModelAgentParameters) +{ + JPS_AnticipationVelocityModelAgentParameters agentParameters{}; + + ASSERT_DOUBLE_EQ(agentParameters.position.x, 0); + ASSERT_DOUBLE_EQ(agentParameters.position.y, 0); + ASSERT_DOUBLE_EQ(agentParameters.journeyId, 0); + ASSERT_DOUBLE_EQ(agentParameters.stageId, 0); + ASSERT_DOUBLE_EQ(agentParameters.time_gap, 1.06); + ASSERT_DOUBLE_EQ(agentParameters.v0, 1.2); + ASSERT_DOUBLE_EQ(agentParameters.radius, 0.15); + ASSERT_DOUBLE_EQ(agentParameters.anticipationTime, 0.5); + ASSERT_DOUBLE_EQ(agentParameters.reactionTime, 0.1); + ASSERT_DOUBLE_EQ(agentParameters.wallBufferDistance, 0.1); +} + TEST(OperationalModel, CanConstructGeneralizedCentrifugalForceModel) { JPS_ErrorMessage errorMsg{}; diff --git a/libsimulator/CMakeLists.txt b/libsimulator/CMakeLists.txt index 32b1db611..c15cc5ad3 100644 --- a/libsimulator/CMakeLists.txt +++ b/libsimulator/CMakeLists.txt @@ -12,12 +12,18 @@ add_library(simulator STATIC src/CollisionFreeSpeedModelBuilder.hpp src/CollisionFreeSpeedModelData.hpp src/CollisionFreeSpeedModelUpdate.hpp - src/CollisionFreeSpeedModelV2.cpp + src/CollisionFreeSpeedModelV2.cpp src/CollisionFreeSpeedModelV2.hpp src/CollisionFreeSpeedModelV2Builder.cpp src/CollisionFreeSpeedModelV2Builder.hpp src/CollisionFreeSpeedModelV2Data.hpp src/CollisionFreeSpeedModelV2Update.hpp + src/AnticipationVelocityModel.cpp + src/AnticipationVelocityModel.hpp + src/AnticipationVelocityModelBuilder.cpp + src/AnticipationVelocityModelBuilder.hpp + src/AnticipationVelocityModelData.hpp + src/AnticipationVelocityModelUpdate.hpp src/CollisionGeometry.cpp src/CollisionGeometry.hpp src/Ellipse.cpp diff --git a/libsimulator/src/AnticipationVelocityModel.cpp b/libsimulator/src/AnticipationVelocityModel.cpp new file mode 100644 index 000000000..23f426123 --- /dev/null +++ b/libsimulator/src/AnticipationVelocityModel.cpp @@ -0,0 +1,335 @@ +// Copyright © 2012-2024 Forschungszentrum Jülich GmbH +// SPDX-License-Identifier: LGPL-3.0-or-later +#include "AnticipationVelocityModel.hpp" +#include "AnticipationVelocityModelData.hpp" +#include "AnticipationVelocityModelUpdate.hpp" +#include "GenericAgent.hpp" +#include "GeometricFunctions.hpp" +#include "Macros.hpp" +#include "OperationalModel.hpp" +#include "SimulationError.hpp" +#include +#include +#include +#include +#include +#include + +OperationalModelType AnticipationVelocityModel::Type() const +{ + return OperationalModelType::ANTICIPATION_VELOCITY_MODEL; +} + +OperationalModelUpdate AnticipationVelocityModel::ComputeNewPosition( + double dT, + const GenericAgent& ped, + const CollisionGeometry& geometry, + const NeighborhoodSearchType& neighborhoodSearch) const +{ + auto neighborhood = neighborhoodSearch.GetNeighboringAgents(ped.pos, _cutOffRadius); + const auto& boundary = geometry.LineSegmentsInApproxDistanceTo(ped.pos); + + // Remove any agent from the neighborhood that is obstructed by geometry and the current + // agent + neighborhood.erase( + std::remove_if( + std::begin(neighborhood), + std::end(neighborhood), + [&ped, &boundary](const auto& neighbor) { + if(ped.id == neighbor.id) { + return true; + } + const auto agent_to_neighbor = LineSegment(ped.pos, neighbor.pos); + if(std::find_if( + boundary.cbegin(), + boundary.cend(), + [&agent_to_neighbor](const auto& boundary_segment) { + return intersects(agent_to_neighbor, boundary_segment); + }) != boundary.end()) { + return true; + } + + return false; + }), + std::end(neighborhood)); + + const auto neighborRepulsion = std::accumulate( + std::begin(neighborhood), + std::end(neighborhood), + Point{}, + [&ped, this](const auto& res, const auto& neighbor) { + return res + NeighborRepulsion(ped, neighbor); + }); + + const auto desiredDirection = (ped.destination - ped.pos).Normalized(); + auto direction = (desiredDirection + neighborRepulsion).Normalized(); + if(direction == Point{}) { + direction = ped.orientation; + } + + const auto& model = std::get(ped.model); + const double wallBufferDistance = model.wallBufferDistance; + // Wall sliding behavior + direction = HandleWallAvoidance(direction, ped.pos, model.radius, boundary, wallBufferDistance); + + // update direction towards the newly calculated direction + direction = UpdateDirection(ped, direction, dT); + const auto spacing = std::accumulate( + std::begin(neighborhood), + std::end(neighborhood), + std::numeric_limits::max(), + [&ped, &direction, this](const auto& res, const auto& neighbor) { + return std::min(res, GetSpacing(ped, neighbor, direction)); + }); + + const auto optimal_speed = OptimalSpeed(ped, spacing, model.timeGap); + const auto velocity = direction * optimal_speed; + return AnticipationVelocityModelUpdate{ + .position = ped.pos + velocity * dT, .velocity = velocity, .orientation = direction}; +}; + +void AnticipationVelocityModel::ApplyUpdate(const OperationalModelUpdate& upd, GenericAgent& agent) + const +{ + const auto& update = std::get(upd); + auto& model = std::get(agent.model); + agent.pos = update.position; + agent.orientation = update.orientation; + model.velocity = update.velocity; +} + +Point AnticipationVelocityModel::UpdateDirection( + const GenericAgent& ped, + const Point& calculatedDirection, + double dt) const +{ + const auto& model = std::get(ped.model); + const Point desiredDirection = (ped.destination - ped.pos).Normalized(); + Point actualDirection = ped.orientation; + Point updatedDirection; + + if(desiredDirection.ScalarProduct(calculatedDirection) * + desiredDirection.ScalarProduct(actualDirection) < + 0) { + updatedDirection = calculatedDirection; + } else { + // Compute the rate of change of direction (Eq. 7) + const Point directionDerivative = + (calculatedDirection.Normalized() - actualDirection) / model.reactionTime; + updatedDirection = actualDirection + directionDerivative * dt; + } + + return updatedDirection.Normalized(); +} + +void AnticipationVelocityModel::CheckModelConstraint( + const GenericAgent& agent, + const NeighborhoodSearchType& neighborhoodSearch, + const CollisionGeometry& geometry) const +{ + const auto& model = std::get(agent.model); + const auto r = model.radius; + constexpr double rMin = 0.; + constexpr double rMax = 2.; + validateConstraint(r, rMin, rMax, "radius", true); + + const auto strengthNeighborRepulsion = model.strengthNeighborRepulsion; + constexpr double snMin = 0.; + constexpr double snMax = 20.; + validateConstraint(strengthNeighborRepulsion, snMin, snMax, "strengthNeighborRepulsion", false); + + const auto rangeNeighborRepulsion = model.rangeNeighborRepulsion; + constexpr double rnMin = 0.; + constexpr double rnMax = 5.; + validateConstraint(rangeNeighborRepulsion, rnMin, rnMax, "rangeNeighborRepulsion", true); + + const auto buff = model.wallBufferDistance; + constexpr double buffMin = 0.; + constexpr double buffMax = 1.; + validateConstraint(buff, buffMin, buffMax, "wallBufferDistance", false); + + const auto v0 = model.v0; + constexpr double v0Min = 0.; + constexpr double v0Max = 10.; + validateConstraint(v0, v0Min, v0Max, "v0"); + + const auto timeGap = model.timeGap; + constexpr double timeGapMin = 0.; + constexpr double timeGapMax = 10.; + validateConstraint(timeGap, timeGapMin, timeGapMax, "timeGap", true); + + const auto anticipationTime = model.anticipationTime; + constexpr double anticipationTimeMin = 0.0; + constexpr double anticipationTimeMax = 5.0; + validateConstraint( + anticipationTime, anticipationTimeMin, anticipationTimeMax, "anticipationTime"); + + const auto reactionTime = model.reactionTime; + constexpr double reactionTimeMin = 0.0; + constexpr double reactionTimeMax = 1.0; + validateConstraint(reactionTime, reactionTimeMin, reactionTimeMax, "reactionTime", true); + + const auto neighbors = neighborhoodSearch.GetNeighboringAgents(agent.pos, 2); + for(const auto& neighbor : neighbors) { + if(agent.id == neighbor.id) { + continue; + } + const auto& neighbor_model = std::get(neighbor.model); + const auto contanctdDist = r + neighbor_model.radius; + const auto distance = (agent.pos - neighbor.pos).Norm(); + if(contanctdDist >= distance) { + throw SimulationError( + "Model constraint violation: Agent {} too close to agent {}: distance {}", + agent.pos, + neighbor.pos, + distance); + } + } + + const auto lineSegments = geometry.LineSegmentsInDistanceTo(r, agent.pos); + if(std::begin(lineSegments) != std::end(lineSegments)) { + throw SimulationError( + "Model constraint violation: Agent {} too close to geometry boundaries, distance " + "<= {}", + agent.pos, + r); + } +} + +std::unique_ptr AnticipationVelocityModel::Clone() const +{ + return std::make_unique(*this); +} + +double AnticipationVelocityModel::OptimalSpeed( + const GenericAgent& ped, + double spacing, + double time_gap) const +{ + const auto& model = std::get(ped.model); + double min_spacing = 0.0; + return std::min(std::max(spacing / time_gap, min_spacing), model.v0); +} + +double AnticipationVelocityModel::GetSpacing( + const GenericAgent& ped1, + const GenericAgent& ped2, + const Point& direction) const +{ + const auto& model1 = std::get(ped1.model); + const auto& model2 = std::get(ped2.model); + const auto distp12 = ped2.pos - ped1.pos; + const auto inFront = direction.ScalarProduct(distp12) >= 0; + if(!inFront) { + return std::numeric_limits::max(); + } + + const auto left = direction.Rotate90Deg(); + const auto l = model1.radius + model2.radius; + bool inCorridor = std::abs(left.ScalarProduct(distp12)) <= l; + if(!inCorridor) { + return std::numeric_limits::max(); + } + return distp12.Norm() - l; +} + +Point AnticipationVelocityModel::CalculateInfluenceDirection( + const Point& desiredDirection, + const Point& predictedDirection) const +{ + // Eq. (5) + static std::mt19937 gen(42); + static std::uniform_int_distribution dist(0, 1); // Random integers: 0 or 1 + + Point orthogonalDirection = Point(-desiredDirection.y, desiredDirection.x).Normalized(); + double alignment = orthogonalDirection.ScalarProduct(predictedDirection); + Point influenceDirection = orthogonalDirection; + if(fabs(alignment) < J_EPS) { + // Choose a random direction (left or right) + if(dist(gen) == 0) { + influenceDirection = -orthogonalDirection; + } + } else if(alignment > 0) { + influenceDirection = -orthogonalDirection; + } + return influenceDirection; +} + +Point AnticipationVelocityModel::NeighborRepulsion( + const GenericAgent& ped1, + const GenericAgent& ped2) const +{ + const auto& model1 = std::get(ped1.model); + const auto& model2 = std::get(ped2.model); + + const auto distp12 = ped2.pos - ped1.pos; + const auto [distance, ep12] = distp12.NormAndNormalized(); + const double adjustedDist = distance - (model1.radius + model2.radius); + + // Pedestrian movement and desired directions + const auto& e1 = ped1.orientation; + const auto& d1 = (ped1.destination - ped1.pos).Normalized(); + const auto& e2 = ped2.orientation; + + // Check perception range (Eq. 1) + const auto inPerceptionRange = d1.ScalarProduct(ep12) >= 0 || e1.ScalarProduct(ep12) >= 0; + if(!inPerceptionRange) + return Point(0, 0); + + double S_Gap = + (model1.velocity - model2.velocity).ScalarProduct(ep12) * model1.anticipationTime; + double R_dist = adjustedDist - S_Gap; + R_dist = std::max(R_dist, 0.0); // Clamp to zero if negative + + // Interaction strength (Eq. 3 & 4) + constexpr double alignmentBase = 1.0; + constexpr double alignmentWeight = 0.5; + const double alignmentFactor = alignmentBase + alignmentWeight * (1.0 - d1.ScalarProduct(e2)); + const double interactionStrength = model1.strengthNeighborRepulsion * alignmentFactor * + std::exp(-R_dist / model1.rangeNeighborRepulsion); + auto newep12 = distp12 + model2.velocity * model2.anticipationTime; // e_ij(t+ta) + + // Compute adjusted influence direction + const auto influenceDirection = CalculateInfluenceDirection(d1, newep12); + return influenceDirection * interactionStrength; +} + +Point AnticipationVelocityModel::HandleWallAvoidance( + const Point& direction, + const Point& agentPosition, + double agentRadius, + const std::vector& boundary, + double wallBufferDistance) const +{ + const double criticalWallDistance = wallBufferDistance + agentRadius; + + auto nearestWallIt = std::min_element( + boundary.cbegin(), boundary.cend(), [&agentPosition](const auto& wall1, const auto& wall2) { + const auto distanceVector1 = agentPosition - wall1.ShortestPoint(agentPosition); + const auto distanceVector2 = agentPosition - wall2.ShortestPoint(agentPosition); + return distanceVector1.Norm() < distanceVector2.Norm(); + }); + + if(nearestWallIt != boundary.end()) { + const auto closestPoint = nearestWallIt->ShortestPoint(agentPosition); + const auto distanceVector = agentPosition - closestPoint; + auto [perpendicularDistance, directionAwayFromBoundary] = + distanceVector.NormAndNormalized(); + + if(perpendicularDistance < criticalWallDistance) { + // Agent is too close to wall + const auto dotProduct = direction.ScalarProduct(directionAwayFromBoundary); + if(dotProduct < 0) { + // Agent is moving towards wall + const auto wallVector = nearestWallIt->p2 - nearestWallIt->p1; + const auto wallDirection = wallVector.Normalized(); + // Project direction onto wall + const auto parallelComponent = + wallDirection * direction.ScalarProduct(wallDirection); + return parallelComponent.Normalized(); + } + } + } + + return direction; // Return original direction if no wall correction needed +} diff --git a/libsimulator/src/AnticipationVelocityModel.hpp b/libsimulator/src/AnticipationVelocityModel.hpp new file mode 100644 index 000000000..c4a360a5e --- /dev/null +++ b/libsimulator/src/AnticipationVelocityModel.hpp @@ -0,0 +1,53 @@ +// Copyright © 2012-2024 Forschungszentrum Jülich GmbH +// SPDX-License-Identifier: LGPL-3.0-or-later +#pragma once + +#include "CollisionGeometry.hpp" +#include "NeighborhoodSearch.hpp" +#include "OperationalModel.hpp" + +struct GenericAgent; + +class AnticipationVelocityModel : public OperationalModel +{ +public: + using NeighborhoodSearchType = NeighborhoodSearch; + +private: + double _cutOffRadius{3}; + +public: + AnticipationVelocityModel() = default; + ~AnticipationVelocityModel() override = default; + OperationalModelType Type() const override; + OperationalModelUpdate ComputeNewPosition( + double dT, + const GenericAgent& ped, + const CollisionGeometry& geometry, + const NeighborhoodSearchType& neighborhoodSearch) const override; + void ApplyUpdate(const OperationalModelUpdate& update, GenericAgent& agent) const override; + void CheckModelConstraint( + const GenericAgent& agent, + const NeighborhoodSearchType& neighborhoodSearch, + const CollisionGeometry& geometry) const override; + std::unique_ptr Clone() const override; + +private: + double OptimalSpeed(const GenericAgent& ped, double spacing, double time_gap) const; + Point CalculateInfluenceDirection( + const Point& desiredDirection, + const Point& predictedDirection) const; + double + GetSpacing(const GenericAgent& ped1, const GenericAgent& ped2, const Point& direction) const; + Point NeighborRepulsion(const GenericAgent& ped1, const GenericAgent& ped2) const; + + Point HandleWallAvoidance( + const Point& direction, + const Point& agentPosition, + double agentRadius, + const std::vector& boundary, + double wallBufferDistance) const; + + Point + UpdateDirection(const GenericAgent& ped, const Point& calculatedDirection, double dt) const; +}; diff --git a/libsimulator/src/AnticipationVelocityModelBuilder.cpp b/libsimulator/src/AnticipationVelocityModelBuilder.cpp new file mode 100644 index 000000000..384ae2748 --- /dev/null +++ b/libsimulator/src/AnticipationVelocityModelBuilder.cpp @@ -0,0 +1,12 @@ +// Copyright © 2012-2024 Forschungszentrum Jülich GmbH +// SPDX-License-Identifier: LGPL-3.0-or-later +#include "AnticipationVelocityModelBuilder.hpp" + +AnticipationVelocityModelBuilder::AnticipationVelocityModelBuilder() +{ +} + +AnticipationVelocityModel AnticipationVelocityModelBuilder::Build() +{ + return AnticipationVelocityModel(); +} diff --git a/libsimulator/src/AnticipationVelocityModelBuilder.hpp b/libsimulator/src/AnticipationVelocityModelBuilder.hpp new file mode 100644 index 000000000..eb4c97ca3 --- /dev/null +++ b/libsimulator/src/AnticipationVelocityModelBuilder.hpp @@ -0,0 +1,11 @@ +// Copyright © 2012-2024 Forschungszentrum Jülich GmbH +// SPDX-License-Identifier: LGPL-3.0-or-later +#pragma once + +#include "AnticipationVelocityModel.hpp" +class AnticipationVelocityModelBuilder +{ +public: + AnticipationVelocityModelBuilder(); + AnticipationVelocityModel Build(); +}; diff --git a/libsimulator/src/AnticipationVelocityModelData.hpp b/libsimulator/src/AnticipationVelocityModelData.hpp new file mode 100644 index 000000000..49e9539b7 --- /dev/null +++ b/libsimulator/src/AnticipationVelocityModelData.hpp @@ -0,0 +1,42 @@ +// Copyright © 2012-2024 Forschungszentrum Jülich GmbH +// SPDX-License-Identifier: LGPL-3.0-or-later +#pragma once + +#include "Point.hpp" + +struct AnticipationVelocityModelData { + double strengthNeighborRepulsion{}; + double rangeNeighborRepulsion{}; + double wallBufferDistance{0.1}; // buff distance of agent to wall + double anticipationTime{0.5}; // anticipation time + double reactionTime{0.1}; // reaction time to update direction + Point velocity{}; + double timeGap{1.06}; + double v0{1.2}; + double radius{0.15}; +}; + +template <> +struct fmt::formatter { + + constexpr auto parse(format_parse_context& ctx) { return ctx.begin(); } + + template + auto format(const AnticipationVelocityModelData& m, FormatContext& ctx) const + { + return fmt::format_to( + ctx.out(), + "AnticipationVelocityModel[strengthNeighborRepulsion={}, " + "rangeNeighborRepulsion={}, wallBufferDistance={}, " + "timeGap={}, v0={}, radius={}, reactionTime={}, anticipationTime={}, velocity={}])", + m.strengthNeighborRepulsion, + m.rangeNeighborRepulsion, + m.wallBufferDistance, + m.timeGap, + m.v0, + m.radius, + m.reactionTime, + m.anticipationTime, + m.velocity); + } +}; diff --git a/libsimulator/src/AnticipationVelocityModelUpdate.hpp b/libsimulator/src/AnticipationVelocityModelUpdate.hpp new file mode 100644 index 000000000..f3c7396c0 --- /dev/null +++ b/libsimulator/src/AnticipationVelocityModelUpdate.hpp @@ -0,0 +1,10 @@ +// Copyright © 2012-2024 Forschungszentrum Jülich GmbH +// SPDX-License-Identifier: LGPL-3.0-or-later +#pragma once +#include "Point.hpp" + +struct AnticipationVelocityModelUpdate { + Point position{}; + Point velocity{}; + Point orientation{}; +}; diff --git a/libsimulator/src/GenericAgent.hpp b/libsimulator/src/GenericAgent.hpp index c53f38aa0..edac57cbc 100644 --- a/libsimulator/src/GenericAgent.hpp +++ b/libsimulator/src/GenericAgent.hpp @@ -1,6 +1,7 @@ // Copyright © 2012-2024 Forschungszentrum Jülich GmbH // SPDX-License-Identifier: LGPL-3.0-or-later #pragma once +#include "AnticipationVelocityModelData.hpp" #include "CollisionFreeSpeedModelData.hpp" #include "CollisionFreeSpeedModelV2Data.hpp" #include "GeneralizedCentrifugalForceModelData.hpp" @@ -33,6 +34,7 @@ struct GenericAgent { GeneralizedCentrifugalForceModelData, CollisionFreeSpeedModelData, CollisionFreeSpeedModelV2Data, + AnticipationVelocityModelData, SocialForceModelData>; Model model{}; diff --git a/libsimulator/src/Mesh.cpp b/libsimulator/src/Mesh.cpp index 62fd01ff3..1f22069b2 100644 --- a/libsimulator/src/Mesh.cpp +++ b/libsimulator/src/Mesh.cpp @@ -416,9 +416,7 @@ std::vector Mesh::FVertices() const std::begin(vertices), std::end(vertices), std::back_inserter(f_vertices), - [](const auto& v) { - return glm::vec2{v.x, v.y}; - }); + [](const auto& v) { return glm::vec2{v.x, v.y}; }); return f_vertices; } diff --git a/libsimulator/src/OperationalModel.hpp b/libsimulator/src/OperationalModel.hpp index 13402d9a3..4d61de642 100644 --- a/libsimulator/src/OperationalModel.hpp +++ b/libsimulator/src/OperationalModel.hpp @@ -4,15 +4,12 @@ #include "Clonable.hpp" #include "CollisionGeometry.hpp" -#include "GeneralizedCentrifugalForceModelData.hpp" #include "OperationalModelType.hpp" #include "OperationalModelUpdate.hpp" #include "Point.hpp" #include "SimulationError.hpp" -#include "UniqueID.hpp" #include -#include template class NeighborhoodSearch; diff --git a/libsimulator/src/OperationalModelType.hpp b/libsimulator/src/OperationalModelType.hpp index d75568247..7343d815f 100644 --- a/libsimulator/src/OperationalModelType.hpp +++ b/libsimulator/src/OperationalModelType.hpp @@ -6,5 +6,6 @@ enum class OperationalModelType { COLLISION_FREE_SPEED, GENERALIZED_CENTRIFUGAL_FORCE, COLLISION_FREE_SPEED_V2, + ANTICIPATION_VELOCITY_MODEL, SOCIAL_FORCE }; diff --git a/libsimulator/src/OperationalModelUpdate.hpp b/libsimulator/src/OperationalModelUpdate.hpp index 19d1368b4..6f949947b 100644 --- a/libsimulator/src/OperationalModelUpdate.hpp +++ b/libsimulator/src/OperationalModelUpdate.hpp @@ -1,5 +1,6 @@ // Copyright © 2012-2024 Forschungszentrum Jülich GmbH // SPDX-License-Identifier: LGPL-3.0-or-later +#include "AnticipationVelocityModelUpdate.hpp" #include "CollisionFreeSpeedModelUpdate.hpp" #include "CollisionFreeSpeedModelV2Update.hpp" #include "GeneralizedCentrifugalForceModelUpdate.hpp" @@ -11,4 +12,5 @@ using OperationalModelUpdate = std::variant< GeneralizedCentrifugalForceModelUpdate, CollisionFreeSpeedModelUpdate, CollisionFreeSpeedModelV2Update, + AnticipationVelocityModelUpdate, SocialForceModelUpdate>; diff --git a/libsimulator/src/Point.cpp b/libsimulator/src/Point.cpp index 05fa67599..bbb0e7ba0 100644 --- a/libsimulator/src/Point.cpp +++ b/libsimulator/src/Point.cpp @@ -169,6 +169,11 @@ Point& Point::operator+=(const Point& p) return *this; } +Point Point::operator-() const +{ + return Point(-x, -y); +} + const Point operator/(const Point& p, double f) { static auto constexpr eps = diff --git a/libsimulator/src/Point.hpp b/libsimulator/src/Point.hpp index d682d832c..b0a1d112c 100644 --- a/libsimulator/src/Point.hpp +++ b/libsimulator/src/Point.hpp @@ -63,7 +63,8 @@ class Point bool operator!=(const Point& p) const; /// Assignement Point& operator+=(const Point& p); - + /// unary negation operator + Point operator-() const; bool operator<(const Point& rhs) const; bool operator>(const Point& rhs) const; diff --git a/notebooks/direct_steering.ipynb b/notebooks/direct_steering.ipynb index 19b02eefc..9bc7be267 100644 --- a/notebooks/direct_steering.ipynb +++ b/notebooks/direct_steering.ipynb @@ -132,7 +132,7 @@ " for idx, waypoint in enumerate(waypoints):\n", " axes.plot(waypoint[0], waypoint[1], \"ro\")\n", " axes.annotate(\n", - " f\"WP {idx+1}\",\n", + " f\"WP {idx + 1}\",\n", " (waypoint[0], waypoint[1]),\n", " textcoords=\"offset points\",\n", " xytext=(10, -15),\n", diff --git a/notebooks/journey.ipynb b/notebooks/journey.ipynb index d4cbfc96e..2cb6f0314 100644 --- a/notebooks/journey.ipynb +++ b/notebooks/journey.ipynb @@ -138,7 +138,7 @@ "for idx, waypoint in enumerate(waypoints):\n", " ax.plot(waypoint[0], waypoint[1], \"ro\")\n", " ax.annotate(\n", - " f\"WP {idx+1}\",\n", + " f\"WP {idx + 1}\",\n", " (waypoint[0], waypoint[1]),\n", " textcoords=\"offset points\",\n", " xytext=(10, -15),\n", diff --git a/notebooks/lane-formation.ipynb b/notebooks/lane-formation.ipynb index 0f8a7125c..10f989b03 100644 --- a/notebooks/lane-formation.ipynb +++ b/notebooks/lane-formation.ipynb @@ -90,7 +90,7 @@ " plt.text(\n", " centroid.x,\n", " centroid.y,\n", - " f\"Start {id+1}\",\n", + " f\"Start {id + 1}\",\n", " ha=\"center\",\n", " va=\"center\",\n", " fontsize=10,\n", @@ -277,7 +277,7 @@ " every_nth_frame=5,\n", " width=1200,\n", " height=400,\n", - " title_note=f\"Ratio: {min(number)/sum(number):0.2f}\",\n", + " title_note=f\"Ratio: {min(number) / sum(number):0.2f}\",\n", " ).show()" ] }, @@ -307,7 +307,7 @@ " traj_width=0.3,\n", " traj_start_marker=\".\",\n", " )\n", - " ax.set_title(f\"Ratio: {min(number)/sum(number):.2f}\")\n", + " ax.set_title(f\"Ratio: {min(number) / sum(number):.2f}\")\n", "plt.tight_layout()\n", "fig.set_size_inches((10, 12))\n", "plt.show()" @@ -385,7 +385,7 @@ " pedpy.plot_density(\n", " density=density_voronoi[number], axes=ax0, color=colors[i]\n", " )\n", - " labels.append(f\"Ratio: {min(number)/sum(number):.3f}\")\n", + " labels.append(f\"Ratio: {min(number) / sum(number):.3f}\")\n", "\n", "ax0.legend(labels)\n", "plt.show()" @@ -474,14 +474,14 @@ " pedpy.plot_speed(\n", " speed=mean_speed_r[number],\n", " axes=ax,\n", - " title=f\"Mean speed. Ratio: {min(number)/sum(number):.3f}\",\n", + " title=f\"Mean speed. Ratio: {min(number) / sum(number):.3f}\",\n", " color=pedpy.PEDPY_BLUE,\n", " )\n", "\n", " if number in mean_speed_l:\n", " pedpy.plot_speed(\n", " speed=mean_speed_l[number],\n", - " title=f\"Mean speed. Ratio: {min(number)/sum(number):.3f}\",\n", + " title=f\"Mean speed. Ratio: {min(number) / sum(number):.3f}\",\n", " axes=ax,\n", " color=pedpy.PEDPY_RED,\n", " )\n", diff --git a/notebooks/model-comparison.ipynb b/notebooks/model-comparison.ipynb new file mode 100644 index 000000000..98fa31b14 --- /dev/null +++ b/notebooks/model-comparison.ipynb @@ -0,0 +1,1134 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "d328babd", + "metadata": {}, + "source": [ + "# Comparative Analysis of Pedestrian Dynamics Models\n", + "\n", + "## Objective\n", + "\n", + "This notebook compares four pedestrian dynamics models by simulating agent behavior in a \n", + "structured environment. Using simplified scenarios, we analyze how each model handles \n", + "navigation, obstacle avoidance, and agent interactions. This comparison reveals the strengths \n", + "and weaknesses of each approach, helping inform model selection when using JuPedSim.\n", + "\n", + "When choosing a pedestrian dynamics model in JuPedSim, the decision largely depends on the \n", + "specific scenario you want to simulate or the behavior you want to reproduce. The available \n", + "models can be divided into two main categories: **force-based** and **velocity-based** models, \n", + "each with their own strengths and limitations.\n", + "\n", + "### Force-Based Models\n", + "\n", + "Force-based models, which include the **Social Force Model (SFM)** and the **Generalized \n", + "Centrifugal Force Model (GCFM)**, excel at simulating physical interactions between pedestrians. \n", + "These models are particularly effective at capturing pushing behavior in dense crowds, making \n", + "them ideal for evacuation scenarios or situations where physical contact between pedestrians \n", + "is common. The **GCFM** offers some improvements over the basic **SFM**, particularly in handling \n", + "body shape effects.\n", + "\n", + "### Velocity-Based Models\n", + "\n", + "On the other hand, velocity-based models like the **Collision-Free Model (CFM)** and the \n", + "**Anticipation Velocity Model (AVM)** are better suited for normal walking situations where \n", + "collision avoidance is the primary concern. These models typically produce smoother, more \n", + "realistic trajectories in regular-density scenarios. The **AVM**, in particular, stands out for \n", + "its superior collision avoidance capabilities and ability to reproduce lane formation behavior, \n", + "as demonstrated in [recent research](https://doi.org/10.1016/j.trc.2021.103464).\n", + "\n", + "### Computational Efficiency\n", + "\n", + "When it comes to computational efficiency, velocity-based models generally have an advantage. \n", + "The **CFM** offers the fastest execution times, while the **AVM** provides a good balance between \n", + "computational efficiency and realistic behavior simulation. Force-based models, while more \n", + "computationally intensive, are necessary when accurate physical interaction modeling is crucial.\n", + "\n", + "### Model Selection Guidance\n", + "\n", + "The choice of model ultimately comes down to your specific requirements:\n", + "\n", + "- **Force-based models (SFM, GCFM):** Best for emergency evacuations or crowded environments \n", + " with significant physical interactions. \n", + "- **Velocity-based models (AVM):** Ideal for typical pedestrian scenarios requiring smooth \n", + " navigation and realistic avoidance behavior. \n", + "- **Collision-Free Model (CFM):** Suitable when computational resources are limited and \n", + " physical interactions are not critical. \n", + "\n", + "The key is to understand that no single model is universally superior—each has its place \n", + "depending on the specific requirements of your simulation scenario.\n", + "\n", + "**So as a final facit, which model to use? The answer is: it depends.**\n", + "\n", + "## Models Under Investigation\n", + "\n", + "In this notebooks the models compared are:\n", + "\n", + "1. [**Collision-Free Speed Model (CSM):**](https://pedestriandynamics.org/models/collision_free_speed_model/) \n", + " A velocity-based model focused on ensuring agents move without collisions \n", + " by dynamically adjusting their speeds.\n", + "\n", + "2. [**Anticipation Velocity Model (AVM):**](https://doi.org/10.1016/j.trc.2021.103464) \n", + " A model that incorporates anticipation and reaction times, allowing agents \n", + " to predict and avoid potential collisions.\n", + "\n", + "3. [**Social Force Model (SFM):**](https://pedestriandynamics.org/models/social_force_model/)\n", + " A force-based model where agents are influenced \n", + " by attractive forces toward goals and repulsive forces \n", + " from obstacles and other agents.\n", + "\n", + "4. [**Generalized Centrifugal Force Model (GCFM):**](https://pedestriandynamics.org/models/generalized_centrifugal_force_model/) \n", + " An enhanced force-based model where agents experience centrifugal forces \n", + " to better simulate realistic avoidance behavior.\n", + "\n", + "> **Note:** \n", + "> All models are utilized with their default parameter settings as defined in **JuPedSim**." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "b982aeaf-a056-4870-bf96-4213db17ddcd", + "metadata": { + "tags": [ + "hide-input" + ] + }, + "outputs": [], + "source": [ + "import pathlib\n", + "\n", + "import jupedsim as jps\n", + "import matplotlib.pyplot as plt\n", + "import pedpy\n", + "from jupedsim.internal.notebook_utils import animate, read_sqlite_file\n", + "from shapely import Polygon\n", + "from shapely.ops import unary_union\n", + "\n", + "\n", + "def initialize_simulation(\n", + " model, agent_parameters, geometry, goals, positions, speeds, trajectory_file\n", + "):\n", + " simulation = jps.Simulation(\n", + " model=model,\n", + " geometry=geometry,\n", + " dt=0.01,\n", + " trajectory_writer=jps.SqliteTrajectoryWriter(\n", + " output_file=pathlib.Path(trajectory_file), every_nth_frame=5\n", + " ),\n", + " )\n", + "\n", + " exit_ids = [simulation.add_exit_stage(goal) for goal in goals]\n", + " journey = jps.JourneyDescription(exit_ids)\n", + " journey_id = simulation.add_journey(journey)\n", + " for pos, v0, exit_id in zip(positions, speeds, exit_ids):\n", + " if agent_parameters == jps.AnticipationVelocityModelAgentParameters:\n", + " simulation.add_agent(\n", + " agent_parameters(\n", + " journey_id=journey_id,\n", + " stage_id=exit_id,\n", + " v0=v0,\n", + " position=pos,\n", + " anticipation_time=1,\n", + " reaction_time=0.3,\n", + " )\n", + " )\n", + " elif agent_parameters == jps.SocialForceModelAgentParameters:\n", + " simulation.add_agent(\n", + " agent_parameters(\n", + " journey_id=journey_id,\n", + " stage_id=exit_id,\n", + " desiredSpeed=v0,\n", + " position=pos,\n", + " )\n", + " )\n", + " else:\n", + " simulation.add_agent(\n", + " agent_parameters(\n", + " journey_id=journey_id,\n", + " stage_id=exit_id,\n", + " position=pos,\n", + " v0=v0,\n", + " )\n", + " )\n", + "\n", + " return simulation\n", + "\n", + "\n", + "def plot_simulation_configuration(geometry, starting_positions, exit_areas):\n", + " \"\"\"Plot setup for visual inspection.\"\"\"\n", + " walkable_area = pedpy.WalkableArea(geometry)\n", + " axes = pedpy.plot_walkable_area(walkable_area=walkable_area)\n", + " for exit_area in exit_areas:\n", + " axes.fill(*exit_area.exterior.xy, color=\"indianred\")\n", + "\n", + " axes.scatter(*zip(*starting_positions), label=\"Starting Position\")\n", + " axes.set_xlabel(\"x/m\")\n", + " axes.set_ylabel(\"y/m\")\n", + " axes.set_aspect(\"equal\")\n", + " axes.grid(True, alpha=0.3)\n", + "\n", + "\n", + "def plot_evacuation_times(times_dict, figsize=(10, 6)):\n", + " \"\"\"\n", + " Plot evacuation times for different pedestrian models.\n", + " \"\"\"\n", + " fig = plt.figure(figsize=figsize)\n", + "\n", + " bars = plt.bar(list(times_dict.keys()), list(times_dict.values()))\n", + "\n", + " plt.title(\"Evacuation Times by Model\")\n", + " plt.ylabel(\"Time (seconds)\")\n", + " plt.grid(axis=\"y\", linestyle=\"--\", alpha=0.3)\n", + "\n", + " # Add value labels on top of each bar\n", + " for bar in bars:\n", + " height = bar.get_height()\n", + " plt.text(\n", + " bar.get_x() + bar.get_width() / 2.0,\n", + " height,\n", + " f\"{height:.1f}s\",\n", + " ha=\"center\",\n", + " va=\"bottom\",\n", + " )\n", + "\n", + " plt.tight_layout()\n", + " return fig\n", + "\n", + "\n", + "def run_simulation(simulation, max_iterations=4000):\n", + " while (\n", + " simulation.agent_count() > 0\n", + " and simulation.iteration_count() < max_iterations\n", + " ):\n", + " simulation.iterate()\n", + " print(f\"Evacuation time: {simulation.elapsed_time():.2f} s\")\n", + " return simulation.elapsed_time()\n", + "\n", + "\n", + "width = 600\n", + "height = 600" + ] + }, + { + "cell_type": "markdown", + "id": "625d2d09-f624-4d7a-b82f-0897cbf8bc35", + "metadata": {}, + "source": [ + "## Scenario 1: Crossing Paths in Rooms\n", + "\n", + "Four agents navigate between interconnected rooms with obstacles. Each agent aims to reach the diagonally opposite goal. Agents are expected to plan paths around obstacles while interacting with other agents in the corridors.\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ad9ec391-b8c6-4dc4-9255-51d0d8d7e194", + "metadata": { + "tags": [ + "hide-input" + ] + }, + "outputs": [], + "source": [ + "def create_geometry_scenario1():\n", + " outer_boundary = Polygon([(0, 0), (10, 0), (10, 10), (0, 10)])\n", + " walls = [\n", + " Polygon([(4.9, 0), (5.1, 0), (5.1, 3.8), (4.9, 3.8)]),\n", + " Polygon([(4.9, 6.2), (5.1, 6.2), (5.1, 10), (4.9, 10)]),\n", + " Polygon([(0, 4.9), (3.8, 4.9), (3.8, 5.1), (0, 5.1)]),\n", + " Polygon([(6.2, 4.9), (10, 4.9), (10, 5.1), (6.2, 5.1)]),\n", + " Polygon([(2.0, 2.0), (3.0, 2.0), (3.0, 3.0), (2.0, 3.0)]),\n", + " Polygon([(7.0, 2.0), (8.0, 2.0), (8.0, 3.0), (7.0, 3.0)]),\n", + " Polygon([(2.0, 7.0), (3.0, 7.0), (3.0, 6.0), (2.0, 6.0)]),\n", + " Polygon([(7.0, 7.0), (8.0, 7.0), (8.0, 6.0), (7.0, 6.0)]),\n", + " ]\n", + "\n", + " geometry = outer_boundary.difference(unary_union(walls))\n", + " return geometry\n", + "\n", + "\n", + "def define_positions_scenario1():\n", + " \"\"\"Define initial positions and desired speeds.\"\"\"\n", + " positions = [(1, 1), (9, 9), (1, 9), (9, 1)]\n", + " speeds = [1.0, 1.0, 1.0, 1.0]\n", + " return positions, speeds\n", + "\n", + "\n", + "def define_goals_scenario1():\n", + " \"\"\"Define goal polygons.\"\"\"\n", + " goals = [\n", + " Polygon([(8.5, 8.5), (9.5, 8.5), (9.5, 9.5), (8.5, 9.5)]),\n", + " Polygon([(0.5, 0.5), (1.5, 0.5), (1.5, 1.5), (0.5, 1.5)]),\n", + " Polygon([(8.5, 0.5), (9.5, 0.5), (9.5, 1.5), (8.5, 1.5)]),\n", + " Polygon([(0.5, 8.5), (1.5, 8.5), (1.5, 9.5), (0.5, 9.5)]),\n", + " ]\n", + " return goals\n", + "\n", + "\n", + "geometry = create_geometry_scenario1()\n", + "goals = define_goals_scenario1()\n", + "positions, speeds = define_positions_scenario1()\n", + "plot_simulation_configuration(geometry, positions, goals)\n", + "times_dict = {}" + ] + }, + { + "cell_type": "markdown", + "id": "6afbde36-10eb-4852-87b2-f181e2e6af5e", + "metadata": {}, + "source": [ + "### CSM Simulation\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "dc5ef18b-4c28-43ae-a764-5fb067d486d1", + "metadata": {}, + "outputs": [], + "source": [ + "model = \"CFM\"\n", + "trajectory_file = f\"{model}.sqlite\"\n", + "simulation = initialize_simulation(\n", + " jps.CollisionFreeSpeedModelV2(),\n", + " jps.CollisionFreeSpeedModelV2AgentParameters,\n", + " geometry,\n", + " goals,\n", + " positions,\n", + " speeds,\n", + " trajectory_file,\n", + ")\n", + "times_dict[model] = run_simulation(simulation, max_iterations=5000)\n", + "trajectories, walkable_area = read_sqlite_file(trajectory_file)\n", + "animate(\n", + " trajectories,\n", + " walkable_area,\n", + " title_note=model,\n", + " every_nth_frame=5,\n", + " width=width,\n", + " height=height,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "3a94b4b1-c4ec-40d9-bb04-6c47559d3ff5", + "metadata": {}, + "source": [ + "### AVM Simulation\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "c9221227-10c5-4869-8591-7f83b16c2c46", + "metadata": {}, + "outputs": [], + "source": [ + "model = \"AVM\"\n", + "trajectory_file = f\"{model}.sqlite\"\n", + "simulation = initialize_simulation(\n", + " jps.AnticipationVelocityModel(),\n", + " jps.AnticipationVelocityModelAgentParameters,\n", + " geometry,\n", + " goals,\n", + " positions,\n", + " speeds,\n", + " trajectory_file,\n", + ")\n", + "times_dict[model] = run_simulation(simulation, max_iterations=5000)\n", + "trajectories, walkable_area = read_sqlite_file(trajectory_file)\n", + "animate(\n", + " trajectories,\n", + " walkable_area,\n", + " title_note=model,\n", + " every_nth_frame=5,\n", + " width=width,\n", + " height=height,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "ca674e50-ff18-49ff-942e-6afd9902622f", + "metadata": {}, + "source": [ + "### SFM Simulation\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d553fe0b-5f46-408b-93c2-1354be3c2d7a", + "metadata": {}, + "outputs": [], + "source": [ + "model = \"SFM\"\n", + "trajectory_file = f\"{model}.sqlite\"\n", + "simulation = initialize_simulation(\n", + " jps.SocialForceModel(),\n", + " jps.SocialForceModelAgentParameters,\n", + " geometry,\n", + " goals,\n", + " positions,\n", + " speeds,\n", + " trajectory_file,\n", + ")\n", + "times_dict[model] = run_simulation(simulation, max_iterations=5000)\n", + "trajectories, walkable_area = read_sqlite_file(trajectory_file)\n", + "animate(\n", + " trajectories,\n", + " walkable_area,\n", + " title_note=model,\n", + " every_nth_frame=5,\n", + " width=width,\n", + " height=height,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "5ec92ce4-914d-4a08-a1e8-a12c6266f9c6", + "metadata": {}, + "source": [ + "### GCFM Simulation\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "f9850b99-1805-4a2e-af9e-1fa76a20dfc3", + "metadata": {}, + "outputs": [], + "source": [ + "model = \"GCFM\"\n", + "trajectory_file = f\"{model}.sqlite\"\n", + "simulation = initialize_simulation(\n", + " jps.GeneralizedCentrifugalForceModel(),\n", + " jps.GeneralizedCentrifugalForceModelAgentParameters,\n", + " geometry,\n", + " goals,\n", + " positions,\n", + " speeds,\n", + " trajectory_file,\n", + ")\n", + "times_dict[model] = run_simulation(simulation, max_iterations=5000)\n", + "trajectories, walkable_area = read_sqlite_file(trajectory_file)\n", + "animate(\n", + " trajectories,\n", + " walkable_area,\n", + " title_note=model,\n", + " every_nth_frame=5,\n", + " width=width,\n", + " height=height,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "884203af-0af3-474e-a4a9-646ab3988478", + "metadata": {}, + "source": [ + "### Evacuation times\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "38454c9e-32b2-4521-9a54-5d608b7345ca", + "metadata": { + "tags": [ + "hide-input" + ] + }, + "outputs": [], + "source": [ + "fig = plot_evacuation_times(times_dict)\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "id": "790f4f70-432b-4bcd-a467-5df58259e054", + "metadata": {}, + "source": [ + "## Scenario 2: Following Behavior\n", + "\n", + "A single-line setup where one agent follows another slower-moving agent, testing how the models handle speed adjustments and eventually overtaking.\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "20c9eb29-34d3-46fc-a206-2273934b290c", + "metadata": { + "tags": [ + "hide-input" + ] + }, + "outputs": [], + "source": [ + "def create_geometry_scenario2():\n", + " geometry = Polygon([(-3, -2), (16, -2), (16, 2), (-3, 2)])\n", + " return geometry\n", + "\n", + "\n", + "def define_positions_scenario2():\n", + " \"\"\"Define initial positions and desired speeds.\"\"\"\n", + " positions = [\n", + " (-2, 0),\n", + " (2, 0),\n", + " ]\n", + " speeds = [\n", + " 1.0,\n", + " 0.0,\n", + " ]\n", + " return positions, speeds\n", + "\n", + "\n", + "def define_goals_scenario2():\n", + " \"\"\"Define goal polygons.\"\"\"\n", + " goals = [\n", + " Polygon([(12, -1), (15, -1), (15, 1), (12, 1)]),\n", + " Polygon([(12, -1), (15, -1), (15, 1), (12, 1)]),\n", + " ]\n", + " return goals\n", + "\n", + "\n", + "geometry = create_geometry_scenario2()\n", + "positions, speeds = define_positions_scenario2()\n", + "goals = define_goals_scenario2()\n", + "plot_simulation_configuration(geometry, positions, goals)\n", + "times_dict = {}" + ] + }, + { + "cell_type": "markdown", + "id": "1dc114b6-0b4f-422d-940a-8c75776f687d", + "metadata": {}, + "source": [ + "### CSM Simulation\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "7225ac10-e170-4733-b0ca-507516f87259", + "metadata": {}, + "outputs": [], + "source": [ + "model = \"CFM\"\n", + "trajectory_file = f\"{model}.sqlite\"\n", + "simulation = initialize_simulation(\n", + " jps.CollisionFreeSpeedModelV2(),\n", + " jps.CollisionFreeSpeedModelV2AgentParameters,\n", + " geometry,\n", + " goals,\n", + " positions,\n", + " speeds,\n", + " trajectory_file,\n", + ")\n", + "times_dict[model] = run_simulation(simulation, max_iterations=2000)\n", + "trajectories, walkable_area = read_sqlite_file(trajectory_file)\n", + "animate(\n", + " trajectories,\n", + " walkable_area,\n", + " title_note=model,\n", + " every_nth_frame=5,\n", + " width=width,\n", + " height=height,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "0a65cae5-f2f3-4c29-b357-a6f2b1f28d1a", + "metadata": {}, + "source": [ + "### AVM Simulation\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "047b1727-210a-4fb5-bdd1-e7432f255193", + "metadata": {}, + "outputs": [], + "source": [ + "model = \"AVM\"\n", + "trajectory_file = f\"{model}.sqlite\"\n", + "simulation = initialize_simulation(\n", + " jps.AnticipationVelocityModel(),\n", + " jps.AnticipationVelocityModelAgentParameters,\n", + " geometry,\n", + " goals,\n", + " positions,\n", + " speeds,\n", + " trajectory_file,\n", + ")\n", + "times_dict[model] = run_simulation(simulation, max_iterations=2000)\n", + "trajectories, walkable_area = read_sqlite_file(trajectory_file)\n", + "animate(\n", + " trajectories,\n", + " walkable_area,\n", + " title_note=model,\n", + " every_nth_frame=5,\n", + " width=width,\n", + " height=height,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "4b02fbfa-251a-43d9-97ed-ca6e5f6784ff", + "metadata": {}, + "source": [ + "### SFM Simulation\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "14cedaae-4b5f-4b0a-8667-a7d350e3a4d8", + "metadata": {}, + "outputs": [], + "source": [ + "model = \"SFM\"\n", + "trajectory_file = f\"{model}.sqlite\"\n", + "simulation = initialize_simulation(\n", + " jps.SocialForceModel(),\n", + " jps.SocialForceModelAgentParameters,\n", + " geometry,\n", + " goals,\n", + " positions,\n", + " speeds,\n", + " trajectory_file,\n", + ")\n", + "times_dict[model] = run_simulation(simulation, max_iterations=2000)\n", + "trajectories, walkable_area = read_sqlite_file(trajectory_file)\n", + "animate(\n", + " trajectories,\n", + " walkable_area,\n", + " title_note=model,\n", + " every_nth_frame=5,\n", + " width=width,\n", + " height=height,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "de75158d-3073-4ef7-a17c-9d1b94f16fbc", + "metadata": {}, + "source": [ + "### GCFM Simulation\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "cd4b3e6f-0c34-4e18-8b62-4e9bb2464e97", + "metadata": {}, + "outputs": [], + "source": [ + "model = \"GCFM\"\n", + "trajectory_file = f\"{model}.sqlite\"\n", + "simulation = initialize_simulation(\n", + " jps.GeneralizedCentrifugalForceModel(),\n", + " jps.GeneralizedCentrifugalForceModelAgentParameters,\n", + " geometry,\n", + " goals,\n", + " positions,\n", + " speeds,\n", + " trajectory_file,\n", + ")\n", + "times_dict[model] = run_simulation(simulation, max_iterations=2000)\n", + "trajectories, walkable_area = read_sqlite_file(trajectory_file)\n", + "animate(\n", + " trajectories,\n", + " walkable_area,\n", + " title_note=model,\n", + " every_nth_frame=5,\n", + " width=width,\n", + " height=height,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "6f9f6b1f", + "metadata": {}, + "source": [ + "## Scenario 3: Head-on Encounter\n", + "\n", + "Two agents approach each other on a straight line, evaluating how different models resolve direct confrontation and determine which agent yields or how they negotiate passing space.\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "8894d9c1", + "metadata": { + "tags": [ + "hide-input" + ] + }, + "outputs": [], + "source": [ + "def create_geometry_scenario3():\n", + " geometry = Polygon([(-10, -2), (10, -2), (10, 2), (-10, 2)])\n", + " return geometry\n", + "\n", + "\n", + "def define_positions_scenario3():\n", + " \"\"\"Define initial positions and desired speeds.\"\"\"\n", + " positions = [\n", + " (-2, 0),\n", + " (2, 0),\n", + " ]\n", + " speeds = [\n", + " 1.0,\n", + " 1.0,\n", + " ]\n", + " return positions, speeds\n", + "\n", + "\n", + "def define_goals_scenario3():\n", + " \"\"\"Define goal polygons.\"\"\"\n", + " goals = [\n", + " Polygon([(6, -1), (9, -1), (9, 1), (6, 1)]),\n", + " Polygon([(-9, -1), (-6, -1), (-6, 1), (-9, 1)]),\n", + " ]\n", + " return goals\n", + "\n", + "\n", + "geometry = create_geometry_scenario3()\n", + "positions, speeds = define_positions_scenario3()\n", + "goals = define_goals_scenario3()\n", + "plot_simulation_configuration(geometry, positions, goals)\n", + "times_dict = {}" + ] + }, + { + "cell_type": "markdown", + "id": "5deb294f", + "metadata": {}, + "source": [ + "### CSM Simulation\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "acdfbfcd", + "metadata": {}, + "outputs": [], + "source": [ + "model = \"CFM\"\n", + "trajectory_file = f\"{model}.sqlite\"\n", + "simulation = initialize_simulation(\n", + " jps.CollisionFreeSpeedModelV2(),\n", + " jps.CollisionFreeSpeedModelV2AgentParameters,\n", + " geometry,\n", + " goals,\n", + " positions,\n", + " speeds,\n", + " trajectory_file,\n", + ")\n", + "times_dict[model] = run_simulation(simulation, max_iterations=2000)\n", + "trajectories, walkable_area = read_sqlite_file(trajectory_file)\n", + "animate(\n", + " trajectories,\n", + " walkable_area,\n", + " title_note=model,\n", + " every_nth_frame=5,\n", + " width=width,\n", + " height=height,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "2fbf4d77", + "metadata": {}, + "source": [ + "### AVM Simulation\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "6e1657f8", + "metadata": {}, + "outputs": [], + "source": [ + "model = \"AVM\"\n", + "trajectory_file = f\"{model}.sqlite\"\n", + "simulation = initialize_simulation(\n", + " jps.AnticipationVelocityModel(),\n", + " jps.AnticipationVelocityModelAgentParameters,\n", + " geometry,\n", + " goals,\n", + " positions,\n", + " speeds,\n", + " trajectory_file,\n", + ")\n", + "times_dict[model] = run_simulation(simulation, max_iterations=2000)\n", + "trajectories, walkable_area = read_sqlite_file(trajectory_file)\n", + "animate(\n", + " trajectories,\n", + " walkable_area,\n", + " title_note=model,\n", + " every_nth_frame=5,\n", + " width=width,\n", + " height=height,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "2448d825", + "metadata": {}, + "source": [ + "### SFM Simulation\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "5a5fdb29", + "metadata": {}, + "outputs": [], + "source": [ + "model = \"SFM\"\n", + "trajectory_file = f\"{model}.sqlite\"\n", + "simulation = initialize_simulation(\n", + " jps.SocialForceModel(),\n", + " jps.SocialForceModelAgentParameters,\n", + " geometry,\n", + " goals,\n", + " positions,\n", + " speeds,\n", + " trajectory_file,\n", + ")\n", + "times_dict[model] = run_simulation(simulation, max_iterations=2000)\n", + "trajectories, walkable_area = read_sqlite_file(trajectory_file)\n", + "animate(\n", + " trajectories,\n", + " walkable_area,\n", + " title_note=model,\n", + " every_nth_frame=5,\n", + " width=width,\n", + " height=height,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "24f23728", + "metadata": {}, + "source": [ + "### GCFM Simulation\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "12c632de", + "metadata": {}, + "outputs": [], + "source": [ + "model = \"GCFM\"\n", + "trajectory_file = f\"{model}.sqlite\"\n", + "simulation = initialize_simulation(\n", + " jps.GeneralizedCentrifugalForceModel(),\n", + " jps.GeneralizedCentrifugalForceModelAgentParameters,\n", + " geometry,\n", + " goals,\n", + " positions,\n", + " speeds,\n", + " trajectory_file,\n", + ")\n", + "times_dict[model] = run_simulation(simulation, max_iterations=5000)\n", + "trajectories, walkable_area = read_sqlite_file(trajectory_file)\n", + "animate(\n", + " trajectories,\n", + " walkable_area,\n", + " title_note=model,\n", + " every_nth_frame=5,\n", + " width=width,\n", + " height=height,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "a9848f9e", + "metadata": {}, + "source": [ + "### Evacuation times" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "9926f3b0-d1c4-4e38-9071-71b20298ed25", + "metadata": { + "tags": [ + "hide-input" + ] + }, + "outputs": [], + "source": [ + "fig = plot_evacuation_times(times_dict)\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "id": "478351a0", + "metadata": {}, + "source": [ + "## Scenario 4: Perpendicular Crossing\n", + "\n", + "Two agents move on intersecting paths - one traveling upward and another moving left to right - creating a potential collision point. This tests how models handle collision avoidance when agents approach at right angles.\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "86c2b199", + "metadata": { + "tags": [ + "hide-input" + ] + }, + "outputs": [], + "source": [ + "def create_geometry_scenario4():\n", + " geometry = Polygon([(-6, -6), (6, -6), (6, 6), (-6, 6)])\n", + " return geometry\n", + "\n", + "\n", + "def define_positions_scenario4():\n", + " \"\"\"Define initial positions and desired speeds.\"\"\"\n", + " positions = [(-2, 0), (0, -2)]\n", + " speeds = [1.0, 0.99]\n", + " return positions, speeds\n", + "\n", + "\n", + "def define_goals_scenario4():\n", + " \"\"\"Define goal polygons.\"\"\"\n", + " goals = [\n", + " Polygon([(5, -2), (6, -2), (6, 2), (5, 2)]),\n", + " Polygon([(-2, 5), (-2, 6), (2, 6), (2, 5)]),\n", + " ]\n", + " return goals\n", + "\n", + "\n", + "geometry = create_geometry_scenario4()\n", + "positions, speeds = define_positions_scenario4()\n", + "goals = define_goals_scenario4()\n", + "plot_simulation_configuration(geometry, positions, goals)\n", + "times_dict = {}" + ] + }, + { + "cell_type": "markdown", + "id": "29092296", + "metadata": {}, + "source": [ + "### CSM Simulation\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "5683d076", + "metadata": {}, + "outputs": [], + "source": [ + "model = \"CFM\"\n", + "trajectory_file = f\"{model}.sqlite\"\n", + "simulation = initialize_simulation(\n", + " jps.CollisionFreeSpeedModelV2(),\n", + " jps.CollisionFreeSpeedModelV2AgentParameters,\n", + " geometry,\n", + " goals,\n", + " positions,\n", + " speeds,\n", + " trajectory_file,\n", + ")\n", + "times_dict[model] = run_simulation(simulation, max_iterations=2000)\n", + "trajectories, walkable_area = read_sqlite_file(trajectory_file)\n", + "animate(\n", + " trajectories,\n", + " walkable_area,\n", + " title_note=model,\n", + " every_nth_frame=5,\n", + " width=width,\n", + " height=height,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "f1767999", + "metadata": {}, + "source": [ + "### AVM Simulation\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "9ac17a40", + "metadata": {}, + "outputs": [], + "source": [ + "model = \"AVM\"\n", + "trajectory_file = f\"{model}.sqlite\"\n", + "simulation = initialize_simulation(\n", + " jps.AnticipationVelocityModel(),\n", + " jps.AnticipationVelocityModelAgentParameters,\n", + " geometry,\n", + " goals,\n", + " positions,\n", + " speeds,\n", + " trajectory_file,\n", + ")\n", + "times_dict[model] = run_simulation(simulation, max_iterations=2000)\n", + "trajectories, walkable_area = read_sqlite_file(trajectory_file)\n", + "animate(\n", + " trajectories,\n", + " walkable_area,\n", + " title_note=model,\n", + " every_nth_frame=5,\n", + " width=width,\n", + " height=height,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "d6a75e02", + "metadata": {}, + "source": [ + "### SFM Simulation\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "14e13c6b", + "metadata": {}, + "outputs": [], + "source": [ + "model = \"SFM\"\n", + "trajectory_file = f\"{model}.sqlite\"\n", + "simulation = initialize_simulation(\n", + " jps.SocialForceModel(),\n", + " jps.SocialForceModelAgentParameters,\n", + " geometry,\n", + " goals,\n", + " positions,\n", + " speeds,\n", + " trajectory_file,\n", + ")\n", + "times_dict[model] = run_simulation(simulation, max_iterations=2000)\n", + "trajectories, walkable_area = read_sqlite_file(trajectory_file)\n", + "animate(\n", + " trajectories,\n", + " walkable_area,\n", + " title_note=model,\n", + " every_nth_frame=5,\n", + " width=width,\n", + " height=height,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "cbfbde63", + "metadata": {}, + "source": [ + "### GCFM Simulation\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "20edcdd7", + "metadata": {}, + "outputs": [], + "source": [ + "model = \"GCFM\"\n", + "trajectory_file = f\"{model}.sqlite\"\n", + "simulation = initialize_simulation(\n", + " jps.GeneralizedCentrifugalForceModel(),\n", + " jps.GeneralizedCentrifugalForceModelAgentParameters,\n", + " geometry,\n", + " goals,\n", + " positions,\n", + " speeds,\n", + " trajectory_file,\n", + ")\n", + "times_dict[model] = run_simulation(simulation, max_iterations=2000)\n", + "trajectories, walkable_area = read_sqlite_file(trajectory_file)\n", + "animate(\n", + " trajectories,\n", + " walkable_area,\n", + " title_note=model,\n", + " every_nth_frame=5,\n", + " width=width,\n", + " height=height,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "79db3354", + "metadata": {}, + "source": [ + "### Evacuation times" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "5cfe84e2-4c16-4b44-b4d7-69c83bd662e0", + "metadata": { + "tags": [ + "hide-input" + ] + }, + "outputs": [], + "source": [ + "fig = plot_evacuation_times(times_dict)\n", + "plt.show()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.8" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/notebooks/routing.ipynb b/notebooks/routing.ipynb index c1d680ce2..e26db3bc5 100644 --- a/notebooks/routing.ipynb +++ b/notebooks/routing.ipynb @@ -93,7 +93,7 @@ "for idx, (waypoint, distance) in enumerate(waypoints):\n", " ax.plot(waypoint[0], waypoint[1], \"ro\")\n", " ax.annotate(\n", - " f\"WP {idx+1}\",\n", + " f\"WP {idx + 1}\",\n", " (waypoint[0], waypoint[1]),\n", " textcoords=\"offset points\",\n", " xytext=(10, -15),\n", diff --git a/python_bindings_jupedsim/CMakeLists.txt b/python_bindings_jupedsim/CMakeLists.txt index fe65c0b9a..2b9e1beae 100644 --- a/python_bindings_jupedsim/CMakeLists.txt +++ b/python_bindings_jupedsim/CMakeLists.txt @@ -6,6 +6,7 @@ add_library(py_jupedsim MODULE generalized_centrifugal_force_model.cpp collision_free_speed_model.cpp collision_free_speed_model_v2.cpp + anticipation_velocity_model.cpp social_force_model.cpp logging.cpp logging.hpp diff --git a/python_bindings_jupedsim/agent.cpp b/python_bindings_jupedsim/agent.cpp index c3919f955..bfd6f1a2c 100644 --- a/python_bindings_jupedsim/agent.cpp +++ b/python_bindings_jupedsim/agent.cpp @@ -70,6 +70,7 @@ void init_agent(py::module_& m) std::unique_ptr, std::unique_ptr, std::unique_ptr, + std::unique_ptr, std::unique_ptr> { switch(JPS_Agent_GetModelType(w.handle)) { case JPS_GeneralizedCentrifugalForceModel: @@ -81,6 +82,9 @@ void init_agent(py::module_& m) case JPS_CollisionFreeSpeedModelV2: return std::make_unique( JPS_Agent_GetCollisionFreeSpeedModelV2State(w.handle, nullptr)); + case JPS_AnticipationVelocityModel: + return std::make_unique( + JPS_Agent_GetAnticipationVelocityModelState(w.handle, nullptr)); case JPS_SocialForceModel: return std::make_unique( JPS_Agent_GetSocialForceModelState(w.handle, nullptr)); diff --git a/python_bindings_jupedsim/anticipation_velocity_model.cpp b/python_bindings_jupedsim/anticipation_velocity_model.cpp new file mode 100644 index 000000000..13076c491 --- /dev/null +++ b/python_bindings_jupedsim/anticipation_velocity_model.cpp @@ -0,0 +1,159 @@ +// Copyright © 2012-2024 Forschungszentrum Jülich GmbH +// SPDX-License-Identifier: LGPL-3.0-or-later +#include "jupedsim/anticipation_velocity_model.h" +#include "conversion.hpp" +#include "wrapper.hpp" + +#include + +#include +#include +#include +#include + +namespace py = pybind11; + +void init_anticipation_velocity_model(py::module_& m) +{ + py::class_( + m, "AnticipationVelocityModelAgentParameters") + .def( + py::init([](std::tuple position, + double time_gap, + double v0, + double radius, + JPS_JourneyId journey_id, + JPS_StageId stage_id, + double strengthNeighborRepulsion, + double rangeNeighborRepulsion, + double wallBufferDistance, + double anticipationTime, + double reactionTime) { + return JPS_AnticipationVelocityModelAgentParameters{ + intoJPS_Point(position), + journey_id, + stage_id, + time_gap, + v0, + radius, + strengthNeighborRepulsion, + rangeNeighborRepulsion, + wallBufferDistance, + anticipationTime, + reactionTime}; + }), + py::kw_only(), + py::arg("position"), + py::arg("time_gap"), + py::arg("v0"), + py::arg("radius"), + py::arg("journey_id"), + py::arg("stage_id"), + py::arg("strength_neighbor_repulsion"), + py::arg("range_neighbor_repulsion"), + py::arg("wall_buffer_distance"), + py::arg("anticipation_time"), + py::arg("reaction_time")) + .def("__repr__", [](const JPS_AnticipationVelocityModelAgentParameters& p) { + return fmt::format( + "position: {}, journey_id: {}, stage_id: {}, " + "time_gap: {}, v0: {}, radius: {}", + "strength_neighbor_repulsion: {}, range_neighbor_repulsion: {}" + "wall_buffer_distance: {}" + "anticipation_time: {}, reaction_time: {}", + intoTuple(p.position), + p.journeyId, + p.stageId, + p.time_gap, + p.v0, + p.radius, + p.strengthNeighborRepulsion, + p.rangeNeighborRepulsion, + p.wallBufferDistance, + p.anticipationTime, + p.reactionTime); + }); + py::class_(m, "AnticipationVelocityModelBuilder") + .def(py::init([]() { + return std::make_unique( + JPS_AnticipationVelocityModelBuilder_Create()); + })) + .def("build", [](JPS_AnticipationVelocityModelBuilder_Wrapper& w) { + JPS_ErrorMessage errorMsg{}; + auto result = JPS_AnticipationVelocityModelBuilder_Build(w.handle, &errorMsg); + if(result) { + return std::make_unique(result); + } + auto msg = std::string(JPS_ErrorMessage_GetMessage(errorMsg)); + JPS_ErrorMessage_Free(errorMsg); + throw std::runtime_error{msg}; + }); + py::class_(m, "AnticipationVelocityModelState") + .def_property( + "time_gap", + [](const JPS_AnticipationVelocityModelState_Wrapper& w) { + return JPS_AnticipationVelocityModelState_GetTimeGap(w.handle); + }, + [](JPS_AnticipationVelocityModelState_Wrapper& w, double time_gap) { + JPS_AnticipationVelocityModelState_SetTimeGap(w.handle, time_gap); + }) + .def_property( + "v0", + [](const JPS_AnticipationVelocityModelState_Wrapper& w) { + return JPS_AnticipationVelocityModelState_GetV0(w.handle); + }, + [](JPS_AnticipationVelocityModelState_Wrapper& w, double v0) { + JPS_AnticipationVelocityModelState_SetV0(w.handle, v0); + }) + .def_property( + "radius", + [](const JPS_AnticipationVelocityModelState_Wrapper& w) { + return JPS_AnticipationVelocityModelState_GetRadius(w.handle); + }, + [](JPS_AnticipationVelocityModelState_Wrapper& w, double radius) { + JPS_AnticipationVelocityModelState_SetRadius(w.handle, radius); + }) + .def_property( + "strength_neighbor_repulsion", + [](const JPS_AnticipationVelocityModelState_Wrapper& w) { + return JPS_AnticipationVelocityModelState_GetStrengthNeighborRepulsion(w.handle); + }, + [](JPS_AnticipationVelocityModelState_Wrapper& w, double strengthNeighborRepulsion) { + JPS_AnticipationVelocityModelState_SetStrengthNeighborRepulsion( + w.handle, strengthNeighborRepulsion); + }) + .def_property( + "range_neighbor_repulsion", + [](const JPS_AnticipationVelocityModelState_Wrapper& w) { + return JPS_AnticipationVelocityModelState_GetRangeNeighborRepulsion(w.handle); + }, + [](JPS_AnticipationVelocityModelState_Wrapper& w, double rangeNeighborRepulsion) { + JPS_AnticipationVelocityModelState_SetRangeNeighborRepulsion( + w.handle, rangeNeighborRepulsion); + }) + .def_property( + "wall_buffer_distance", + [](const JPS_AnticipationVelocityModelState_Wrapper& w) { + return JPS_AnticipationVelocityModelState_GetWallBufferDistance(w.handle); + }, + [](JPS_AnticipationVelocityModelState_Wrapper& w, double wallBufferDistance) { + JPS_AnticipationVelocityModelState_SetWallBufferDistance( + w.handle, wallBufferDistance); + }) + .def_property( + "anticipation_time", + [](const JPS_AnticipationVelocityModelState_Wrapper& w) { + return JPS_AnticipationVelocityModelState_GetAnticipationTime(w.handle); + }, + [](JPS_AnticipationVelocityModelState_Wrapper& w, double anticipationTime) { + JPS_AnticipationVelocityModelState_SetAnticipationTime(w.handle, anticipationTime); + }) + .def_property( + "reaction_time", + [](const JPS_AnticipationVelocityModelState_Wrapper& w) { + return JPS_AnticipationVelocityModelState_GetReactionTime(w.handle); + }, + [](JPS_AnticipationVelocityModelState_Wrapper& w, double reactionTime) { + JPS_AnticipationVelocityModelState_SetReactionTime(w.handle, reactionTime); + }); +} diff --git a/python_bindings_jupedsim/bindings_jupedsim.cpp b/python_bindings_jupedsim/bindings_jupedsim.cpp index 054cfe13b..68aa8b283 100644 --- a/python_bindings_jupedsim/bindings_jupedsim.cpp +++ b/python_bindings_jupedsim/bindings_jupedsim.cpp @@ -10,6 +10,7 @@ void init_trace(py::module_& m); void init_generalized_centrifugal_force_model(py::module_& m); void init_collision_free_speed_model(py::module_& m); void init_collision_free_speed_model_v2(py::module_& m); +void init_anticipation_velocity_model(py::module_& m); void init_social_force_model(py::module_& m); void init_geometry(py::module_& m); void init_routing(py::module_& m); @@ -27,6 +28,7 @@ PYBIND11_MODULE(py_jupedsim, m) init_generalized_centrifugal_force_model(m); init_collision_free_speed_model(m); init_collision_free_speed_model_v2(m); + init_anticipation_velocity_model(m); init_social_force_model(m); init_geometry(m); init_routing(m); diff --git a/python_bindings_jupedsim/simulation.cpp b/python_bindings_jupedsim/simulation.cpp index 5c4901d3b..8c6fda9ac 100644 --- a/python_bindings_jupedsim/simulation.cpp +++ b/python_bindings_jupedsim/simulation.cpp @@ -156,6 +156,20 @@ void init_simulation(py::module_& m) JPS_ErrorMessage_Free(errorMsg); throw std::runtime_error{msg}; }) + .def( + "add_agent", + [](JPS_Simulation_Wrapper& simulation, + JPS_AnticipationVelocityModelAgentParameters& parameters) { + JPS_ErrorMessage errorMsg{}; + auto result = JPS_Simulation_AddAnticipationVelocityModelAgent( + simulation.handle, parameters, &errorMsg); + if(result) { + return result; + } + auto msg = std::string(JPS_ErrorMessage_GetMessage(errorMsg)); + JPS_ErrorMessage_Free(errorMsg); + throw std::runtime_error{msg}; + }) .def( "add_agent", [](JPS_Simulation_Wrapper& simulation, diff --git a/python_bindings_jupedsim/wrapper.hpp b/python_bindings_jupedsim/wrapper.hpp index e03749677..8369f0221 100644 --- a/python_bindings_jupedsim/wrapper.hpp +++ b/python_bindings_jupedsim/wrapper.hpp @@ -1,6 +1,7 @@ // Copyright © 2012-2024 Forschungszentrum Jülich GmbH // SPDX-License-Identifier: LGPL-3.0-or-later #pragma once +#include "jupedsim/anticipation_velocity_model.h" #include #define OWNED_WRAPPER(cls) \ @@ -38,6 +39,7 @@ OWNED_WRAPPER(JPS_GeometryBuilder); OWNED_WRAPPER(JPS_OperationalModel); OWNED_WRAPPER(JPS_CollisionFreeSpeedModelBuilder); OWNED_WRAPPER(JPS_CollisionFreeSpeedModelV2Builder); +OWNED_WRAPPER(JPS_AnticipationVelocityModelBuilder); OWNED_WRAPPER(JPS_GeneralizedCentrifugalForceModelBuilder); OWNED_WRAPPER(JPS_SocialForceModelBuilder); OWNED_WRAPPER(JPS_JourneyDescription); @@ -55,4 +57,5 @@ WRAPPER(JPS_Agent); WRAPPER(JPS_GeneralizedCentrifugalForceModelState); WRAPPER(JPS_CollisionFreeSpeedModelState); WRAPPER(JPS_CollisionFreeSpeedModelV2State); +WRAPPER(JPS_AnticipationVelocityModelState); WRAPPER(JPS_SocialForceModelState); diff --git a/python_modules/jupedsim/jupedsim/__init__.py b/python_modules/jupedsim/jupedsim/__init__.py index b923adefe..1cc13298f 100644 --- a/python_modules/jupedsim/jupedsim/__init__.py +++ b/python_modules/jupedsim/jupedsim/__init__.py @@ -25,6 +25,11 @@ set_info_callback, set_warning_callback, ) +from jupedsim.models.anticipation_velocity_model import ( + AnticipationVelocityModel, + AnticipationVelocityModelAgentParameters, + AnticipationVelocityModelState, +) from jupedsim.models.collision_free_speed import ( CollisionFreeSpeedModel, CollisionFreeSpeedModelAgentParameters, @@ -105,6 +110,9 @@ "CollisionFreeSpeedModelV2AgentParameters", "CollisionFreeSpeedModelV2", "CollisionFreeSpeedModelV2State", + "AnticipationVelocityModelAgentParameters", + "AnticipationVelocityModel", + "AnticipationVelocityModelState", "SocialForceModelAgentParameters", "SocialForceModel", "SocialForceModelState", diff --git a/python_modules/jupedsim/jupedsim/agent.py b/python_modules/jupedsim/jupedsim/agent.py index 9623e58ef..00096970b 100644 --- a/python_modules/jupedsim/jupedsim/agent.py +++ b/python_modules/jupedsim/jupedsim/agent.py @@ -2,6 +2,9 @@ # SPDX-License-Identifier: LGPL-3.0-or-later import jupedsim.native as py_jps +from jupedsim.models.anticipation_velocity_model import ( + AnticipationVelocityModelState, +) from jupedsim.models.collision_free_speed import CollisionFreeSpeedModelState from jupedsim.models.collision_free_speed_v2 import ( CollisionFreeSpeedModelV2State, @@ -105,6 +108,8 @@ def model( ) -> ( GeneralizedCentrifugalForceModelState | CollisionFreeSpeedModelState + | CollisionFreeSpeedModelV2State + | AnticipationVelocityModelState | SocialForceModelState ): """Access model specific state of this agent.""" @@ -115,6 +120,8 @@ def model( return CollisionFreeSpeedModelState(model) elif isinstance(model, py_jps.CollisionFreeSpeedModelV2State): return CollisionFreeSpeedModelV2State(model) + elif isinstance(model, py_jps.AnticipationVelocityModelState): + return AnticipationVelocityModelState(model) elif isinstance(model, py_jps.SocialForceModelState): return SocialForceModelState(model) else: diff --git a/python_modules/jupedsim/jupedsim/internal/notebook_utils.py b/python_modules/jupedsim/jupedsim/internal/notebook_utils.py index 00b2db41b..daab8c50e 100644 --- a/python_modules/jupedsim/jupedsim/internal/notebook_utils.py +++ b/python_modules/jupedsim/jupedsim/internal/notebook_utils.py @@ -46,7 +46,7 @@ def _speed_to_color(speed, min_speed, max_speed): """Map a speed value to a color using a colormap.""" normalized_speed = (speed - min_speed) / (max_speed - min_speed) r, g, b = plt.cm.jet_r(normalized_speed)[:3] - return f"rgba({r*255:.0f}, {g*255:.0f}, {b*255:.0f}, 0.5)" + return f"rgba({r * 255:.0f}, {g * 255:.0f}, {b * 255:.0f}, 0.5)" def _get_line_color(disk_color): diff --git a/python_modules/jupedsim/jupedsim/models/anticipation_velocity_model.py b/python_modules/jupedsim/jupedsim/models/anticipation_velocity_model.py new file mode 100644 index 000000000..0b6aad5b0 --- /dev/null +++ b/python_modules/jupedsim/jupedsim/models/anticipation_velocity_model.py @@ -0,0 +1,174 @@ +# Copyright © 2012-2024 Forschungszentrum Jülich GmbH +# SPDX-License-Identifier: LGPL-3.0-or-later + +from dataclasses import dataclass + +import jupedsim.native as py_jps + + +@dataclass(kw_only=True) +class AnticipationVelocityModel: + """Anticipation Velocity Model (AVM). + + The AVM incorporates pedestrian anticipation, divided into three phases: + 1. Perception of the current situation. + 2. Prediction of future situations. + 3. Strategy selection leading to action. + + This model quantitatively reproduces bidirectional pedestrian flow by accounting for: + - Anticipation of changes in neighboring pedestrians' positions. + - The strategy of following others' movement. The AVM is a model that takes into consideration + the anticipation of pedestrians. For this, the process of anticipation is divided into three parts: + - perception of the actual situation, + - prediction of a future situation and + - selection of a strategy leading to action. + + + A general description of the AVM can be found in the originating publication + https://doi.org/10.1016/j.trc.2021.103464 + """ + + pass + + +@dataclass(kw_only=True) +class AnticipationVelocityModelAgentParameters: + """ + Agent parameters for Anticipation Velocity Model (AVM). + + See publication for more details about this model + https://doi.org/10.1016/j.trc.2021.103464 + + .. note:: + + Instances of this type are copied when creating the agent, you can safely + create one instance of this type and modify it between calls to `add_agent` + + E.g.: + + .. code:: python + + positions = [...] # List of initial agent positions + params = AnticipationVelocityModelAgentParameters(v0=0.9) # all agents are slower + for p in positions: + params.position = p + sim.add_agent(params) + + Attributes: + position: Position of the agent. + time_gap: Time constant that describe how fast pedestrian close gaps. + v0: Maximum speed of the agent. + radius: Radius of the agent. + journey_id: Id of the journey the agent follows. + stage_id: Id of the stage the agent targets. + strength_neighbor_repulsion: Strength of the repulsion from neighbors + range_neighbor_repulsion: Range of the repulsion from neighbors + wall_buffer_distance: Buffer distance of agents to the walls. + anticipation_time: Anticipation time of an agent. + reaction_time: reaction time of an agent to change its direction. + """ + + position: tuple[float, float] = (0.0, 0.0) + time_gap: float = 1.06 + v0: float = 1.2 + radius: float = 0.2 + journey_id: int = 0 + stage_id: int = 0 + strength_neighbor_repulsion: float = 8.0 + range_neighbor_repulsion: float = 0.1 + wall_buffer_distance: float = 0.1 + anticipation_time: float = 1.0 + reaction_time: float = 0.3 + + def as_native( + self, + ) -> py_jps.AnticipationVelocityModelAgentParameters: + return py_jps.AnticipationVelocityModelAgentParameters( + position=self.position, + time_gap=self.time_gap, + v0=self.v0, + radius=self.radius, + journey_id=self.journey_id, + stage_id=self.stage_id, + strength_neighbor_repulsion=self.strength_neighbor_repulsion, + range_neighbor_repulsion=self.range_neighbor_repulsion, + wall_buffer_distance=self.wall_buffer_distance, + anticipation_time=self.anticipation_time, + reaction_time=self.reaction_time, + ) + + +class AnticipationVelocityModelState: + def __init__(self, backing): + self._obj = backing + + @property + def time_gap(self) -> float: + return self._obj.time_gap + + @time_gap.setter + def time_gap(self, time_gap): + self._obj.time_gap = time_gap + + @property + def v0(self) -> float: + """Maximum speed of this agent.""" + return self._obj.v0 + + @v0.setter + def v0(self, v0): + self._obj.v0 = v0 + + @property + def radius(self) -> float: + """Radius of this agent.""" + return self._obj.radius + + @radius.setter + def radius(self, radius): + self._obj.radius = radius + + @property + def strength_neighbor_repulsion(self) -> float: + """Strength of the repulsion from neighbors of this agent.""" + return self._obj.strength_neighbor_repulsion + + @strength_neighbor_repulsion.setter + def strength_neighbor_repulsion(self, strength_neighbor_repulsion): + self._obj.strength_neighbor_repulsion = strength_neighbor_repulsion + + @property + def range_neighbor_repulsion(self) -> float: + """Range of the repulsion from neighbors of this agent.""" + return self._obj.range_neighbor_repulsion + + @range_neighbor_repulsion.setter + def range_neighbor_repulsion(self, range_neighbor_repulsion): + self._obj.range_neighbor_repulsion = range_neighbor_repulsion + + @property + def wall_buffer_distance(self): + """Wall buffer distance of agent to walls.""" + return self._obj.wall_buffer_distance + + @wall_buffer_distance.setter + def wall_buffer_distance(self, wall_buffer_distance): + self._obj.wall_buffer_distance = wall_buffer_distance + + @property + def anticipation_time(self) -> float: + """Anticipation time of this agent.""" + return self._obj.anticipation_time + + @anticipation_time.setter + def anticipation_time(self, anticipation_time): + self._obj.anticipation_time = anticipation_time + + @property + def reaction_time(self) -> float: + """Reaction time of this agent.""" + return self._obj.reaction_time + + @reaction_time.setter + def reaction_time(self, reaction_time): + self._obj.reaction_time = reaction_time diff --git a/python_modules/jupedsim/jupedsim/simulation.py b/python_modules/jupedsim/jupedsim/simulation.py index cf61544a5..cc9acab2c 100644 --- a/python_modules/jupedsim/jupedsim/simulation.py +++ b/python_modules/jupedsim/jupedsim/simulation.py @@ -11,6 +11,10 @@ from jupedsim.geometry_utils import build_geometry from jupedsim.internal.tracing import Trace from jupedsim.journey import JourneyDescription +from jupedsim.models.anticipation_velocity_model import ( + AnticipationVelocityModel, + AnticipationVelocityModelAgentParameters, +) from jupedsim.models.collision_free_speed import ( CollisionFreeSpeedModel, CollisionFreeSpeedModelAgentParameters, @@ -53,6 +57,7 @@ def __init__( CollisionFreeSpeedModel | GeneralizedCentrifugalForceModel | CollisionFreeSpeedModelV2 + | AnticipationVelocityModel | SocialForceModel ), geometry: ( @@ -110,6 +115,9 @@ def __init__( elif isinstance(model, CollisionFreeSpeedModelV2): model_builder = py_jps.CollisionFreeSpeedModelV2Builder() py_jps_model = model_builder.build() + elif isinstance(model, AnticipationVelocityModel): + model_builder = py_jps.AnticipationVelocityModelBuilder() + py_jps_model = model_builder.build() elif isinstance(model, GeneralizedCentrifugalForceModel): model_builder = py_jps.GeneralizedCentrifugalForceModelBuilder( strength_neighbor_repulsion=model.strength_neighbor_repulsion, @@ -248,6 +256,7 @@ def add_agent( GeneralizedCentrifugalForceModelAgentParameters | CollisionFreeSpeedModelAgentParameters | CollisionFreeSpeedModelV2AgentParameters + | AnticipationVelocityModelAgentParameters | SocialForceModelAgentParameters ), ) -> int: diff --git a/systemtest/test_model_constraints.py b/systemtest/test_model_constraints.py index d59cf242f..2a6000699 100644 --- a/systemtest/test_model_constraints.py +++ b/systemtest/test_model_constraints.py @@ -605,3 +605,446 @@ def test_generalized_centrifugal_force_model_can_not_add_agent_too_close_to_wall b_max=b_max, ) ) + + +@pytest.fixture +def square_room_100x100_avm(): + simulation = jps.Simulation( + model=jps.AnticipationVelocityModel(), + geometry=[(-50, -50), (50, -50), (50, 50), (-50, 50)], + ) + + exit_id = simulation.add_exit_stage([(49, -3), (49, 3), (50, 3), (50, -3)]) + + journey = jps.JourneyDescription([exit_id]) + journey_id = simulation.add_journey(journey) + agent_position = (0, 0) + + return simulation, journey_id, exit_id, agent_position + + +@pytest.mark.parametrize("strength", [*np.arange(0.1, 10, 1)]) +def test_anticipation_velocity_model_can_set_strength_neighbor_repulsion( + square_room_100x100_avm, strength +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + strength_neighbor_repulsion=strength, + ) + ) + + +def test_anticipation_velocity_model_can_not_set_strength_neighbor_repulsion_too_small( + square_room_100x100_avm, +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: strengthNeighborRepulsion -0.1 .*", + ): + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + strength_neighbor_repulsion=-0.1, + ) + ) + + +def test_anticipation_velocity_model_can_not_set_strength_neighbor_repulsion_too_large( + square_room_100x100_avm, +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: strengthNeighborRepulsion 20.1 .*", + ): + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + strength_neighbor_repulsion=20.1, + ) + ) + + +@pytest.mark.parametrize("range_repulsion", [*np.arange(0.1, 5, 1)]) +def test_anticipation_velocity_model_can_set_range_neighbor_repulsion( + square_room_100x100_avm, range_repulsion +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + range_neighbor_repulsion=range_repulsion, + ) + ) + + +def test_anticipation_velocity_model_can_not_set_range_neighbor_repulsion_too_small( + square_room_100x100_avm, +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: rangeNeighborRepulsion 0 .*", + ): + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + range_neighbor_repulsion=0, + ) + ) + + +def test_anticipation_velocity_model_can_not_set_range_neighbor_repulsion_too_large( + square_room_100x100_avm, +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: rangeNeighborRepulsion 5.1 .*", + ): + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + range_neighbor_repulsion=5.1, + ) + ) + + +@pytest.mark.parametrize("wall_buffer", [*np.arange(0.1, 1, 0.2)]) +def test_anticipation_velocity_model_can_set_wall_buffer_distance( + square_room_100x100_avm, wall_buffer +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + wall_buffer_distance=wall_buffer, + ) + ) + + +def test_anticipation_velocity_model_can_not_set_wall_buffer_distance_too_small( + square_room_100x100_avm, +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: wallBufferDistance -0.1 .*", + ): + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + wall_buffer_distance=-0.1, + ) + ) + + +def test_anticipation_velocity_model_can_not_set_wall_buffer_distance_too_large( + square_room_100x100_avm, +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: wallBufferDistance 2.1 .*", + ): + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + wall_buffer_distance=2.1, + ) + ) + + +@pytest.mark.parametrize("anticipation_time", [*np.arange(0.1, 5, 0.5)]) +def test_anticipation_velocity_model_can_set_anticipation_time( + square_room_100x100_avm, anticipation_time +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + anticipation_time=anticipation_time, + ) + ) + + +def test_anticipation_velocity_model_can_not_set_anticipation_time_too_small( + square_room_100x100_avm, +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: anticipationTime -0.1 .*", + ): + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + anticipation_time=-0.1, + ) + ) + + +def test_anticipation_velocity_model_can_not_set_anticipation_time_too_large( + square_room_100x100_avm, +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: anticipationTime 5.1 .*", + ): + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + anticipation_time=5.1, + ) + ) + + +@pytest.mark.parametrize("reaction_time", [*np.arange(0.1, 1, 0.2)]) +def test_anticipation_velocity_model_can_set_reaction_time( + square_room_100x100_avm, reaction_time +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + reaction_time=reaction_time, + ) + ) + + +def test_anticipation_velocity_model_can_not_set_reaction_time_too_small( + square_room_100x100_avm, +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + with pytest.raises( + RuntimeError, match=r"Model constraint violation: reactionTime 0 .*" + ): + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + reaction_time=0, + ) + ) + + +def test_anticipation_velocity_model_can_not_set_reaction_time_too_large( + square_room_100x100_avm, +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + with pytest.raises( + RuntimeError, match=r"Model constraint violation: reactionTime 2.1 .*" + ): + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + reaction_time=2.1, + ) + ) + + +@pytest.mark.parametrize("time_gap", [*np.arange(0.1, 10, 1)]) +def test_anticipation_velocity_model_can_set_time_gap( + square_room_100x100_avm, time_gap +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + time_gap=time_gap, + ) + ) + + +def test_anticipation_velocity_model_can_not_set_time_gap_too_small( + square_room_100x100_avm, +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + with pytest.raises( + RuntimeError, match=r"Model constraint violation: timeGap 0 .*" + ): + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + time_gap=0, + ) + ) + + +def test_anticipation_velocity_model_can_not_set_time_gap_too_large( + square_room_100x100_avm, +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + with pytest.raises( + RuntimeError, match=r"Model constraint violation: timeGap 10.1 .*" + ): + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + time_gap=10.1, + ) + ) + + +@pytest.mark.parametrize("v0", [*np.arange(0.1, 10, 1)]) +def test_anticipation_velocity_model_can_set_v0(square_room_100x100_avm, v0): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + v0=v0, + ) + ) + + +def test_anticipation_velocity_model_can_not_set_v0_too_small( + square_room_100x100_avm, +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + with pytest.raises( + RuntimeError, match=r"Model constraint violation: v0 -0.1 .*" + ): + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + v0=-0.1, + ) + ) + + +def test_anticipation_velocity_model_can_not_set_v0_too_large( + square_room_100x100_avm, +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + with pytest.raises( + RuntimeError, match=r"Model constraint violation: v0 10.1 .*" + ): + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + v0=10.1, + ) + ) + + +@pytest.mark.parametrize("radius", [*np.arange(0.1, 2, 0.2)]) +def test_anticipation_velocity_model_can_set_radius( + square_room_100x100_avm, radius +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + radius=radius, + ) + ) + + +def test_anticipation_velocity_model_can_not_set_radius_too_small( + square_room_100x100_avm, +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + with pytest.raises( + RuntimeError, match=r"Model constraint violation: radius 0 .*" + ): + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + radius=0, + ) + ) + + +def test_anticipation_velocity_model_can_not_set_radius_too_large( + square_room_100x100_avm, +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + with pytest.raises( + RuntimeError, match=r"Model constraint violation: radius 2.1 .*" + ): + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + radius=2.1, + ) + ) + + +@pytest.mark.parametrize("radius", [*np.arange(0.1, 0.5, 0.1)]) +def test_anticipation_velocity_model_can_not_add_agent_too_close_to_wall( + square_room_100x100_avm, + radius, +): + simulation, journey_id, exit_id, agent_position = square_room_100x100_avm + + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: Agent (.+) too close to geometry boundaries, distance .*", + ): + simulation.add_agent( + jps.AnticipationVelocityModelAgentParameters( + position=(50 - (0.99 * radius), 0), + journey_id=journey_id, + stage_id=exit_id, + radius=radius, + ) + ) diff --git a/systemtest/test_model_properties.py b/systemtest/test_model_properties.py index afdd75b2d..92b0d7f57 100644 --- a/systemtest/test_model_properties.py +++ b/systemtest/test_model_properties.py @@ -83,6 +83,53 @@ def test_set_model_parameters_collision_free_speed_model_v2( assert sim.agent(agent_id).model.range_geometry_repulsion == 8.0 +@pytest.fixture +def simulation_with_anticipation_velocity_model(): + return jps.Simulation( + model=jps.AnticipationVelocityModel(), + geometry=[(0, 0), (10, 0), (10, 10), (0, 10)], + ) + + +def test_set_model_parameters_anticipation_velocity_model( + simulation_with_anticipation_velocity_model, +): + sim = simulation_with_anticipation_velocity_model + wp = sim.add_waypoint_stage((10, 1), 0.5) + journey_id = sim.add_journey(jps.JourneyDescription([wp])) + + agent = jps.AnticipationVelocityModelAgentParameters( + journey_id=journey_id, + stage_id=wp, + position=(1, 1), + ) + agent_id = sim.add_agent(agent) + + sim.agent(agent_id).model.v0 = 2.0 + assert sim.agent(agent_id).model.v0 == 2.0 + + sim.agent(agent_id).model.time_gap = 3.0 + assert sim.agent(agent_id).model.time_gap == 3.0 + + sim.agent(agent_id).model.radius = 4.0 + assert sim.agent(agent_id).model.radius == 4.0 + + sim.agent(agent_id).model.strength_neighbor_repulsion = 5.0 + assert sim.agent(agent_id).model.strength_neighbor_repulsion == 5.0 + + sim.agent(agent_id).model.range_neighbor_repulsion = 6.0 + assert sim.agent(agent_id).model.range_neighbor_repulsion == 6.0 + + sim.agent(agent_id).model.wall_buffer_distance = 1.1 + assert sim.agent(agent_id).model.wall_buffer_distance == 1.1 + + sim.agent(agent_id).model.anticipation_time = 2.1 + assert sim.agent(agent_id).model.anticipation_time == 2.1 + + sim.agent(agent_id).model.reaction_time = 0.31 + assert sim.agent(agent_id).model.reaction_time == 0.31 + + @pytest.fixture def simulation_with_collision_free_speed_model(): return jps.Simulation(