diff --git a/src/graphics/render/WorldRenderer.cpp b/src/graphics/render/WorldRenderer.cpp
index d7290998..149f5766 100644
--- a/src/graphics/render/WorldRenderer.cpp
+++ b/src/graphics/render/WorldRenderer.cpp
@@ -387,33 +387,42 @@ void WorldRenderer::renderFrame(
         skybox->draw(ctx, camera, assets, worldInfo.daytime, clouds);
 
         // In-world lines
-        for (const auto& [_, agent] : level.pathfinding->getAgents()) {
-            const auto& route = agent.route;
-            if (!route.found)
-                continue;
-            for (int i = 1; i < route.nodes.size(); i++) {
-                const auto& a = route.nodes.at(i - 1);
-                const auto& b = route.nodes.at(i);
+        if (debug) {
+            for (const auto& [_, agent] : level.pathfinding->getAgents()) {
+                const auto& route = agent.route;
+                if (!route.found)
+                    continue;
+                for (const auto& blocked : route.visited) {
+                    lines->pushLine(
+                        glm::vec3(blocked) + glm::vec3(0.5f),
+                        glm::vec3(blocked) + glm::vec3(0.5f, 1.0f, 0.5f),
+                        glm::vec4(1, 0, 0, 1)
+                    );
+                }
+                for (int i = 1; i < route.nodes.size(); i++) {
+                    const auto& a = route.nodes.at(i - 1);
+                    const auto& b = route.nodes.at(i);
+
+                    if (i == 1) {
+                        lines->pushLine(
+                            glm::vec3(a.pos) + glm::vec3(0.5f),
+                            glm::vec3(a.pos) + glm::vec3(0.5f, 1.0f, 0.5f),
+                            glm::vec4(1, 1, 1, 1)
+                        );
+                    }
 
-                if (i == 1) {
                     lines->pushLine(
                         glm::vec3(a.pos) + glm::vec3(0.5f),
-                        glm::vec3(a.pos) + glm::vec3(0.5f, 1.0f, 0.5f),
+                        glm::vec3(b.pos) + glm::vec3(0.5f),
+                        glm::vec4(1, 0, 1, 1)
+                    );
+
+                    lines->pushLine(
+                        glm::vec3(b.pos) + glm::vec3(0.5f),
+                        glm::vec3(b.pos) + glm::vec3(0.5f, 1.0f, 0.5f),
                         glm::vec4(1, 1, 1, 1)
                     );
                 }
-
-                lines->pushLine(
-                    glm::vec3(a.pos) + glm::vec3(0.5f),
-                    glm::vec3(b.pos) + glm::vec3(0.5f),
-                    glm::vec4(1, 0, 1, 1)
-                );
-
-                lines->pushLine(
-                    glm::vec3(b.pos) + glm::vec3(0.5f),
-                    glm::vec3(b.pos) + glm::vec3(0.5f, 1.0f, 0.5f),
-                    glm::vec4(1, 1, 1, 1)
-                );
             }
         }
 
diff --git a/src/logic/scripting/lua/libs/libpathfinding.cpp b/src/logic/scripting/lua/libs/libpathfinding.cpp
index 96ac0d13..e66b1f2a 100644
--- a/src/logic/scripting/lua/libs/libpathfinding.cpp
+++ b/src/logic/scripting/lua/libs/libpathfinding.cpp
@@ -46,10 +46,18 @@ static int l_make_route(lua::State* L) {
     return 0;
 }
 
+static int l_set_max_visited_blocks(lua::State* L) {
+    if (auto agent = get_agent(L)) {
+        agent->maxVisitedBlocks = lua::tointeger(L, 2);
+    }
+    return 0;
+}
+
 const luaL_Reg pathfindinglib[] = {
     {"create_agent", lua::wrap<l_create_agent>},
     {"set_enabled", lua::wrap<l_set_enabled>},
     {"is_enabled", lua::wrap<l_is_enabled>},
     {"make_route", lua::wrap<l_make_route>},
+    {"set_max_visited", lua::wrap<l_set_max_visited_blocks>},
     {NULL, NULL}
 };
diff --git a/src/voxels/Pathfinding.cpp b/src/voxels/Pathfinding.cpp
index 54020cdd..829bbde8 100644
--- a/src/voxels/Pathfinding.cpp
+++ b/src/voxels/Pathfinding.cpp
@@ -1,10 +1,6 @@
 #include "Pathfinding.hpp"
 
-#define GLM_ENABLE_EXPERIMENTAL
-#include <glm/gtx/hash.hpp>
-
 #include <queue>
-#include <unordered_set>
 #include <unordered_map>
 
 #include "world/Level.hpp"
@@ -13,6 +9,8 @@
 #include "voxels/blocks_agent.hpp"
 #include "content/Content.hpp"
 
+inline constexpr float SQRT2 = 1.4142135623730951f; // sqrt(2)
+
 using namespace voxels;
 
 struct Node {
@@ -28,7 +26,7 @@ struct NodeLess {
     }
 };
 
-static float distance(const glm::ivec3& a, const glm::ivec3& b) {
+static float heuristic(const glm::ivec3& a, const glm::ivec3& b) {
     return glm::distance(glm::vec3(a), glm::vec3(b));
 }
 
@@ -60,10 +58,10 @@ static bool check_passability(
 
 static void restore_route(
     Route& route,
-    const Node& node,
+    const glm::ivec3& lastPos,
     const std::unordered_map<glm::ivec3, Node>& parents
 ) {
-    auto pos = node.pos;
+    auto pos = lastPos;
     while (true) {
         const auto& found = parents.find(pos);
         if (found == parents.end()) {
@@ -89,7 +87,7 @@ Route Pathfinding::perform(
     Route route {};
 
     std::priority_queue<Node, std::vector<Node>, NodeLess> queue;
-    queue.push({start, {}, 0, distance(start, end)});
+    queue.push({start, {}, 0, heuristic(start, end)});
 
     std::unordered_set<glm::ivec3> blocked;
     std::unordered_map<glm::ivec3, Node> parents;
@@ -97,18 +95,31 @@ Route Pathfinding::perform(
     const auto& chunks = *level.chunks;
     int height = std::max(agent.height, 1);
 
+    glm::ivec3 nearest = start;
+    float minHScore = heuristic(start, end);
+
     while (!queue.empty()) {
         if (blocked.size() == agent.maxVisitedBlocks) {
+            if (agent.mayBeIncomplete) {
+                restore_route(route, nearest, parents);
+                route.nodes.push_back({start});
+                route.found = true;
+                route.visited = std::move(blocked);
+                return route;
+            }
             break;
         }
         auto node = queue.top();
         queue.pop();
 
-        if (node.pos.x == end.x && glm::abs((node.pos.y - end.y) / height) == 0 && node.pos.z == end.z) {
-            restore_route(route, node, parents);
+        if (node.pos.x == end.x &&
+            glm::abs((node.pos.y - end.y) / height) == 0 &&
+            node.pos.z == end.z) {
+            restore_route(route, node.pos, parents);
             route.nodes.push_back({start});
             route.found = true;
-            break;
+            route.visited = std::move(blocked);
+            return route;
         }
 
         blocked.emplace(node.pos);
@@ -128,6 +139,9 @@ Route Pathfinding::perform(
             }
             pos.y = surface;
             auto point = pos + glm::ivec3(offset.x, 0, offset.y);
+            if (blocked.find(point) != blocked.end()) {
+                continue;
+            }
 
             if (is_obstacle_at(chunks, pos.x, pos.y + agent.height / 2, pos.z)) {
                 continue;
@@ -135,18 +149,21 @@ Route Pathfinding::perform(
             if (!check_passability(agent, chunks, node, offset, i >= 4)) {
                 continue;
             }
-            if (blocked.find(point) != blocked.end()) {
-                continue;
-            }
-            int score = glm::abs(node.pos.y - pos.y);
+
+            int score = glm::abs(node.pos.y - pos.y) * 10;
+            float sum = glm::abs(offset.x) + glm::abs(offset.y);
             float gScore =
-                node.gScore + glm::abs(offset.x) + glm::abs(offset.y) + score;
+                node.gScore + glm::max(sum, SQRT2) * 0.5f + sum * 0.5f + score;
             const auto& found = parents.find(point);
-            if (found == parents.end() || gScore < found->second.gScore) {
-                float hScore = distance(point, end);
+            if (found == parents.end()) {
+                float hScore = heuristic(point, end);
+                if (hScore < minHScore) {
+                    minHScore = hScore;
+                    nearest = point;
+                }
                 float fScore = gScore + hScore;
                 Node nNode {point, node.pos, gScore, fScore};
-                parents[point] = Node {node.pos, node.parent, gScore, fScore};
+                parents[point] = node;
                 queue.push(nNode);
             }
         }
diff --git a/src/voxels/Pathfinding.hpp b/src/voxels/Pathfinding.hpp
index 04e039cb..4b0b0008 100644
--- a/src/voxels/Pathfinding.hpp
+++ b/src/voxels/Pathfinding.hpp
@@ -1,10 +1,14 @@
 #pragma once
 
+#define GLM_ENABLE_EXPERIMENTAL
+#include <glm/gtx/hash.hpp>
+
 #include <vector>
 #include <memory>
 #include <glm/vec2.hpp>
 #include <glm/vec3.hpp>
 #include <unordered_map>
+#include <unordered_set>
 
 class Level;
 class GlobalChunks;
@@ -17,12 +21,14 @@ namespace voxels {
     struct Route {
         bool found;
         std::vector<RouteNode> nodes;
+        std::unordered_set<glm::ivec3> visited;
     };
 
     struct Agent {
         bool enabled = false;
-        int height = 1;
-        int maxVisitedBlocks = 1e5;
+        bool mayBeIncomplete = true;
+        int height = 2;
+        int maxVisitedBlocks = 1e3;
         glm::ivec3 start;
         glm::ivec3 target;
         Route route;