Add model constraint checks

PedestrianDynamics · Oct 17, 2023 · 4eb5ce7 · 4eb5ce7
1 parent 5fc3b68
commit 4eb5ce7
Show file tree

Hide file tree

Showing 10 changed files with 576 additions and 23 deletions.
diff --git 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<CollisionFreeSpeedModelData>(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<CollisionFreeSpeedModelData>(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<OperationalModel> CollisionFreeSpeedModel::Clone() const

diff --git 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<OperationalModel> Clone() const override;
 
 private:

diff --git 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<GeneralizedCentrifugalForceModelData>(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<OperationalModel> GeneralizedCentrifugalForceModel::Clone() const

diff --git 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<OperationalModel> Clone() const override;
 
 private:

diff --git a/libsimulator/src/OperationalDecisionSystem.hpp b/libsimulator/src/OperationalDecisionSystem.hpp
@@ -63,8 +63,9 @@ class OperationalDecisionSystem
 
     void ValidateAgent(
         const GenericAgent& agent,
-        const NeighborhoodSearch<GenericAgent>& neighborhoodSearch) const
+        const NeighborhoodSearch<GenericAgent>& 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
@@ -43,6 +43,40 @@ struct fmt::formatter<PedestrianUpdate> {
     }
 };
 
+template <typename T>
+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<OperationalModel>
 {
 public:
@@ -57,7 +91,8 @@ class OperationalModel : public Clonable<OperationalModel>
         const NeighborhoodSearch<GenericAgent>& neighborhoodSearch) const = 0;
 
     virtual void ApplyUpdate(const OperationalModelUpdate& update, GenericAgent& agent) const = 0;
-    virtual void CheckDistanceConstraint(
+    virtual void CheckModelConstraint(
         const GenericAgent& agent,
-        const NeighborhoodSearch<GenericAgent>& neighborhoodSearch) const = 0;
+        const NeighborhoodSearch<GenericAgent>& neighborhoodSearch,
+        const CollisionGeometry& geometry) const = 0;
 };
diff --git 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
@@ -240,7 +240,8 @@ PYBIND11_MODULE(py_jupedsim, m)
                 p.b_min,
                 p.b_max);
         });
-    py::class_<JPS_CollisionFreeSpeedModelAgentParameters>(m, "CollisionFreeSpeedModelAgentParameters")
+    py::class_<JPS_CollisionFreeSpeedModelAgentParameters>(
+        m, "CollisionFreeSpeedModelAgentParameters")
         .def(
             py::init([](std::tuple<double, double> position,
                         double time_gap,
@@ -351,7 +352,7 @@ PYBIND11_MODULE(py_jupedsim, m)
             throw std::runtime_error{msg};
         });
     py::class_<JPS_GeneralizedCentrifugalForceModelModelBuilder_Wrapper>(
-        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;
                 }