diff --git a/public/client.js b/public/client.js index ba7a8fc..9d295f9 100644 --- a/public/client.js +++ b/public/client.js @@ -13,12 +13,14 @@ camera.rotation.set((60 / 180) * Math.PI, 0, 0) const entityMaterial = new THREE.MeshToonMaterial({ color: 0xffffff }) const terrainMaterial = new THREE.MeshToonMaterial({ color: 0xffd700 }) -const minimapCamera = new THREE.OrthographicCamera(-10, 10, 10, -10) +// const minimapCamera = new THREE.OrthographicCamera(-10, 10, 10, -10) +const minimapCamera = new THREE.OrthographicCamera(-6, 6, 6, -6) const minimapRenderer = new THREE.WebGLRenderer() minimapRenderer.setSize(600, 600) minimapRenderer.setAnimationLoop(minimapRender) -minimapCamera.position.set(10, 10, 10) +// minimapCamera.position.set(10, 10, 10) +minimapCamera.position.set(6, 6, 6) const entities = {} const terrains = {} diff --git a/src/entity.js b/src/entity.js index cb1b635..89d1c72 100644 --- a/src/entity.js +++ b/src/entity.js @@ -2,6 +2,7 @@ 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() @@ -94,15 +95,29 @@ export default class Entity { if (this.#path.length < 1 || !this.#path.at(-1).equals(fixedDest)) { console.time('pathfinding') - // console.time('waypoints') - const waypoints = this.waypoints().concat([this.position, fixedDest]) - // console.timeEnd('waypoints') - // console.time('graph') + console.time('waypoints') + const start = SATX.vectorToFloat32Array(this.position) + const goal = SATX.vectorToFloat32Array(fixedDest) + const nonUniqueWaypoints = this.waypoints().map((w) => SATX.vectorToFloat32Array(w)).concat([start, goal]) + const waypoints = Pathfind.uniqueWaypoints(nonUniqueWaypoints) + console.timeEnd('waypoints') + console.time('graph') const graph = Pathfind.buildGraph(waypoints, collidables, this.radius) - // console.timeEnd('graph') - // console.time('path') - this.#path = Pathfind.shortestPath(graph, this.position, fixedDest) - // console.timeEnd('path') + + // console.log(Pathfind.formatFloat32Array(graph, 5, true)) + // const tunnels = [] + // for (let i = 0; i < graph.length; i += 5) { + // tunnels.push(SATX.entityTunnel(graph[i], graph[i + 1], graph[i + 2], graph[i + 3], 1)) + // } + + // tunnels.map((t) => SATX.satPolygonToVectors(t)).forEach((t) => this.#game.add_terrain(new Terrain(t))) + // this.#dest = null + + console.timeEnd('graph') + console.time('path') + this.#path = Pathfind.shortestPath(graph, start, goal).map((waypoint) => new Vector2(waypoint[0], waypoint[1])) + console.log(this.#path) + console.timeEnd('path') console.timeEnd('pathfinding') } @@ -140,6 +155,10 @@ export default class Entity { } waypoints() { - return this.game?.unadjustedWaypoints.map(([waypoint, direction]) => waypoint.clone().add(direction.clone().multiplyScalar(this.radius))) ?? [] + const entityColliders = (this.game?.entities ?? []).filter((e) => e.id != this.id) + const terrainColliders = (this.game?.terrains ?? []) + const unadjustedWaypoints = entityColliders.concat(terrainColliders).map((e) => e.unadjustedWaypoints).flat() + + return unadjustedWaypoints.map(([waypoint, direction]) => waypoint.clone().add(direction.clone().multiplyScalar(this.radius))) ?? [] } } diff --git a/src/index.js b/src/index.js index 9ae8bed..83cf888 100644 --- a/src/index.js +++ b/src/index.js @@ -46,66 +46,78 @@ app.listen(port, () => { entity1.radius = 50 game.spawn_entity(entity1) - const entity2 = new Entity() - entity2.id = '2' - entity2.teleport(110, 110) - entity2.radius = 50 - game.spawn_entity(entity2) - - const horseshoe = new Terrain([ + // const entity2 = new Entity() + // entity2.id = '2' + // entity2.teleport(110, 110) + // entity2.radius = 50 + // game.spawn_entity(entity2) + + const triangle = new Terrain([ { x: 400, y: 200 }, - { x: 600, y: 200 }, - { x: 700, y: 300 }, - { x: 650, y: 600 }, { x: 400, y: 600 }, - { x: 400, y: 450 }, - { x: 600, y: 500 }, { x: 600, y: 300 }, - { x: 400, y: 300 }, ]) - horseshoe.id = 'horseshoe' - game.add_terrain(horseshoe) + triangle.id = 'triangle' + game.add_terrain(triangle) - 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 horseshoe = new Terrain([ + // { x: 400, y: 200 }, + // { x: 600, y: 200 }, + // { x: 700, y: 300 }, + // { x: 650, y: 600 }, + // { x: 400, y: 600 }, + // { x: 400, y: 450 }, + // { x: 600, y: 500 }, + // { x: 600, y: 300 }, + // { x: 400, y: 300 }, + // ]) + // horseshoe.id = 'horseshoe' + // game.add_terrain(horseshoe) - 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 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 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 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 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) + // 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) + + entity1.moveAction(1000, 500) + + // setTimeout(() => entity1.moveAction(100, 400), 10) game.start() }) diff --git a/src/pathfind.js b/src/pathfind.js index 8620963..30d679f 100644 --- a/src/pathfind.js +++ b/src/pathfind.js @@ -1,12 +1,42 @@ -import Entity from "./entity.js" -import PriorityQueue from "./priority-queue.js" -import SATX from "./satx.js" +import { Path, Vector2 } from 'three' +import Entity from './entity.js' +import PriorityQueue from './priority-queue.js' +import SATX from './satx.js' export default class Pathfind { + static precision = 0.001 + static multiplier = 1000 // (1 / this.precision) + static key(pos) { return `${pos.x},${pos.y}` } + static floatKey4(a, b, c, d) { + return Math.floor(a * Pathfind.multiplier) + + Math.floor(b * Pathfind.multiplier) * Pathfind.multiplier + + Math.floor(c * Pathfind.multiplier) * Pathfind.multiplier ** 2 + + Math.floor(d * Pathfind.multiplier) * Pathfind.multiplier ** 3 + } + + static floatKey2(a, b) { + return Math.floor(a * Pathfind.multiplier) + + Math.floor(b * Pathfind.multiplier) * Pathfind.multiplier + } + + static uniqueWaypoints(waypoints) { + const included = new Set() + const uniqueWaypoints = [] + for (const waypoint of waypoints) { + const key = Pathfind.floatKey2(waypoint[0], waypoint[1]) + if (!included.has(key)) { + included.add(key) + uniqueWaypoints.push(waypoint) + } + } + + return uniqueWaypoints + } + static shortestPath(graph, start, goal) { const queue = new PriorityQueue((a, b) => a[1] < b[1]) const visited = new Map() @@ -17,19 +47,26 @@ export default class Pathfind { const [path, cost] = queue.pop() const waypoint = path.at(-1) - if (waypoint.equals(goal)) { + if (Math.abs(waypoint[0] - goal[0]) < Pathfind.precision && Math.abs(waypoint[1] - goal[1]) < Pathfind.precision) { path.shift() return path } - const waypointKey = this.key(waypoint) + const waypointKey = Pathfind.floatKey2(waypoint) if (!visited.has(waypointKey) || visited.get(waypointKey) > cost) { visited.set(waypointKey, cost) - for (const { to, distance } of graph.filter(e => e.from.equals(waypoint))) { - const toKey = this.key(to) - if (!visited.has(toKey) || visited.get(toKey) > cost + distance) { - queue.push([[...path, to], cost + distance]) + for (let i = 0; i < graph.length; i += 5) { + if (Math.abs(waypoint[0] - graph[i]) < Pathfind.precision && Math.abs(waypoint[1] - graph[i + 1]) < Pathfind.precision) { + continue + } + + const nextKey = `${graph[i + 2]},${graph[i + 3]}` + if (!visited.has(nextKey) || visited.get(nextKey) > cost + graph[i + 4]) { + const next = new Float32Array(2) + next[0] = graph[i + 2] + next[1] = graph[i + 3] + queue.push([[...path, next], cost + graph[i + 4]]) } } } @@ -38,36 +75,108 @@ export default class Pathfind { return [] } - static buildGraph(waypoints = [], colliders = [], radius = 0) { - const graph = [] - const calculated = new Set() + static buildGraph(waypoints = [], colliders = [], radius = 0, mergeNodes = true) { + const filteredWaypoints = [] + const checked = new Set() - for (const from of waypoints) { - for (const to of waypoints) { - if (from.equals(to)) { + if (radius > 0) { + for (const waypoint of waypoints) { + const collider = Entity.collider(waypoint[0], waypoint[1], radius) + const waypointAvailable = !SATX.collideObjects(collider, colliders) + if (waypointAvailable) { + filteredWaypoints.push(waypoint) + } + } + } + + const mergedWaypoints = new Float32Array(filteredWaypoints.length * 2) + let mergedWaypointsIndex = 0 + for (const waypoint of filteredWaypoints) { + mergedWaypoints[mergedWaypointsIndex] = waypoint[0] + mergedWaypoints[mergedWaypointsIndex + 1] = waypoint[1] + mergedWaypointsIndex += 2 + } + + const nodes = [] + for (let i = 0; i < mergedWaypoints.length; i += 2) { + for (let j = 0; j < mergedWaypoints.length; j += 2) { + if (i == j) { + continue + } + + if (Math.abs(mergedWaypoints[i] - mergedWaypoints[j]) < Pathfind.precision && Math.abs(mergedWaypoints[i + 1] - mergedWaypoints[j + 1]) < Pathfind.precision) { 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 key = Pathfind.floatKey4(mergedWaypoints[i], mergedWaypoints[i + 1], mergedWaypoints[j], mergedWaypoints[j + 1]) + if (!checked.has(key)) { + checked.add(key) + checked.add(Pathfind.floatKey4(mergedWaypoints[j], mergedWaypoints[j + 1], mergedWaypoints[i], mergedWaypoints[i + 1])) - const tunnel = SATX.entityTunnel(from, to, radius) - const collider = Entity.collider(from.x, from.y, radius) + const tunnel = SATX.entityTunnel(mergedWaypoints[i], mergedWaypoints[i + 1], mergedWaypoints[j], mergedWaypoints[j + 1], radius) + + if (!SATX.collideObjects(tunnel, colliders)) { + const node = new Float32Array(5) + node[0] = mergedWaypoints[i] + node[1] = mergedWaypoints[i + 1] + node[2] = mergedWaypoints[j] + node[3] = mergedWaypoints[j + 1] + node[4] = Math.hypot(mergedWaypoints[j] - mergedWaypoints[i], mergedWaypoints[j + 1] - mergedWaypoints[i + 1]) + nodes.push(node) - 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 }) - graph.push({ from: to, to: from, distance }) + const reverseNode = new Float32Array(5) + reverseNode[0] = mergedWaypoints[j] + reverseNode[1] = mergedWaypoints[j + 1] + reverseNode[2] = mergedWaypoints[i] + reverseNode[3] = mergedWaypoints[i + 1] + reverseNode[4] = node[4] // distance is the same, copying is less expensive + nodes.push(reverseNode) } } } } + if (!mergeNodes) { + return nodes + } + + const graph = new Float32Array(nodes.length * 5) + let graphIndex = 0 + for (const node of nodes) { + graph[graphIndex] = node[0] + graph[graphIndex + 1] = node[1] + graph[graphIndex + 2] = node[2] + graph[graphIndex + 3] = node[3] + graph[graphIndex + 4] = node[4] + graphIndex += 5 + } + return graph } + + static formatFloat32Array(array, columns = 2, text = false) { + const formatted = [] + let columnWidth = 0 + for (let i = 0; i < array.length; i += columns) { + const row = [] + for (let j = i; j < i + columns; j++) { + if (text) { + row.push(`${array[j]}`) + if (`${array[j]}`.length > columnWidth) { + columnWidth = `${array[j]}`.length + } + } + else { + row.push(array[j]) + } + } + formatted.push(row) + } + + if (text) { + return formatted.map((row) => row.map((v) => v.padEnd(columnWidth, ' ')).join(' | ')).join('\n') + } + + return formatted + } } diff --git a/src/satx.js b/src/satx.js index fab4dcb..d940df8 100644 --- a/src/satx.js +++ b/src/satx.js @@ -49,24 +49,24 @@ export default class SATX { return 1 / Math.cos(Math.PI / numberOfVertices) } - static entityTunnel(from, to, radius = 0) { + static entityTunnel(fromX, fromY, toX, toY, radius = 0) { if (radius <= 0) { - return this.line(from, to) + return this.line(fromX, fromY, toX, toY) } - 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 sides = new Float32Array(5) + sides[0] = toX - fromX + sides[1] = toY - fromY + sides[4] = Math.hypot(sides[0], sides[1]) + sides[2] = (sides[1] / sides[4]) * -radius // optimization: negation and swapping rotates + sides[3] = (sides[0] / sides[4]) * radius - 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)]) + return new SAT.Polygon(new SAT.Vector(fromX - sides[2], fromY - sides[3]), [ + new SAT.Vector(), + new SAT.Vector(sides[0], sides[1]), + new SAT.Vector(sides[0] + (2 * sides[2]), sides[1] + (2 * sides[3])), + new SAT.Vector(2 * sides[2], 2 * sides[3]), + ]) } static fixCollisions(entityPosition, colliders, radius = 0) { @@ -83,12 +83,28 @@ export default class SATX { return position } - static line(from, to) { - return new SAT.Polygon(new SAT.Vector(...from.toArray()), [new SAT.Vector(), new SAT.Vector(...to.clone().sub(from).toArray())]) + static line(fromX, fromY, toX, toY) { + return new SAT.Polygon(new SAT.Vector(fromX, fromY), [new SAT.Vector(), new SAT.Vector(toX - fromX, toY - fromY)]) } - static polygonToThreeVector2(polygon) { + static satPolygonToVectors(polygon) { const position = new Vector2(polygon.pos.x, polygon.pos.y) return polygon.points.map((p) => new Vector2(p.x, p.y).add(position)) } + + static vectorToFloat32Array(vector) { + const array = new Float32Array(2) + array[0] = vector.x + array[1] = vector.y + + return array + } + + static float32ArrayToVector(array) { + return new Vector2(array[0], array[1]) + } + + static float32ArrayWithIndexToVector(array, index) { + return new Vector2(array[index], array[index + 1]) + } }