519 lines
15 KiB
C++
519 lines
15 KiB
C++
#include "navigation/PathFinder.h"
|
|
#include "external/nlohmann/json.hpp"
|
|
#include "utils/Utils.h"
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <iostream>
|
|
#include <limits>
|
|
#include <queue>
|
|
|
|
namespace ZL {
|
|
|
|
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;
|
|
if (!item.contains("polygon") || !item["polygon"].is_array()) {
|
|
return polygon;
|
|
}
|
|
|
|
for (const auto& point : item["polygon"]) {
|
|
if (point.is_array() && point.size() >= 2) {
|
|
polygon.emplace_back(point[0].get<float>(), point[1].get<float>());
|
|
} else if (point.is_object()) {
|
|
polygon.emplace_back(point.value("x", 0.0f), point.value("z", 0.0f));
|
|
}
|
|
}
|
|
|
|
return polygon;
|
|
}
|
|
}
|
|
|
|
void PathFinder::build(const std::vector<ObstacleMesh>& obstacleMeshes,
|
|
const std::string& configPath,
|
|
const std::string& zipPath)
|
|
{
|
|
ready = false;
|
|
loadedConfigPath = configPath;
|
|
loadedZipPath = zipPath;
|
|
obstacles = obstacleMeshes;
|
|
|
|
loadConfig(configPath, zipPath);
|
|
resetGridBounds();
|
|
|
|
if (gridWidth <= 0 || gridDepth <= 0) {
|
|
std::cerr << "[nav] Could not build grid: no navigation polygons\n";
|
|
walkable.clear();
|
|
return;
|
|
}
|
|
|
|
rebuildWalkableGrid();
|
|
ready = true;
|
|
|
|
std::cout << "[nav] Polygon grid built: " << gridWidth << "x" << gridDepth
|
|
<< ", cell=" << cellSize << ", areas=" << areas.size() << "\n";
|
|
}
|
|
|
|
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) ||
|
|
!findNearestWalkableCell(end, endCell)) {
|
|
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), 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)) {
|
|
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) || !isCellWalkable(vertical)) {
|
|
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);
|
|
|
|
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;
|
|
}
|
|
|
|
bool PathFinder::setAreaAvailable(const std::string& areaName, bool available)
|
|
{
|
|
bool found = false;
|
|
bool changed = false;
|
|
for (NavigationArea& area : areas) {
|
|
if (area.name == areaName) {
|
|
found = true;
|
|
changed = changed || area.available != available;
|
|
area.available = available;
|
|
}
|
|
}
|
|
|
|
if (changed) {
|
|
rebuildWalkableGrid();
|
|
}
|
|
|
|
return found;
|
|
}
|
|
|
|
bool PathFinder::isWalkable(const Eigen::Vector3f& point) const
|
|
{
|
|
Cell cell;
|
|
return worldToCell(point, cell) && isCellWalkable(cell);
|
|
}
|
|
|
|
void PathFinder::loadConfig(const std::string& configPath, const std::string& zipPath)
|
|
{
|
|
cellSize = 0.4f;
|
|
agentRadius = 0.45f;
|
|
floorY = 0.0f;
|
|
objectPadding = 0.25f;
|
|
areas.clear();
|
|
|
|
try {
|
|
std::string content;
|
|
if (!zipPath.empty()) {
|
|
const std::vector<char> bytes = readFileFromZIP(configPath, zipPath);
|
|
content.assign(bytes.begin(), bytes.end());
|
|
} else {
|
|
content = readTextFile(configPath);
|
|
}
|
|
|
|
if (content.empty()) {
|
|
std::cerr << "[nav] Navigation config is empty or missing: " << configPath << "\n";
|
|
return;
|
|
}
|
|
|
|
const json root = json::parse(content);
|
|
cellSize = root.value("cellSize", cellSize);
|
|
agentRadius = root.value("agentRadius", agentRadius);
|
|
floorY = root.value("floorY", floorY);
|
|
objectPadding = root.value("objectPadding", objectPadding);
|
|
|
|
if (!root.contains("areas") || !root["areas"].is_array()) {
|
|
std::cerr << "[nav] Navigation config has no 'areas' array: " << configPath << "\n";
|
|
return;
|
|
}
|
|
|
|
for (const auto& item : root["areas"]) {
|
|
NavigationArea area;
|
|
area.name = item.value("name", "");
|
|
area.available = item.value("available", true);
|
|
area.polygon = readPolygon(item);
|
|
|
|
if (area.name.empty()) {
|
|
std::cerr << "[nav] Skipping navigation area without name\n";
|
|
continue;
|
|
}
|
|
if (area.polygon.size() < 3) {
|
|
std::cerr << "[nav] Skipping navigation area '" << area.name << "': polygon has fewer than 3 points\n";
|
|
continue;
|
|
}
|
|
|
|
areas.push_back(area);
|
|
}
|
|
} catch (const std::exception& e) {
|
|
std::cerr << "[nav] Failed to load config '" << configPath << "': " << e.what() << "\n";
|
|
}
|
|
|
|
cellSize = (std::max)(cellSize, 0.1f);
|
|
agentRadius = (std::max)(agentRadius, 0.0f);
|
|
objectPadding = (std::max)(objectPadding, 0.0f);
|
|
}
|
|
|
|
void PathFinder::resetGridBounds()
|
|
{
|
|
if (areas.empty()) {
|
|
gridWidth = 0;
|
|
gridDepth = 0;
|
|
return;
|
|
}
|
|
|
|
minX = areas.front().polygon.front().x();
|
|
float maxX = minX;
|
|
minZ = areas.front().polygon.front().y();
|
|
float maxZ = minZ;
|
|
|
|
auto includePoint = [&](float x, float z) {
|
|
minX = (std::min)(minX, x);
|
|
maxX = (std::max)(maxX, x);
|
|
minZ = (std::min)(minZ, z);
|
|
maxZ = (std::max)(maxZ, z);
|
|
};
|
|
|
|
for (const NavigationArea& area : areas) {
|
|
for (const Eigen::Vector2f& point : area.polygon) {
|
|
includePoint(point.x(), point.y());
|
|
}
|
|
}
|
|
|
|
for (const ObstacleMesh& obstacle : obstacles) {
|
|
if (!obstacle.mesh) {
|
|
continue;
|
|
}
|
|
for (const Eigen::Vector3f& point : obstacle.mesh->PositionData) {
|
|
const Eigen::Vector3f worldPoint = point + obstacle.offset;
|
|
includePoint(worldPoint.x(), worldPoint.z());
|
|
}
|
|
}
|
|
|
|
const float padding = cellSize * 2.0f + agentRadius + objectPadding;
|
|
minX -= padding;
|
|
minZ -= padding;
|
|
maxX += padding;
|
|
maxZ += padding;
|
|
|
|
gridWidth = static_cast<int>(std::ceil((maxX - minX) / cellSize));
|
|
gridDepth = static_cast<int>(std::ceil((maxZ - minZ) / cellSize));
|
|
}
|
|
|
|
void PathFinder::rebuildWalkableGrid()
|
|
{
|
|
walkable.assign(static_cast<size_t>(gridWidth * gridDepth), 0);
|
|
markAvailableAreasWalkable();
|
|
markObstacleMeshesBlocked();
|
|
}
|
|
|
|
void PathFinder::markAvailableAreasWalkable()
|
|
{
|
|
for (int z = 0; z < gridDepth; ++z) {
|
|
for (int x = 0; x < gridWidth; ++x) {
|
|
const Cell cell{ x, z };
|
|
const Eigen::Vector3f center = cellCenter(cell);
|
|
|
|
for (const NavigationArea& area : areas) {
|
|
if (!area.available) {
|
|
continue;
|
|
}
|
|
if (pointInPolygon(center.x(), center.z(), area.polygon)) {
|
|
walkable[static_cast<size_t>(indexOf(cell))] = 1;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void PathFinder::markObstacleMeshesBlocked()
|
|
{
|
|
const float padding = agentRadius + objectPadding;
|
|
|
|
for (const ObstacleMesh& obstacle : obstacles) {
|
|
if (!obstacle.mesh || obstacle.mesh->PositionData.empty()) {
|
|
continue;
|
|
}
|
|
|
|
Eigen::Vector3f minPoint = obstacle.mesh->PositionData[0] + obstacle.offset;
|
|
Eigen::Vector3f maxPoint = minPoint;
|
|
for (const Eigen::Vector3f& localPoint : obstacle.mesh->PositionData) {
|
|
const Eigen::Vector3f point = localPoint + obstacle.offset;
|
|
minPoint = minPoint.cwiseMin(point);
|
|
maxPoint = maxPoint.cwiseMax(point);
|
|
}
|
|
|
|
Cell from;
|
|
Cell to;
|
|
worldToCell({ minPoint.x() - padding, floorY, minPoint.z() - padding }, from);
|
|
worldToCell({ maxPoint.x() + padding, floorY, maxPoint.z() + padding }, to);
|
|
from.x = (std::max)(from.x, 0);
|
|
from.z = (std::max)(from.z, 0);
|
|
to.x = (std::min)(to.x, gridWidth - 1);
|
|
to.z = (std::min)(to.z, gridDepth - 1);
|
|
|
|
for (int z = from.z; z <= to.z; ++z) {
|
|
for (int x = from.x; x <= to.x; ++x) {
|
|
const Cell cell{ x, z };
|
|
if (isInsideGrid(cell)) {
|
|
walkable[static_cast<size_t>(indexOf(cell))] = 0;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
bool PathFinder::worldToCell(const Eigen::Vector3f& point, Cell& out) const
|
|
{
|
|
if (gridWidth <= 0 || gridDepth <= 0) {
|
|
out = {};
|
|
return false;
|
|
}
|
|
|
|
out.x = static_cast<int>(std::floor((point.x() - minX) / cellSize));
|
|
out.z = static_cast<int>(std::floor((point.z() - minZ) / cellSize));
|
|
return isInsideGrid(out);
|
|
}
|
|
|
|
Eigen::Vector3f PathFinder::cellCenter(const Cell& cell) const
|
|
{
|
|
return {
|
|
minX + (static_cast<float>(cell.x) + 0.5f) * cellSize,
|
|
floorY,
|
|
minZ + (static_cast<float>(cell.z) + 0.5f) * cellSize
|
|
};
|
|
}
|
|
|
|
bool PathFinder::isInsideGrid(const Cell& cell) const
|
|
{
|
|
return cell.x >= 0 && cell.z >= 0 && cell.x < gridWidth && cell.z < gridDepth;
|
|
}
|
|
|
|
bool PathFinder::isCellWalkable(const Cell& cell) const
|
|
{
|
|
return isInsideGrid(cell) && walkable[static_cast<size_t>(indexOf(cell))] != 0;
|
|
}
|
|
|
|
int PathFinder::indexOf(const Cell& cell) const
|
|
{
|
|
return cell.z * gridWidth + cell.x;
|
|
}
|
|
|
|
bool PathFinder::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out) const
|
|
{
|
|
Cell origin;
|
|
if (!worldToCell(point, origin)) {
|
|
const int x = static_cast<int>(std::floor((point.x() - minX) / cellSize));
|
|
const int z = static_cast<int>(std::floor((point.z() - minZ) / cellSize));
|
|
origin.x = (std::min)((std::max)(x, 0), gridWidth - 1);
|
|
origin.z = (std::min)((std::max)(z, 0), gridDepth - 1);
|
|
}
|
|
|
|
if (isCellWalkable(origin)) {
|
|
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 dz = -radius; dz <= radius; ++dz) {
|
|
for (int dx = -radius; dx <= radius; ++dx) {
|
|
if (std::abs(dx) != radius && std::abs(dz) != radius) {
|
|
continue;
|
|
}
|
|
|
|
Cell candidate{ origin.x + dx, origin.z + dz };
|
|
if (isCellWalkable(candidate)) {
|
|
out = candidate;
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
bool PathFinder::hasLineOfSight(const Cell& from, const Cell& to) const
|
|
{
|
|
const Eigen::Vector3f a = cellCenter(from);
|
|
const Eigen::Vector3f b = cellCenter(to);
|
|
const float distance = (b - a).norm();
|
|
const int steps = (std::max)(1, static_cast<int>(std::ceil(distance / (cellSize * 0.5f))));
|
|
|
|
for (int i = 0; i <= steps; ++i) {
|
|
const float t = static_cast<float>(i) / static_cast<float>(steps);
|
|
const Eigen::Vector3f p = a * (1.0f - t) + b * t;
|
|
Cell cell;
|
|
if (!worldToCell(p, cell) || !isCellWalkable(cell)) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
std::vector<PathFinder::Cell> PathFinder::smoothCells(const std::vector<Cell>& cells) const
|
|
{
|
|
if (cells.size() <= 2) {
|
|
return cells;
|
|
}
|
|
|
|
std::vector<Cell> result;
|
|
size_t anchor = 0;
|
|
result.push_back(cells[anchor]);
|
|
|
|
while (anchor < cells.size() - 1) {
|
|
size_t next = cells.size() - 1;
|
|
while (next > anchor + 1 && !hasLineOfSight(cells[anchor], cells[next])) {
|
|
--next;
|
|
}
|
|
|
|
result.push_back(cells[next]);
|
|
anchor = next;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
bool PathFinder::pointInPolygon(float x, float z, const std::vector<Eigen::Vector2f>& polygon)
|
|
{
|
|
bool inside = false;
|
|
size_t previous = polygon.size() - 1;
|
|
|
|
for (size_t current = 0; current < polygon.size(); ++current) {
|
|
const Eigen::Vector2f& a = polygon[current];
|
|
const Eigen::Vector2f& b = polygon[previous];
|
|
|
|
const bool crossesZ = (a.y() > z) != (b.y() > z);
|
|
if (crossesZ) {
|
|
const float t = (z - a.y()) / (b.y() - a.y());
|
|
const float intersectionX = a.x() + t * (b.x() - a.x());
|
|
if (x < intersectionX) {
|
|
inside = !inside;
|
|
}
|
|
}
|
|
|
|
previous = current;
|
|
}
|
|
|
|
return inside;
|
|
}
|
|
|
|
} // namespace ZL
|