Working about pathfinding
This commit is contained in:
Vladislav Khorev committed
1 parent
6a5adb1c79
commit
c6d98bd72a
25 files changed
+1997
-508
No files matched your search
@@ -0,0 +1,269 @@
|
||||
#include "CharacterState.h"
|
||||
#include "LocationGameState.h"
|
||||
#include "navigation/PathFinder.h"
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
#include <filesystem>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace {
|
||||
|
||||
int fail(const char* message)
|
||||
{
|
||||
std::cout << "PathFinderTests: " << message << std::endl;
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
|
||||
std::filesystem::path writeGrid(
|
||||
const std::string& name, int width, int depth, bool corridor)
|
||||
{
|
||||
const auto path = std::filesystem::temp_directory_path() / name;
|
||||
std::ofstream output(path);
|
||||
output << "cellSize 1\nagentRadius 0.1\nfloorY 0\n"
|
||||
<< "minX 0\nminZ 0\ngridWidth " << width
|
||||
<< "\ngridDepth " << depth << "\n";
|
||||
for (int z = 0; z < depth; ++z) {
|
||||
for (int x = 0; x < width; ++x)
|
||||
output << (!corridor || z == depth / 2 ? '1' : '0');
|
||||
output << '\n';
|
||||
}
|
||||
return path;
|
||||
}
|
||||
|
||||
bool samePath(
|
||||
const std::vector<Eigen::Vector3f>& lhs,
|
||||
const std::vector<Eigen::Vector3f>& rhs)
|
||||
{
|
||||
if (lhs.size() != rhs.size()) return false;
|
||||
for (size_t i = 0; i < lhs.size(); ++i)
|
||||
if (lhs[i] != rhs[i]) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
float segmentDistanceSquared(
|
||||
const Eigen::Vector3f& point,
|
||||
const Eigen::Vector3f& start,
|
||||
const Eigen::Vector3f& end)
|
||||
{
|
||||
const Eigen::Vector2f p(point.x(), point.z());
|
||||
const Eigen::Vector2f a(start.x(), start.z());
|
||||
const Eigen::Vector2f segment(end.x() - start.x(), end.z() - start.z());
|
||||
const float lengthSquared = segment.squaredNorm();
|
||||
const float t = lengthSquared > 1e-8f
|
||||
? (std::max)(0.f, (std::min)(1.f, (p - a).dot(segment) / lengthSquared))
|
||||
: 0.f;
|
||||
return (p - (a + segment * t)).squaredNorm();
|
||||
}
|
||||
|
||||
void runBenchmark(FRG::PathFinder& pathFinder, int queries)
|
||||
{
|
||||
std::vector<FRG::PathFinder::DynamicObstacle> obstacles;
|
||||
for (int i = 0; i < queries; ++i) {
|
||||
const float x = 4.5f + static_cast<float>((i * 7) % 70);
|
||||
const float z = 4.5f + static_cast<float>((i * 13) % 70);
|
||||
obstacles.push_back({Eigen::Vector3f(x, 0.f, z), 0.25f});
|
||||
}
|
||||
const auto start = std::chrono::steady_clock::now();
|
||||
size_t waypointCount = 0;
|
||||
for (int i = 0; i < queries; ++i) {
|
||||
const Eigen::Vector3f from(
|
||||
1.5f, 0.f, 1.5f + static_cast<float>(i % 30));
|
||||
const Eigen::Vector3f to(
|
||||
78.5f, 0.f, 78.5f - static_cast<float>(i % 30));
|
||||
waypointCount +=
|
||||
pathFinder.findPathToNearest(from, to, obstacles).size();
|
||||
}
|
||||
const auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::steady_clock::now() - start);
|
||||
std::cout << "PathFinder benchmark " << queries << " characters/queries: "
|
||||
<< elapsed.count() << " ms, " << waypointCount
|
||||
<< " waypoints" << std::endl;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
const bool benchmark = argc > 1 && std::string(argv[1]) == "--benchmark";
|
||||
const auto openGrid = writeGrid("frg_pathfinder_open.txt", 80, 80, false);
|
||||
FRG::PathFinder pathFinder;
|
||||
pathFinder.build(openGrid.string());
|
||||
if (!pathFinder.isReady()) return fail("Open test grid did not load");
|
||||
|
||||
const FRG::PathFinder::DynamicObstacle first{
|
||||
Eigen::Vector3f(8.5f, 0.f, 8.5f), 0.75f};
|
||||
const FRG::PathFinder::DynamicObstacle second{
|
||||
Eigen::Vector3f(12.5f, 0.f, 10.5f), 0.75f};
|
||||
const Eigen::Vector3f start(2.5f, 0.f, 8.5f);
|
||||
const Eigen::Vector3f end(18.5f, 0.f, 8.5f);
|
||||
const auto ordered = pathFinder.findPathToNearest(
|
||||
start, end, {first, second});
|
||||
const auto reversed = pathFinder.findPathToNearest(
|
||||
start, end, {second, first});
|
||||
if (ordered.empty() || !samePath(ordered, reversed))
|
||||
return fail("Obstacle insertion order changed the route");
|
||||
Eigen::Vector3f previous = start;
|
||||
for (const Eigen::Vector3f& point : ordered) {
|
||||
if (segmentDistanceSquared(
|
||||
first.position, previous, point) < 0.85f * 0.85f)
|
||||
return fail("Smoothed path crossed a dynamic obstacle");
|
||||
previous = point;
|
||||
}
|
||||
|
||||
const auto occupiedDestination =
|
||||
pathFinder.findPathToNearest(start, first.position, {first});
|
||||
if (occupiedDestination.empty())
|
||||
return fail("Occupied destination did not produce a nearest route");
|
||||
const Eigen::Vector3f occupiedEnd = occupiedDestination.back();
|
||||
const Eigen::Vector2f occupiedDelta(
|
||||
occupiedEnd.x() - first.position.x(),
|
||||
occupiedEnd.z() - first.position.z());
|
||||
if (occupiedDelta.squaredNorm() < 0.85f * 0.85f)
|
||||
return fail("Occupied destination ended inside the obstacle");
|
||||
|
||||
const auto corridorGrid =
|
||||
writeGrid("frg_pathfinder_corridor.txt", 20, 3, true);
|
||||
FRG::PathFinder corridor;
|
||||
corridor.build(corridorGrid.string());
|
||||
const FRG::PathFinder::DynamicObstacle blocker{
|
||||
Eigen::Vector3f(10.5f, 0.f, 1.5f), 0.45f};
|
||||
const auto nearest = corridor.findPathToNearest(
|
||||
Eigen::Vector3f(1.5f, 0.f, 1.5f),
|
||||
Eigen::Vector3f(18.5f, 0.f, 1.5f), {blocker});
|
||||
if (nearest.empty() || nearest.back().x() >= blocker.position.x())
|
||||
return fail("Nearest fallback ignored a dynamic blocker");
|
||||
|
||||
FRG::CharacterState guardedMover;
|
||||
guardedMover.configureAsPlayer();
|
||||
guardedMover.enabled = true;
|
||||
guardedMover.walkTarget = Eigen::Vector3f(2.f, 0.f, 0.f);
|
||||
guardedMover.requestedWalkTarget = guardedMover.walkTarget;
|
||||
guardedMover.pathWaypoints = {guardedMover.walkTarget};
|
||||
const std::vector<FRG::CharacterMovementObstacle> movementObstacles{
|
||||
{"mover", Eigen::Vector3f::Zero(), 0.45f},
|
||||
{"blocker", Eigen::Vector3f(0.8f, 0.f, 0.f), 0.45f}};
|
||||
FRG::CharacterUpdateContext guardedContext;
|
||||
guardedContext.characterId = "mover";
|
||||
guardedContext.movementObstacles = &movementObstacles;
|
||||
FRG::CharacterUpdateEvents blockedEvents;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
blockedEvents = guardedMover.update(100, guardedContext);
|
||||
if (!guardedMover.position.isZero() ||
|
||||
blockedEvents.forceNavigationReplan ||
|
||||
!guardedMover.pathWaypoints.empty() ||
|
||||
guardedMover.requestedWalkTarget != guardedMover.position ||
|
||||
guardedMover.currentState != FRG::AnimationState::STAND)
|
||||
return fail("Blocked player action was not cancelled");
|
||||
|
||||
FRG::CharacterState retryingNpc;
|
||||
retryingNpc.enabled = true;
|
||||
retryingNpc.configureNavigationRetryCooldowns(
|
||||
"retrying_npc", {2, 3, 4, 5, 6, 7, 8, 9, 10, 11});
|
||||
FRG::CharacterState otherNpc;
|
||||
otherNpc.configureNavigationRetryCooldowns(
|
||||
"other_npc", {2, 3, 4, 5, 6, 7, 8, 9, 10, 11});
|
||||
if (otherNpc.navigationRetryCooldownTicks ==
|
||||
retryingNpc.navigationRetryCooldownTicks)
|
||||
return fail("Stable NPC IDs produced the same retry sequence");
|
||||
const int firstCooldown =
|
||||
retryingNpc.navigationRetryCooldownTicks.front();
|
||||
retryingNpc.walkTarget = Eigen::Vector3f(2.f, 0.f, 0.f);
|
||||
retryingNpc.requestedWalkTarget = retryingNpc.walkTarget;
|
||||
retryingNpc.pathWaypoints = {retryingNpc.walkTarget};
|
||||
const std::vector<FRG::CharacterMovementObstacle> movingBlocker{
|
||||
{"b", Eigen::Vector3f(0.8f, 0.f, 0.f), 0.45f, true,
|
||||
Eigen::Vector3f(-1.f, 0.f, 0.f)}};
|
||||
FRG::CharacterUpdateContext retryContext;
|
||||
retryContext.characterId = "retrying_npc";
|
||||
retryContext.movementObstacles = &movingBlocker;
|
||||
FRG::CharacterUpdateEvents retryEvents;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
retryEvents = retryingNpc.update(100, retryContext);
|
||||
if (retryEvents.forceNavigationReplan ||
|
||||
!retryingNpc.pathWaypoints.empty() ||
|
||||
retryingNpc.navigationRetryTicks != firstCooldown ||
|
||||
retryingNpc.requestedWalkTarget != Eigen::Vector3f(2.f, 0.f, 0.f))
|
||||
return fail("Blocked NPC did not stop with a deterministic cooldown");
|
||||
for (int i = 0; i < firstCooldown; ++i)
|
||||
retryEvents = retryingNpc.update(100, retryContext);
|
||||
if (!retryEvents.forceNavigationReplan ||
|
||||
retryEvents.blockingCharacterId != "b")
|
||||
return fail("Blocked NPC did not retry after its cooldown");
|
||||
|
||||
retryingNpc.movedLastTick = true;
|
||||
nlohmann::json blockedState;
|
||||
retryingNpc.save(blockedState);
|
||||
FRG::CharacterState restoredMover;
|
||||
restoredMover.load(blockedState);
|
||||
if (restoredMover.movementBlockedTicks !=
|
||||
retryingNpc.movementBlockedTicks ||
|
||||
restoredMover.navigationRetryTicks !=
|
||||
retryingNpc.navigationRetryTicks ||
|
||||
restoredMover.navigationRetryCooldownTicks !=
|
||||
retryingNpc.navigationRetryCooldownTicks ||
|
||||
restoredMover.navigationRetryCooldownIndex !=
|
||||
retryingNpc.navigationRetryCooldownIndex ||
|
||||
!restoredMover.movedLastTick)
|
||||
return fail("Blocked movement state did not round trip");
|
||||
|
||||
FRG::LocationGameState occupancyState;
|
||||
auto stationary = std::make_shared<FRG::CharacterState>();
|
||||
stationary->enabled = true;
|
||||
stationary->hp = 10.f;
|
||||
auto moving = std::make_shared<FRG::CharacterState>();
|
||||
moving->enabled = true;
|
||||
moving->hp = 10.f;
|
||||
moving->pathWaypoints = {Eigen::Vector3f(4.f, 0.f, 0.f)};
|
||||
moving->movedLastTick = true;
|
||||
FRG::PlayerStateMap occupancyPlayers{
|
||||
{"stationary", stationary}, {"moving", moving}};
|
||||
if (occupancyState.collectPathObstacles(
|
||||
"requester", occupancyPlayers).size() != 1 ||
|
||||
occupancyState.collectPathObstacles(
|
||||
"requester", occupancyPlayers, "moving").size() != 2 ||
|
||||
!occupancyState.collectPathObstacles(
|
||||
"stationary", occupancyPlayers).empty())
|
||||
return fail("Stationary occupancy policy is incorrect");
|
||||
moving->movedLastTick = false;
|
||||
if (occupancyState.collectPathObstacles(
|
||||
"requester", occupancyPlayers).size() != 2)
|
||||
return fail("Physically stalled character was omitted from planning");
|
||||
|
||||
FRG::LocationGameState approachingState;
|
||||
auto left = std::make_shared<FRG::CharacterState>();
|
||||
left->configureAsPlayer();
|
||||
left->enabled = true;
|
||||
left->position = Eigen::Vector3f(0.f, 0.f, 0.f);
|
||||
left->walkTarget = Eigen::Vector3f(2.f, 0.f, 0.f);
|
||||
left->requestedWalkTarget = left->walkTarget;
|
||||
left->pathWaypoints = {left->walkTarget};
|
||||
auto right = std::make_shared<FRG::CharacterState>();
|
||||
right->configureAsPlayer();
|
||||
right->enabled = true;
|
||||
right->position = Eigen::Vector3f(1.4f, 0.f, 0.f);
|
||||
right->walkTarget = Eigen::Vector3f(-1.f, 0.f, 0.f);
|
||||
right->requestedWalkTarget = right->walkTarget;
|
||||
right->pathWaypoints = {right->walkTarget};
|
||||
FRG::PlayerStateMap approachingPlayers{{"a", left}, {"b", right}};
|
||||
for (int i = 0; i < 5; ++i)
|
||||
approachingState.update(100, approachingPlayers, false);
|
||||
const float separation = (right->position - left->position).norm();
|
||||
if (separation + 1e-5f <
|
||||
left->movementContactRadius + right->movementContactRadius ||
|
||||
!left->pathWaypoints.empty() || !right->pathWaypoints.empty())
|
||||
return fail("Colliding players did not stop without overlap");
|
||||
|
||||
if (benchmark) {
|
||||
runBenchmark(pathFinder, 100);
|
||||
runBenchmark(pathFinder, 1000);
|
||||
}
|
||||
std::filesystem::remove(openGrid);
|
||||
std::filesystem::remove(corridorGrid);
|
||||
return EXIT_SUCCESS;
|
||||
}
|
||||
Reference in new issue
Block a user