diff --git a/core/include/vf/Pathfind.h b/core/include/vf/Pathfind.h new file mode 100644 index 0000000..3eb5c1c --- /dev/null +++ b/core/include/vf/Pathfind.h @@ -0,0 +1,26 @@ +#pragma once + +#include +#include "World.h" +#include "Math.h" + +namespace vf { + +struct PathConfig { + int mobHeight = 2; // vertical voxels the walker occupies + int maxStepUp = 1; // how high a single step can climb + int maxDrop = 3; // how far a single step can fall + int maxExpansions = 40000; // A* node budget before giving up +}; + +// A cell is standable when the walker's body fits (non-solid) and there is solid +// ground directly beneath it. +bool isStandable(const World& world, const Vec3i& cell, const PathConfig& config = {}); + +// A* over standable voxel cells. Returns the cell path from start to goal +// inclusive, or an empty path when the goal is unreachable, not standable, or the +// search exceeds the node budget. Deterministic: ties break on a fixed ordering. +std::vector findPath(const World& world, const Vec3i& start, const Vec3i& goal, + const PathConfig& config = {}); + +} // namespace vf diff --git a/core/src/Pathfind.cpp b/core/src/Pathfind.cpp new file mode 100644 index 0000000..2de0b7f --- /dev/null +++ b/core/src/Pathfind.cpp @@ -0,0 +1,108 @@ +#include "vf/Pathfind.h" + +#include +#include +#include +#include + +namespace vf { + +bool isStandable(const World& world, const Vec3i& cell, const PathConfig& config) { + if (world.solidAt(cell.x, cell.y - 1, cell.z)) { + for (int h = 0; h < config.mobHeight; ++h) + if (world.solidAt(cell.x, cell.y + h, cell.z)) return false; + return true; + } + return false; +} + +namespace { + +const int horiz[4][2] = {{1, 0}, {-1, 0}, {0, 1}, {0, -1}}; + +void successors(const World& world, const Vec3i& cell, const PathConfig& config, + std::vector& out) { + for (auto& dir : horiz) { + int nx = cell.x + dir[0], nz = cell.z + dir[1]; + for (int dy = config.maxStepUp; dy >= -config.maxDrop; --dy) { + Vec3i cand{nx, cell.y + dy, nz}; + if (!isStandable(world, cand, config)) continue; + if (dy > 0 && world.solidAt(cell.x, cell.y + config.mobHeight, cell.z)) + continue; // ceiling blocks the step up + out.push_back(cand); + } + } +} + +int heuristic(const Vec3i& a, const Vec3i& b) { + return std::abs(a.x - b.x) + std::abs(a.z - b.z); +} + +struct Frontier { + int f; + int order; + Vec3i cell; +}; + +struct FrontierGreater { + bool operator()(const Frontier& a, const Frontier& b) const { + if (a.f != b.f) return a.f > b.f; + return a.order > b.order; // stable tie-break by insertion order + } +}; + +} // namespace + +std::vector findPath(const World& world, const Vec3i& start, const Vec3i& goal, + const PathConfig& config) { + if (!isStandable(world, start, config) || !isStandable(world, goal, config)) return {}; + if (start == goal) return {start}; + + std::priority_queue, FrontierGreater> open; + std::unordered_map gScore; + std::unordered_map cameFrom; + + gScore[start] = 0; + int order = 0; + open.push({heuristic(start, goal), order++, start}); + + int expansions = 0; + std::vector neighbours; + + while (!open.empty()) { + Frontier current = open.top(); + open.pop(); + + auto known = gScore.find(current.cell); + if (known == gScore.end() || current.f - heuristic(current.cell, goal) > known->second) + continue; // stale entry + + if (current.cell == goal) { + std::vector path; + Vec3i step = goal; + path.push_back(step); + while (step != start) { + step = cameFrom[step]; + path.push_back(step); + } + std::reverse(path.begin(), path.end()); + return path; + } + + if (++expansions > config.maxExpansions) return {}; + + neighbours.clear(); + successors(world, current.cell, config, neighbours); + int nextG = known->second + 1; + for (const Vec3i& next : neighbours) { + auto it = gScore.find(next); + if (it != gScore.end() && it->second <= nextG) continue; + gScore[next] = nextG; + cameFrom[next] = current.cell; + open.push({nextG + heuristic(next, goal), order++, next}); + } + } + return {}; +} + +} // namespace vf