thayol/instructions-clear
Archived
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix pathfinding nekimegyafalnak style

Browse Source
This commit is contained in:
Thayol
2024-12-25 17:19:45 +09:00
parent 0f8a73911f
commit 8fe48fb679
5 changed files with 266 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files
public/client.js
+4 -2
View File
@@ -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 = {}
src/entity.js
+28 -9
View File
@@ -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))) ?? []
}
}
src/index.js
+63 -51
View File
@@ -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 entity2 = new Entity()
// entity2.id = '2'
// entity2.teleport(110, 110)
// entity2.radius = 50
// game.spawn_entity(entity2)
const horseshoe = new Terrain([
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()
})
src/pathfind.js
+136 -27
View File
@@ -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
}
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}`)
if (Math.abs(mergedWaypoints[i] - mergedWaypoints[j]) < Pathfind.precision && Math.abs(mergedWaypoints[i + 1] - mergedWaypoints[j + 1]) < Pathfind.precision) {
continue
}
const tunnel = SATX.entityTunnel(from, to, radius)
const collider = Entity.collider(from.x, from.y, radius)
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 tunnelClear = !SATX.collideObjects(tunnel, colliders)
const waypointAvailable = !SATX.collideObjects(collider, colliders)
const tunnel = SATX.entityTunnel(mergedWaypoints[i], mergedWaypoints[i + 1], mergedWaypoints[j], mergedWaypoints[j + 1], radius)
if (waypointAvailable && tunnelClear) {
const distance = from.distanceTo(to)
graph.push({ from, to, distance })
graph.push({ from: to, to: from, distance })
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 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
}
}
src/satx.js
+33 -17
View File
@@ -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])
}
}