lecodes-sdk 0.19.0

package/src/gl/NavAgent.ts

@@ -0,0 +1,337 @@
+// A navmesh agent, as an aspect on a 3D Node (docs/navmesh-plan.md §5). The GAME decides where it
+// goes and when; DetourCrowd (creator-nav, one update per fixed step for the whole crowd) does the
+// corridor, steering, path replanning and local avoidance; this aspect applies its slot to the node:
+//
+//   const op = new Node().aspect(NavAgent, { radius: 0.4, speed: 3.2 })
+//   scene.add(op)
+//   op.agent.goTo(player.position)                 // false = nothing to walk on there
+//   op.agent.on('arrive', () => engage())
+//   if (op.agent.state === 'blocked') op.agent.goTo(nav.randomPoint(op.position, 8)!)
+//
+// Two ways to move the node (`drive`):
+//   • 'position' (default) — the crowd owns the transform: every host including headless, no
+//     physics. The node is placed on the navmesh surface (heights are quantized to the bake's
+//     cell height, 10 cm by default).
+//   • 'controller' — the node has a CharacterController: the crowd's velocity becomes its `move`
+//     command each fixed step, the capsule collides with everything, and the crowd slot is warped
+//     back to the body when they diverge (a push-back off the mesh is corrected by Detour itself).
+// Facing turns toward the velocity at `turnSpeed` (0 = leave the rotation alone).
+//
+// Without a loaded NavMesh the aspect is inert — `goTo` returns false, nothing throws — so a
+// project stays runnable on a host without navigation.
+
+import { Aspect } from "../core/Aspect"
+import type { FieldMeta } from "../core/fields"
+import { Vec3, cx, cy, cz, type Vec3Like } from "../math/vec"
+import { CharacterController } from "./CharacterController"
+import { NavCrowd, NavMesh, NAV_AGENT, NAV_READ } from "./NavMesh"
+import type { Node } from "./Node"
+
+export type NavAgentState = "idle" | "moving" | "arrived" | "blocked" | "offmesh"
+export type NavAgentAvoidance = "off" | "low" | "medium" | "high"
+export type NavAgentDrive = "position" | "controller"
+
+export type NavAgentEvents = {
+  /** Reached the target (within `stopDistance`). */
+  arrive: () => void
+  /** The target cannot be reached — the agent stopped at the closest point (or nowhere near). */
+  blocked: () => void
+}
+
+const AVOIDANCE: Record<NavAgentAvoidance, number> = { off: -1, low: 0, medium: 1, high: 3 }
+const DEG2RAD = Math.PI / 180
+const JOIN_RETRY_STEPS = 30
+
+export class NavAgent extends Aspect<"agent", Node, NavAgentEvents> {
+  static readonly aspect = "agent"
+  /** The crowd advances first, then every agent applies its slot. */
+  static after = [ NavCrowd ]
+
+  static fields: FieldMeta<NavAgent> = {
+    radius: { min: 0.05, max: 5, step: 0.05 },
+    height: { min: 0.1, max: 10, step: 0.1 },
+    speed: { min: 0, max: 50, step: 0.1 },
+    acceleration: { min: 0.1, max: 200, step: 0.5 },
+    turnSpeed: { min: 0, max: 3600, step: 10 },
+    separation: { min: 0, max: 10, step: 0.1 },
+    stopDistance: { min: 0, max: 10, step: 0.05 },
+  }
+
+  private _radius = 0.4
+  private _height = 1.8
+  private _speed = 3
+  private _acceleration = 8
+  private _avoidance: NavAgentAvoidance = "medium"
+  private _separation = 2
+  private _include: string[] | undefined
+  private _exclude: string[] | undefined
+  /** deg/s the node turns toward its velocity; 0 = never touch the rotation. */
+  turnSpeed = 540
+  /** How the node follows its slot. */
+  drive: NavAgentDrive = "position"
+  /** Reached = within this many metres (along the corridor) of the target. */
+  stopDistance = 0.3
+
+  /** Agent radius (metres). Defaults to the navmesh's. */
+  get radius(): number { return this._radius }
+  set radius(v: number) { this._radius = v; this._dirty = true }
+  get height(): number { return this._height }
+  set height(v: number) { this._height = v; this._dirty = true }
+  /** Top speed (m/s). */
+  get speed(): number { return this._speed }
+  set speed(v: number) { this._speed = v; this._dirty = true }
+  get acceleration(): number { return this._acceleration }
+  set acceleration(v: number) { this._acceleration = v; this._dirty = true }
+  /** Local (agent-vs-agent) avoidance quality. */
+  get avoidance(): NavAgentAvoidance { return this._avoidance }
+  set avoidance(v: NavAgentAvoidance) { this._avoidance = v; this._dirty = true }
+  /** Separation force weight (0 = none). */
+  get separation(): number { return this._separation }
+  set separation(v: number) { this._separation = v; this._dirty = true }
+  /** Area names this agent may walk (default: all). */
+  get include(): string[] | undefined { return this._include }
+  set include(v: string[] | undefined) { this._include = v; this._dirty = true }
+  /** Area names this agent never enters. */
+  get exclude(): string[] | undefined { return this._exclude }
+  set exclude(v: string[] | undefined) { this._exclude = v; this._dirty = true }
+
+  state: NavAgentState = "idle"
+  /** The current velocity after avoidance (m/s). */
+  readonly velocity = new Vec3(0, 0, 0)
+  /** The velocity the corridor asked for, before avoidance. */
+  readonly desiredVelocity = new Vec3(0, 0, 0)
+  /** Metres left along the corridor (-1 = no target). */
+  remainingDistance = -1
+  /** Where the agent is going, or null. */
+  get target(): Vec3 | null { return this._target }
+  get hasPath(): boolean { return this._target !== null }
+
+  private _crowd: NavCrowd | null = null
+  private _slot = -1
+  private _target: Vec3 | null = null
+  /** velocity-driven (`move`) rather than target-driven */
+  private _steering = false
+  private _dirty = false
+  private _faceX = 0
+  private _faceZ = -1
+  private _retryAt = 0
+  private _lastSeen = -1
+  private _still = 0
+  private _near = 0
+  private _slow = 0
+
+  onAttach(): void {
+    this.state = "idle"
+    this._join()
+  }
+
+  onReconfigure(): void { this._dirty = true }
+
+  onDetach(): void {
+    this._leave()
+  }
+
+  /** Walk to a point. False = no walkable polygon near it (or no navmesh) — the current target stays. */
+  goTo(target: Vec3Like): boolean {
+    if (this._slot < 0 && !this._join()) return false
+    const ok = _creatorNav.crowdSetTarget(this._crowd!._id, this._slot, cx(target), cy(target), cz(target))
+    if (!ok) return false
+    this._target = new Vec3(cx(target), cy(target), cz(target))
+    this._steering = false
+    this.state = "moving"
+    this._still = 0
+    this._near = 0
+    this._slow = 0
+    return true
+  }
+
+  /** Drop the target and decelerate to a stop. */
+  stop(): void {
+    if (this._slot >= 0) _creatorNav.crowdResetTarget(this._crowd!._id, this._slot)
+    this._target = null
+    this._steering = false
+    this.remainingDistance = -1
+    this.state = "idle"
+  }
+
+  /**
+   * Steer by velocity instead of a target — a player on WASD, a scripted push: the crowd keeps
+   * the node on the mesh and avoids the other agents, no path and no arrival. Call it every fixed
+   * step (the last velocity holds until the next call); `move(0, 0)` stops. Cancels `goTo`.
+   * False = no navmesh / off the mesh.
+   */
+  move(vx: number, vz: number): boolean {
+    if (this._slot < 0 && !this._join()) return false
+    this._target = null
+    this.remainingDistance = -1
+    if (vx === 0 && vz === 0) {
+      if (this._steering) { _creatorNav.crowdResetTarget(this._crowd!._id, this._slot); this._steering = false }
+      this.state = "idle"
+      return true
+    }
+    _creatorNav.crowdSetVelocity(this._crowd!._id, this._slot, vx, 0, vz)
+    this._steering = true
+    this.state = "moving"
+    return true
+  }
+
+  /** Teleport (a spawn, a respawn). The target, if any, is kept. */
+  warp(p: Vec3Like): void {
+    this.node.position = [ cx(p), cy(p), cz(p) ]
+    if (this._slot >= 0 && !_creatorNav.crowdWarp(this._crowd!._id, this._slot, cx(p), cy(p), cz(p))) {
+      // off the mesh now — leave the crowd and re-join when it is back on
+      this._leave()
+      this._retryAt = 0
+    }
+  }
+
+  private params(): Float32Array {
+    const p = new Float32Array(NAV_AGENT.COUNT)
+    const mesh = this._crowd?._mesh
+    p[NAV_AGENT.RADIUS] = this._radius
+    p[NAV_AGENT.HEIGHT] = this._height
+    p[NAV_AGENT.MAX_SPEED] = this._speed
+    p[NAV_AGENT.MAX_ACCELERATION] = this._acceleration
+    p[NAV_AGENT.COLLISION_QUERY_RANGE] = 0
+    p[NAV_AGENT.PATH_OPTIMIZATION_RANGE] = 0
+    p[NAV_AGENT.SEPARATION_WEIGHT] = this._separation
+    p[NAV_AGENT.AVOIDANCE] = AVOIDANCE[this._avoidance] ?? 1
+    p[NAV_AGENT.INCLUDE_FLAGS] = mesh ? mesh.flags(this._include) : 0xffff
+    p[NAV_AGENT.EXCLUDE_FLAGS] = mesh && this._exclude ? mesh.flags(this._exclude) : 0
+    p[NAV_AGENT.ANTICIPATE_TURNS] = 1
+    p[NAV_AGENT.OPTIMIZE_VIS] = 1
+    p[NAV_AGENT.OPTIMIZE_TOPO] = 1
+    return p
+  }
+
+  /** @internal Register with the active navmesh's crowd. False = no navmesh / off the mesh. */
+  _join(): boolean {
+    const mesh = NavMesh.active
+    if (!mesh || typeof _creatorNav === "undefined") return false
+    const crowd = mesh._crowd()
+    if (!crowd._ensureCapacity()) return false
+    if (mesh._maxRadius < this._radius) mesh._maxRadius = this._radius
+    const p = this.node.worldPosition
+    const slot = _creatorNav.crowdAdd(crowd._id, p.x, p.y, p.z, this.params())
+    if (slot < 0) { this._retryAt = crowd._step + JOIN_RETRY_STEPS; return false }
+    this._crowd = crowd
+    this._slot = slot
+    this._dirty = false
+    this._lastSeen = -1
+    crowd._agents.add(this)
+    if (this._target && !_creatorNav.crowdSetTarget(crowd._id, slot, this._target.x, this._target.y, this._target.z)) this._target = null
+    return true
+  }
+
+  /** @internal The crowd was re-created (capacity grew): take a fresh slot, keep the target. */
+  _rejoin(): void {
+    const crowd = this._crowd
+    if (!crowd) return
+    const p = this.node.worldPosition
+    const slot = _creatorNav.crowdAdd(crowd._id, p.x, p.y, p.z, this.params())
+    this._slot = slot
+    if (slot >= 0 && this._target) _creatorNav.crowdSetTarget(crowd._id, slot, this._target.x, this._target.y, this._target.z)
+  }
+
+  /** @internal Leave the crowd (detach, navmesh disposal). */
+  _leave(): void {
+    if (this._crowd && this._slot >= 0 && typeof _creatorNav !== "undefined" && this._crowd._id) {
+      _creatorNav.crowdRemove(this._crowd._id, this._slot)
+    }
+    this._crowd?._agents.delete(this)
+    this._crowd = null
+    this._slot = -1
+  }
+
+  updateFixed(dt: number): void {
+    if (this._slot < 0) {
+      // no crowd yet (the navmesh loaded after the attach, or the spawn was off the mesh): retry
+      if (NavMesh.active && (this._crowd === null ? true : this._crowd._step >= this._retryAt)) {
+        if (!this._join()) this._retryAt = (NavMesh.active._crowd()._step) + JOIN_RETRY_STEPS
+      }
+      return
+    }
+    const crowd = this._crowd!
+    if (crowd._mesh !== NavMesh.active) { this._leave(); return }
+    if (this._dirty) { this._dirty = false; _creatorNav.crowdSetParams(crowd._id, this._slot, this.params()) }
+    if (crowd._step === this._lastSeen) return   // the crowd has not advanced since (a frame with no step)
+    this._lastSeen = crowd._step
+    const b = crowd._buf
+    const o = this._slot * NAV_READ.STRIDE
+    const st = b[o + NAV_READ.STATE]!
+    if (st === 0) { this._leave(); return }
+    const px = b[o + NAV_READ.POS_X]!, py = b[o + NAV_READ.POS_Y]!, pz = b[o + NAV_READ.POS_Z]!
+    const vx = b[o + NAV_READ.VEL_X]!, vy = b[o + NAV_READ.VEL_Y]!, vz = b[o + NAV_READ.VEL_Z]!
+    this.velocity.x = vx; this.velocity.y = vy; this.velocity.z = vz
+    this.desiredVelocity.x = b[o + NAV_READ.DVEL_X]!; this.desiredVelocity.y = b[o + NAV_READ.DVEL_Y]!; this.desiredVelocity.z = b[o + NAV_READ.DVEL_Z]!
+    const remaining = b[o + NAV_READ.REMAINING]!
+    this.remainingDistance = remaining
+
+    // the node
+    const node = this.node
+    if (this.drive === "controller") {
+      const cc = node.get(CharacterController)
+      if (cc) {
+        cc.move(vx, vz)
+        // the body is the truth: re-sync the slot when the capsule ended up elsewhere
+        const wp = node.worldPosition
+        const dx = wp.x - px, dz = wp.z - pz
+        if (dx * dx + dz * dz > this._radius * this._radius * 0.25) _creatorNav.crowdWarp(crowd._id, this._slot, wp.x, wp.y, wp.z)
+      } else {
+        node.position = [ px, py, pz ]
+      }
+    } else {
+      node.position = [ px, py, pz ]
+    }
+    if (this.turnSpeed > 0) this.face(vx, vz, dt)
+
+    // the state machine — arrival / blocked are decided here, in JS. A crowd never lets everyone
+    // reach the same point: the first agent stops ON it and avoidance holds the rest jostling in a
+    // ring around it — so a second spent within a few radii of the goal counts as arrived, and
+    // standing still anywhere else for 3 s (a jam, a door that closed) counts as blocked.
+    if (this._target) {
+      const ts = b[o + NAV_READ.TARGET_STATE]!
+      const partial = b[o + NAV_READ.PARTIAL]! > 0.5
+      const speed = Math.hypot(vx, vz)
+      if (ts === 1) {
+        this.finish("blocked")
+      } else if (ts === 2 && remaining >= 0 && remaining <= this.stopDistance) {
+        this.finish(partial ? "blocked" : "arrived")
+      } else {
+        // near = a few radii out (the first ring); slow-near = creeping in the outer rings of a big crowd
+        const r = this._radius
+        const valid = ts === 2 && remaining >= 0
+        this._near = valid && remaining <= this.stopDistance + r * 8 ? this._near + 1 : 0
+        this._slow = valid && remaining <= this.stopDistance + r * 16 && speed < 0.7 ? this._slow + 1 : 0
+        this._still = speed < 0.15 ? this._still + 1 : 0
+        if (this._near >= 60 || this._slow >= 120) this.finish(partial ? "blocked" : "arrived")
+        else if (this._still >= 180) this.finish("blocked")
+        else this.state = st === 3 ? "offmesh" : "moving"
+      }
+    }
+  }
+
+  private finish(state: "arrived" | "blocked"): void {
+    _creatorNav.crowdResetTarget(this._crowd!._id, this._slot)
+    this._target = null
+    this.state = state
+    this.remainingDistance = 0
+    this.emit(state === "arrived" ? "arrive" : "blocked")
+  }
+
+  private face(vx: number, vz: number, dt: number): void {
+    const speed = Math.hypot(vx, vz)
+    if (speed < 0.05) return
+    const a0 = Math.atan2(this._faceX, this._faceZ)
+    const a1 = Math.atan2(vx / speed, vz / speed)
+    let delta = a1 - a0
+    while (delta > Math.PI) delta -= 2 * Math.PI
+    while (delta < -Math.PI) delta += 2 * Math.PI
+    const max = this.turnSpeed * DEG2RAD * dt
+    const a = a0 + Math.max(-max, Math.min(max, delta))
+    this._faceX = Math.sin(a)
+    this._faceZ = Math.cos(a)
+    const p = this.node.position
+    this.node.lookAt([ cx(p) + this._faceX, cy(p), cz(p) + this._faceZ ], "-z")
+  }
+}