diff --git a/engine/breeze/map/map.c b/engine/breeze/map/map.c
index a017440..8975c96 100644
--- a/engine/breeze/map/map.c
+++ b/engine/breeze/map/map.c
@@ -5,6 +5,7 @@
 #include "../math/vec2i.h"
 
 #include <cute_tiled.h>
+#include <raymath.h>
 #include <string.h>
 
 BzTileMap BZ_TILEMAP_INVALID = {.isValid = false};
@@ -381,6 +382,80 @@ BzTileObjectGroup *bzTileMapGetObjects(BzTileMap *map, i32 slotID) {
     BZ_ASSERT(slotID >= 0 && slotID < map->objectGroupCount);
     return &map->objectGroups[slotID];
 }
+static f32 abs(f32 x) {
+    return x > 0 ? x : -x;
+}
+
+f32 bzTileMapRayCast(BzTileMap *map, Vector2 from, Vector2 to, f32 maxDst, Vector2 *outIntersection) {
+    // Digital Differential Analyzer (DDA)
+    Vector2 scale = {1.0f / map->tileWidth, 1.0f / map->tileHeight};
+    from = Vector2Multiply(from, scale);
+    to = Vector2Multiply(to, scale);
+
+    Vector2 dir = Vector2Subtract(to, from);
+    dir = Vector2Normalize(dir);
+
+    //Vector2 stepSize = {
+    //        sqrtf(1 + (dir.y / dir.x) * (dir.y / dir.x)),
+    //        sqrtf(1 + (dir.x / dir.y) * (dir.x / dir.y))
+    //};
+    Vector2 stepSize = {abs(1.0f / dir.x), abs(1.0f / dir.y)};
+
+    i32 cellX = (i32) from.x;
+    i32 cellY = (i32) from.y;
+    if (bzTileMapHasCollision(map, cellX, cellY)) {
+        return 0.0f;
+    }
+    Vector2 rayLength = Vector2Zero();
+    i32 stepX, stepY;
+
+    if (dir.x < 0) {
+        stepX = -1;
+        rayLength.x = (from.x - (f32) (cellX)) * stepSize.x;
+    } else {
+        stepX = 1;
+        rayLength.x = ((f32) cellX + 1.0f - from.x) * stepSize.x;
+    }
+
+    if (dir.y < 0) {
+        stepY = -1;
+        rayLength.y = (from.y - (f32) cellY) * stepSize.y;
+    } else {
+        stepY = 1;
+        rayLength.y = ((f32) cellY + 1 - from.y) * stepSize.y;
+    }
+    f32 distance = 0.0f;
+    while (distance < maxDst) {
+        if (rayLength.x < rayLength.y) {
+            cellX += stepX;
+            distance = rayLength.x * map->tileWidth;
+            rayLength.x += stepSize.x;
+        } else {
+            cellY += stepY;
+            distance = rayLength.y * map->tileHeight;
+            rayLength.y += stepSize.y;
+        }
+        if (cellX >= 0 && cellX < map->width && cellY >= 0 && cellY < map->height &&
+            map->collisionMap[cellY * map->width + cellX]) {
+            break;
+        }
+    }
+
+    if (outIntersection) {
+        Vector2 intersection = from;
+        intersection = Vector2Add(intersection, Vector2Scale(dir, distance));
+        intersection.x *= map->tileWidth;
+        intersection.y *= map->tileHeight;
+        *outIntersection = intersection;
+    }
+
+    return distance;
+}
+bool bzTileMapCanRayCastLine(BzTileMap *map, Vector2 from, Vector2 to) {
+    f32 dst = Vector2Distance(from, to);
+    f32 rayDst = bzTileMapRayCast(map, from, to, dst, NULL);
+    return rayDst >= dst;
+}
 
 void bzTileMapDraw(BzTileMap *map) {
     for (i32 i = 0; i < map->layerCount; i++) {
diff --git a/engine/breeze/map/map.h b/engine/breeze/map/map.h
index c870674..1c04c26 100644
--- a/engine/breeze/map/map.h
+++ b/engine/breeze/map/map.h
@@ -124,6 +124,9 @@ void bzTileMapOverrideObjectGroup(BzTileMap *map, i32 slotID, BzTileObjectsFunc
 BzTileLayer *bzTileMapGetLayer(BzTileMap *map, i32 slotID);
 BzTileObjectGroup *bzTileMapGetObjects(BzTileMap *map, i32 slotID);
 
+f32 bzTileMapRayCast(BzTileMap *map, Vector2 from, Vector2 to, f32 maxDst, Vector2 *outIntersection);
+bool bzTileMapCanRayCastLine(BzTileMap *map, Vector2 from, Vector2 to);
+
 void bzTileMapDraw(BzTileMap *map);
 void bzTileMapDrawCollisions(BzTileMap *map);
 bool bzTileMapHasCollision(BzTileMap *map, i32 x, i32 y);
diff --git a/game/pathfinding.c b/game/pathfinding.c
index d2e2f07..69a7e2f 100644
--- a/game/pathfinding.c
+++ b/game/pathfinding.c
@@ -29,21 +29,6 @@ static PathData *pushPathWaypoint(PathData *pathData, Position waypoint, BzObjec
     return pathData;
 }
 
-static bool canRayCastLine(BzTileMap *map, Position from, Position to) {
-    Vector2 step = Vector2Subtract(to, from);
-    step = Vector2Normalize(step);
-
-    while (Vector2DistanceSqr(from, to) > 1.0f) {
-        from = Vector2Add(from, step);
-
-        BzTile tileX = 0, tileY = 0;
-        bzTileMapPosToTile(map, from, &tileX, &tileY);
-        if (bzTileMapHasCollision(map, tileX, tileY))
-            return false;
-    }
-    return true;
-}
-
 static void reversePath(PathData *pathData) {
     while (pathData) {
         for (i32 i = 0; i < pathData->numWaypoints / 2; i++) {
@@ -61,8 +46,9 @@ static void reversePath(PathData *pathData) {
 static void smoothPath(BzTileMap *map, PathData *pathData, BzObjectPool *pool) {
     // Our smoothed path
     PathData *outPath = pathData;
-    size_t outIdx = 1;
     Position outPos = outPath->waypoints[0];
+    size_t outIdx = 1;
+    Position lastPos = outPos;
 
 #define NEXT_WAYPOINT(path, idx, len)                       \
 do {                                                        \
@@ -74,18 +60,20 @@ do {                                                        \
 } while (0)
 
     PathData *currPath = pathData;
-    size_t currIdx = 0;
     PathData *nextPath = pathData;
+    size_t currIdx = 0;
     size_t nextIdx = 0;
-    Position lastPos = outPos;
     // Needed, because we overwrite numWaypoints
     size_t currPathLen = currPath->numWaypoints;
     size_t nextPathLen = nextPath->numWaypoints;
 
     // Second element
+    currIdx++;
     NEXT_WAYPOINT(currPath, currIdx, currPathLen);
     // Third element
+    nextIdx++;
     NEXT_WAYPOINT(nextPath, nextIdx, nextPathLen);
+    nextIdx++;
     NEXT_WAYPOINT(nextPath, nextIdx, nextPathLen);
 
     outPath->numWaypoints = 1;
@@ -100,7 +88,7 @@ do {                                                        \
         NEXT_WAYPOINT(currPath, currIdx, currPathLen);
         NEXT_WAYPOINT(nextPath, nextIdx, nextPathLen);
 
-        if (!canRayCastLine(map, outPos, nextPos)) {
+        if (!bzTileMapCanRayCastLine(map, outPos, nextPos)) {
             outPos = currPos;
             outPath->waypoints[outIdx++] = currPos;
             outPath->numWaypoints = outIdx;
@@ -113,7 +101,6 @@ do {                                                        \
         }
     }
 #undef NEXT_WAYPOINT
-    BZ_ASSERT(lastPos.x != INFINITY && lastPos.y != INFINITY);
     outPath->waypoints[outIdx++] = lastPos;
     outPath->numWaypoints = outIdx;
 
diff --git a/game/systems_entity.c b/game/systems_entity.c
index e8a02d4..da88f30 100644
--- a/game/systems_entity.c
+++ b/game/systems_entity.c
@@ -171,7 +171,7 @@ void renderDebugPath(ecs_iter_t *it) {
     Path *path = ecs_field(it, Path, 1);
 
     for (i32 i = 0; i < it->count; i++) {
-        PathData *pathData = path->paths;
+        PathData *pathData = path[i].paths;
         bool first = true;
         while (pathData) {
             for (i32 iPath = 0; iPath < pathData->numWaypoints; iPath++) {