diff --git a/src/voxels/Pathfinding.cpp b/src/voxels/Pathfinding.cpp
index f235b554..59da59f2 100644
--- a/src/voxels/Pathfinding.cpp
+++ b/src/voxels/Pathfinding.cpp
@@ -22,7 +22,6 @@ struct Node {
     float fScore;
 };
 
-
 struct NodeLess {
     bool operator()(const Node& l, const Node& r) const {
         return l.fScore > r.fScore;
@@ -33,9 +32,54 @@ static float distance(const glm::ivec3& a, const glm::ivec3& b) {
     return glm::distance(glm::vec3(a), glm::vec3(b));
 }
 
-Pathfinding::Pathfinding(const Level& level) : level(level) {}
+Pathfinding::Pathfinding(const Level& level)
+    : level(level), chunks(*level.chunks) {
+}
+
+static bool check_passability(
+    const Agent& agent,
+    const GlobalChunks& chunks,
+    const Node& node,
+    const glm::ivec2& offset,
+    bool diagonal
+) {
+    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 Node& node,
+    const std::unordered_map<glm::ivec3, Node>& parents
+) {
+    auto pos = node.pos;
+    while (true) {
+        const auto& found = parents.find(pos);
+        if (found == parents.end()) {
+            route.nodes.push_back({pos});
+            break;
+        }
+        route.nodes.push_back({pos});
+        pos = found->second.pos;
+    }
+}
+
+Route Pathfinding::perform(
+    const Agent& agent, const glm::ivec3& start, const glm::ivec3& end
+) {
+    using namespace blocks_agent;
 
-Route Pathfinding::perform(const glm::ivec3& start, const glm::ivec3& end) {
     Route route {};
 
     std::priority_queue<Node, std::vector<Node>, NodeLess> queue;
@@ -50,24 +94,8 @@ Route Pathfinding::perform(const glm::ivec3& start, const glm::ivec3& end) {
         auto node = queue.top();
         queue.pop();
 
-        if (blocked.find(node.pos) != blocked.end()) {
-            continue;
-        }
-
         if (node.pos.x == end.x && node.pos.z == end.z) {
-            auto prev = glm::ivec3();
-            auto pos = node.pos;
-            while (pos != start) {
-                const auto& found = parents.find(pos);
-                if (found == parents.end()) {
-                    route.nodes.push_back({pos});
-                    break;
-                }
-                route.nodes.push_back({pos});
-
-                prev = pos;
-                pos = found->second.pos;
-            }
+            restore_route(route, node, parents);
             route.nodes.push_back({start});
             route.found = true;
             break;
@@ -81,28 +109,30 @@ Route Pathfinding::perform(const glm::ivec3& start, const glm::ivec3& end) {
 
         for (int i = 0; i < sizeof(neighbors) / sizeof(glm::ivec2); i++) {
             auto offset = neighbors[i];
-            auto point = node.pos + glm::ivec3(offset.x, 0, offset.y);
-            if (blocks_agent::is_obstacle_at(
-                    chunks, point.x + 0.5f, point.y + 0.5f, point.z + 0.5f
-                )) {
+            auto pos = node.pos;
+
+            int surface = getSurfaceAt(pos + glm::ivec3(offset.x, 0, offset.y), 1);
+
+            if (surface == -1) {
                 continue;
             }
-            if (i >= 4) {
-                auto a = node.pos + glm::ivec3(offset.x, 0, offset.y);
-                auto b = node.pos + glm::ivec3(offset.x, 0, offset.y);
-                if (blocks_agent::is_obstacle_at(chunks, a.x, a.y, a.z))
-                    continue;
-                if (blocks_agent::is_obstacle_at(chunks, b.x, b.y, b.z))
-                    continue;
+            pos.y = surface;
+            auto point = pos + glm::ivec3(offset.x, 0, offset.y);
+
+            if (is_obstacle_at(chunks, pos.x, pos.y + agent.height / 2, pos.z)) {
+                continue;
+            }
+            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);
             float gScore =
-                node.gScore + glm::abs(offset.x) + glm::abs(offset.y);
-            const auto& foundParent = parents.find(point);
-            bool queued = foundParent != parents.end();
-            if (!queued || gScore < foundParent->second.gScore) {
+                node.gScore + glm::abs(offset.x) + glm::abs(offset.y) + score;
+            const auto& found = parents.find(point);
+            if (found == parents.end() || gScore < found->second.gScore) {
                 float hScore = distance(point, end);
                 float fScore = gScore + hScore;
                 Node nNode {point, node.pos, gScore, fScore};
@@ -113,3 +143,46 @@ Route Pathfinding::perform(const glm::ivec3& start, const glm::ivec3& end) {
     }
     return route;
 }
+
+static int check_point(
+    const ContentUnitIndices<Block>& defs,
+    const GlobalChunks& chunks,
+    int x,
+    int y,
+    int z
+) {
+    auto vox = blocks_agent::get(chunks, x, y, z);
+    if (vox == nullptr) {
+        return 0;
+    }
+    const auto& def = defs.require(vox->id);
+    if (def.obstacle) {
+        return 0;
+    }
+    if (def.translucent) {
+        return -1;
+    }
+    return 1;
+}
+
+int Pathfinding::getSurfaceAt(const glm::ivec3& pos, int maxDelta) {
+    using namespace blocks_agent;
+
+    const auto& defs = level.content.getIndices()->blocks;
+
+    int status;
+    int surface = pos.y;
+    if (check_point(defs, chunks, pos.x, surface, pos.z) <= 0) {
+        if (check_point(defs, chunks, pos.x, surface + 1, pos.z) <= 0)
+            return -1;
+        else
+            return surface + 1;
+    } else if ((status = check_point(defs, chunks, pos.x, surface - 1, pos.z)) <= 0) {
+        if (status == -1)
+            return -1;
+        return surface;
+    } else if (check_point(defs, chunks, pos.x, surface - 2, pos.z) == 0) {
+        return surface - 1;
+    }
+    return -1;
+}
diff --git a/src/voxels/Pathfinding.hpp b/src/voxels/Pathfinding.hpp
index cefecdb2..f743fd1c 100644
--- a/src/voxels/Pathfinding.hpp
+++ b/src/voxels/Pathfinding.hpp
@@ -6,6 +6,7 @@
 #include <glm/vec3.hpp>
 
 class Level;
+class GlobalChunks;
 
 namespace voxels {
     struct RouteNode {
@@ -17,6 +18,10 @@ namespace voxels {
         std::vector<RouteNode> nodes;
     };
 
+    struct Agent {
+        int height;
+    };
+
     struct Map {
         int width;
         int height;
@@ -40,9 +45,14 @@ namespace voxels {
     class Pathfinding {
     public:
         Pathfinding(const Level& level);
-        
-        Route perform(const glm::ivec3& start, const glm::ivec3& end);
+
+        Route perform(
+            const Agent& agent, const glm::ivec3& start, const glm::ivec3& end
+        );
     private:
         const Level& level;
+        const GlobalChunks& chunks;
+
+        int getSurfaceAt(const glm::ivec3& pos, int maxDelta);
     };
 }