GizStackGizStack
View .mdOpen in ChatGPTOpen in Claude

gizgame/navigation

where an agent can walk, and the way there. build voxelizes the scene's triangles into columns of CELL meters; a surface is walkable when it is flat enough, has the agent's height of room above it and lies more than the agent's radius from an edge or a wall (erosion). Walkable cells that step into each other (within CLIMB) merge into convex rectangles, greedily row by row, and two rectangles that share an edge share a portal. path finds the rectangles by the shared A* (gizlang/collections) and pulls the string through their portals (the funnel), so a way is the fewest straight legs. Several floors over one column are kept apart: a node is a cell and one surface of it.

gizlang
import * as Navigation from "gizgame/navigation";

Types

Agent

gizlang
type Agent = { radius: f64; height: f64; climb: f64; slope: f64; }

Who walks: its radius, its height, the most it steps up and the steepest slope (radians) it stands on.

  • radius: f64
  • height: f64
  • climb: f64
  • slope: f64

Mesh

gizlang
class Mesh { cell: f64; origin: Point3; columns: i64; rows: i64; agent: Agent; private nodes: Vec<Node> = []; private atCell: Vec<Vec<i64>> = []; rects: Vec<Rect> = []; constructor(cell: f64, origin: Point3, columns: i64, rows: i64, agent: Agent) { this.cell = cell; this.origin = origin; this.columns = columns; this.rows = rows; this.agent = agent; for (const at of range(0, columns*rows)) { this.atCell.push([]); } } private nodeNear(x: i64, z: i64, y: f64): i64 { if (x <0 || z <0 || x> = this.columns || z> = this.rows) { return -1; } let best = -1; let gap = 1e300; for (const n of this.atCell[z*this.columns+x]) { const d = Math.abs(this.nodes[n].y-y); if (this.nodes[n].walkable && d <gap && this.nodes[n].y-y <= this.agent.climb && y-this.nodes[n].y <= 2.0) { best = n; gap = d; } } return best; } nodeAt(p: Point3): i64 { const x = i64(Math.floor((p.x-this.origin.x)/this.cell)); const z = i64(Math.floor((p.z-this.origin.z)/this.cell)); return this.nodeNear(x, z, p.y); } private centerOf(n: i64): Point3 { const node = this.nodes[n]; return point3(this.origin.x+(f64(node.x)+0.5)*this.cell, node.y, this.origin.z+(f64(node.z)+0.5)*this.cell); } build(positions: Vec<f64>, indices: Vec<i64>): void { let heights: Vec<Vec<f64>> = []; let flats: Vec<Vec<bool>> = []; for (const at of range(0, this.columns*this.rows)) { heights.push([]); flats.push([]); } const most = Math.cos(this.agent.slope); for (const t of range(0, indices.length/3)) { const p = (k: i64): Point3 => { const i = indices[3*t+k]*3; return point3(positions[i], positions[i+1], positions[i+2]); }; const a = p(0); const b = p(1); const c = p(2); const n = cross(sub(b, a), sub(c, a)); const nl = length(n); if (nl <1e-12) { continue; } const flat = Math.abs(n.y)/nl> = most; const x0 = Math.maxI64(0, i64(Math.floor((Math.min(a.x, Math.min(b.x, c.x))-this.origin.x)/this.cell))); const x1 = Math.minI64(this.columns-1, i64(Math.floor((Math.max(a.x, Math.max(b.x, c.x))-this.origin.x)/this.cell))); const z0 = Math.maxI64(0, i64(Math.floor((Math.min(a.z, Math.min(b.z, c.z))-this.origin.z)/this.cell))); const z1 = Math.minI64(this.rows-1, i64(Math.floor((Math.max(a.z, Math.max(b.z, c.z))-this.origin.z)/this.cell))); const den = (b.z-c.z)*(a.x-c.x)+(c.x-b.x)*(a.z-c.z); if (Math.abs(den) <1e-12) { continue; } for (const z of range(z0, z1+1)) { for (const x of range(x0, x1+1)) { const px = this.origin.x+(f64(x)+0.5)*this.cell; const pz = this.origin.z+(f64(z)+0.5)*this.cell; const u = ((b.z-c.z)*(px-c.x)+(c.x-b.x)*(pz-c.z))/den; const v = ((c.z-a.z)*(px-c.x)+(a.x-c.x)*(pz-c.z))/den; const w = 1.0-u-v; if (u <-1e-9 || v <-1e-9 || w <-1e-9) { continue; } heights[z*this.columns+x].push(u*a.y+v*b.y+w*c.y); flats[z*this.columns+x].push(flat); } } } for (const z of range(0, this.rows)) { for (const x of range(0, this.columns)) { const k = z*this.columns+x; const hs = heights[k]; for (const i of range(0, hs.length)) { if (!flats[k][i]) { continue; } let free = true; for (const j of range(0, hs.length)) { if (j !== i && hs[j]> hs[i]+0.01 && hs[j] <hs[i]+this.agent.height) { free = false; break; } } if (free && !this.atCell[k].some(n => Math.abs(this.nodes[n].y-hs[i]) <0.05)) { this.atCell[k].push(this.nodes.length); this.nodes.push({ x, z, y: hs[i], walkable: true, rect: -1 }); } } } } this.erode(); this.merge(); } private erode(): void { const passes = i64(Math.ceil(this.agent.radius/this.cell)); for (const pass of range(0, passes)) { let edge: Vec<i64> = []; for (const n of range(0, this.nodes.length)) { const node = this.nodes[n]; if (!node.walkable) { continue; } for (const d of [[1, 0], [-1, 0], [0, 1], [0, -1]]) { if (this.nodeNear(node.x+d[0], node.z+d[1], node.y) <0) { edge.push(n); break; } } } for (const n of edge) { this.nodes[n].walkable = false; } } } private merge(): void { for (const n of range(0, this.nodes.length)) { const start = this.nodes[n]; if (!start.walkable || start.rect> = 0) { continue; } const id = this.rects.length; let x1 = start.x; let row: Vec<i64> = [n]; while (true) { const next = this.nodeNear(x1+1, start.z, this.nodes[row[row.length-1]].y); if (next <0 || this.nodes[next].rect> = 0) { break; } row.push(next); x1+ = 1; } let rows: Vec<Vec<i64>> = [row]; let z1 = start.z; while (true) { let next: Vec<i64> = []; let ok = true; for (const at of range(0, row.length)) { const m = this.nodeNear(start.x+at, z1+1, this.nodes[rows[rows.length-1][at]].y); if (m <0 || this.nodes[m].rect> = 0) { ok = false; break; } next.push(m); } if (!ok) { break; } rows.push(next); z1+ = 1; } let sum = 0.0; let count = 0; for (const r of rows) { for (const m of r) { this.nodes[m].rect = id; sum+ = this.nodes[m].y; count+ = 1; } } this.rects.push({ x0: start.x, z0: start.z, x1, z1, y: sum/f64(count), portals: [] }); } for (const n of range(0, this.nodes.length)) { const node = this.nodes[n]; if (node.rect <0) { continue; } for (const d of [[1, 0], [0, 1]]) { const m = this.nodeNear(node.x+d[0], node.z+d[1], node.y); if (m <0 || this.nodes[m].rect <0 || this.nodes[m].rect === node.rect) { continue; } const y = (node.y+this.nodes[m].y)*0.5; const ex = this.origin.x+f64(node.x+d[0])*this.cell; const ez = this.origin.z+f64(node.z+d[1])*this.cell; const a = d[0] === 1 ? point3(ex, y, ez): point3(ex, y, ez); const b = d[0] === 1 ? point3(ex, y, ez+this.cell): point3(ex+this.cell, y, ez); this.widen(node.rect, this.nodes[m].rect, a, b); this.widen(this.nodes[m].rect, node.rect, a, b); } } } private widen(from: i64, to: i64, a: Point3, b: Point3): void { let r = this.rects[from]; for (const at of range(0, r.portals.length)) { let p = r.portals[at]; if (p.to === to && (Math.abs(p.a.x-a.x) <1e-9 && Math.abs(p.b.x-b.x) <1e-9 || Math.abs(p.a.z-a.z) <1e-9 && Math.abs(p.b.z-b.z) <1e-9)) { p.a = point3(Math.min(p.a.x, a.x), p.a.y, Math.min(p.a.z, a.z)); p.b = point3(Math.max(p.b.x, b.x), p.b.y, Math.max(p.b.z, b.z)); r.portals[at] = p; this.rects[from] = r; return; } } r.portals.push({ to, a, b }); this.rects[from] = r; } path(from: Point3, to: Point3): Vec<Point3> { const s = this.nodeAt(from); const g = this.nodeAt(to); if (s <0 || g <0) { return []; } const start = this.nodes[s].rect; const goal = this.nodes[g].rect; if (start === goal) { return [from, to]; } const middle = (r: i64): Point3 => { const q = this.rects[r]; return point3(this.origin.x+f64(q.x0+q.x1+1)*0.5*this.cell, q.y, this.origin.z+f64(q.z0+q.z1+1)*0.5*this.cell); }; let rects: Vec<i64> = []; try { rects = shortestPath(start, goal, (r: i64): Vec<PathEdge> => this.rects[r].portals.map((p: Portal): PathEdge => ({ to: p.to, cost: length(sub(middle(r), middle(p.to))) })), (r: i64, end: i64): f64 => length(sub(middle(r), middle(end)))); } catch { return []; } if (rects.length === 0) { return []; } let portals: Vec<Portal> = []; for (const k of range(1, rects.length)) { for (const p of this.rects[rects[k-1]].portals) { if (p.to === rects[k]) { portals.push(p); break; } } } return funnel(from, to, portals); } }

The walkable rectangles of a scene.

  • cell: f64
  • origin: gizlang_geometry.Point3
  • columns: i64
  • rows: i64
  • agent: M185.Agent
  • nodes: Vec<M185.Node>
  • atCell: Vec<Vec<i64>>
  • rects: Vec<M185.Rect>

Walker

gizlang
type Walker = { position: Point3; velocity: Point3; way: Vec<Point3>; next: i64; speed: f64; }

A walker of a crowd: where it is, its velocity, its way and the corner it walks to.

  • position: gizlang_geometry.Point3
  • velocity: gizlang_geometry.Point3
  • way: Vec<gizlang_geometry.Point3>
  • next: i64
  • speed: f64

Functions

agent

gizlang
function agent(radius: f64 = 0.4, height: f64 = 1.8): Agent

An agent of RADIUS and HEIGHT that steps up 0.4 and stands on slopes up to 45 degrees.

constructor

gizlang
constructor(cell: f64, origin: Point3, columns: i64, rows: i64, agent: Agent)

An empty mesh of COLUMNS x ROWS cells of CELL meters from ORIGIN, for AGENT; build fills it.

Effects: fail, read, write, alloc.

walker

gizlang
function walker(position: Point3, speed: f64): Walker

A walker at POSITION of SPEED meters a second, with no way yet.

Effects: fail, alloc.

steer

gizlang
function steer(mesh: Mesh, w: Walker, others: Vec<Walker>): Walker

The velocity that W wants: toward its next corner (it passes a corner within a half cell), and away from the walkers of OTHERS within two radii of it, so a crowd spreads instead of piling up.

Effects: fail, read.

Methods

Mesh.nodeAt

gizlang
nodeAt(p: Point3): i64

The walkable node under the world point P, else -1.

Effects: fail, read.

Mesh.build

gizlang
build(positions: Vec<f64>, indices: Vec<i64>): void

Builds the nodes, erodes them and merges them into rectangles with portals.

Effects: fail, read, write, alloc.

Mesh.path

gizlang
path(from: Point3, to: Point3): Vec<Point3>

The corners of the shortest way from FROM to TO over walkable ground, both ends included; empty when TO cannot be reached.

Effects: fail, read, write, console, io, alloc.

Walker.toward

gizlang
function Walker.toward(self: Walker, mesh: Mesh, goal: Point3): Walker

W with a new way on MESH from where it is to GOAL.

Effects: fail, read, write, console, io, alloc.