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

Restore back old navigation system

Browse Source
This commit is contained in:
Vladislav Khorev committed 2026-10-22 23:02:25 +06:00
1 parent c9c6b7e7fb
commit ea8f64c940
7 files changed
+44 -275
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

src/navigation/PathFinder.cpp
+1 -184
View File
@@ -124,68 +124,6 @@ void PathFinder::build(const std::string& configPath,
<< ", cell=" << cellSize << ", areas=" << areas.size() << "\n";
}
std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
const Eigen::Vector3f& end) const
{
return runAStar(start, end, walkable, 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;
// --- Логика внесения dynamicObstacles остается здесь ---
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;
}
}
}
}
}
}
return runAStar(start, end, walkableGrid, false);
}
std::vector<Eigen::Vector3f> PathFinder::findNearestReachableImpl(
const Eigen::Vector3f& start,
const Eigen::Vector3f& end,
const std::vector<unsigned char>& walkableGrid) const
{
return runAStar(start, end, walkableGrid, true);
}
/*
std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
const Eigen::Vector3f& end) const
{
@@ -556,7 +494,7 @@ std::vector<Eigen::Vector3f> PathFinder::findNearestReachableImpl(
path.erase(path.begin());
return path;
}*/
}
std::vector<Eigen::Vector3f> PathFinder::findPathToNearest(
const Eigen::Vector3f& start, const Eigen::Vector3f& end) const
@@ -1137,125 +1075,4 @@ bool PathFinder::pointInPolygon(float x, float z, const std::vector<Eigen::Vecto
return inside;
}
std::vector<Eigen::Vector3f> PathFinder::runAStar(
const Eigen::Vector3f& start,
const Eigen::Vector3f& end,
const std::vector<unsigned char>& grid,
bool returnNearestOnFail) const
{
if (!ready || grid.empty()) return {};
Cell startCell, endCell;
if (!findNearestWalkableCell(start, startCell, grid) ||
!findNearestWalkableCell(end, endCell, grid)) {
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 { return priority > other.priority; }
};
const int cellCount = gridWidth * gridDepth;
std::vector<float> cost(static_cast<size_t>(cellCount), (std::numeric_limits<float>::max)());
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 });
int bestIndex = startIndex;
float bestDist = distanceCells(startCell, endCell);
static const int offsets[8][2] = {
{ 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 },
{ 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 }
};
bool reached = false;
while (!open.empty()) {
const QueueNode current = open.top();
open.pop();
if (current.index == endIndex) {
bestIndex = endIndex;
reached = true;
break;
}
const Cell currentCell{ current.index % gridWidth, current.index / gridWidth };
if (cost[static_cast<size_t>(current.index)] < current.priority - distanceCells(currentCell, endCell)) {
continue;
}
if (returnNearestOnFail) {
const float d = distanceCells(currentCell, endCell);
if (d < bestDist) {
bestDist = d;
bestIndex = current.index;
}
}
for (const auto& offset : offsets) {
Cell next{ currentCell.x + offset[0], currentCell.z + offset[1] };
if (!isCellWalkable(next, grid)) 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, grid) || !isCellWalkable(v, grid)) 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) });
}
}
if (!reached && !returnNearestOnFail) return {};
if (bestIndex == startIndex) return {};
std::vector<Cell> cells;
for (int cur = bestIndex; cur != -1; cur = cameFrom[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, grid);
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 (reached && !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;
}
} // namespace FRG
src/navigation/PathFinder.h
-5
View File
@@ -109,11 +109,6 @@ private:
std::vector<Eigen::Vector3f> findNearestReachableImpl(const Eigen::Vector3f& start,
const Eigen::Vector3f& end,
const std::vector<unsigned char>& walkableGrid) const;
std::vector<Eigen::Vector3f> runAStar(const Eigen::Vector3f& start,
const Eigen::Vector3f& end,
const std::vector<unsigned char>& grid,
bool returnNearestOnFail) const;
};
} // namespace FRG