add unoptimized pathfinding

public/client.js

@@ -16,7 +16,7 @@ const terrainMaterial = new THREE.MeshToonMaterial({ color: 0xffd700 })
 const minimapCamera = new THREE.OrthographicCamera(-10, 10, 10, -10)
 const minimapRenderer = new THREE.WebGLRenderer()
 
-minimapRenderer.setSize(300, 300)
+minimapRenderer.setSize(600, 600)
 minimapRenderer.setAnimationLoop(minimapRender)
 minimapCamera.position.set(10, 10, 10)
 
@@ -101,7 +101,8 @@ function connectWebSocket() {
         entity = entities[e.id]
       }
       else {
-        entity = new THREE.Mesh(new THREE.SphereGeometry(e.radius / 100), entityMaterial)
+        entity = new THREE.Mesh(new THREE.CylinderGeometry(e.radius / 100, e.radius / 100, e.radius / 50), entityMaterial)
+        entity.rotation.x = Math.PI / 2
         entity.userData.type = 'entity'
         entity.userData.id = e.id
         scene.add(entity)

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,18 +86,23 @@ 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()
+      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.#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 ? fixedDest : stepTaken
+        const position = distance <= speed ? destination : stepTaken
   
         const collider = Entity.collider(position.x, position.y, this.radius)
         const isColliding = SATX.collideObjects(collider, this.collidables)
@@ -84,11 +111,18 @@ export default class Entity {
           this.position.copy(position)
         }
   
-      if (this.x == this.#dest?.x && this.y == this.#dest?.y) {
+        if (this.position.equals(destination)) {
+          this.#path = this.#path.slice(1)
+          if (this.#path.length > 0) {
+            this.takeStep(distance)
+          }
+          else {
             this.#dest = null
           }
         }
       }
+    }
+  }
 
   async update() {
     await Promise.allSettled([

src/game.js

@@ -3,8 +3,8 @@ import { EventEmitter } from 'node:events'
 export default class Game {
   tickRate = 30
   currentTick = 0
-  width = 4000
-  height = 4000
+  width = 2000
+  height = 2000
 
   #entities = []
   #eventEmitter = new EventEmitter()
@@ -16,11 +16,20 @@ export default class Game {
   get terrains() { return this.#terrains }
   get tickBudget() { return this.#tickBudget }
 
+  get unadjustedWaypoints() {
+    return this.terrains.map((t) => t.unadjustedWaypoints).concat(this.entities.map((e) => e.unadjustedWaypoints)).flat()
+  }
+
   spawn_entity(entity) {
     this.#entities.push(entity)
     entity.game = this
   }
 
+  despawn(entity) {
+    this.#entities = this.#entities.filter((e) => e.id != entity.id)
+    entity.game = null
+  }
+
   add_terrain(terrain) {
     this.#terrains.push(terrain)
   }

src/index.js

@@ -4,6 +4,10 @@ import Game from './game.js'
 import Entity from './entity.js'
 import Terrain from './terrain.js'
 
+import { Vector2 } from 'three'
+import Pathfind from './pathfind.js'
+import SATX from './satx.js'
+
 const app = new WebSocketExpress()
 const port = 1280
 const game = new Game()
@@ -42,17 +46,17 @@ app.listen(port, () => {
 
   const entity1 = new Entity()
   entity1.id = '1'
-  entity1.teleport(350, 500)
+  entity1.teleport(200, 500)
   entity1.radius = 50
   game.spawn_entity(entity1)
 
   const entity2 = new Entity()
   entity2.id = '2'
-  entity2.teleport(35, 35)
-  entity2.radius = 35
+  entity2.teleport(110, 110)
+  entity2.radius = 50
   game.spawn_entity(entity2)
 
-  const vertices = [
+  const horseshoe = new Terrain([
     { x: 400, y: 200 },
     { x: 600, y: 200 },
     { x: 700, y: 300 },
@@ -62,11 +66,50 @@ app.listen(port, () => {
     { x: 600, y: 500 },
     { x: 600, y: 300 },
     { x: 400, y: 300 },
-  ]
+  ])
+  horseshoe.id = 'horseshoe'
+  game.add_terrain(horseshoe)
 
-  const terrain1 = new Terrain(vertices)
-  terrain1.id = 'a'
-  game.add_terrain(terrain1)
+  const stopsign = new Terrain([
+    { x: 800, y: 800 },
+    { x: 900, y: 900 },
+    { x: 900, y: 1000 },
+    { x: 800, y: 1100 },
+    { x: 800, y: 1100 },
+    { x: 700, y: 1100 },
+    { x: 600, y: 1000 },
+    { x: 600, y: 900 },
+    { x: 700, y: 800 },
+  ])
+  stopsign.id = 'stopsign'
+  game.add_terrain(stopsign)
+
+  const box = new Terrain([
+    { x: 1200, y: 700 },
+    { x: 1200, y: 800 },
+    { x: 1300, y: 800 },
+    { x: 1300, y: 700 },
+  ])
+  box.id = 'box'
+  game.add_terrain(box)
+
+  const diamond = new Terrain([
+    { x: 1000, y: 300 },
+    { x: 1100, y: 400 },
+    { x: 1000, y: 500 },
+    { x: 900, y: 400 },
+  ])
+  diamond.id = 'diamond'
+  game.add_terrain(diamond)
+
+  const pole = new Terrain([
+    { x: 400, y: 1000 },
+    { x: 410, y: 1000 },
+    { x: 410, y: 1010 },
+    { x: 400, y: 1010 },
+  ])
+  pole.id = 'pole'
+  game.add_terrain(pole)
 
   game.start()
 })

src/pathfind.js

@@ -0,0 +1,60 @@
+import Entity from "./entity.js"
+import PriorityQueue from "./priority-queue.js"
+import SATX from "./satx.js"
+
+export default class Pathfind {
+  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], 0])
+
+    while (!queue.isEmpty()) {
+      const [path, cost] = queue.pop()
+      const waypoint = path.at(-1)
+
+      if (waypoint.equals(goal)) {
+        path.shift()
+        return path
+      }
+
+      if (!visited.has(key(waypoint)) || visited.get(key(waypoint)) > cost) {
+        visited.set(key(waypoint), cost)
+
+        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])
+          }
+        }
+      }
+    }
+
+    return []
+  }
+
+  static buildGraph(waypoints = [], colliders = [], radius = 0) {
+    const graph = []
+
+    for (const from of waypoints) {
+      for (const to of waypoints) {
+        if (from.equals(to)) {
+          continue
+        }
+
+        const tunnel = SATX.entityTunnel(from, to, radius)
+        const collider = Entity.collider(from.x, from.y, radius)
+
+        const tunnelClear = !SATX.collideObjects(tunnel, colliders)
+        const waypointAvailable = !SATX.collideObjects(collider, colliders)
+
+        if (waypointAvailable && tunnelClear) {
+          const distance = from.distanceTo(to)
+          graph.push({ from, to, distance })
+        }
+      }
+    }
+
+    return graph
+  }
+}

src/priority-queue.js

@@ -0,0 +1,78 @@
+const top = 0;
+const parent = i => ((i + 1) >>> 1) - 1;
+const left = i => (i << 1) + 1;
+const right = i => (i + 1) << 1;
+
+export default class PriorityQueue {
+  #heap
+  #comparator
+
+  constructor(comparator = (a, b) => a > b) {
+    this.#heap = []
+    this.#comparator = comparator
+  }
+
+  get length() { return this.#heap.length }
+
+  isEmpty() {
+    return this.length < 1
+  }
+
+  peek() {
+    return this.#heap[top]
+  }
+
+  push(...values) {
+    values.forEach(value => {
+      this.#heap.push(value)
+      this.#siftUp();
+    });
+    return this.length;
+  }
+
+  pop() {
+    const poppedValue = this.peek()
+    const bottom = this.length - 1
+    if (bottom > top) {
+      this.#swap(top, bottom)
+    }
+    this.#heap.pop()
+    this.#siftDown()
+    return poppedValue
+  }
+
+  replace(value) {
+    const replacedValue = this.peek()
+    this.#heap[top] = value
+    this.#siftDown()
+    return replacedValue
+  }
+
+  #greater(i, j) {
+    return this.#comparator(this.#heap[i], this.#heap[j])
+  }
+
+  #swap(i, j) {
+    [this.#heap[i], this.#heap[j]] = [this.#heap[j], this.#heap[i]]
+  }
+
+  #siftUp() {
+    let node = this.length - 1
+    while (node > top && this.#greater(node, parent(node))) {
+      this.#swap(node, parent(node))
+      node = parent(node)
+    }
+  }
+
+  #siftDown() {
+    let node = top;
+    while (
+      (left(node) < this.length && this.#greater(left(node), node)) ||
+      (right(node) < this.length && this.#greater(right(node), node))
+    ) {
+      let maxChild = (right(node) < this.length && this.#greater(right(node), left(node))) ? right(node) : left(node)
+      this.#swap(node, maxChild)
+      node = maxChild
+    }
+  }
+}

src/satx.js

@@ -1,21 +1,41 @@
 import SAT from 'sat'
+import { Vector2 } from 'three'
+import Entity from './entity.js'
 
 export default class SATX {
-  static collideObject(collider1, collider2) {
+  static clamp(vectorOrObject, maxX = Infinity, maxY = Infinity, radius = 0) {
+    let modified = null
+    if (vectorOrObject instanceof Vector2) {
+      modified = vectorOrObject.clone()
+    }
+    else if (vectorOrObject instanceof SAT.Vector) {
+      modified = new SAT.Vector(vectorOrObject.x, vectorOrObject.y)
+    }
+    else {
+      modified = { x: vectorOrObject.x, y: vectorOrObject.y }
+    }
+
+    modified.x = Math.min(Math.max(radius, vectorOrObject.x), (maxX ?? Infinity) - radius)
+    modified.y = Math.min(Math.max(radius, vectorOrObject.y), (maxY ?? Infinity) - radius)
+
+    return modified
+  }
+
+  static collideObject(collider1, collider2, result = null) {
     if (collider1 instanceof SAT.Circle && collider2 instanceof SAT.Circle) {
-      return SAT.testCircleCircle(collider1, collider2)
+      return SAT.testCircleCircle(collider1, collider2, result)
     }
 
     if (collider1 instanceof SAT.Circle && collider2 instanceof SAT.Polygon) {
-      return SAT.testCirclePolygon(collider1, collider2)
+      return SAT.testCirclePolygon(collider1, collider2, result)
     }
 
     if (collider1 instanceof SAT.Polygon && collider2 instanceof SAT.Circle) {
-      return SAT.testPolygonCircle(collider1, collider2)
+      return SAT.testPolygonCircle(collider1, collider2, result)
     }
 
     if (collider1 instanceof SAT.Polygon && collider2 instanceof SAT.Polygon) {
-      return SAT.testPolygonPolygon(collider1, collider2)
+      return SAT.testPolygonPolygon(collider1, collider2, result)
     }
 
     return false
@@ -24,4 +44,43 @@ export default class SATX {
   static collideObjects(collider1, colliders) {
     return colliders.some((c) => this.collideObject(collider1, c))
   }
+
+  static enclosingRegularPolygonRadius(numberOfVertices) {
+    return 1 / Math.cos(Math.PI / numberOfVertices)
+  }
+
+  static entityTunnel(from, to, radius) {
+    const length = to.clone().sub(from)
+    const halfWidth = length.clone().normalize().multiplyScalar(radius).rotateAround(new Vector2(), Math.PI / 2)
+    const width = halfWidth.clone().multiplyScalar(2)
+
+    const origin = from.clone().sub(halfWidth)
+    const satPoints = [
+      new SAT.Vector(...origin.toArray()),
+      new SAT.Vector(...from.clone().sub(halfWidth).add(length).sub(origin).toArray()),
+      new SAT.Vector(...from.clone().sub(halfWidth).add(length.clone().add(width)).sub(origin).toArray()),
+      new SAT.Vector(...from.clone().sub(halfWidth).add(width).sub(origin).toArray()),
+    ]
+
+    return new SAT.Polygon(satPoints[0], [new SAT.Vector(), ...satPoints.slice(1)])
+  }
+
+  static fixCollisions(entityPosition, colliders, radius = 0) {
+    const position = entityPosition.clone()
+    let collider = Entity.collider(position.x, position.y, radius)
+    colliders.forEach((c) => {
+      let result = new SAT.Response()
+      if (this.collideObject(collider, c, result)) {
+        position.sub(new Vector2(result.overlapV.x, result.overlapV.y))
+        collider = Entity.collider(position.x, position.y, radius)
+      }
+    })
+
+    return position
+  }
+
+  static polygonToThreeVector2(polygon) {
+    const position = new Vector2(polygon.pos.x, polygon.pos.y)
+    return polygon.points.map((p) => new Vector2(p.x, p.y).add(position))
+  }
 }

src/terrain.js

@@ -9,30 +9,58 @@ export default class Terrain {
 
   #colliders = []
   #vertices = []
+  #unadjustedWaypoints = []
 
   constructor(vertices) {
     this.#vertices = vertices.map((v) => new Vector2(v.x, v.y))
+    if (ShapeUtils.isClockWise(this.#vertices)) {
+      this.#vertices.reverse()
+    }
+
     this.#calculateColliders()
     this.#calculatePosition()
     this.#calculateRelativeVertices()
+    this.#calculateUnadjustedWaypoints()
   }
 
   get colliders() { return this.#colliders }
+  get unadjustedWaypoints() { return this.#unadjustedWaypoints }
   get vertices() { return this.#vertices }
 
+  static waypointsForSide(fromVertex, toVertex, isClockwise = false) {
+    const from = isClockwise ? toVertex : fromVertex
+    const to = isClockwise ? fromVertex : toVertex
+    const origin = new Vector2()
+    const sideNormal = to.clone().sub(from).clone().normalize()
+
+    const margin = sideNormal.clone().rotateAround(origin, -3 * Math.PI / 4)
+    const offset = margin.clone().multiplyScalar(Math.SQRT2)
+    const inverseMargin = sideNormal.clone().negate().rotateAround(origin, 3 * Math.PI / 4)
+    const inverseOffset = inverseMargin.clone().multiplyScalar(Math.SQRT2)
+
+    return [
+      [margin.clone().add(from), offset],
+      [inverseMargin.clone().add(to), inverseOffset],
+    ]
+  }
+
   state() {
     return {
       ...this,
     }
   }
 
-  #calculateColliders() {
+  #shape() {
     const complexShape = new Shape()
 
     complexShape.moveTo(this.#vertices.at(0).x, this.#vertices.at(0).y)
     this.#vertices.slice(1).forEach((v) => complexShape.lineTo(v.x, v.y))
 
-    const points = complexShape.extractPoints(16)
+    return complexShape
+  }
+
+  #calculateColliders() {
+    const points = this.#shape().extractPoints(16)
 
     const indicesToPolygon = (indices) => {
       const satPoints = [
@@ -54,4 +82,8 @@ export default class Terrain {
   #calculateRelativeVertices() {
     this.relativeVertices = this.#vertices.map((v) => v.clone().sub(this.position))
   }
+
+  #calculateUnadjustedWaypoints() {
+    this.#unadjustedWaypoints = this.#vertices.map((v, i, arr) => Terrain.waypointsForSide(v, i + 1 < arr.length ? arr[i + 1] : arr[0])).flat()
+  }
 }