add unoptimized pathfinding

src/entity.js

@@ -1,6 +1,7 @@
 import { Vector2 } from 'three'
 import SAT from 'sat'
 import SATX from './satx.js'
+import Pathfind from './pathfind.js'
 
 export default class Entity {
   id = crypto.randomUUID()
@@ -10,6 +11,7 @@ export default class Entity {
   #position = new Vector2()
   #dest = null
   #game = null
+  #path = []
 
   static collider(x, y, radius) {
     return new SAT.Circle(new SAT.Vector(x, y), radius)
@@ -42,6 +44,26 @@ export default class Entity {
     return [this.collider]
   }
 
+  get unadjustedWaypoints() {
+    const numberOfWaypoints = 8
+    const margin = 1
+    const enclosingRegularPolygonRadius = SATX.enclosingRegularPolygonRadius(numberOfWaypoints)
+    const radius = this.radius * enclosingRegularPolygonRadius + margin
+    const baseWaypoint = new Vector2(radius, 0)
+    const waypoints = []
+
+    const origin = new Vector2
+    const unitOfRotation = (Math.PI * 2 / numberOfWaypoints)
+    for (let i = 0; i < numberOfWaypoints; i++) {
+      waypoints.push(baseWaypoint.clone().rotateAround(origin, unitOfRotation * i))
+    }
+
+    return waypoints.map((w) => [
+      w.clone().add(this.position),
+      w.clone().normalize().multiplyScalar(enclosingRegularPolygonRadius),
+    ])
+  }
+
   isColliding(...colliders) {
     return SATX.collideObjects(this.collider, colliders)
   }
@@ -64,28 +86,40 @@ export default class Entity {
     this.position.set(x, y)
   }
 
-  takeStep() {
-    const speed = this.speed / (this.game?.tickBudget ?? 1000)
+  takeStep(distanceTraveled = 0) {
+    const speed = (this.speed / (this.game?.tickBudget ?? 1000)) - distanceTraveled
     if (this.#dest != null) {
-      const fixedDest = new Vector2(
-        Math.min(Math.max(this.radius, this.#dest.x), (this.game?.width ?? Infinity) - this.radius),
-        Math.min(Math.max(this.radius, this.#dest.y), (this.game?.height ?? Infinity) - this.radius),
-      )
+      const fixedDest = SATX.clamp(SATX.fixCollisions(this.#dest, this.collidables, this.radius), this.game?.width, this.game?.height, this.radius)
 
-      const distance = this.position.clone().sub(fixedDest).length()
-      const direction = fixedDest.clone().sub(this.position).normalize()
-      const stepTaken = this.position.clone().add(direction.multiplyScalar(speed))
-      const position = distance <= speed ? fixedDest : stepTaken
-
-      const collider = Entity.collider(position.x, position.y, this.radius)
-      const isColliding = SATX.collideObjects(collider, this.collidables)
-
-      if (!isColliding) {
-        this.position.copy(position)
+      if (this.#path.length < 1 || !this.#path.at(-1).equals(this.#dest)) {
+        const waypoints = (this.game?.unadjustedWaypoints.map(([unadjusted, direction]) => unadjusted.clone().add(direction.clone().multiplyScalar(this.radius))) ?? []).concat([this.position, fixedDest])
+        const graph = Pathfind.buildGraph(waypoints, this.collidables, this.radius)
+        this.#path = Pathfind.shortestPath(graph, this.position, fixedDest)
       }
 
-      if (this.x == this.#dest?.x && this.y == this.#dest?.y) {
-        this.#dest = null
+      if (this.#path.length > 0) {
+        const destination = this.#path.at(0)
+        const distance = this.position.clone().sub(destination).length()
+        const direction = destination.clone().sub(this.position).normalize()
+        const stepTaken = this.position.clone().add(direction.multiplyScalar(speed))
+        const position = distance <= speed ? destination : stepTaken
+  
+        const collider = Entity.collider(position.x, position.y, this.radius)
+        const isColliding = SATX.collideObjects(collider, this.collidables)
+  
+        if (!isColliding) {
+          this.position.copy(position)
+        }
+  
+        if (this.position.equals(destination)) {
+          this.#path = this.#path.slice(1)
+          if (this.#path.length > 0) {
+            this.takeStep(distance)
+          }
+          else {
+            this.#dest = null
+          }
+        }
       }
     }
   }