Improved pathfinding logic
This commit is contained in:
Vladislav Khorev committed
1 parent
2659951d8e
commit
3a73f3fbde
36 files changed
+458337
-871
No files matched your search
@@ -397,6 +397,124 @@ std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
|
||||
return path;
|
||||
}
|
||||
|
||||
std::vector<Eigen::Vector3f> PathFinder::findNearestReachableImpl(
|
||||
const Eigen::Vector3f& start,
|
||||
const Eigen::Vector3f& end,
|
||||
const std::vector<unsigned char>& walkableGrid) const
|
||||
{
|
||||
if (!ready || walkableGrid.empty()) return {};
|
||||
|
||||
Cell startCell;
|
||||
Cell endCell;
|
||||
if (!findNearestWalkableCell(start, startCell, walkableGrid)) return {};
|
||||
if (!findNearestWalkableCell(end, endCell, walkableGrid)) return {};
|
||||
|
||||
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), 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);
|
||||
|
||||
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) {
|
||||
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;
|
||||
bestIndex = current.index;
|
||||
}
|
||||
|
||||
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 h{ currentCell.x + offset[0], currentCell.z };
|
||||
Cell v{ currentCell.x, currentCell.z + offset[1] };
|
||||
if (!isCellWalkable(h, walkableGrid) || !isCellWalkable(v, 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;
|
||||
open.push({ nextIndex, newCost + distanceCells(next, endCell) });
|
||||
}
|
||||
}
|
||||
|
||||
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, 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());
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
bool PathFinder::setAreaAvailable(const std::string& areaName, bool available)
|
||||
{
|
||||
bool found = false;
|
||||
|
||||
@@ -41,6 +41,14 @@ public:
|
||||
const Eigen::Vector3f& end,
|
||||
const std::vector<DynamicObstacle>& dynamicObstacles) const;
|
||||
|
||||
// Like findPath, but when the destination is unreachable the character
|
||||
// walks as close as possible instead of not moving at all.
|
||||
std::vector<Eigen::Vector3f> findPathToNearest(const Eigen::Vector3f& start,
|
||||
const Eigen::Vector3f& end) const;
|
||||
std::vector<Eigen::Vector3f> findPathToNearest(const Eigen::Vector3f& start,
|
||||
const Eigen::Vector3f& end,
|
||||
const std::vector<DynamicObstacle>& dynamicObstacles) const;
|
||||
|
||||
bool setAreaAvailable(const std::string& areaName, bool available);
|
||||
bool isReady() const { return ready; }
|
||||
bool isWalkable(const Eigen::Vector3f& point) const;
|
||||
@@ -95,6 +103,12 @@ private:
|
||||
bool findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out, const std::vector<unsigned char>& walkableGrid) const;
|
||||
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;
|
||||
};
|
||||
|
||||
} // namespace ZL
|
||||
Reference in new issue
Block a user