salmon-engine-projects/shadow-over-bishkek001
14
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Working about pathfinding

Browse Source
This commit is contained in:
Vladislav Khorev committed 2026-11-08 17:24:15 +06:00
1 parent 6a5adb1c79
commit c6d98bd72a
25 files changed
+1997 -508
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

src/navigation/PathFinder.cpp
+214 -333
View File
@@ -14,15 +14,6 @@ namespace FRG {
using json = nlohmann::json;
namespace {
const float INF_COST = (std::numeric_limits<float>::max)();
float distanceCells(const PathFinder::Cell& a, const PathFinder::Cell& b)
{
const float dx = static_cast<float>(a.x - b.x);
const float dz = static_cast<float>(a.z - b.z);
return std::sqrt(dx * dx + dz * dz);
}
std::vector<Eigen::Vector2f> readPolygon(const json& item)
{
std::vector<Eigen::Vector2f> polygon;
@@ -127,397 +118,287 @@ void PathFinder::build(const std::string& configPath,
std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
const Eigen::Vector3f& end) const
{
if (!ready || walkable.empty()) {
return {};
}
Cell startCell;
Cell endCell;
if (!findNearestWalkableCell(start, startCell, walkable) ||
!findNearestWalkableCell(end, endCell, walkable)) {
return {};
}
if (startCell.x == endCell.x && startCell.z == endCell.z) {
return { cellCenter(endCell) };
}
struct QueueNode {
int index = 0;
float priority = 0.0f;
bool operator<(const QueueNode& other) const
{
if (priority != other.priority) return priority > other.priority;
return index > other.index;
}
};
const int cellCount = gridWidth * gridDepth;
std::vector<float> cost(static_cast<size_t>(cellCount), INF_COST);
std::vector<int> cameFrom(static_cast<size_t>(cellCount), -1);
std::priority_queue<QueueNode> open;
const int startIndex = indexOf(startCell);
const int endIndex = indexOf(endCell);
cost[static_cast<size_t>(startIndex)] = 0.0f;
open.push({ startIndex, 0.0f });
static const int offsets[8][2] = {
{ 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 },
{ 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 }
};
while (!open.empty()) {
const QueueNode current = open.top();
open.pop();
if (current.index == endIndex) {
break;
}
const Cell currentCell{ current.index % gridWidth, current.index / gridWidth };
for (const auto& offset : offsets) {
Cell next{ currentCell.x + offset[0], currentCell.z + offset[1] };
if (!isCellWalkable(next, walkable)) {
continue;
}
const bool diagonal = offset[0] != 0 && offset[1] != 0;
if (diagonal) {
Cell horizontal{ currentCell.x + offset[0], currentCell.z };
Cell vertical{ currentCell.x, currentCell.z + offset[1] };
if (!isCellWalkable(horizontal, walkable) || !isCellWalkable(vertical, walkable)) {
continue;
}
}
const int nextIndex = indexOf(next);
const float stepCost = diagonal ? 1.41421356f : 1.0f;
const float newCost = cost[static_cast<size_t>(current.index)] + stepCost;
if (newCost >= cost[static_cast<size_t>(nextIndex)]) {
continue;
}
cost[static_cast<size_t>(nextIndex)] = newCost;
cameFrom[static_cast<size_t>(nextIndex)] = current.index;
const float priority = newCost + distanceCells(next, endCell);
open.push({ nextIndex, priority });
}
}
if (cameFrom[static_cast<size_t>(endIndex)] == -1) {
return {};
}
std::vector<Cell> cells;
for (int current = endIndex; current != -1; current = cameFrom[static_cast<size_t>(current)]) {
cells.push_back({ current % gridWidth, current / gridWidth });
if (current == startIndex) {
break;
}
}
std::reverse(cells.begin(), cells.end());
cells = smoothCells(cells, walkable);
std::vector<Eigen::Vector3f> path;
path.reserve(cells.size());
for (const Cell& cell : cells) {
path.push_back(cellCenter(cell));
}
if (!path.empty() && (path.front() - Eigen::Vector3f(start.x(), floorY, start.z())).norm() < cellSize * 0.75f) {
path.erase(path.begin());
}
if (!path.empty()) {
Cell requestedEndCell;
if (worldToCell(end, requestedEndCell) &&
requestedEndCell.x == endCell.x &&
requestedEndCell.z == endCell.z) {
path.back() = Eigen::Vector3f(end.x(), floorY, end.z());
}
}
return path;
const std::lock_guard<std::mutex> lock(*queryMutex);
return runSearch(start, end, 0, false);
}
std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
const Eigen::Vector3f& end,
const std::vector<DynamicObstacle>& dynamicObstacles) const
{
if (!ready || walkable.empty()) {
return {};
}
std::vector<unsigned char> walkableGrid = walkable;
if (!dynamicObstacles.empty()) {
for (const DynamicObstacle& obstacle : dynamicObstacles) {
const float radius = obstacle.radius + agentRadius;
if (radius <= 0.0f) continue;
const float minWorldX = obstacle.position.x() - radius;
const float maxWorldX = obstacle.position.x() + radius;
const float minWorldZ = obstacle.position.z() - radius;
const float maxWorldZ = obstacle.position.z() + radius;
const int minCellX = (std::max)(0, static_cast<int>(std::floor((minWorldX - minX) / cellSize)));
const int maxCellX = (std::min)(gridWidth - 1, static_cast<int>(std::floor((maxWorldX - minX) / cellSize)));
const int minCellZ = (std::max)(0, static_cast<int>(std::floor((minWorldZ - minZ) / cellSize)));
const int maxCellZ = (std::min)(gridDepth - 1, static_cast<int>(std::floor((maxWorldZ - minZ) / cellSize)));
const float radiusSq = radius * radius;
for (int z = minCellZ; z <= maxCellZ; ++z) {
for (int x = minCellX; x <= maxCellX; ++x) {
const Cell cell{ x, z };
const Eigen::Vector3f center = cellCenter(cell);
const float dx = center.x() - obstacle.position.x();
const float dz = center.z() - obstacle.position.z();
if (dx * dx + dz * dz <= radiusSq) {
const int idx = indexOf(cell);
if (idx >= 0 && idx < static_cast<int>(walkableGrid.size())) {
walkableGrid[static_cast<size_t>(idx)] = 0;
}
}
}
}
}
}
Cell startCell;
Cell endCell;
if (!findNearestWalkableCell(start, startCell, walkableGrid) ||
!findNearestWalkableCell(end, endCell, walkableGrid)) {
return {};
}
if (startCell.x == endCell.x && startCell.z == endCell.z) {
return { cellCenter(endCell) };
}
struct QueueNode {
int index = 0;
float priority = 0.0f;
bool operator<(const QueueNode& other) const
{
if (priority != other.priority) return priority > other.priority;
return index > other.index;
}
};
const int cellCount = gridWidth * gridDepth;
std::vector<float> cost(static_cast<size_t>(cellCount), INF_COST);
std::vector<int> cameFrom(static_cast<size_t>(cellCount), -1);
std::priority_queue<QueueNode> open;
const int startIndex = indexOf(startCell);
const int endIndex = indexOf(endCell);
cost[static_cast<size_t>(startIndex)] = 0.0f;
open.push({ startIndex, 0.0f });
static const int offsets[8][2] = {
{ 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 },
{ 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 }
};
while (!open.empty()) {
const QueueNode current = open.top();
open.pop();
if (current.index == endIndex) {
break;
}
const Cell currentCell{ current.index % gridWidth, current.index / gridWidth };
for (const auto& offset : offsets) {
Cell next{ currentCell.x + offset[0], currentCell.z + offset[1] };
if (!isCellWalkable(next, walkableGrid)) {
continue;
}
const bool diagonal = offset[0] != 0 && offset[1] != 0;
if (diagonal) {
Cell horizontal{ currentCell.x + offset[0], currentCell.z };
Cell vertical{ currentCell.x, currentCell.z + offset[1] };
if (!isCellWalkable(horizontal, walkableGrid) || !isCellWalkable(vertical, walkableGrid)) {
continue;
}
}
const int nextIndex = indexOf(next);
const float stepCost = diagonal ? 1.41421356f : 1.0f;
const float newCost = cost[static_cast<size_t>(current.index)] + stepCost;
if (newCost >= cost[static_cast<size_t>(nextIndex)]) {
continue;
}
cost[static_cast<size_t>(nextIndex)] = newCost;
cameFrom[static_cast<size_t>(nextIndex)] = current.index;
const float priority = newCost + distanceCells(next, endCell);
open.push({ nextIndex, priority });
}
}
if (cameFrom[static_cast<size_t>(endIndex)] == -1) {
return {};
}
std::vector<Cell> cells;
for (int current = endIndex; current != -1; current = cameFrom[static_cast<size_t>(current)]) {
cells.push_back({ current % gridWidth, current / gridWidth });
if (current == startIndex) {
break;
}
}
std::reverse(cells.begin(), cells.end());
cells = smoothCells(cells, walkableGrid);
std::vector<Eigen::Vector3f> path;
path.reserve(cells.size());
for (const Cell& cell : cells) {
path.push_back(cellCenter(cell));
}
if (!path.empty() && (path.front() - Eigen::Vector3f(start.x(), floorY, start.z())).norm() < cellSize * 0.75f) {
path.erase(path.begin());
}
if (!path.empty()) {
Cell requestedEndCell;
if (worldToCell(end, requestedEndCell) &&
requestedEndCell.x == endCell.x &&
requestedEndCell.z == endCell.z) {
path.back() = Eigen::Vector3f(end.x(), floorY, end.z());
}
}
return path;
const std::lock_guard<std::mutex> lock(*queryMutex);
return runSearch(start, end, buildDynamicOverlay(dynamicObstacles), false);
}
std::vector<Eigen::Vector3f> PathFinder::findNearestReachableImpl(
std::uint32_t PathFinder::buildDynamicOverlay(
const std::vector<DynamicObstacle>& dynamicObstacles) const
{
const size_t cellCount = walkable.size();
if (dynamicBlockedGeneration.size() != cellCount) {
dynamicBlockedGeneration.assign(cellCount, 0);
dynamicGeneration = 0;
}
if (++dynamicGeneration == 0) {
std::fill(dynamicBlockedGeneration.begin(), dynamicBlockedGeneration.end(), 0);
dynamicGeneration = 1;
}
for (const DynamicObstacle& obstacle : dynamicObstacles) {
// Include the cell half-diagonal so a segment between adjacent
// unblocked cell centres cannot geometrically cut through the circle.
const float radius =
obstacle.radius + agentRadius + cellSize * 0.70710678f;
if (radius <= 0.f) continue;
const int minCellX = (std::max)(
0, static_cast<int>(std::floor((obstacle.position.x() - radius - minX) / cellSize)));
const int maxCellX = (std::min)(
gridWidth - 1, static_cast<int>(std::floor((obstacle.position.x() + radius - minX) / cellSize)));
const int minCellZ = (std::max)(
0, static_cast<int>(std::floor((obstacle.position.z() - radius - minZ) / cellSize)));
const int maxCellZ = (std::min)(
gridDepth - 1, static_cast<int>(std::floor((obstacle.position.z() + radius - minZ) / cellSize)));
const float radiusSq = radius * radius;
for (int z = minCellZ; z <= maxCellZ; ++z) {
for (int x = minCellX; x <= maxCellX; ++x) {
const Cell cell{x, z};
const Eigen::Vector3f center = cellCenter(cell);
const float dx = center.x() - obstacle.position.x();
const float dz = center.z() - obstacle.position.z();
if (dx * dx + dz * dz <= radiusSq)
dynamicBlockedGeneration[static_cast<size_t>(indexOf(cell))] =
dynamicGeneration;
}
}
}
return dynamicGeneration;
}
bool PathFinder::isCellWalkableOverlay(
const Cell& cell, std::uint32_t generation) const
{
if (!isCellWalkable(cell)) return false;
return generation == 0 ||
dynamicBlockedGeneration[static_cast<size_t>(indexOf(cell))] != generation;
}
bool PathFinder::findNearestWalkableCellOverlay(
const Eigen::Vector3f& point, Cell& out, std::uint32_t generation) const
{
Cell origin;
if (!worldToCell(point, origin)) {
origin.x = (std::max)(0, (std::min)(
gridWidth - 1, static_cast<int>(std::floor((point.x() - minX) / cellSize))));
origin.z = (std::max)(0, (std::min)(
gridDepth - 1, static_cast<int>(std::floor((point.z() - minZ) / cellSize))));
}
if (isCellWalkableOverlay(origin, generation)) {
out = origin;
return true;
}
const int maxRadius =
(std::max)(4, static_cast<int>(std::ceil(8.0f / cellSize)));
for (int radius = 1; radius <= maxRadius; ++radius) {
for (int z = origin.z - radius; z <= origin.z + radius; ++z) {
for (int x = origin.x - radius; x <= origin.x + radius; ++x) {
if (std::abs(x - origin.x) != radius &&
std::abs(z - origin.z) != radius) continue;
const Cell candidate{x, z};
if (isCellWalkableOverlay(candidate, generation)) {
out = candidate;
return true;
}
}
}
}
return false;
}
bool PathFinder::hasLineOfSightOverlay(
const Cell& from, const Cell& to, std::uint32_t generation) const
{
int x = from.x;
int z = from.z;
const int dx = std::abs(to.x - from.x);
const int dz = std::abs(to.z - from.z);
const int sx = from.x < to.x ? 1 : -1;
const int sz = from.z < to.z ? 1 : -1;
int error = dx - dz;
while (true) {
if (!isCellWalkableOverlay({x, z}, generation)) return false;
if (x == to.x && z == to.z) return true;
const int twiceError = error * 2;
const int oldX = x;
const int oldZ = z;
if (twiceError > -dz) {
error -= dz;
x += sx;
}
if (twiceError < dx) {
error += dx;
z += sz;
}
if (x != oldX && z != oldZ &&
(!isCellWalkableOverlay({x, oldZ}, generation) ||
!isCellWalkableOverlay({oldX, z}, generation))) {
return false;
}
}
}
std::vector<PathFinder::Cell> PathFinder::smoothCellsOverlay(
const std::vector<Cell>& cells, std::uint32_t generation) const
{
if (cells.size() <= 2) return cells;
std::vector<Cell> result;
result.push_back(cells.front());
size_t anchor = 0;
while (anchor + 1 < cells.size()) {
size_t next = cells.size() - 1;
while (next > anchor + 1 &&
!hasLineOfSightOverlay(cells[anchor], cells[next], generation)) {
--next;
}
result.push_back(cells[next]);
anchor = next;
}
return result;
}
std::vector<Eigen::Vector3f> PathFinder::runSearch(
const Eigen::Vector3f& start,
const Eigen::Vector3f& end,
const std::vector<unsigned char>& walkableGrid) const
std::uint32_t obstacleGeneration,
bool nearestReachable) const
{
if (!ready || walkableGrid.empty()) return {};
if (!ready || walkable.empty()) return {};
Cell startCell;
Cell endCell;
if (!findNearestWalkableCell(start, startCell, walkableGrid)) return {};
if (!findNearestWalkableCell(end, endCell, walkableGrid)) return {};
if (!findNearestWalkableCellOverlay(start, startCell, obstacleGeneration) ||
!findNearestWalkableCellOverlay(end, endCell, obstacleGeneration)) return {};
if (startCell.x == endCell.x && startCell.z == endCell.z)
return {cellCenter(endCell)};
const size_t cellCount = walkable.size();
if (searchCost.size() != cellCount) {
searchCost.resize(cellCount);
searchCameFrom.resize(cellCount);
searchVisitedGeneration.assign(cellCount, 0);
searchGeneration = 0;
}
if (++searchGeneration == 0) {
std::fill(searchVisitedGeneration.begin(), searchVisitedGeneration.end(), 0);
searchGeneration = 1;
}
const auto heuristic = [](const Cell& a, const Cell& b) {
const int dx = std::abs(a.x - b.x);
const int dz = std::abs(a.z - b.z);
return 14 * (std::min)(dx, dz) + 10 * (std::max(dx, dz) - (std::min)(dx, dz));
};
struct QueueNode {
int index = 0;
float priority = 0.0f;
int index;
int priority;
int cost;
bool operator<(const QueueNode& other) const {
if (priority != other.priority) return priority > other.priority;
if (cost != other.cost) return cost > other.cost;
return index > other.index;
}
};
const int cellCount = gridWidth * gridDepth;
std::vector<float> cost(static_cast<size_t>(cellCount), INF_COST);
std::vector<int> cameFrom(static_cast<size_t>(cellCount), -1);
std::priority_queue<QueueNode> open;
const int startIndex = indexOf(startCell);
const int endIndex = indexOf(endCell);
cost[static_cast<size_t>(startIndex)] = 0.f;
open.push({ startIndex, 0.f });
int bestIndex = startIndex;
float bestDist = distanceCells(startCell, endCell);
const int endIndex = indexOf(endCell);
searchVisitedGeneration[static_cast<size_t>(startIndex)] = searchGeneration;
searchCost[static_cast<size_t>(startIndex)] = 0;
searchCameFrom[static_cast<size_t>(startIndex)] = -1;
open.push({startIndex, heuristic(startCell, endCell), 0});
int bestIndex = startIndex;
int bestDistanceSq =
(startCell.x - endCell.x) * (startCell.x - endCell.x) +
(startCell.z - endCell.z) * (startCell.z - endCell.z);
static const int offsets[8][2] = {
{ 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 },
{ 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 }
};
while (!open.empty()) {
const QueueNode current = open.top();
open.pop();
const size_t currentOffset = static_cast<size_t>(current.index);
if (searchVisitedGeneration[currentOffset] != searchGeneration ||
searchCost[currentOffset] != current.cost) continue;
if (current.index == endIndex) {
bestIndex = endIndex;
break;
}
if (cost[static_cast<size_t>(current.index)] < current.priority - distanceCells({ current.index % gridWidth, current.index / gridWidth }, endCell)) {
continue; // stale entry
}
const Cell currentCell{ current.index % gridWidth, current.index / gridWidth };
const float d = distanceCells(currentCell, endCell);
if (d < bestDist) {
bestDist = d;
const int dx = currentCell.x - endCell.x;
const int dz = currentCell.z - endCell.z;
const int distanceSq = dx * dx + dz * dz;
if (distanceSq < bestDistanceSq ||
(distanceSq == bestDistanceSq && current.index < bestIndex)) {
bestDistanceSq = distanceSq;
bestIndex = current.index;
}
for (const auto& offset : offsets) {
Cell next{ currentCell.x + offset[0], currentCell.z + offset[1] };
if (!isCellWalkable(next, walkableGrid)) continue;
if (!isCellWalkableOverlay(next, obstacleGeneration)) continue;
const bool diagonal = offset[0] != 0 && offset[1] != 0;
if (diagonal) {
Cell h{ currentCell.x + offset[0], currentCell.z };
Cell v{ currentCell.x, currentCell.z + offset[1] };
if (!isCellWalkable(h, walkableGrid) || !isCellWalkable(v, walkableGrid)) continue;
if (!isCellWalkableOverlay(h, obstacleGeneration) ||
!isCellWalkableOverlay(v, obstacleGeneration)) continue;
}
const int nextIndex = indexOf(next);
const float stepCost = diagonal ? 1.41421356f : 1.0f;
const float newCost = cost[static_cast<size_t>(current.index)] + stepCost;
if (newCost >= cost[static_cast<size_t>(nextIndex)]) continue;
cost[static_cast<size_t>(nextIndex)] = newCost;
cameFrom[static_cast<size_t>(nextIndex)] = current.index;
open.push({ nextIndex, newCost + distanceCells(next, endCell) });
const int nextIndex = indexOf(next);
const size_t nextOffset = static_cast<size_t>(nextIndex);
const int newCost = current.cost + (diagonal ? 14 : 10);
if (searchVisitedGeneration[nextOffset] == searchGeneration &&
newCost >= searchCost[nextOffset]) continue;
searchVisitedGeneration[nextOffset] = searchGeneration;
searchCost[nextOffset] = newCost;
searchCameFrom[nextOffset] = current.index;
open.push({nextIndex, newCost + heuristic(next, endCell), newCost});
}
}
if (!nearestReachable && bestIndex != endIndex) return {};
if (bestIndex == startIndex) return {};
std::vector<Cell> cells;
for (int cur = bestIndex; cur != -1; cur = cameFrom[static_cast<size_t>(cur)]) {
for (int cur = bestIndex; cur != -1;
cur = searchCameFrom[static_cast<size_t>(cur)]) {
cells.push_back({ cur % gridWidth, cur / gridWidth });
if (cur == startIndex) break;
}
std::reverse(cells.begin(), cells.end());
cells = smoothCells(cells, walkableGrid);
cells = smoothCellsOverlay(cells, obstacleGeneration);
std::vector<Eigen::Vector3f> path;
path.reserve(cells.size());
for (const Cell& cell : cells) path.push_back(cellCenter(cell));
if (!path.empty() && (path.front() - Eigen::Vector3f(start.x(), floorY, start.z())).norm() < cellSize * 0.75f)
if (!path.empty() &&
(path.front() - Eigen::Vector3f(start.x(), floorY, start.z())).norm() <
cellSize * 0.75f)
path.erase(path.begin());
if (bestIndex == endIndex && !path.empty()) {
Cell requestedEndCell;
if (worldToCell(end, requestedEndCell) &&
requestedEndCell.x == endCell.x && requestedEndCell.z == endCell.z) {
path.back() = Eigen::Vector3f(end.x(), floorY, end.z());
}
}
return path;
}
std::vector<Eigen::Vector3f> PathFinder::findPathToNearest(
const Eigen::Vector3f& start, const Eigen::Vector3f& end) const
{
auto path = findPath(start, end);
if (!path.empty()) return path;
return findNearestReachableImpl(start, end, walkable);
const std::lock_guard<std::mutex> lock(*queryMutex);
return runSearch(start, end, 0, true);
}
std::vector<Eigen::Vector3f> PathFinder::findPathToNearest(
const Eigen::Vector3f& start, const Eigen::Vector3f& end,
const std::vector<DynamicObstacle>& dynamicObstacles) const
{
auto path = findPath(start, end, dynamicObstacles);
if (!path.empty()) return path;
path = findPath(start, end);
if (!path.empty()) return path;
return findNearestReachableImpl(start, end, walkable);
const std::lock_guard<std::mutex> lock(*queryMutex);
return runSearch(
start, end, buildDynamicOverlay(dynamicObstacles), true);
}
bool PathFinder::setAreaAvailable(const std::string& areaName, bool available)
src/navigation/PathFinder.h
+28 -5
View File
@@ -1,6 +1,9 @@
#pragma once
#include <Eigen/Core>
#include <cstdint>
#include <memory>
#include <mutex>
#include <string>
#include <vector>
@@ -79,6 +82,17 @@ private:
std::vector<unsigned char> walkable;
std::vector<NavigationArea> areas;
// Reused by synchronous authoritative path queries. Generation stamps avoid
// copying/clearing the complete map for every dynamic-obstacle search.
mutable std::vector<std::uint32_t> dynamicBlockedGeneration;
mutable std::uint32_t dynamicGeneration = 0;
mutable std::vector<int> searchCost;
mutable std::vector<int> searchCameFrom;
mutable std::vector<std::uint32_t> searchVisitedGeneration;
mutable std::uint32_t searchGeneration = 0;
mutable std::shared_ptr<std::mutex> queryMutex =
std::make_shared<std::mutex>();
void loadConfig(const std::string& configPath, const std::string& zipPath);
bool loadGrid(const std::string& path, const std::string& zipPath);
void resetGridBounds();
@@ -103,11 +117,20 @@ private:
bool hasLineOfSight(const Cell& from, const Cell& to, const std::vector<unsigned char>& walkableGrid) const;
std::vector<Cell> smoothCells(const std::vector<Cell>& cells, const std::vector<unsigned char>& walkableGrid) const;
// A* that runs to exhaustion and returns a path to the reachable cell
// geometrically closest to end when end itself is unreachable.
std::vector<Eigen::Vector3f> findNearestReachableImpl(const Eigen::Vector3f& start,
const Eigen::Vector3f& end,
const std::vector<unsigned char>& walkableGrid) const;
std::uint32_t buildDynamicOverlay(
const std::vector<DynamicObstacle>& dynamicObstacles) const;
bool isCellWalkableOverlay(const Cell& cell, std::uint32_t generation) const;
bool findNearestWalkableCellOverlay(
const Eigen::Vector3f& point, Cell& out, std::uint32_t generation) const;
bool hasLineOfSightOverlay(
const Cell& from, const Cell& to, std::uint32_t generation) const;
std::vector<Cell> smoothCellsOverlay(
const std::vector<Cell>& cells, std::uint32_t generation) const;
std::vector<Eigen::Vector3f> runSearch(
const Eigen::Vector3f& start,
const Eigen::Vector3f& end,
std::uint32_t obstacleGeneration,
bool nearestReachable) const;
};
} // namespace FRG