Merge pull request #1071 from MihailRis/pathfinding-update

pathfinding update
This commit is contained in:
MihailRis 2026-11-01 19:08:18 +03:00 • committed by GitHub
commit c2777eba09
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
6 changed files with 399 additions and 193 deletions

View file

@ -55,6 +55,12 @@ pathfinding.pull_route(agent: int) --> table<vec3> or nil
--- Set the maximum number of visited blocks for the agent. Used to limit the amount of work of the pathfinding algorithm.
pathfinding.set_max_visited(agent: int, max_visited: int)
--- Sets the agent's width in blocks. Defaults to 0.6
pathfinding.set_width(agent: int, width: number)
--- Sets the agent's height in blocks. Defaults to 2
pathfinding.set_height(agent: int, height: int)
--- Adding an avoided blocks tag
pathfinding.avoid_tag(
agent: int,

View file

@ -55,6 +55,12 @@ pathfinding.pull_route(agent: int) -> table<vec3> или nil
--- Установка максимального количества посещенных блоков для агента. Используется для ограничения объема работы алгоритма поиска пути.
pathfinding.set_max_visited(agent: int, max_visited: int)
--- Установка ширины агента в блоках. По-умолчанию 0.6
pathfinding.set_width(agent: int, width: number)
--- Установка высоты агента в блоках. По-умолчанию 2
pathfinding.set_height(agent: int, width: int)
--- Добавление тега избегаемых блоков
pathfinding.avoid_tag(
agent: int,
@ -63,4 +69,4 @@ pathfinding.avoid_tag(
-- стоимость пересечения блока
cost: int = 10
)
```
```

View file

@ -5,8 +5,11 @@ local started
local tsf = entity.transform
local body = entity.rigidbody
local size = body:get_size()
agent = pathfinding.create_agent()
pathfinding.set_max_visited(agent, 1e3)
pathfinding.set_width(agent, math.max(size[1], size[3]))
pathfinding.set_height(agent, math.ceil(size[2]))
pathfinding.avoid_tag(agent, "core:liquid", 8)
function set_target(new_target)

View file

@ -100,6 +100,20 @@ static int l_set_jump_height(lua::State* L) {
return 0;
}
static int l_set_width(lua::State* L) {
if (auto agent = get_agent(L)) {
agent->width = lua::tonumber(L, 2);
}
return 0;
}
static int l_set_height(lua::State* L) {
if (auto agent = get_agent(L)) {
agent->height = lua::touinteger(L, 2);
}
return 0;
}
static int l_avoid_tag(lua::State* L) {
if (auto agent = get_agent(L)) {
int index =
@ -125,6 +139,8 @@ const luaL_Reg pathfindinglib[] = {
{"pull_route", lua::wrap<l_pull_route>},
{"set_max_visited", lua::wrap<l_set_max_visited_blocks>},
{"set_jump_height", lua::wrap<l_set_jump_height>},
{"set_width", lua::wrap<l_set_width>},
{"set_height", lua::wrap<l_set_height>},
{"avoid_tag", lua::wrap<l_avoid_tag>},
{nullptr, nullptr}
};

View file

@ -1,62 +1,336 @@
#include "Pathfinding.hpp"
#include "constants.hpp"
#include "content/Content.hpp"
#include "maths/voxmaths.hpp"
#include "voxels/Block.hpp"
#include "voxels/Chunk.hpp"
#include "voxels/GlobalChunks.hpp"
#include "voxels/blocks_agent.hpp"
#include "world/Level.hpp"
inline constexpr float SQRT2 = 1.4142135623730951f; // sqrt(2)
#include <algorithm>
#include <cmath>
#include <iterator>
#include <limits>
static constexpr float SQRT2 = 1.4142135623730951f; // sqrt(2)
static constexpr int16_t SOLID = -1;
static constexpr int16_t UNKNOWN = std::numeric_limits<int16_t>::min();
static constexpr int NO_SURFACE = std::numeric_limits<int>::min();
static constexpr int MAX_DROP = 1;
static constexpr float SEGMENT_SAMPLE_STEP = 0.2f;
static constexpr size_t MAX_SEGMENT_NODES = 32;
using namespace voxels;
static const glm::ivec2 NEIGHBORS[] {
{0, 1}, {1, 0}, {0, -1}, {-1, 0},
{-1, -1}, {1, -1}, {1, 1}, {-1, 1},
};
static float heuristic(const glm::ivec3& a, const glm::ivec3& b) {
return glm::distance(glm::vec3(a), glm::vec3(b));
}
class BlocksSampler {
public:
BlocksSampler(
const GlobalChunks& chunks,
const ContentUnitIndices<Block, blockid_t>& defs,
const Agent& agent,
std::vector<int16_t>& costs
)
: height(std::max(agent.height, 1)),
radius(static_cast<int>(
std::ceil(std::max(agent.width - 1.0f, 0.0f) * 0.5f - 1e-4f)
)),
halfWidth(std::max(agent.width * 0.5f, 0.05f)),
jumpHeight(std::max(agent.jumpHeight, 0)),
chunks(chunks),
defs(defs),
agent(agent),
costs(costs) {
if (costs.size() != defs.count()) {
costs.assign(defs.count(), UNKNOWN);
}
}
int16_t at(int x, int y, int z) {
if (y < 0) {
return SOLID;
}
if (y >= CHUNK_H) {
return 0;
}
int cx = floordiv<CHUNK_W>(x);
int cz = floordiv<CHUNK_D>(z);
if (!hasChunk || cx != chunkX || cz != chunkZ) {
chunk = chunks.getChunk(cx, cz);
chunkX = cx;
chunkZ = cz;
hasChunk = true;
}
if (chunk == nullptr) {
return SOLID;
}
int lx = x - cx * CHUNK_W;
int lz = z - cz * CHUNK_D;
blockid_t id = chunk->voxels[(y * CHUNK_D + lz) * CHUNK_W + lx].id;
if (id >= costs.size()) {
return SOLID;
}
auto& cost = costs[id];
if (cost == UNKNOWN) {
cost = classify(id);
}
return cost;
}
int16_t getMaxCostInBox(int x0, int z0, int x1, int z1, int y0, int y1) {
int16_t result = 0;
for (int y = y0; y < y1; y++) {
for (int z = z0; z <= z1; z++) {
for (int x = x0; x <= x1; x++) {
int16_t cost = at(x, y, z);
if (cost == SOLID) {
return SOLID;
}
result = std::max(result, cost);
}
}
}
return result;
}
int16_t getFootprintCost(int x, int z, int y0, int y1) {
return getMaxCostInBox(
x - radius, z - radius, x + radius, z + radius, y0, y1
);
}
int16_t getStandingCost(const glm::ivec3& pos) {
return getFootprintCost(pos.x, pos.z, pos.y, pos.y + height);
}
int findSurfaceHeight(int x, int y, int z) {
if (at(x, y, z) == SOLID) {
for (int i = 1; i <= jumpHeight; i++) {
if (at(x, y + i, z) != SOLID) {
return y + i;
}
}
return NO_SURFACE;
}
for (int i = 0; i <= MAX_DROP; i++) {
if (at(x, y - i - 1, z) == SOLID) {
return y - i;
}
}
return NO_SURFACE;
}
int16_t getMovementCost(const glm::ivec3& from, const glm::ivec3& to) {
int top = std::max(from.y, to.y) + height;
int16_t cost = getFootprintCost(to.x, to.z, to.y, top);
if (cost == SOLID) {
return SOLID;
}
if (to.y > from.y &&
getFootprintCost(from.x, from.z, from.y + height, top) == SOLID) {
return SOLID;
}
int dx = to.x - from.x;
int dz = to.z - from.z;
if (dx == 0 || dz == 0) {
return cost;
}
int bottom = std::min(from.y, to.y);
int ax = from.x + dx * (radius + 1);
int az = from.z - dz * radius;
int bx = from.x - dx * radius;
int bz = from.z + dz * (radius + 1);
int16_t a = getMaxCostInBox(ax, az, ax, az, bottom, top);
int16_t b = getMaxCostInBox(bx, bz, bx, bz, bottom, top);
if (a == SOLID || b == SOLID) {
return SOLID;
}
return std::max({cost, a, b});
}
int16_t getLineCost(glm::vec2 a, glm::vec2 b, int y0, int y1) {
auto min = [this](float a, float b) {
return static_cast<int>(std::floor(std::min(a, b) - halfWidth));
};
auto max = [this](float a, float b) {
return static_cast<int>(std::ceil(std::max(a, b) + halfWidth));
};
int x0 = min(a.x, b.x);
int z0 = min(a.y, b.y);
int x1 = max(a.x, b.x);
int z1 = max(a.y, b.y);
return getMaxCostInBox(
x0, z0, std::max(x0, x1 - 1), std::max(z0, z1 - 1), y0, y1
);
}
bool canWalkDirectly(
const glm::ivec3& a, const glm::ivec3& b, int16_t maxCost
) {
if (b.y > a.y) {
return false;
}
glm::vec2 from(a.x + 0.5f, a.z + 0.5f);
glm::vec2 to(b.x + 0.5f, b.z + 0.5f);
int steps = std::max(
1,
static_cast<int>(
std::ceil(glm::distance(from, to) / SEGMENT_SAMPLE_STEP)
)
);
int cx = a.x;
int cz = a.z;
int y = a.y;
glm::vec2 prev = from;
auto isBlocked = [&](glm::vec2 p, int y0, int y1) {
int16_t cost = getLineCost(prev, p, y0, y1);
return cost == SOLID || cost > maxCost;
};
for (int i = 1; i <= steps; i++) {
float t = i / static_cast<float>(steps);
glm::vec2 p = from + (to - from) * t;
int px = static_cast<int>(std::floor(p.x));
int pz = static_cast<int>(std::floor(p.y));
int top = y + height;
if (px != cx || pz != cz) {
int ny = findSurfaceHeight(px, y, pz);
if (ny == NO_SURFACE || ny > y) {
return false;
}
if (ny < y) {
if (isBlocked(p, y, top)) {
return false;
}
prev = p;
}
cx = px;
cz = pz;
y = ny;
}
if (isBlocked(p, y, top)) {
return false;
}
prev = p;
}
return y == b.y;
}
std::vector<glm::ivec3> simplifyPath(
const std::vector<glm::ivec3>& path
) {
if (path.size() <= 2) {
return path;
}
std::vector<int16_t> nodeCosts(path.size());
for (size_t i = 0; i < path.size(); i++) {
nodeCosts[i] = std::max<int16_t>(getStandingCost(path[i]), 0);
}
auto isDirect = [&](size_t from, size_t to) {
int16_t maxCost = std::max(nodeCosts[from], nodeCosts[to]);
return canWalkDirectly(path[from], path[to], maxCost);
};
const size_t last = path.size() - 1;
std::vector<glm::ivec3> result {path.front()};
size_t anchor = 0;
while (anchor < last) {
size_t limit = std::min(last, anchor + MAX_SEGMENT_NODES);
size_t good = anchor + 1;
size_t bad = limit + 1;
for (size_t step = 2; good < limit; step *= 2) {
size_t next = std::min(anchor + step, limit);
if (!isDirect(anchor, next)) {
bad = next;
break;
}
good = next;
}
while (bad - good > 1) {
size_t mid = (good + bad) / 2;
(isDirect(anchor, mid) ? good : bad) = mid;
}
result.push_back(path[good]);
anchor = good;
}
return result;
}
const int height;
const int radius;
const float halfWidth;
const int jumpHeight;
private:
const GlobalChunks& chunks;
const ContentUnitIndices<Block, blockid_t>& defs;
const Agent& agent;
std::vector<int16_t>& costs;
const Chunk* chunk = nullptr;
int chunkX = 0;
int chunkZ = 0;
bool hasChunk = false;
int16_t classify(blockid_t id) const {
const auto& def = defs.require(id);
if (def.obstacle) {
return SOLID;
}
int cost = 0;
for (const auto& [tag, tagCost] : agent.avoidTags) {
if (def.rt.tags.find(tag) != def.rt.tags.end()) {
cost = std::max(cost, tagCost);
}
}
return static_cast<int16_t>(
std::clamp(cost, 0, static_cast<int>(INT16_MAX))
);
}
};
Pathfinding::Pathfinding(const Level& level)
: level(level),
chunks(*level.chunks),
blockDefs(level.content.getIndices()->blocks) {
}
static bool check_passability(
const Agent& agent,
const GlobalChunks& chunks,
const Node& node,
const glm::ivec2& offset,
bool diagonal
static Route finish_route(
Agent& agent, BlocksSampler& sampler, const glm::ivec3& last, bool found
) {
if (!diagonal) {
return true;
}
auto a = node.pos + glm::ivec3(offset.x, 0, 0);
auto b = node.pos + glm::ivec3(0, 0, offset.y);
for (int i = 0; i < agent.height; i++) {
if (blocks_agent::is_obstacle_at(chunks, a.x, a.y + i, a.z))
return false;
if (blocks_agent::is_obstacle_at(chunks, b.x, b.y + i, b.z))
return false;
}
return true;
}
static void restore_route(
Route& route,
const glm::ivec3& lastPos,
const std::unordered_map<glm::ivec3, Node>& parents
) {
auto pos = lastPos;
while (true) {
const auto& found = parents.find(pos);
if (found == parents.end()) {
route.nodes.push_back({pos});
auto& state = agent.state;
std::vector<glm::ivec3> path;
for (auto pos = last;;) {
path.push_back(pos);
const auto& visit = state.visits.find(pos);
if (visit == state.visits.end() || visit->second.parent == pos) {
break;
}
route.nodes.push_back({pos});
pos = found->second.pos;
pos = visit->second.parent;
}
std::reverse(path.begin(), path.end());
path = sampler.simplifyPath(path);
Route route {};
route.found = found;
route.totalVisited = state.closedCount;
route.nodes.reserve(path.size());
for (auto it = path.rbegin(); it != path.rend(); ++it) {
route.nodes.push_back({*it});
}
state.finished = true;
state.queue = {};
state.visits = {};
agent.route = route;
return route;
}
int Pathfinding::createAgent() {
@ -83,124 +357,77 @@ void Pathfinding::performAllAsync(int stepsPerAgent) {
}
}
static Route finish_route(Agent& agent, State&& state) {
Route route {};
restore_route(route, state.nearest, state.parents);
route.totalVisited = state.blocked.size();
route.nodes.push_back({agent.start});
route.found = true;
state.finished = true;
agent.state = std::move(state);
agent.route = route;
return route;
}
enum Passability {
NON_PASSABLE = -1,
OBSTACLE = 0,
PASSABLE = 1,
};
Route Pathfinding::perform(Agent& agent, int maxVisited) {
using namespace blocks_agent;
auto& state = agent.state;
BlocksSampler sampler(chunks, blockDefs, agent, state.blockCosts);
State state = std::move(agent.state);
if (state.queue.empty()) {
state.queue.push(
{agent.start, {}, 0, heuristic(agent.start, agent.target)}
);
}
const auto& chunks = *level.chunks;
int height = std::max(agent.height, 1);
if (state.nearest == glm::ivec3(0)) {
if (state.visits.empty()) {
float hScore = heuristic(agent.start, agent.target);
state.queue.push({agent.start, 0.0f, hScore});
state.visits[agent.start] = {agent.start, 0.0f, false};
state.closedCount = 0;
state.nearest = agent.start;
state.minHScore = heuristic(agent.start, agent.target);
state.minHScore = hScore;
}
int visited = -1;
int visited = 0;
while (!state.queue.empty()) {
if (state.blocked.size() == agent.maxVisitedBlocks) {
if (agent.mayBeIncomplete) {
return finish_route(agent, std::move(state));
}
break;
if (state.closedCount >= agent.maxVisitedBlocks) {
return finish_route(
agent, sampler, state.nearest, agent.mayBeIncomplete
);
}
visited++;
if (visited == maxVisited) {
if (visited++ == maxVisited) {
state.finished = false;
agent.state = std::move(state);
return {};
}
auto node = state.queue.top();
state.queue.pop();
if (node.pos.x == agent.target.x &&
glm::abs((node.pos.y - agent.target.y) / height) == 0 &&
node.pos.z == agent.target.z) {
return finish_route(agent, std::move(state));
auto& current = state.visits[node.pos];
if (current.closed) {
continue;
}
current.closed = true;
state.closedCount++;
float gScore = current.gScore;
if (node.pos.x == agent.target.x && node.pos.z == agent.target.z &&
glm::abs(node.pos.y - agent.target.y) < sampler.height) {
return finish_route(agent, sampler, node.pos, true);
}
state.blocked.emplace(node.pos);
glm::ivec2 neighbors[8] {
{0, 1},
{1, 0},
{0, -1},
{-1, 0},
{-1, -1},
{1, -1},
{1, 1},
{-1, 1},
};
for (int i = 0; i < sizeof(neighbors) / sizeof(glm::ivec2); i++) {
auto offset = neighbors[i];
auto pos = node.pos;
float cost = glm::abs(node.pos.y - pos.y) * 10;
int surface = getSurfaceAt(
agent, pos + glm::ivec3(offset.x, 0, offset.y), 1, cost
);
if (surface == NON_PASSABLE) {
for (size_t i = 0; i < std::size(NEIGHBORS); i++) {
const auto& offset = NEIGHBORS[i];
glm::ivec3 point(node.pos.x + offset.x, 0, node.pos.z + offset.y);
point.y = sampler.findSurfaceHeight(point.x, node.pos.y, point.z);
if (point.y == NO_SURFACE) {
continue;
}
pos.y = surface;
auto point = pos + glm::ivec3(offset.x, 0, offset.y);
if (state.blocked.find(point) != state.blocked.end()) {
auto found = state.visits.find(point);
if (found != state.visits.end() && found->second.closed) {
continue;
}
if (is_obstacle_at(
chunks, pos.x, pos.y + agent.jumpHeight, pos.z
)) {
int16_t cost = sampler.getMovementCost(node.pos, point);
if (cost == SOLID) {
continue;
}
if (!check_passability(agent, chunks, node, offset, i >= 4)) {
float nextGScore = gScore + (i >= 4 ? SQRT2 : 1.0f) + cost;
if (found != state.visits.end() &&
found->second.gScore <= nextGScore) {
continue;
}
float sum = glm::abs(offset.x) + glm::abs(offset.y);
float gScore = node.gScore + sum + cost;
const auto& found = state.parents.find(point);
if (found == state.parents.end()) {
float hScore = heuristic(point, agent.target);
if (hScore < state.minHScore) {
state.minHScore = hScore;
state.nearest = point;
}
float fScore = gScore * 0.75f + hScore;
Node nNode {point, node.pos, gScore, fScore};
state.parents[point] = node;
state.queue.push(nNode);
float hScore = heuristic(point, agent.target);
if (hScore < state.minHScore) {
state.minHScore = hScore;
state.nearest = point;
}
state.visits[point] = {node.pos, nextGScore, false};
state.queue.push({point, nextGScore, nextGScore * 0.75f + hScore});
}
}
state.finished = true;
agent.state = std::move(state);
return finish_route(agent, std::move(agent.state));
return finish_route(agent, sampler, state.nearest, agent.mayBeIncomplete);
}
Agent* Pathfinding::getAgent(int id) {
@ -214,56 +441,3 @@ Agent* Pathfinding::getAgent(int id) {
const std::unordered_map<int, Agent>& Pathfinding::getAgents() const {
return agents;
}
int Pathfinding::checkPoint(const Agent& agent, int x, int y, int z, int& cost) {
auto vox = blocks_agent::get(chunks, x, y, z);
if (vox == nullptr) {
return OBSTACLE;
}
const auto& def = blockDefs.require(vox->id);
if (def.obstacle) {
return OBSTACLE;
}
for (const auto& pair : agent.avoidTags) {
if (def.rt.tags.find(pair.first) != def.rt.tags.end()) {
cost = pair.second;
return NON_PASSABLE;
}
}
return PASSABLE;
}
int Pathfinding::getSurfaceAt(
const Agent& agent, const glm::ivec3& pos, int maxDelta, float& cost
) {
using namespace blocks_agent;
int status;
int surface = pos.y;
int ncost = 0;
if ((status = checkPoint(agent, pos.x, surface, pos.z, ncost)) == OBSTACLE) {
if ((status = checkPoint(agent, pos.x, surface + 1, pos.z, ncost)) == OBSTACLE) {
return NON_PASSABLE;
} else if (status == NON_PASSABLE) {
cost += 5;
}
cost += ncost;
return surface + 1;
} else {
if (status == NON_PASSABLE) {
cost += 5;
}
if ((status = checkPoint(agent, pos.x, surface - 1, pos.z, ncost)) == OBSTACLE) {
cost += ncost;
return surface;
} else if (status == NON_PASSABLE) {
cost += 5;
}
if ((status = checkPoint(agent, pos.x, surface - 2, pos.z, ncost)) == OBSTACLE) {
cost += ncost;
return surface - 1;
}
return NON_PASSABLE;
}
return NON_PASSABLE;
}

View file

@ -4,11 +4,11 @@
#include <glm/gtx/hash.hpp>
#include <glm/vec2.hpp>
#include <glm/vec3.hpp>
#include <cstdint>
#include <memory>
#include <queue>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <vector>
#include "typedefs.hpp"
@ -26,14 +26,13 @@ namespace voxels {
};
struct Route {
bool found;
std::vector<RouteNode> nodes;
int totalVisited;
bool found;
};
struct Node {
glm::ivec3 pos;
glm::ivec3 parent;
float gScore;
float fScore;
};
@ -44,12 +43,19 @@ namespace voxels {
}
};
struct Visit {
glm::ivec3 parent;
float gScore;
bool closed;
};
struct State {
std::priority_queue<Node, std::vector<Node>, NodeLess> queue;
std::unordered_set<glm::ivec3> blocked;
std::unordered_map<glm::ivec3, Node> parents;
glm::ivec3 nearest;
float minHScore;
std::unordered_map<glm::ivec3, Visit> visits;
std::vector<int16_t> blockCosts;
int closedCount = 0;
glm::ivec3 nearest {};
float minHScore = 0.0f;
bool finished = true;
};
@ -57,6 +63,7 @@ namespace voxels {
bool enabled = false;
bool mayBeIncomplete = true;
int height = 2;
float width = 0.6f;
int jumpHeight = 1;
int maxVisitedBlocks = 1e3;
glm::ivec3 start;
@ -87,11 +94,5 @@ namespace voxels {
const ContentUnitIndices<Block, blockid_t>& blockDefs;
std::unordered_map<int, Agent> agents;
int nextAgent = 1;
int getSurfaceAt(
const Agent& agent, const glm::ivec3& pos, int maxDelta, float& cost
);
int checkPoint(const Agent& agent, int x, int y, int z, int& cost);
};
}