From 05360208b0ac96f046d1eb9a28c3d90130be0b26 Mon Sep 17 00:00:00 2001 From: Thayol Date: Wed, 25 Dec 2024 00:32:33 +0900 Subject: [PATCH] add unoptimized pathfinding --- public/client.js | 5 +-- src/entity.js | 70 ++++++++++++++++++++++++++++---------- src/game.js | 13 ++++++-- src/index.js | 59 +++++++++++++++++++++++++++----- src/pathfind.js | 60 +++++++++++++++++++++++++++++++++ src/priority-queue.js | 78 +++++++++++++++++++++++++++++++++++++++++++ src/satx.js | 69 +++++++++++++++++++++++++++++++++++--- src/terrain.js | 36 ++++++++++++++++++-- 8 files changed, 353 insertions(+), 37 deletions(-) create mode 100644 src/pathfind.js create mode 100644 src/priority-queue.js diff --git a/public/client.js b/public/client.js index 3ca9d68..ba7a8fc 100644 --- a/public/client.js +++ b/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) diff --git a/src/entity.js b/src/entity.js index 786df39..f435ead 100644 --- a/src/entity.js +++ b/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 + } + } } } } diff --git a/src/game.js b/src/game.js index d6570e8..8888d80 100644 --- a/src/game.js +++ b/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) } diff --git a/src/index.js b/src/index.js index 64c1402..3d6c440 100644 --- a/src/index.js +++ b/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() }) diff --git a/src/pathfind.js b/src/pathfind.js new file mode 100644 index 0000000..3972651 --- /dev/null +++ b/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 + } +} diff --git a/src/priority-queue.js b/src/priority-queue.js new file mode 100644 index 0000000..a431f46 --- /dev/null +++ b/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 + } + } +} diff --git a/src/satx.js b/src/satx.js index 26b07f9..c2e544f 100644 --- a/src/satx.js +++ b/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)) + } } diff --git a/src/terrain.js b/src/terrain.js index 25e4efb..a257480 100644 --- a/src/terrain.js +++ b/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() + } }