diff --git a/doc/en/scripting/builtins/libpathfinding.md b/doc/en/scripting/builtins/libpathfinding.md index 19562177b..5976a9e49 100644 --- a/doc/en/scripting/builtins/libpathfinding.md +++ b/doc/en/scripting/builtins/libpathfinding.md @@ -55,6 +55,12 @@ pathfinding.pull_route(agent: int) --> table 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, diff --git a/doc/ru/scripting/builtins/libpathfinding.md b/doc/ru/scripting/builtins/libpathfinding.md index 3d0a07e2e..eb991730e 100644 --- a/doc/ru/scripting/builtins/libpathfinding.md +++ b/doc/ru/scripting/builtins/libpathfinding.md @@ -55,6 +55,12 @@ pathfinding.pull_route(agent: int) -> table или 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 ) -``` \ No newline at end of file +``` diff --git a/res/scripts/components/pathfinding.lua b/res/scripts/components/pathfinding.lua index fd5990280..bda08ae5a 100644 --- a/res/scripts/components/pathfinding.lua +++ b/res/scripts/components/pathfinding.lua @@ -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) diff --git a/src/logic/scripting/lua/libs/libpathfinding.cpp b/src/logic/scripting/lua/libs/libpathfinding.cpp index ae880912d..d702633e3 100644 --- a/src/logic/scripting/lua/libs/libpathfinding.cpp +++ b/src/logic/scripting/lua/libs/libpathfinding.cpp @@ -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}, {"set_max_visited", lua::wrap}, {"set_jump_height", lua::wrap}, + {"set_width", lua::wrap}, + {"set_height", lua::wrap}, {"avoid_tag", lua::wrap}, {nullptr, nullptr} }; diff --git a/src/voxels/Pathfinding.cpp b/src/voxels/Pathfinding.cpp index 999711f0b..7a3285b79 100644 --- a/src/voxels/Pathfinding.cpp +++ b/src/voxels/Pathfinding.cpp @@ -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 +#include +#include +#include + +static constexpr float SQRT2 = 1.4142135623730951f; // sqrt(2) +static constexpr int16_t SOLID = -1; +static constexpr int16_t UNKNOWN = std::numeric_limits::min(); +static constexpr int NO_SURFACE = std::numeric_limits::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& defs, + const Agent& agent, + std::vector& costs + ) + : height(std::max(agent.height, 1)), + radius(static_cast( + 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(x); + int cz = floordiv(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(std::floor(std::min(a, b) - halfWidth)); + }; + auto max = [this](float a, float b) { + return static_cast(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( + 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(steps); + glm::vec2 p = from + (to - from) * t; + int px = static_cast(std::floor(p.x)); + int pz = static_cast(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 simplifyPath( + const std::vector& path + ) { + if (path.size() <= 2) { + return path; + } + std::vector nodeCosts(path.size()); + for (size_t i = 0; i < path.size(); i++) { + nodeCosts[i] = std::max(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 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& defs; + const Agent& agent; + std::vector& 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( + std::clamp(cost, 0, static_cast(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& 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 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& 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; -} diff --git a/src/voxels/Pathfinding.hpp b/src/voxels/Pathfinding.hpp index 2e079076e..d81b51bee 100644 --- a/src/voxels/Pathfinding.hpp +++ b/src/voxels/Pathfinding.hpp @@ -4,11 +4,11 @@ #include #include #include +#include #include #include #include #include -#include #include #include "typedefs.hpp" @@ -26,14 +26,13 @@ namespace voxels { }; struct Route { - bool found; std::vector 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, NodeLess> queue; - std::unordered_set blocked; - std::unordered_map parents; - glm::ivec3 nearest; - float minHScore; + std::unordered_map visits; + std::vector 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& blockDefs; std::unordered_map 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); }; }