diff --git a/methods.lua b/methods.lua
index 034d34b..d41738b 100644
--- a/methods.lua
+++ b/methods.lua
@@ -9,6 +9,9 @@ local floor = math.floor
 local random = math.random
 local rad = math.rad
 
+local sin = math.sin
+local cos = math.cos
+
 local function diff(a, b) -- Get difference between 2 angles
     return math.atan2(math.sin(b - a), math.cos(b - a))
 end
@@ -47,6 +50,40 @@ local function debugpart(pos, time, tex)
     })
 end
 
+---------------------
+-- Local Utilities --
+---------------------
+
+local function get_collision(self, yaw)
+    local width = self.width
+    local height = self.height
+    local pos = self.object:get_pos()
+    pos.y = pos.y + 1
+    local pos2 = vec_add(pos, vec_multi(yaw2dir(yaw), width + 5))
+    for x = -width, width, width / math.ceil(width) do
+        for y = 0, height, height / math.ceil(height) do
+            local vec1 = {
+                x = math.cos(yaw) * ((pos.x + x) - pos.x) + pos.x,
+                y = pos.y + y,
+                z = math.sin(yaw) * ((pos.x + x) - pos.x) + pos.z
+            }
+            local vec2 = {
+                x = math.cos(yaw) * ((pos2.x + x) - pos2.x) + pos2.x,
+                y = vec1.y,
+                z = math.sin(yaw) * ((pos2.x + x) - pos2.x) + pos2.z
+            }
+            local ray = minetest.raycast(vec1, vec2, false, true)
+            for pointed_thing in ray do
+                if pointed_thing
+                and pointed_thing.type == "node" then
+                    return true, pointed_thing.intersection_point
+                end
+            end
+        end 
+    end
+    return false
+end
+
 -------------
 -- Actions --
 -------------
@@ -151,7 +188,9 @@ creatura.register_movement_method("creatura:pathfind", function(self, pos2)
     local speed = movement_data.speed or 5
     local path = self._path
     if not path or #path < 2 then
-        self._path = creatura.find_path(self, pos, pos2, self.width, self.height, 200) or {}
+        if get_collision(self, dir2yaw(vec_dir(pos, pos2))) then
+            self._path = creatura.find_path(self, pos, pos2, self.width, self.height, 200) or {}
+        end
     else
         waypoint = self._path[2]
         if self:pos_in_box({x = waypoint.x, y = pos.y + self.height * 0.5, z = waypoint.z}) then
@@ -279,34 +318,37 @@ end)
 
 -- Obstacle Avoidance
 
-local moveable = creatura.is_pos_moveable
-
-local function get_obstacle_avoidance(self, pos2)
-    local pos = self.object:get_pos()
-    local yaw = minetest.dir_to_yaw(vec_dir(pos, pos2))
-    pos.y = pos.y + self.stepheight
-    local height = self.height
+local function get_obstacle_avoidance(self, goal)
     local width = self.width
-    local outset = vec_center(vec_add(pos, vec_multi(yaw2dir(yaw), width + 0.2)))
-    local pos2
-    if not moveable(outset, width, height) then
-        yaw = self.object:get_yaw()
-        for i = 1, 89, 45 do
+    local height = self.height
+    local pos = self.object:get_pos()
+    pos.y = pos.y + 1
+    local yaw2goal = dir2yaw(vec_dir(pos, goal))
+    local outset = vec_add(pos, vec_multi(yaw2dir(yaw2goal), width))
+    local collide, col_pos = get_collision(self, outset, yaw2goal)
+    local avd_pos
+    if collide then
+        for i = 45, 180, 45 do
             angle = rad(i)
-            dir = vec_multi(yaw2dir(yaw + angle), width + 0.2)
-            pos2 = vec_center(vec_add(pos, dir))
-            if moveable(pos2, width, height) then
+            dir = vec_multi(yaw2dir(yaw2goal + angle), width)
+            avd_pos = vec_center(vec_add(pos, dir))
+            if not get_collision(self, avd_pos, yaw2goal) then
                 break
             end
             angle = -rad(i)
-            dir = vec_multi(yaw2dir(yaw + angle), width + 0.2)
-            pos2 = vec_center(vec_add(pos, dir))
-            if moveable(pos2, width, height) then
+            dir = vec_multi(yaw2dir(yaw2goal + angle), width)
+            avd_pos = vec_center(vec_add(pos, dir))
+            if not get_collision(self, avd_pos, yaw2goal) then
                 break
             end
         end
+        if col_pos.y - (pos.y + height * 0.5) > 1 then
+            avd_pos.y = avd_pos.y - 3
+        elseif (pos.y + height * 0.5) - col_pos.y > 1 then
+            avd_pos.y = avd_pos.y + 3
+        end
     end
-    return pos2
+    return avd_pos
 end
 
 creatura.register_movement_method("creatura:obstacle_avoidance", function(self, pos2)