diff --git a/src/entity.js b/src/entity.js
index 537cb73..55de6d1 100644
--- a/src/entity.js
+++ b/src/entity.js
@@ -2,7 +2,6 @@ import { Vector2 } from 'three'
 import SAT from 'sat'
 import SATX from './satx.js'
 import Pathfind from './pathfind.js'
-import Terrain from './terrain.js'
 
 export default class Entity {
   id = crypto.randomUUID()
@@ -95,18 +94,13 @@ export default class Entity {
       if (this.#path.length < 1 || !this.#path.at(-1).equals(this.#dest)) {
         console.time('pathfinding')
         console.time('waypoints')
-        const extraWaypoints = [[this.position, new Vector2()], [fixedDest, new Vector2()]]
-        const waypoints = (this.game?.unadjustedWaypoints ?? []).concat(extraWaypoints)
+        const waypoints = (this.game?.unadjustedWaypoints.map(([unadjusted, direction]) => unadjusted.clone().add(direction.clone().multiplyScalar(this.radius))) ?? []).concat([this.position, fixedDest])
         console.timeEnd('waypoints')
-        console.time('extraEntries')
-        const extraGraphEntries = Pathfind.buildGraph(waypoints, this.collidables(), extraWaypoints)
-        console.timeEnd('extraEntries')
-        console.time('unadjustedGraph')
-        const unadjustedGraph = (this.game?.waypointGraph ?? []).concat(extraGraphEntries)
-        console.timeEnd('unadjustedGraph')
+        console.time('graph')
+        const graph = Pathfind.buildGraph(waypoints, this.collidables(), this.radius)
+        console.timeEnd('graph')
         console.time('path')
-        this.#path = Pathfind.shortestPath(unadjustedGraph, this.position, fixedDest).map(([unadjusted, direction]) => unadjusted.clone().add(direction.clone().multiplyScalar(this.radius)))
-        console.log(this.#path)
+        this.#path = Pathfind.shortestPath(graph, this.position, fixedDest)
         console.timeEnd('path')
         console.timeEnd('pathfinding')
       }
@@ -119,15 +113,12 @@ export default class Entity {
         const position = distance <= speed ? destination : stepTaken
   
         const collider = Entity.collider(position.x, position.y, this.radius)
-        // TODO: fix collisions while pathfinding
-        // const isColliding = SATX.collideObjects(collider, this.collidables())
+        const isColliding = SATX.collideObjects(collider, this.collidables())
   
-        // if (!isColliding) {
-        //   this.position.copy(position)
-        // }
+        if (!isColliding) {
+          this.position.copy(position)
+        }
   
-        this.position.copy(position)
-
         if (this.position.equals(destination)) {
           this.#path = this.#path.slice(1)
           if (this.#path.length > 0) {
diff --git a/src/game.js b/src/game.js
index 8015c13..8888d80 100644
--- a/src/game.js
+++ b/src/game.js
@@ -1,5 +1,4 @@
 import { EventEmitter } from 'node:events'
-import Pathfind from './pathfind.js'
 
 export default class Game {
   tickRate = 30
@@ -11,23 +10,14 @@ export default class Game {
   #eventEmitter = new EventEmitter()
   #terrains = []
   #tickBudget = Math.floor(1000 / this.tickRate)
-  #waypointGraph = []
 
   get entities() { return this.#entities }
   get eventEmitter() { return this.#eventEmitter }
   get terrains() { return this.#terrains }
   get tickBudget() { return this.#tickBudget }
-  get waypointGraph() { return this.#waypointGraph }
 
   get unadjustedWaypoints() {
-    // const terrainAndEntities = this.entities.concat(this.terrains)
-    const terrainAndEntities = this.terrains
-    return terrainAndEntities.map((e) => e.unadjustedWaypoints).flat()
-  }
-
-  colliders() {
-    const terrainAndEntities = this.entities.concat(this.terrains)
-    return terrainAndEntities.map((e) => e.colliders).flat()
+    return this.terrains.map((t) => t.unadjustedWaypoints).concat(this.entities.map((e) => e.unadjustedWaypoints)).flat()
   }
 
   spawn_entity(entity) {
@@ -42,7 +32,6 @@ export default class Game {
 
   add_terrain(terrain) {
     this.#terrains.push(terrain)
-    this.#calculateWaypointGraph()
   }
 
   state() {
@@ -64,8 +53,4 @@ export default class Game {
     this.currentTick++
     this.eventEmitter.emit('tick')
   }
-
-  #calculateWaypointGraph() {
-    this.#waypointGraph = Pathfind.buildGraph(this.unadjustedWaypoints, this.colliders())
-  }
 }
diff --git a/src/index.js b/src/index.js
index 98afaaf..9ae8bed 100644
--- a/src/index.js
+++ b/src/index.js
@@ -52,14 +52,6 @@ app.listen(port, () => {
   entity2.radius = 50
   game.spawn_entity(entity2)
 
-  // const triangle = new Terrain([
-  //   { x: 400, y: 200 },
-  //   { x: 400, y: 600 },
-  //   { x: 600, y: 300 },
-  // ])
-  // triangle.id = 'triangle'
-  // game.add_terrain(triangle)
-
   const horseshoe = new Terrain([
     { x: 400, y: 200 },
     { x: 600, y: 200 },
@@ -115,7 +107,5 @@ app.listen(port, () => {
   pole.id = 'pole'
   game.add_terrain(pole)
 
-  entity1.moveAction(entity1.x + 600, entity1.y)
-
   game.start()
 })
diff --git a/src/pathfind.js b/src/pathfind.js
index 365d03a..3972651 100644
--- a/src/pathfind.js
+++ b/src/pathfind.js
@@ -3,33 +3,28 @@ import PriorityQueue from "./priority-queue.js"
 import SATX from "./satx.js"
 
 export default class Pathfind {
-  static keyFor(pos) {
-    return `${pos.x},${pos.y}`
-  }
-
   static shortestPath(graph, start, goal) {
+    const key = (pos) => `${pos.x},${pos.y}`
     const queue = new PriorityQueue((a, b) => a[1] < b[1])
     const visited = new Map()
 
-    queue.push([[[start, null]], 0])
+    queue.push([[start], 0])
 
     while (!queue.isEmpty()) {
       const [path, cost] = queue.pop()
-      const waypoint = path.at(-1).at(0)
+      const waypoint = path.at(-1)
 
       if (waypoint.equals(goal)) {
         path.shift()
         return path
       }
 
-      const key = this.keyFor(waypoint)
-      if (!visited.has(key) || visited.get(key) > cost) {
-        visited.set(key, cost)
+      if (!visited.has(key(waypoint)) || visited.get(key(waypoint)) > cost) {
+        visited.set(key(waypoint), cost)
 
-        for (const { to, direction, distance } of graph.filter(e => e.from.equals(waypoint))) {
-          const keyTo = this.keyFor(to)
-          if (!visited.has(keyTo) || visited.get(keyTo) > cost + distance) {
-            queue.push([[...path, [to, direction]], cost + distance])
+        for (const { to, distance } of graph.filter(e => e.from.equals(waypoint))) {
+          if (!visited.has(key(to)) || visited.get(key(to)) > cost + distance) {
+            queue.push([[...path, to], cost + distance])
           }
         }
       }
@@ -38,28 +33,24 @@ export default class Pathfind {
     return []
   }
 
-  static buildGraph(toWaypoints, colliders, fromWaypoints = null) {
+  static buildGraph(waypoints = [], colliders = [], radius = 0) {
     const graph = []
-    const calculated = new Set()
 
-    for (const [from, reverseDirection] of (fromWaypoints ?? toWaypoints)) {
-      for (const [to, direction] of toWaypoints) {
+    for (const from of waypoints) {
+      for (const to of waypoints) {
         if (from.equals(to)) {
           continue
         }
 
-        const key = `${from.x},${from.y};${to.x},${to.y}`
-        if (!calculated.has(key)) {
-          calculated.add(key)
-          calculated.add(`${to.x},${to.y};${from.x},${from.y}`)
+        const tunnel = SATX.entityTunnel(from, to, radius)
+        const collider = Entity.collider(from.x, from.y, radius)
 
-          const lineOfSight = !SATX.collideObjects(SATX.line(from, to), colliders)
+        const tunnelClear = !SATX.collideObjects(tunnel, colliders)
+        const waypointAvailable = !SATX.collideObjects(collider, colliders)
 
-          if (lineOfSight) {
-            const distance = from.distanceTo(to)
-            graph.push({ from, to, distance, direction })
-            graph.push({ to: from, from: to, distance, direction: reverseDirection })
-          }
+        if (waypointAvailable && tunnelClear) {
+          const distance = from.distanceTo(to)
+          graph.push({ from, to, distance })
         }
       }
     }