Skip to content

Commit aaa7988

Browse files
learncoldlearncold
committed
fix stair movement
1 parent f12fc83 commit aaa7988

7 files changed

Lines changed: 175 additions & 24 deletions

‎src/domain/ScenarioSimulationInternal.cpp‎

Lines changed: 30 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -542,6 +542,10 @@ const std::vector<ScenarioConnectionTraversal>& cachedTraversalsForZone(
542542
return it == cache.traversableConnectionsByZone.end() ? empty : it->second;
543543
}
544544

545+
std::string agentCollisionFloorId(const EvacuationRoute& route) {
546+
return route.displayFloorId.empty() ? route.currentFloorId : route.displayFloorId;
547+
}
548+
545549
std::string zoneAt(const ScenarioLayoutCacheResource& cache, const Point2D& point, const std::string& floorId) {
546550
const auto& floorLayout = cachedLayoutForFloor(cache, floorId);
547551
for (const auto& zone : floorLayout.zones) {
@@ -728,15 +732,19 @@ AgentSpatialIndex buildAgentSpatialIndex(
728732
double cellSize) {
729733
AgentSpatialIndex index;
730734
index.cellSize = cellSize;
731-
index.cells.reserve(entities.size() * 2);
735+
index.cellsByFloor.reserve(4);
732736

733737
for (const auto entity : entities) {
734738
const auto& status = query.get<EvacuationStatus>(entity);
735739
if (status.evacuated) {
736740
continue;
737741
}
738742
const auto& position = query.get<Position>(entity);
739-
index.cells[spatialKey(spatialCellFor(position.value, cellSize))].push_back(entity);
743+
const auto floorId = query.contains<EvacuationRoute>(entity)
744+
? agentCollisionFloorId(query.get<EvacuationRoute>(entity))
745+
: std::string{};
746+
auto& floorCells = index.cellsByFloor[floorId];
747+
floorCells[spatialKey(spatialCellFor(position.value, cellSize))].push_back(entity);
740748
}
741749
return index;
742750
}
@@ -745,14 +753,20 @@ std::vector<engine::Entity> nearbyAgents(
745753
engine::WorldQuery& query,
746754
const AgentSpatialIndex& index,
747755
const Point2D& point,
756+
const std::string& floorId,
748757
double radius) {
749758
std::vector<engine::Entity> candidates;
759+
const auto floorIt = index.cellsByFloor.find(floorId);
760+
if (floorIt == index.cellsByFloor.end()) {
761+
return candidates;
762+
}
763+
750764
const auto center = spatialCellFor(point, index.cellSize);
751765
const auto range = std::max(1, static_cast<int>(std::ceil(radius / index.cellSize)));
752766
for (int dy = -range; dy <= range; ++dy) {
753767
for (int dx = -range; dx <= range; ++dx) {
754-
const auto it = index.cells.find(spatialKey({.x = center.x + dx, .y = center.y + dy}));
755-
if (it == index.cells.end()) {
768+
const auto it = floorIt->second.find(spatialKey({.x = center.x + dx, .y = center.y + dy}));
769+
if (it == floorIt->second.end()) {
756770
continue;
757771
}
758772
for (const auto entity : it->second) {
@@ -766,6 +780,14 @@ std::vector<engine::Entity> nearbyAgents(
766780
return candidates;
767781
}
768782

783+
std::vector<engine::Entity> nearbyAgents(
784+
engine::WorldQuery& query,
785+
const AgentSpatialIndex& index,
786+
const Point2D& point,
787+
double radius) {
788+
return nearbyAgents(query, index, point, std::string{}, radius);
789+
}
790+
769791
Point2D deterministicFallbackDirection(engine::Entity entity) {
770792
const auto seed = static_cast<double>((entity.index % 17U) + 1U);
771793
return normalizedOr({.x = std::cos(seed * 1.37), .y = std::sin(seed * 1.37)}, {.x = 1.0, .y = 0.0});
@@ -817,6 +839,10 @@ Point2D forwardPreservingAgentAvoidanceVelocity(
817839
}
818840
const auto& otherPosition = query.get<Position>(other);
819841
const auto& otherAgent = query.get<Agent>(other);
842+
const auto& otherRoute = query.get<EvacuationRoute>(other);
843+
if (agentCollisionFloorId(otherRoute) != agentCollisionFloorId(route)) {
844+
continue;
845+
}
820846
const auto offsetToOther = otherPosition.value - position.value;
821847
const auto distance = lengthOf(offsetToOther);
822848
const auto desiredDistance = static_cast<double>(agent.radius + otherAgent.radius) + kPersonalSpaceBuffer;
@@ -825,7 +851,6 @@ Point2D forwardPreservingAgentAvoidanceVelocity(
825851

826852
bool headOn = false;
827853
if (route.nextWaypointIndex < route.waypoints.size() && distance <= kHeadOnLookAheadDistance) {
828-
const auto& otherRoute = query.get<EvacuationRoute>(other);
829854
if (otherRoute.currentFloorId == route.currentFloorId
830855
&& otherRoute.nextWaypointIndex < otherRoute.waypoints.size()) {
831856
const auto otherTarget = routeWaypointTarget(otherRoute, otherPosition.value);

‎src/domain/ScenarioSimulationInternal.h‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -63,7 +63,7 @@ struct SpatialCell {
6363

6464
struct AgentSpatialIndex {
6565
double cellSize{1.0};
66-
std::unordered_map<long long, std::vector<engine::Entity>> cells{};
66+
std::unordered_map<std::string, std::unordered_map<long long, std::vector<engine::Entity>>> cellsByFloor{};
6767
};
6868

6969
struct LayoutBounds {
@@ -136,12 +136,19 @@ std::string cachedFloorIdForZone(const ScenarioLayoutCacheResource& cache, const
136136
const std::vector<ScenarioConnectionTraversal>& cachedTraversalsForZone(
137137
const ScenarioLayoutCacheResource& cache,
138138
const std::string& zoneId);
139+
std::string agentCollisionFloorId(const EvacuationRoute& route);
139140
std::string zoneAt(const ScenarioLayoutCacheResource& cache, const Point2D& point, const std::string& floorId);
140141
bool routePassageCrossed(const FacilityLayout2D& layout, const EvacuationRoute& route, const Point2D& position, double agentRadius);
141142
double speedOf(const Point2D& velocity);
142143
std::vector<engine::Entity> simulationEntities(engine::WorldQuery& query);
143144
AgentSpatialIndex buildAgentSpatialIndex(engine::WorldQuery& query, const std::vector<engine::Entity>& entities, double cellSize);
144145
std::vector<engine::Entity> nearbyAgents(engine::WorldQuery& query, const AgentSpatialIndex& index, const Point2D& point, double radius);
146+
std::vector<engine::Entity> nearbyAgents(
147+
engine::WorldQuery& query,
148+
const AgentSpatialIndex& index,
149+
const Point2D& point,
150+
const std::string& floorId,
151+
double radius);
145152
Point2D deterministicFallbackDirection(engine::Entity entity);
146153
Point2D forwardPreservingAgentAvoidanceVelocity(
147154
engine::WorldQuery& query,

‎src/domain/ScenarioSimulationMotionSystem.cpp‎

Lines changed: 24 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -107,9 +107,15 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem {
107107
const auto neighborRadius = std::max(
108108
static_cast<double>(agent.radius) + kDefaultAgentRadius + kPersonalSpaceBuffer,
109109
kHeadOnLookAheadDistance);
110+
const auto collisionFloorId = agentCollisionFloorId(route);
110111
const auto neighborCandidates = resources.contains<ScenarioAgentSpatialIndexResource>()
111-
? scenarioNearbyAgents(query, resources.get<ScenarioAgentSpatialIndexResource>(), position.value, neighborRadius)
112-
: nearbyAgents(query, localNeighborIndex, position.value, neighborRadius);
112+
? scenarioNearbyAgents(
113+
query,
114+
resources.get<ScenarioAgentSpatialIndexResource>(),
115+
position.value,
116+
collisionFloorId,
117+
neighborRadius)
118+
: nearbyAgents(query, localNeighborIndex, position.value, collisionFloorId, neighborRadius);
113119
const auto avoidanceVelocity =
114120
forwardPreservingAgentAvoidanceVelocity(
115121
query,
@@ -698,13 +704,16 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem {
698704
continue;
699705
}
700706

701-
auto& firstPosition = query.get<Position>(first);
702-
const auto& firstAgent = query.get<Agent>(first);
703-
const auto candidates = nearbyAgents(
704-
query,
705-
spatialIndex,
706-
firstPosition.value,
707-
static_cast<double>(firstAgent.radius) + kDefaultAgentRadius);
707+
auto& firstPosition = query.get<Position>(first);
708+
const auto& firstAgent = query.get<Agent>(first);
709+
const auto& firstRoute = query.get<EvacuationRoute>(first);
710+
const auto firstCollisionFloorId = agentCollisionFloorId(firstRoute);
711+
const auto candidates = nearbyAgents(
712+
query,
713+
spatialIndex,
714+
firstPosition.value,
715+
firstCollisionFloorId,
716+
static_cast<double>(firstAgent.radius) + kDefaultAgentRadius);
708717
for (const auto second : candidates) {
709718
if (first == second) {
710719
continue;
@@ -730,11 +739,13 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem {
730739
if (distance >= minimumDistance) {
731740
continue;
732741
}
733-
734-
const auto direction = normalizedOr(delta, deterministicFallbackDirection(first));
735-
const auto push = std::min(0.08, (minimumDistance - distance) * 0.35);
736-
const auto& firstRoute = query.get<EvacuationRoute>(first);
742+
743+
const auto direction = normalizedOr(delta, deterministicFallbackDirection(first));
744+
const auto push = std::min(0.08, (minimumDistance - distance) * 0.35);
737745
const auto& secondRoute = query.get<EvacuationRoute>(second);
746+
if (agentCollisionFloorId(secondRoute) != firstCollisionFloorId) {
747+
continue;
748+
}
738749
firstPosition.value = constrainedMove(
739750
cachedLayoutForFloor(layoutCache, firstRoute.currentFloorId),
740751
firstPosition.value,

‎src/domain/ScenarioSimulationSystems.cpp‎

Lines changed: 14 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -291,14 +291,20 @@ std::vector<engine::Entity> scenarioNearbyAgents(
291291
engine::WorldQuery& query,
292292
const ScenarioAgentSpatialIndexResource& index,
293293
const Point2D& point,
294+
const std::string& floorId,
294295
double radius) {
295296
std::vector<engine::Entity> candidates;
297+
const auto floorIt = index.cellsByFloor.find(floorId);
298+
if (floorIt == index.cellsByFloor.end()) {
299+
return candidates;
300+
}
301+
296302
const auto center = spatialCellFor(point, index.cellSize);
297303
const auto range = std::max(1, static_cast<int>(std::ceil(radius / index.cellSize)));
298304
for (int dy = -range; dy <= range; ++dy) {
299305
for (int dx = -range; dx <= range; ++dx) {
300-
const auto it = index.cells.find(spatialKey({.x = center.x + dx, .y = center.y + dy}));
301-
if (it == index.cells.end()) {
306+
const auto it = floorIt->second.find(spatialKey({.x = center.x + dx, .y = center.y + dy}));
307+
if (it == floorIt->second.end()) {
302308
continue;
303309
}
304310
for (const auto entity : it->second) {
@@ -325,14 +331,18 @@ void ScenarioSpatialIndexSystem::update(engine::EngineWorld& world, const engine
325331
index.cellSize = cellSize_;
326332

327333
const auto entities = query.view<Position, Agent, EvacuationStatus>();
328-
index.cells.reserve(entities.size() * 2);
334+
index.cellsByFloor.reserve(4);
329335
for (const auto entity : entities) {
330336
const auto& status = query.get<EvacuationStatus>(entity);
331337
if (status.evacuated) {
332338
continue;
333339
}
334340
const auto& position = query.get<Position>(entity);
335-
index.cells[spatialKey(spatialCellFor(position.value, index.cellSize))].push_back(entity);
341+
const auto floorId = query.contains<EvacuationRoute>(entity)
342+
? simulation_internal::agentCollisionFloorId(query.get<EvacuationRoute>(entity))
343+
: std::string{};
344+
auto& floorCells = index.cellsByFloor[floorId];
345+
floorCells[spatialKey(spatialCellFor(position.value, index.cellSize))].push_back(entity);
336346
}
337347

338348
resources.set(std::move(index));

‎src/domain/ScenarioSimulationSystems.h‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22

33
#include <cstdint>
44
#include <memory>
5+
#include <string>
56
#include <unordered_map>
67
#include <vector>
78

@@ -32,7 +33,7 @@ struct ScenarioSimulationStepResource {
3233

3334
struct ScenarioAgentSpatialIndexResource {
3435
double cellSize{1.0};
35-
std::unordered_map<long long, std::vector<engine::Entity>> cells{};
36+
std::unordered_map<std::string, std::unordered_map<long long, std::vector<engine::Entity>>> cellsByFloor{};
3637
};
3738

3839
struct ScenarioConnectionTraversal {
@@ -82,6 +83,7 @@ std::vector<engine::Entity> scenarioNearbyAgents(
8283
engine::WorldQuery& query,
8384
const ScenarioAgentSpatialIndexResource& index,
8485
const Point2D& point,
86+
const std::string& floorId,
8587
double radius);
8688

8789
std::unique_ptr<engine::EngineSystem> makeScenarioSimulationMotionSystem();

‎tests/ScenarioSimulationRunnerTests.cpp‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -509,6 +509,41 @@ SC_TEST(ScenarioSimulationRunnerUsesPlacementFloorForOverlappingCoordinates) {
509509
SC_EXPECT_TRUE(runner.frame().agents.front().velocity.x < 0.0);
510510
}
511511

512+
SC_TEST(ScenarioSimulationRunnerDoesNotSlowAgentsOnDifferentFloorsAtSameCoordinates) {
513+
safecrowd::domain::InitialPlacement2D lower;
514+
lower.id = "agent-l1";
515+
lower.floorId = "L1";
516+
lower.zoneId = "room-l1";
517+
lower.targetAgentCount = 1;
518+
lower.initialVelocity = {.x = 1.5, .y = 0.0};
519+
lower.area.outline = {{.x = 1.0, .y = 1.0}};
520+
521+
safecrowd::domain::InitialPlacement2D upper;
522+
upper.id = "agent-l2";
523+
upper.floorId = "L2";
524+
upper.zoneId = "room-l2";
525+
upper.targetAgentCount = 1;
526+
upper.initialVelocity = {.x = 1.5, .y = 0.0};
527+
upper.area.outline = {{.x = 1.0, .y = 1.0}};
528+
529+
safecrowd::domain::ScenarioDraft scenario;
530+
scenario.population.initialPlacements.push_back(lower);
531+
scenario.population.initialPlacements.push_back(upper);
532+
scenario.execution.timeLimitSeconds = 5.0;
533+
534+
safecrowd::domain::ScenarioSimulationRunner runner(overlappingTwoFloorExitLayout(), scenario);
535+
runner.step(0.1);
536+
537+
const auto& agents = runner.frame().agents;
538+
SC_EXPECT_EQ(agents.size(), std::size_t{2});
539+
const auto& first = agents[0].floorId == "L1" ? agents[0] : agents[1];
540+
const auto& second = agents[0].floorId == "L2" ? agents[0] : agents[1];
541+
SC_EXPECT_EQ(first.floorId, std::string{"L1"});
542+
SC_EXPECT_EQ(second.floorId, std::string{"L2"});
543+
SC_EXPECT_TRUE(first.velocity.x > 1.0);
544+
SC_EXPECT_TRUE(second.velocity.x < -1.0);
545+
}
546+
512547
SC_TEST(ScenarioSimulationRunnerHonorsAllowedStairEntryDirection) {
513548
safecrowd::domain::InitialPlacement2D placement;
514549
placement.id = "agent-1";

‎tests/ScenarioSimulationSystemsTests.cpp‎

Lines changed: 61 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,32 @@ class ConfigureScenarioAgentsSystem final : public safecrowd::engine::EngineSyst
3434
}
3535
};
3636

37+
class ConfigureOverlappingFloorAgentsSystem final : public safecrowd::engine::EngineSystem {
38+
public:
39+
void configure(safecrowd::engine::EngineWorld& world) override {
40+
world.resources().set(safecrowd::domain::ScenarioSimulationClockResource{
41+
.elapsedSeconds = 0.0,
42+
.timeLimitSeconds = 10.0,
43+
.complete = false,
44+
});
45+
world.commands().spawnEntity(
46+
safecrowd::domain::Position{.value = {.x = 1.0, .y = 1.0}},
47+
safecrowd::domain::Agent{.radius = 0.25f, .maxSpeed = 1.5f},
48+
safecrowd::domain::Velocity{.value = {.x = 0.0, .y = 0.0}},
49+
safecrowd::domain::EvacuationRoute{.currentFloorId = "L1", .displayFloorId = "L1"},
50+
safecrowd::domain::EvacuationStatus{});
51+
world.commands().spawnEntity(
52+
safecrowd::domain::Position{.value = {.x = 1.0, .y = 1.0}},
53+
safecrowd::domain::Agent{.radius = 0.25f, .maxSpeed = 1.5f},
54+
safecrowd::domain::Velocity{.value = {.x = 0.0, .y = 0.0}},
55+
safecrowd::domain::EvacuationRoute{.currentFloorId = "L2", .displayFloorId = "L2"},
56+
safecrowd::domain::EvacuationStatus{});
57+
}
58+
59+
void update(safecrowd::engine::EngineWorld&, const safecrowd::engine::EngineStepContext&) override {
60+
}
61+
};
62+
3763
class ConfigureEvacuatedAgentsSystem final : public safecrowd::engine::EngineSystem {
3864
public:
3965
void configure(safecrowd::engine::EngineWorld& world) override {
@@ -198,10 +224,45 @@ SC_TEST(ScenarioSpatialIndexSystem_BuildsNearbyAgentResource) {
198224
runtime.world().query(),
199225
index,
200226
{.x = 1.0, .y = 1.0},
227+
std::string{},
201228
0.4);
202229
SC_EXPECT_EQ(nearby.size(), std::size_t{1});
203230
}
204231

232+
SC_TEST(ScenarioSpatialIndexSystem_SeparatesNearbyAgentsByFloor) {
233+
safecrowd::engine::EngineRuntime runtime({
234+
.fixedDeltaTime = 1.0 / 30.0,
235+
.maxCatchUpSteps = 1,
236+
.baseSeed = 31,
237+
});
238+
runtime.addSystem(std::make_unique<ConfigureOverlappingFloorAgentsSystem>());
239+
runtime.addSystem(
240+
std::make_unique<safecrowd::domain::ScenarioSpatialIndexSystem>(1.0),
241+
{.phase = safecrowd::engine::UpdatePhase::PreSimulation,
242+
.triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame});
243+
244+
runtime.play();
245+
runtime.stepFrame(1.0 / 30.0);
246+
247+
const auto& index = runtime.world().resources().get<safecrowd::domain::ScenarioAgentSpatialIndexResource>();
248+
auto l1Nearby = safecrowd::domain::scenarioNearbyAgents(
249+
runtime.world().query(),
250+
index,
251+
{.x = 1.0, .y = 1.0},
252+
"L1",
253+
0.4);
254+
auto l2Nearby = safecrowd::domain::scenarioNearbyAgents(
255+
runtime.world().query(),
256+
index,
257+
{.x = 1.0, .y = 1.0},
258+
"L2",
259+
0.4);
260+
261+
SC_EXPECT_EQ(l1Nearby.size(), std::size_t{1});
262+
SC_EXPECT_EQ(l2Nearby.size(), std::size_t{1});
263+
SC_EXPECT_TRUE(l1Nearby.front() != l2Nearby.front());
264+
}
265+
205266
SC_TEST(ScenarioClockSystem_AdvancesClockResourceOnFixedSteps) {
206267
safecrowd::engine::EngineRuntime runtime({
207268
.fixedDeltaTime = 0.25,

0 commit comments

Comments
 (0)