|
| 1 | +#include "domain/CompressionSystem.h" |
| 2 | + |
| 3 | +#include "domain/AgentComponents.h" |
| 4 | +#include "domain/FacilityLayout2D.h" |
| 5 | +#include "domain/Metrics.h" |
| 6 | + |
| 7 | +#include <algorithm> |
| 8 | +#include <cmath> |
| 9 | + |
| 10 | +namespace safecrowd::domain { |
| 11 | +namespace { |
| 12 | + |
| 13 | +constexpr float kForceThreshold = 0.5f; |
| 14 | +constexpr float kExposureThreshold = 2.0f; |
| 15 | + |
| 16 | +double distanceBetween(const Point2D& lhs, const Point2D& rhs) { |
| 17 | + const double dx = lhs.x - rhs.x; |
| 18 | + const double dy = lhs.y - rhs.y; |
| 19 | + return std::sqrt(dx * dx + dy * dy); |
| 20 | +} |
| 21 | + |
| 22 | +double distancePointToSegment(const Point2D& point, const Point2D& start, const Point2D& end) { |
| 23 | + const double dx = end.x - start.x; |
| 24 | + const double dy = end.y - start.y; |
| 25 | + const double lengthSquared = (dx * dx) + (dy * dy); |
| 26 | + |
| 27 | + if (lengthSquared == 0.0) { |
| 28 | + return distanceBetween(point, start); |
| 29 | + } |
| 30 | + |
| 31 | + const double t = std::clamp( |
| 32 | + (((point.x - start.x) * dx) + ((point.y - start.y) * dy)) / lengthSquared, |
| 33 | + 0.0, |
| 34 | + 1.0); |
| 35 | + const Point2D projection{ |
| 36 | + .x = start.x + (t * dx), |
| 37 | + .y = start.y + (t * dy), |
| 38 | + }; |
| 39 | + return distanceBetween(point, projection); |
| 40 | +} |
| 41 | + |
| 42 | +double barrierCompression(const Barrier2D& barrier, const Point2D& position, double radius) { |
| 43 | + if (!barrier.blocksMovement || barrier.geometry.vertices.size() < 2) { |
| 44 | + return 0.0; |
| 45 | + } |
| 46 | + |
| 47 | + double force = 0.0; |
| 48 | + const auto& vertices = barrier.geometry.vertices; |
| 49 | + |
| 50 | + for (std::size_t index = 0; index + 1 < vertices.size(); ++index) { |
| 51 | + const double distance = distancePointToSegment(position, vertices[index], vertices[index + 1]); |
| 52 | + if (distance < radius) { |
| 53 | + force += radius - distance; |
| 54 | + } |
| 55 | + } |
| 56 | + |
| 57 | + if (barrier.geometry.closed) { |
| 58 | + const double distance = distancePointToSegment(position, vertices.back(), vertices.front()); |
| 59 | + if (distance < radius) { |
| 60 | + force += radius - distance; |
| 61 | + } |
| 62 | + } |
| 63 | + |
| 64 | + return force; |
| 65 | +} |
| 66 | + |
| 67 | +} // namespace |
| 68 | + |
| 69 | +CompressionSystem::CompressionSystem(double timeStepSeconds) |
| 70 | + : timeStepSeconds_(static_cast<float>(std::max(0.0, timeStepSeconds))) { |
| 71 | +} |
| 72 | + |
| 73 | +void CompressionSystem::update(engine::EngineWorld& world, |
| 74 | + const engine::EngineStepContext& step) { |
| 75 | + (void)step; |
| 76 | + |
| 77 | + auto& query = world.query(); |
| 78 | + const auto agentEntities = query.view<Position, Agent, CompressionData>(); |
| 79 | + const auto barrierEntities = query.view<Barrier2D>(); |
| 80 | + |
| 81 | + for (const auto entity : agentEntities) { |
| 82 | + const auto& position = query.get<Position>(entity); |
| 83 | + const auto& agent = query.get<Agent>(entity); |
| 84 | + auto& compression = query.get<CompressionData>(entity); |
| 85 | + |
| 86 | + double currentForce = 0.0; |
| 87 | + |
| 88 | + for (const auto otherEntity : agentEntities) { |
| 89 | + if (otherEntity == entity) { |
| 90 | + continue; |
| 91 | + } |
| 92 | + |
| 93 | + const auto& otherPosition = query.get<Position>(otherEntity); |
| 94 | + const auto& otherAgent = query.get<Agent>(otherEntity); |
| 95 | + const double distance = distanceBetween(position.value, otherPosition.value); |
| 96 | + const double combinedRadius = static_cast<double>(agent.radius + otherAgent.radius); |
| 97 | + |
| 98 | + if (distance < combinedRadius) { |
| 99 | + currentForce += combinedRadius - distance; |
| 100 | + } |
| 101 | + } |
| 102 | + |
| 103 | + for (const auto barrierEntity : barrierEntities) { |
| 104 | + currentForce += barrierCompression( |
| 105 | + query.get<Barrier2D>(barrierEntity), |
| 106 | + position.value, |
| 107 | + static_cast<double>(agent.radius)); |
| 108 | + } |
| 109 | + |
| 110 | + compression.force = static_cast<float>(currentForce); |
| 111 | + if (compression.force > kForceThreshold) { |
| 112 | + compression.exposure += timeStepSeconds_; |
| 113 | + } |
| 114 | + |
| 115 | + compression.isCritical = |
| 116 | + compression.force > kForceThreshold && |
| 117 | + compression.exposure >= kExposureThreshold; |
| 118 | + } |
| 119 | +} |
| 120 | + |
| 121 | +} // namespace safecrowd::domain |
0 commit comments