diff --git a/libsimulator/src/CollisionFreeSpeedModel.cpp b/libsimulator/src/CollisionFreeSpeedModel.cpp index b886520155..add4a7162d 100644 --- a/libsimulator/src/CollisionFreeSpeedModel.cpp +++ b/libsimulator/src/CollisionFreeSpeedModel.cpp @@ -110,13 +110,29 @@ void CollisionFreeSpeedModel::ApplyUpdate(const OperationalModelUpdate& upd, Gen agent.orientation = update.orientation; } -void CollisionFreeSpeedModel::CheckDistanceConstraint( +void CollisionFreeSpeedModel::CheckModelConstraint( const GenericAgent& agent, - const NeighborhoodSearchType& neighborhoodSearch) const + const NeighborhoodSearchType& neighborhoodSearch, + const CollisionGeometry& geometry) const { - const auto neighbors = neighborhoodSearch.GetNeighboringAgents(agent.pos, 2); 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 v0 = model.v0; + constexpr double v0Min = 0.; + constexpr double v0Max = 10.; + validateConstraint(v0, v0Min, v0Max, "v0", true); + + const auto timeGap = model.timeGap; + constexpr double timeGapMin = 0.1; + constexpr double timeGapMax = 10.; + validateConstraint(timeGap, timeGapMin, timeGapMax, "timeGap"); + + const auto neighbors = neighborhoodSearch.GetNeighboringAgents(agent.pos, 2); for(const auto& neighbor : neighbors) { const auto& neighbor_model = std::get(neighbor.model); const auto contanctdDist = r + neighbor_model.radius; @@ -129,6 +145,15 @@ void CollisionFreeSpeedModel::CheckDistanceConstraint( distance); } } + + const auto lineSegments = geometry.LineSegmentsInDistanceTo(r / 2., 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 CollisionFreeSpeedModel::Clone() const diff --git a/libsimulator/src/CollisionFreeSpeedModel.hpp b/libsimulator/src/CollisionFreeSpeedModel.hpp index ac0e589309..4244c4febf 100644 --- a/libsimulator/src/CollisionFreeSpeedModel.hpp +++ b/libsimulator/src/CollisionFreeSpeedModel.hpp @@ -36,9 +36,10 @@ class CollisionFreeSpeedModel : public OperationalModel const CollisionGeometry& geometry, const NeighborhoodSearchType& neighborhoodSearch) const override; void ApplyUpdate(const OperationalModelUpdate& update, GenericAgent& agent) const override; - void CheckDistanceConstraint( + void CheckModelConstraint( const GenericAgent& agent, - const NeighborhoodSearchType& neighborhoodSearch) const override; + const NeighborhoodSearchType& neighborhoodSearch, + const CollisionGeometry& geometry) const override; std::unique_ptr Clone() const override; private: diff --git a/libsimulator/src/GeneralizedCentrifugalForceModel.cpp b/libsimulator/src/GeneralizedCentrifugalForceModel.cpp index 433bbf32ba..5331e569e1 100644 --- a/libsimulator/src/GeneralizedCentrifugalForceModel.cpp +++ b/libsimulator/src/GeneralizedCentrifugalForceModel.cpp @@ -90,10 +90,48 @@ void GeneralizedCentrifugalForceModel::ApplyUpdate( } } -void GeneralizedCentrifugalForceModel::CheckDistanceConstraint( +void GeneralizedCentrifugalForceModel::CheckModelConstraint( const GenericAgent& agent, - const NeighborhoodSearchType& neighborhoodSearch) const + const NeighborhoodSearchType& neighborhoodSearch, + const CollisionGeometry& geometry) const { + const auto& model = std::get(agent.model); + + const auto mass = model.mass; + constexpr double massMin = 1.; + constexpr double massMax = 100.; + validateConstraint(mass, massMin, massMax, "mass"); + + const auto tau = model.tau; + constexpr double tauMin = 0.1; + constexpr double tauMax = 10.; + validateConstraint(tau, tauMin, tauMax, "tau"); + + const auto v0 = model.v0; + constexpr double v0Min = 0.; + constexpr double v0Max = 10.; + validateConstraint(v0, v0Min, v0Max, "v0", true); + + const auto Av = model.Av; + constexpr double AvMin = 0.; + constexpr double AvMax = 10.; + validateConstraint(Av, AvMin, AvMax, "Av"); + + const auto AMin = model.AMin; + constexpr double AMinMin = 0.1; + constexpr double AMinMax = 1.; + validateConstraint(AMin, AMinMin, AMinMax, "AMin"); + + const auto BMin = model.BMin; + constexpr double BMinMin = 0.1; + constexpr double BMinMax = 1.; + validateConstraint(BMin, BMinMin, BMinMax, "BMin"); + + const auto BMax = model.BMax; + const double BMaxMin = BMin; + constexpr double BMaxMax = 2.; + validateConstraint(BMax, BMaxMin, BMaxMax, "BMax"); + const auto neighbors = neighborhoodSearch.GetNeighboringAgents(agent.pos, 2); for(const auto& neighbor : neighbors) { const auto contanctDist = AgentToAgentSpacing(agent, neighbor); @@ -108,6 +146,15 @@ void GeneralizedCentrifugalForceModel::CheckDistanceConstraint( distance - contanctDist); } } + + const auto maxRadius = std::max(AMin, BMax) / 2.; + const auto lineSegments = geometry.LineSegmentsInDistanceTo(maxRadius, agent.pos); + if(std::begin(lineSegments) != std::end(lineSegments)) { + throw SimulationError( + "Model constraint violation: Agent {} too close to geometry boundaries, distance <= {}", + agent.pos, + maxRadius); + } } std::unique_ptr GeneralizedCentrifugalForceModel::Clone() const diff --git a/libsimulator/src/GeneralizedCentrifugalForceModel.hpp b/libsimulator/src/GeneralizedCentrifugalForceModel.hpp index 2b137da6aa..43c6266511 100644 --- a/libsimulator/src/GeneralizedCentrifugalForceModel.hpp +++ b/libsimulator/src/GeneralizedCentrifugalForceModel.hpp @@ -44,9 +44,10 @@ class GeneralizedCentrifugalForceModel : public OperationalModel const CollisionGeometry& geometry, const NeighborhoodSearchType& neighborhoodSearch) const override; void ApplyUpdate(const OperationalModelUpdate& upate, GenericAgent& agent) const override; - void CheckDistanceConstraint( + void CheckModelConstraint( const GenericAgent& agent, - const NeighborhoodSearchType& neighborhoodSearch) const override; + const NeighborhoodSearchType& neighborhoodSearch, + const CollisionGeometry& geometry) const override; std::unique_ptr Clone() const override; private: diff --git a/libsimulator/src/OperationalDecisionSystem.hpp b/libsimulator/src/OperationalDecisionSystem.hpp index 810f074d6a..589c360b04 100644 --- a/libsimulator/src/OperationalDecisionSystem.hpp +++ b/libsimulator/src/OperationalDecisionSystem.hpp @@ -63,8 +63,9 @@ class OperationalDecisionSystem void ValidateAgent( const GenericAgent& agent, - const NeighborhoodSearch& neighborhoodSearch) const + const NeighborhoodSearch& neighborhoodSearch, + const CollisionGeometry& geometry) const { - _model->CheckDistanceConstraint(agent, neighborhoodSearch); + _model->CheckModelConstraint(agent, neighborhoodSearch, geometry); } }; diff --git a/libsimulator/src/OperationalModel.hpp b/libsimulator/src/OperationalModel.hpp index 9bf14a2173..fed5d3da08 100644 --- a/libsimulator/src/OperationalModel.hpp +++ b/libsimulator/src/OperationalModel.hpp @@ -43,6 +43,40 @@ struct fmt::formatter { } }; +template +void validateConstraint( + T value, + T valueMin, + T valueMax, + const std::string& name, + bool includeMin = false) +{ + if(includeMin) { + if(value <= valueMin || value > valueMax) { + throw SimulationError( + "Model constraint violation: {} {} not in allowed range, " + "{} needs to be in ({},{}]", + name, + value, + name, + valueMin, + valueMax); + } + + } else { + if(value < valueMin || value > valueMax) { + throw SimulationError( + "Model constraint violation: {} {} not in allowed range, " + "{} needs to be in [{},{}]", + name, + value, + name, + valueMin, + valueMax); + } + } +} + class OperationalModel : public Clonable { public: @@ -57,7 +91,8 @@ class OperationalModel : public Clonable const NeighborhoodSearch& neighborhoodSearch) const = 0; virtual void ApplyUpdate(const OperationalModelUpdate& update, GenericAgent& agent) const = 0; - virtual void CheckDistanceConstraint( + virtual void CheckModelConstraint( const GenericAgent& agent, - const NeighborhoodSearch& neighborhoodSearch) const = 0; + const NeighborhoodSearch& neighborhoodSearch, + const CollisionGeometry& geometry) const = 0; }; diff --git a/libsimulator/src/Simulation.cpp b/libsimulator/src/Simulation.cpp index 34fc1fbd20..32908a016c 100644 --- a/libsimulator/src/Simulation.cpp +++ b/libsimulator/src/Simulation.cpp @@ -128,8 +128,12 @@ BaseStage::ID Simulation::AddStage(const StageDescription stageDescription) GenericAgent::ID Simulation::AddAgent(GenericAgent&& agent) { + if(!_geometry->InsideGeometry(agent.pos)) { + throw SimulationError("Agent {} not inside walkable area", agent.pos); + } + agent.orientation = agent.orientation.Normalized(); - _operationalDecisionSystem.ValidateAgent(agent, _neighborhoodSearch); + _operationalDecisionSystem.ValidateAgent(agent, _neighborhoodSearch, *_geometry.get()); if(_journeys.count(agent.journeyId) == 0) { throw SimulationError("Unknown journey id: {}", agent.journeyId); diff --git a/python_bindings_jupedsim/bindings_jupedsim.cpp b/python_bindings_jupedsim/bindings_jupedsim.cpp index f3fe112ff8..b82571a1c1 100644 --- a/python_bindings_jupedsim/bindings_jupedsim.cpp +++ b/python_bindings_jupedsim/bindings_jupedsim.cpp @@ -240,7 +240,8 @@ PYBIND11_MODULE(py_jupedsim, m) p.b_min, p.b_max); }); - py::class_(m, "CollisionFreeSpeedModelAgentParameters") + py::class_( + m, "CollisionFreeSpeedModelAgentParameters") .def( py::init([](std::tuple position, double time_gap, @@ -351,7 +352,7 @@ PYBIND11_MODULE(py_jupedsim, m) throw std::runtime_error{msg}; }); py::class_( - m, "GeneralizedCentrifugalForceModelModelBuilder") + m, "GeneralizedCentrifugalForceModelBuilder") .def( py::init([](double strengthNeighborRepulsion, double strengthGeometryRepulsion, @@ -797,10 +798,11 @@ PYBIND11_MODULE(py_jupedsim, m) }) .def( "add_agent", - [](JPS_Simulation_Wrapper& simulation, JPS_CollisionFreeSpeedModelAgentParameters& parameters) { + [](JPS_Simulation_Wrapper& simulation, + JPS_CollisionFreeSpeedModelAgentParameters& parameters) { JPS_ErrorMessage errorMsg{}; - auto result = - JPS_Simulation_AddCollisionFreeSpeedModelAgent(simulation.handle, parameters, &errorMsg); + auto result = JPS_Simulation_AddCollisionFreeSpeedModelAgent( + simulation.handle, parameters, &errorMsg); if(result) { return result; } diff --git a/systemtest/test_model_constraints.py b/systemtest/test_model_constraints.py new file mode 100644 index 0000000000..29062df781 --- /dev/null +++ b/systemtest/test_model_constraints.py @@ -0,0 +1,386 @@ +# Copyright © 2012-2023 Forschungszentrum Jülich GmbH +# SPDX-License-Identifier: LGPL-3.0-or-later +import pytest + +import jupedsim as jps + + +def test_collision_free_speed_model_constraints(): + messages = [] + + def log_msg_handler(msg): + messages.append(msg) + + jps.set_info_callback(log_msg_handler) + jps.set_warning_callback(log_msg_handler) + jps.set_error_callback(log_msg_handler) + + simulation = jps.Simulation( + model=jps.CollisionFreeSpeedModel(), + geometry=[(0, 0), (100, 0), (100, 100), (0, 100)], + ) + + exit_id = simulation.add_exit_stage( + [(99, 45), (99, 55), (100, 55), (100, 45)] + ) + + journey = jps.JourneyDescription([exit_id]) + journey_id = simulation.add_journey(journey) + + agent_position = (50, 50) + + # Radius + with pytest.raises( + RuntimeError, match=r"Model constraint violation: radius 0 .*" + ): + assert simulation.add_agent( + jps.CollisionFreeSpeedModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + radius=0, + ) + ) + + with pytest.raises( + RuntimeError, match=r"Model constraint violation: radius -1 .*" + ): + assert simulation.add_agent( + jps.CollisionFreeSpeedModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + radius=-1, + ) + ) + + with pytest.raises( + RuntimeError, match=r"Model constraint violation: radius 2.1 .*" + ): + assert simulation.add_agent( + jps.CollisionFreeSpeedModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + radius=2.1, + ) + ) + + # v0 + with pytest.raises( + RuntimeError, match=r"Model constraint violation: v0 0 .*" + ): + assert simulation.add_agent( + jps.CollisionFreeSpeedModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + v0=0, + ) + ) + + with pytest.raises( + RuntimeError, match=r"Model constraint violation: v0 -1 .*" + ): + assert simulation.add_agent( + jps.CollisionFreeSpeedModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + v0=-1, + ) + ) + + with pytest.raises( + RuntimeError, match=r"Model constraint violation: v0 10.1 .*" + ): + assert simulation.add_agent( + jps.CollisionFreeSpeedModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + v0=10.1, + ) + ) + + # time gap + with pytest.raises( + RuntimeError, match=r"Model constraint violation: timeGap 0.09 .*" + ): + assert simulation.add_agent( + jps.CollisionFreeSpeedModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + time_gap=0.09, + ) + ) + + with pytest.raises( + RuntimeError, match=r"Model constraint violation: timeGap 10.1 .*" + ): + assert simulation.add_agent( + jps.CollisionFreeSpeedModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + time_gap=10.1, + ) + ) + + # too close to wall + radius = 0.2 + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: Agent (.+) too close to geometry boundaries, distance <= \d+\.?\d*", + ): + assert simulation.add_agent( + jps.CollisionFreeSpeedModelAgentParameters( + position=(0 + 0.5 * radius, 10), + journey_id=journey_id, + stage_id=exit_id, + radius=radius, + ) + ) + + +def test_generalized_centrifugal_force_constraints(): + messages = [] + + def log_msg_handler(msg): + messages.append(msg) + + jps.set_info_callback(log_msg_handler) + jps.set_warning_callback(log_msg_handler) + jps.set_error_callback(log_msg_handler) + + simulation = jps.Simulation( + model=jps.GeneralizedCentrifugalForceModel(), + geometry=[(0, 0), (100, 0), (100, 100), (0, 100)], + ) + + exit_id = simulation.add_exit_stage( + [(99, 45), (99, 55), (100, 55), (100, 45)] + ) + + journey = jps.JourneyDescription([exit_id]) + journey_id = simulation.add_journey(journey) + + agent_position = (50, 50) + + # mass + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: mass [+-]?\d+\.?\d* .+", + ): + assert simulation.add_agent( + jps.GeneralizedCentrifugalForceModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + mass=0.99, + ) + ) + + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: mass [+-]?\d+\.?\d* .+", + ): + assert simulation.add_agent( + jps.GeneralizedCentrifugalForceModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + mass=100.1, + ) + ) + + # tau + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: tau [+-]?\d+\.?\d* .+", + ): + assert simulation.add_agent( + jps.GeneralizedCentrifugalForceModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + tau=0.09, + ) + ) + + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: tau [+-]?\d+\.?\d* .+", + ): + assert simulation.add_agent( + jps.GeneralizedCentrifugalForceModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + tau=10.1, + ) + ) + + # v0 + with pytest.raises( + RuntimeError, match=r"Model constraint violation: v0 [+-]?\d+\.?\d* .+" + ): + assert simulation.add_agent( + jps.GeneralizedCentrifugalForceModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + v0=0, + ) + ) + + with pytest.raises( + RuntimeError, match=r"Model constraint violation: v0 [+-]?\d+\.?\d* .+" + ): + assert simulation.add_agent( + jps.GeneralizedCentrifugalForceModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + v0=-1, + ) + ) + + with pytest.raises( + RuntimeError, match=r"Model constraint violation: v0 [+-]?\d+\.?\d* .+" + ): + assert simulation.add_agent( + jps.GeneralizedCentrifugalForceModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + v0=10.1, + ) + ) + + # Av + with pytest.raises( + RuntimeError, match=r"Model constraint violation: Av [+-]?\d+\.?\d* .+" + ): + assert simulation.add_agent( + jps.GeneralizedCentrifugalForceModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + a_v=-0.001, + ) + ) + + with pytest.raises( + RuntimeError, match=r"Model constraint violation: Av [+-]?\d+\.?\d* .+" + ): + assert simulation.add_agent( + jps.GeneralizedCentrifugalForceModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + a_v=10.1, + ) + ) + + # AMin + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: AMin [+-]?\d+\.?\d* .+", + ): + assert simulation.add_agent( + jps.GeneralizedCentrifugalForceModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + a_min=0.099, + ) + ) + + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: AMin [+-]?\d+\.?\d* .+", + ): + assert simulation.add_agent( + jps.GeneralizedCentrifugalForceModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + a_min=1.1, + ) + ) + + # BMin + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: BMin [+-]?\d+\.?\d* .+", + ): + assert simulation.add_agent( + jps.GeneralizedCentrifugalForceModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + b_min=0.099, + ) + ) + + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: BMin [+-]?\d+\.?\d* .+", + ): + assert simulation.add_agent( + jps.GeneralizedCentrifugalForceModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + b_min=1.1, + ) + ) + + # BMax + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: BMax [+-]?\d+\.?\d* .+", + ): + b_min = 0.3 + + simulation.add_agent( + jps.GeneralizedCentrifugalForceModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + b_min=b_min, + b_max=b_min - 0.001, + ) + ) + + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: BMax [+-]?\d+\.?\d* .+", + ): + simulation.add_agent( + jps.GeneralizedCentrifugalForceModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + b_max=2.01, + ) + ) + + # too close to wall + a_min = 0.3 + b_max = a_min + with pytest.raises( + RuntimeError, + match=r"Model constraint violation: Agent (.+) too close to geometry boundaries, distance <= \d+\.?\d*", + ): + assert simulation.add_agent( + jps.GeneralizedCentrifugalForceModelAgentParameters( + position=(0 + 0.5 * a_min, 10), + journey_id=journey_id, + stage_id=exit_id, + a_min=a_min, + b_max=b_max, + ) + ) diff --git a/systemtest/test_py_jupedsim.py b/systemtest/test_py_jupedsim.py index 88ad939f7f..0145bbf416 100644 --- a/systemtest/test_py_jupedsim.py +++ b/systemtest/test_py_jupedsim.py @@ -36,13 +36,13 @@ def log_msg_handler(msg): (50, 10), ] - expected_agent_ids = set() + expected_agent_ids = list() for new_pos, id in zip( initial_agent_positions, range(10, 10 + len(initial_agent_positions)), ): - expected_agent_ids.add( + expected_agent_ids.append( simulation.add_agent( jps.CollisionFreeSpeedModelAgentParameters( position=new_pos, @@ -60,8 +60,15 @@ def log_msg_handler(msg): simulation.agents_in_polygon([(39, 11), (39, 9), (51, 9), (51, 11)]) ) - assert actual_ids_in_range == [3, 4, 5] - assert actual_ids_in_polygon == [5, 6] + assert actual_ids_in_range == [ + expected_agent_ids[1], + expected_agent_ids[2], + expected_agent_ids[3], + ] + assert actual_ids_in_polygon == [ + expected_agent_ids[3], + expected_agent_ids[4], + ] def test_can_run_simulation(): @@ -456,3 +463,47 @@ def log_msg_handler(msg): simulation.iterate() actual_agent_ids = {agent.id for agent in simulation.agents()} assert actual_agent_ids == expected_agent_ids + + +def test_agent_can_not_be_added_outside_geometry(): + messages = [] + + def log_msg_handler(msg): + messages.append(msg) + + jps.set_info_callback(log_msg_handler) + jps.set_warning_callback(log_msg_handler) + jps.set_error_callback(log_msg_handler) + + simulation = jps.Simulation( + model=jps.CollisionFreeSpeedModel(), + geometry=[(0, 0), (100, 0), (100, 100), (0, 100)], + ) + + exit_id = simulation.add_exit_stage( + [(99, 45), (99, 55), (100, 55), (100, 45)] + ) + + journey = jps.JourneyDescription([exit_id]) + journey_id = simulation.add_journey(journey) + + agent_position = (50, 50) + + simulation.add_agent( + jps.CollisionFreeSpeedModelAgentParameters( + position=agent_position, + journey_id=journey_id, + stage_id=exit_id, + ) + ) + + with pytest.raises( + RuntimeError, match=r"Agent \(-50, -50\) not inside walkable area" + ): + assert simulation.add_agent( + jps.CollisionFreeSpeedModelAgentParameters( + position=(-50, -50), + journey_id=journey_id, + stage_id=exit_id, + ) + )