Add model constraint checks

PedestrianDynamics · Oct 16, 2023 · e33541b · e33541b
1 parent 5fc3b68
commit e33541b
Show file tree

Hide file tree

Showing 7 changed files with 106 additions and 14 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");
+
+    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.; // TODO
+    constexpr double timeGapMax = 10.; // TODO
+    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 = 0.; // TODO
+    constexpr double massMax = 2.; // TODO
+    validateConstraint(mass, massMin, massMax, "mass");
+
+    const auto tau = model.tau;
+    constexpr double tauMin = 0.; // TODO
+    constexpr double tauMax = 2.; // TODO
+    validateConstraint(tau, tauMin, tauMax, "tau");
+
+    const auto v0 = model.v0;
+    constexpr double v0Min = 0.; // TODO
+    constexpr double v0Max = 10.; // TODO
+    validateConstraint(v0, v0Min, v0Max, "v0");
+
+    const auto Av = model.Av;
+    constexpr double AvMin = 0.; // TODO
+    constexpr double AvMax = 10.; // TODO
+    validateConstraint(Av, AvMin, AvMax, "Av");
+
+    const auto AMin = model.AMin;
+    constexpr double AMinMin = 0.; // TODO
+    constexpr double AMinMax = 10.; // TODO
+    validateConstraint(AMin, AMinMin, AMinMax, "AMin");
+
+    const auto BMin = model.BMin;
+    constexpr double BMinMin = 0.; // TODO
+    constexpr double BMinMax = 10.; // TODO
+    validateConstraint(BMin, BMinMin, BMinMax, "BMin");
+
+    const auto BMax = model.BMax;
+    constexpr double BMaxMin = 0.; // TODO
+    constexpr double BMaxMax = 10.; // TODO
+    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,22 @@ struct fmt::formatter<PedestrianUpdate> {
     }
 };
 
+template <typename T>
+void validateConstraint(T value, T valueMin, T valueMax, const std::string& name)
+{
+    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 +73,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
@@ -129,7 +129,7 @@ BaseStage::ID Simulation::AddStage(const StageDescription stageDescription)
 GenericAgent::ID Simulation::AddAgent(GenericAgent&& agent)
 {
     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);