From e33541bed653887acbb4bd93688022ccc429ff18 Mon Sep 17 00:00:00 2001 From: schroedtert Date: Mon, 16 Oct 2023 15:27:49 +0200 Subject: [PATCH] Add model constraint checks --- libsimulator/src/CollisionFreeSpeedModel.cpp | 31 +++++++++-- libsimulator/src/CollisionFreeSpeedModel.hpp | 5 +- .../src/GeneralizedCentrifugalForceModel.cpp | 51 ++++++++++++++++++- .../src/GeneralizedCentrifugalForceModel.hpp | 5 +- .../src/OperationalDecisionSystem.hpp | 5 +- libsimulator/src/OperationalModel.hpp | 21 +++++++- libsimulator/src/Simulation.cpp | 2 +- 7 files changed, 106 insertions(+), 14 deletions(-) diff --git a/libsimulator/src/CollisionFreeSpeedModel.cpp b/libsimulator/src/CollisionFreeSpeedModel.cpp index b886520155..408d35cfb0 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"); + + 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(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..a3ce4434eb 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 = 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 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..ef8cd99e52 100644 --- a/libsimulator/src/OperationalModel.hpp +++ b/libsimulator/src/OperationalModel.hpp @@ -43,6 +43,22 @@ struct fmt::formatter { } }; +template +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 { public: @@ -57,7 +73,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..d8057e9cd3 100644 --- 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);