Skip to content

A fast 2D and 3D game engine — TypeScript SDK, C++/WebAssembly core, visual editor. Ship one project to web, desktop, WeChat MiniGames, playable ads, and native Android / iOS.

feat(nav): agents give way to each other

esenginec1dbbeafda33
diff --git a/CHANGELOG.md b/CHANGELOG.md
index 0e07f3ce6..5e441ee5d 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -170,6 +170,26 @@ published separately; it ships inside the editor.
than the box around them. Nothing waits for the solver any more: the geometry is
in the scene from the first frame.
+- **Agents give way to each other.** A route is planned against a world that does
+ not move; agents do, and a dozen sent to one place all planned the same route
+ and walked it as one body. An agent whose `radius` is greater than zero now
+ declares that it IS a body and steers round the other bodies on the same ground:
+ a spread of candidate velocities around the one it wanted, scored on how soon
+ each runs into somebody and how far it strays. Reciprocal — everyone assumes
+ everyone else is giving way too, so each takes half and two meeting head-on part
+ instead of both dodging the same way and meeting again. Where the arrangement is
+ a perfect mirror, the convention that parts them is keep right.
+
+ Sampled rather than solved, because a sample can be REFUSED: every candidate is
+ checked against the navigable world before it is taken, so steering never walks
+ an agent into a wall. Two bodies too wide to pass in one corridor therefore do
+ not pass — waiting is the honest answer and going through the wall is not.
+ `NavSurface` gained the question that makes it possible, `isNavigable`, which is
+ not the same question as "is there a route here". An agent with `radius` left at
+ `0` is routed as a point and neither gives way nor is given way to, which is what
+ every flat game had before this. New editor check `nav3d-crowd` walks four agents
+ across the same square and watches how close they ever come.
+
- **A scene can say there is a way where the ground has none.** A mesh baked from
what an agent can walk says two floors with a gap between them are two places,
and it is right. `NavLink` is where a scene says what the floor does not: this
diff --git a/desktop b/desktop
index 2a0db5583..a90cf390c 160000
--- a/desktop
+++ b/desktop
@@ -1 +1 @@
-Subproject commit 2a0db558337c0247c26db35a4ee1390ccf0d3c65
+Subproject commit a90cf390c9cfaee8776997d3a913f628f68478b4
diff --git a/docs/astro/src/content/docs/gameplay/ai/navigation.mdx b/docs/astro/src/content/docs/gameplay/ai/navigation.mdx
index 21ed8bd16..21c8d94a7 100644
--- a/docs/astro/src/content/docs/gameplay/ai/navigation.mdx
+++ b/docs/astro/src/content/docs/gameplay/ai/navigation.mdx
@@ -252,6 +252,33 @@ twice `halfHeight` plus the radius) so the mesh is the set of places that body f
Without a controller the agent's Transform is moved along the path directly, which
is what every flat game wants and what a 3D one gets before it has a body.
+## Agents give way to each other
+
+A route is planned against a world that does not move. Agents do, and a dozen of
+them sent to one place all plan the same route and walk it as one body. An agent
+whose `radius` is greater than zero declares that it IS a body, and from then on
+it steers round the other bodies walking the same ground:
+
+```typescript
+cmds.spawn()
+ .insert(Transform, { position: { x: 0, y: 0, z: 0 } })
+ .insert(NavAgent, { speed: 300, radius: 40, arriveRadius: 60 });
+```
+
+Everyone assumes everyone else is doing the same, so each takes half the
+avoidance and two meeting head-on part rather than both dodging the same way and
+meeting again. Where that is not enough to break the tie — a perfect mirror —
+the convention is keep right.
+
+A step aside is checked against the navigable world before it is taken, so
+steering never walks an agent into a wall. Two bodies too wide to pass in one
+corridor therefore do not pass: waiting is the honest answer, and going through
+the wall is not.
+
+An agent with `radius` left at `0` is routed as a point, gives way to nobody and
+is given way to by nobody — which is what every flat game had before this, and
+what a scripted mover still wants.
+
## Agents have width
On a **grid**, set `radius` to the body's actual half-width and planning keeps it
diff --git a/docs/astro/src/content/docs/zh-cn/gameplay/ai/navigation.mdx b/docs/astro/src/content/docs/zh-cn/gameplay/ai/navigation.mdx
index ec31cd52a..e509e187b 100644
--- a/docs/astro/src/content/docs/zh-cn/gameplay/ai/navigation.mdx
+++ b/docs/astro/src/content/docs/zh-cn/gameplay/ai/navigation.mdx
@@ -222,6 +222,27 @@ cmds.spawn()
没有控制器时,agent 的 Transform 会被直接沿路径推进——平面游戏要的就是这个,3D 游戏在还没有
身体之前拿到的也是这个。
+## agent 之间互相让路
+
+路线是对着一个不动的世界规划的。agent 会动,而十几个被派往同一个地方的 agent 会规划出同一条
+路线,然后像一个整体一样走过去。`radius` 大于零的 agent 就是在声明「我是一个**身体**」,
+从此它会绕开走在同一片地上的其它身体:
+
+```typescript
+cmds.spawn()
+ .insert(Transform, { position: { x: 0, y: 0, z: 0 } })
+ .insert(NavAgent, { speed: 300, radius: 40, arriveRadius: 60 });
+```
+
+每个 agent 都假设其它 agent 也在让,所以各让一半——迎面相遇的两个会分开,而不是同时往一边
+躲然后又撞上。当对称到连这也分不出高下时(完全的镜像),约定是**靠右**。
+
+侧身的那一步在迈出去之前会先对着可走世界检查一遍,所以让路永远不会把 agent 让进墙里。
+因此**两个宽到在同一条走廊里错不开的身体就是错不开**:等着才是诚实的答案,穿墙不是。
+
+`radius` 留在 `0` 的 agent 按质点寻路,既不给谁让路也没人给它让路——这就是所有平面游戏
+在此之前的行为,也是一个被脚本驱动的移动物体仍然想要的。
+
## agent 是有宽度的
在**格子**上,把 `radius` 设成身体的实际半宽,寻路就会绕开它挤不过去的缝隙。留在 `0` 表示
diff --git a/sdk/etc/index.api.md b/sdk/etc/index.api.md
index 8ead49420..d9b9eecdb 100644
--- a/sdk/etc/index.api.md
+++ b/sdk/etc/index.api.md
@@ -4725,6 +4725,7 @@ findWorldPath: (from: NavPoint, to: NavPoint, opts?: NavQueryOptions) => Vec3[]
height: number
inBounds: (gx: number, gy: number) => boolean
index: (gx: number, gy: number) => number
+isNavigable: (p: NavPoint) => boolean
isWalkable: (gx: number, gy: number) => boolean
nearestWalkable: (gx: number, gy: number, maxRadius?: number, clearance?: number) => Cell | null
originX: number
@@ -4781,6 +4782,7 @@ describe: (sink: NavSurfaceSink) => void
findPoly: (p: NavPoint, out?: Vec3, snap?: number) => number
findWorldPath: (from: NavPoint, to: NavPoint, opts?: NavQueryOptions) => Vec3[] | null
heightAt: (p: number, x: number, z: number) => number
+isNavigable: (p: NavPoint) => boolean
linkCount: number
maxVertsPerPoly: number
neis: Int32Array<ArrayBufferLike>
@@ -4849,6 +4851,7 @@ radius: number | undefined
```
describe: (sink: NavSurfaceSink) => void
findWorldPath: (from: NavPoint, to: NavPoint, opts?: NavQueryOptions) => Vec3[] | null
+isNavigable: (p: NavPoint) => boolean
up: Vec3
```
diff --git a/sdk/etc/index.native.api.md b/sdk/etc/index.native.api.md
index 68b35cddc..b581d7c3b 100644
--- a/sdk/etc/index.native.api.md
+++ b/sdk/etc/index.native.api.md
@@ -4903,6 +4903,7 @@ findWorldPath: (from: NavPoint, to: NavPoint, opts?: NavQueryOptions) => Vec3[]
height: number
inBounds: (gx: number, gy: number) => boolean
index: (gx: number, gy: number) => number
+isNavigable: (p: NavPoint) => boolean
isWalkable: (gx: number, gy: number) => boolean
nearestWalkable: (gx: number, gy: number, maxRadius?: number, clearance?: number) => Cell | null
originX: number
@@ -4959,6 +4960,7 @@ describe: (sink: NavSurfaceSink) => void
findPoly: (p: NavPoint, out?: Vec3, snap?: number) => number
findWorldPath: (from: NavPoint, to: NavPoint, opts?: NavQueryOptions) => Vec3[] | null
heightAt: (p: number, x: number, z: number) => number
+isNavigable: (p: NavPoint) => boolean
linkCount: number
maxVertsPerPoly: number
neis: Int32Array<ArrayBufferLike>
@@ -5027,6 +5029,7 @@ radius: number | undefined
```
describe: (sink: NavSurfaceSink) => void
findWorldPath: (from: NavPoint, to: NavPoint, opts?: NavQueryOptions) => Vec3[] | null
+isNavigable: (p: NavPoint) => boolean
up: Vec3
```
diff --git a/sdk/etc/index.node.api.md b/sdk/etc/index.node.api.md
index 40a27d2af..532bd6706 100644
--- a/sdk/etc/index.node.api.md
+++ b/sdk/etc/index.node.api.md
@@ -4720,6 +4720,7 @@ findWorldPath: (from: NavPoint, to: NavPoint, opts?: NavQueryOptions) => Vec3[]
height: number
inBounds: (gx: number, gy: number) => boolean
index: (gx: number, gy: number) => number
+isNavigable: (p: NavPoint) => boolean
isWalkable: (gx: number, gy: number) => boolean
nearestWalkable: (gx: number, gy: number, maxRadius?: number, clearance?: number) => Cell | null
originX: number
@@ -4776,6 +4777,7 @@ describe: (sink: NavSurfaceSink) => void
findPoly: (p: NavPoint, out?: Vec3, snap?: number) => number
findWorldPath: (from: NavPoint, to: NavPoint, opts?: NavQueryOptions) => Vec3[] | null
heightAt: (p: number, x: number, z: number) => number
+isNavigable: (p: NavPoint) => boolean
linkCount: number
maxVertsPerPoly: number
neis: Int32Array<ArrayBufferLike>
@@ -4844,6 +4846,7 @@ radius: number | undefined
```
describe: (sink: NavSurfaceSink) => void
findWorldPath: (from: NavPoint, to: NavPoint, opts?: NavQueryOptions) => Vec3[] | null
+isNavigable: (p: NavPoint) => boolean
up: Vec3
```
diff --git a/sdk/etc/minigame.api.md b/sdk/etc/minigame.api.md
index 75f748681..019908493 100644
--- a/sdk/etc/minigame.api.md
+++ b/sdk/etc/minigame.api.md
@@ -4965,6 +4965,7 @@ findWorldPath: (from: NavPoint, to: NavPoint, opts?: NavQueryOptions) => Vec3[]
height: number
inBounds: (gx: number, gy: number) => boolean
index: (gx: number, gy: number) => number
+isNavigable: (p: NavPoint) => boolean
isWalkable: (gx: number, gy: number) => boolean
nearestWalkable: (gx: number, gy: number, maxRadius?: number, clearance?: number) => Cell | null
originX: number
@@ -5021,6 +5022,7 @@ describe: (sink: NavSurfaceSink) => void
findPoly: (p: NavPoint, out?: Vec3, snap?: number) => number
findWorldPath: (from: NavPoint, to: NavPoint, opts?: NavQueryOptions) => Vec3[] | null
heightAt: (p: number, x: number, z: number) => number
+isNavigable: (p: NavPoint) => boolean
linkCount: number
maxVertsPerPoly: number
neis: Int32Array<ArrayBufferLike>
@@ -5089,6 +5091,7 @@ radius: number | undefined
```
describe: (sink: NavSurfaceSink) => void
findWorldPath: (from: NavPoint, to: NavPoint, opts?: NavQueryOptions) => Vec3[] | null
+isNavigable: (p: NavPoint) => boolean
up: Vec3
```
diff --git a/sdk/etc/wechat.api.md b/sdk/etc/wechat.api.md
index c34bd011c..593a46c1f 100644
--- a/sdk/etc/wechat.api.md
+++ b/sdk/etc/wechat.api.md
@@ -4727,6 +4727,7 @@ findWorldPath: (from: NavPoint, to: NavPoint, opts?: NavQueryOptions) => Vec3[]
height: number
inBounds: (gx: number, gy: number) => boolean
index: (gx: number, gy: number) => number
+isNavigable: (p: NavPoint) => boolean
isWalkable: (gx: number, gy: number) => boolean
nearestWalkable: (gx: number, gy: number, maxRadius?: number, clearance?: number) => Cell | null
originX: number
@@ -4783,6 +4784,7 @@ describe: (sink: NavSurfaceSink) => void
findPoly: (p: NavPoint, out?: Vec3, snap?: number) => number
findWorldPath: (from: NavPoint, to: NavPoint, opts?: NavQueryOptions) => Vec3[] | null
heightAt: (p: number, x: number, z: number) => number
+isNavigable: (p: NavPoint) => boolean
linkCount: number
maxVertsPerPoly: number
neis: Int32Array<ArrayBufferLike>
@@ -4851,6 +4853,7 @@ radius: number | undefined
```
describe: (sink: NavSurfaceSink) => void
findWorldPath: (from: NavPoint, to: NavPoint, opts?: NavQueryOptions) => Vec3[] | null
+isNavigable: (p: NavPoint) => boolean
up: Vec3
```
diff --git a/sdk/src/ai/nav/NavGrid.ts b/sdk/src/ai/nav/NavGrid.ts
index 7e5f938bc..138a040bd 100644
--- a/sdk/src/ai/nav/NavGrid.ts
+++ b/sdk/src/ai/nav/NavGrid.ts
@@ -184,6 +184,11 @@ export class NavGrid implements NavSurface {
return cells ? pathToWorld(this, shortenPath(this, cells, clearance)) : null;
}
+ isNavigable(p: NavPoint): boolean {
+ const cell = this.worldToCell(p);
+ return this.isWalkable(cell.x, cell.y);
+ }
+
/**
* Every walkable cell as a quad, and every edge where the walkable world
* stops. The corner array is reused between calls — see {@link NavSurfaceSink}.
diff --git a/sdk/src/ai/nav/NavMesh.ts b/sdk/src/ai/nav/NavMesh.ts
index 3cb5693ce..ebab3c2fc 100644
--- a/sdk/src/ai/nav/NavMesh.ts
+++ b/sdk/src/ai/nav/NavMesh.ts
@@ -533,6 +533,10 @@ export class NavMesh implements NavSurface {
return followGround(out, atPortal, left, right);
}
+ isNavigable(p: NavPoint): boolean {
+ return this.findPoly(p) >= 0;
+ }
+
describe(sink: NavSurfaceSink): void {
for (const link of this.links_) sink.link(link.start, link.end);
const corners: Vec3[] = [];
diff --git a/sdk/src/ai/nav/NavPlugin.ts b/sdk/src/ai/nav/NavPlugin.ts
index f532d42e4..c14083628 100644
--- a/sdk/src/ai/nav/NavPlugin.ts
+++ b/sdk/src/ai/nav/NavPlugin.ts
@@ -40,6 +40,7 @@ import { NavMesh, type NavLinkSegment } from './NavMesh';
import { NavLink } from './NavLink';
import { setupNavDebugDraw } from './NavDebugDraw';
import { advanceAlongPath } from './follow';
+import { avoidVelocity, type AvoidanceNeighbour } from './avoidance';
/** Per-entity runtime path state, owned by the driving system (not serialized). */
export interface AgentRuntime {
@@ -50,6 +51,10 @@ export interface AgentRuntime {
plannedZ: number;
repathTimer: number;
reachable: boolean;
+ /** What it travelled at last frame, on the two axes it steers on. Everyone
+ * avoiding everyone needs to know what everyone is already doing. */
+ velocityA: number;
+ velocityB: number;
}
/** Where the bake gets its triangles. Null until the world can answer — a mesh
@@ -211,7 +216,13 @@ export function stepNavigation(
): void {
if (dt <= 0) return;
- for (const entity of world.getEntitiesWithComponents([NavAgent, Transform])) {
+ // Which two axes an agent steers on: the ones the surface's own up leaves out.
+ // A flat scene is walked in x/y; ground with height in it, in x/z.
+ const flat = Math.abs(nav.surface?.up.z ?? 0) > Math.abs(nav.surface?.up.y ?? 1);
+ const entities = world.getEntitiesWithComponents([NavAgent, Transform]);
+ const crowd = gatherCrowd(world, entities, runtimes, flat);
+
+ for (const entity of entities) {
const agent = world.get(entity, NavAgent);
if (!agent.hasTarget) {
runtimes.delete(entity);
@@ -250,6 +261,8 @@ export function stepNavigation(
plannedZ: agent.targetZ,
repathTimer: agent.repathInterval,
reachable: path !== null,
+ velocityA: rt?.velocityA ?? 0,
+ velocityB: rt?.velocityB ?? 0,
};
}
runtimes.set(entity, rt);
@@ -264,9 +277,12 @@ export function stepNavigation(
// falls; a follower writing the Transform would fight all three.
const character = world.has(entity, CharacterController3D)
? world.get(entity, CharacterController3D) : null;
+ const steering = agent.radius > 0
+ ? neighboursOf(crowd, entity, tf.position, flat, agent.radius)
+ : null;
const done = character && character.enabled !== false
- ? steerCharacter(world, entity, agent, rt, character, dt)
- : walkTransform(world, entity, agent, rt, tf, dt);
+ ? steerCharacter(world, entity, agent, rt, character, dt, flat, steering, nav)
+ : walkTransform(world, entity, agent, rt, tf, dt, flat, steering, nav);
if (done) {
agent.arrived = true;
@@ -277,13 +293,103 @@ export function stepNavigation(
}
}
+/**
+ * Everyone on the surface who is a BODY, in the two axes they steer on. An agent
+ * routed as a point declares no body and neither gives way nor is given way to.
+ */
+function gatherCrowd(
+ world: NavWorldView, entities: readonly Entity[],
+ runtimes: Map<Entity, AgentRuntime>, flat: boolean,
+): Array<AvoidanceNeighbour & { entity: Entity }> {
+ const out: Array<AvoidanceNeighbour & { entity: Entity }> = [];
+ for (const entity of entities) {
+ const agent = world.get(entity, NavAgent);
+ if (agent.radius <= 0) continue;
+ const at = world.get(entity, Transform).position;
+ const rt = runtimes.get(entity);
+ out.push({
+ entity,
+ x: at.x,
+ z: flat ? at.y : at.z,
+ vx: rt?.velocityA ?? 0,
+ vz: rt?.velocityB ?? 0,
+ radius: agent.radius,
+ });
+ }
+ return out;
+}
+
+/** How far away another body is worth steering round, as a multiple of the two
+ * radii — past this the two cannot meet inside the horizon anyway. */
+const NEIGHBOUR_REACH = 6;
+
+function neighboursOf(
+ crowd: ReadonlyArray<AvoidanceNeighbour & { entity: Entity }>,
+ self: Entity, at: Vec3, flat: boolean, radius: number,
+): AvoidanceNeighbour[] | null {
+ const x = at.x;
+ const z = flat ? at.y : at.z;
+ const out: AvoidanceNeighbour[] = [];
+ for (const other of crowd) {
+ if (other.entity === self) continue;
+ const reach = (radius + other.radius) * NEIGHBOUR_REACH;
+ if (Math.hypot(other.x - x, other.z - z) > reach) continue;
+ out.push(other);
+ }
+ return out.length > 0 ? out : null;
+}
+
+/**
+ * The velocity to travel at this frame: straight at the next waypoint, bent round
+ * whoever else is walking there. Only a body steers — an agent routed as a point
+ * has nothing to be in the way of, and nothing to be in its way.
+ */
+function steerTo(
+ target: Vec3, at: Vec3, agent: ComponentData<typeof NavAgent>, rt: AgentRuntime,
+ flat: boolean, neighbours: AvoidanceNeighbour[] | null, nav: Navigation,
+): { a: number; b: number } {
+ const a = at.x;
+ const b = flat ? at.y : at.z;
+ const da = target.x - a;
+ const db = (flat ? target.y : target.z) - b;
+ const length = Math.hypot(da, db);
+ const want = {
+ x: length > 0 ? (da / length) * agent.speed : 0,
+ z: length > 0 ? (db / length) * agent.speed : 0,
+ };
+ if (!neighbours) return { a: want.x, b: want.z };
+
+ const surface = nav.surface;
+ const height = at.y;
+ const v = avoidVelocity(
+ { x: a, z: b, vx: rt.velocityA, vz: rt.velocityB, radius: agent.radius },
+ want, neighbours,
+ {
+ horizon: AVOID_HORIZON,
+ maxSpeed: agent.speed,
+ canStand: surface
+ ? (px, pz) => surface.isNavigable(
+ flat ? { x: px, y: pz, z: height } : { x: px, y: height, z: pz })
+ : undefined,
+ },
+ );
+ return { a: v.x, b: v.z };
+}
+
+/** How far ahead a collision with another body is worth steering round. */
+const AVOID_HORIZON = 1.6;
+
/** Move the Transform along the path directly. Returns whether the goal is reached. */
function walkTransform(
world: NavWorldView, entity: Entity, agent: ComponentData<typeof NavAgent>,
rt: AgentRuntime, tf: ComponentData<typeof Transform>, dt: number,
+ flat: boolean, neighbours: AvoidanceNeighbour[] | null, nav: Navigation,
): boolean {
+ if (neighbours) return walkAmongBodies(world, entity, agent, rt, tf, dt, flat, neighbours, nav);
const pos = { x: tf.position.x, y: tf.position.y, z: tf.position.z };
rt.index = advanceAlongPath(pos, rt.waypoints, rt.index, agent.speed * dt);
+ rt.velocityA = 0;
+ rt.velocityB = 0;
tf.position.x = pos.x;
tf.position.y = pos.y;
tf.position.z = pos.z;
@@ -298,6 +404,57 @@ function walkTransform(
return rt.index >= rt.waypoints.length || withinGoal;
}
+/**
+ * A body walking among other bodies: it is steered rather than marched, because a
+ * route walked exactly is a route that goes through whoever is standing on it. The
+ * waypoint moves on by proximity, since a body cannot be snapped onto a point.
+ */
+function walkAmongBodies(
+ world: NavWorldView, entity: Entity, agent: ComponentData<typeof NavAgent>,
+ rt: AgentRuntime, tf: ComponentData<typeof Transform>, dt: number,
+ flat: boolean, neighbours: AvoidanceNeighbour[], nav: Navigation,
+): boolean {
+ const pos = tf.position;
+ const reach = Math.max(agent.speed * dt * 1.5, WAYPOINT_REACH);
+ while (rt.index < rt.waypoints.length) {
+ const wp = rt.waypoints[rt.index]!;
+ if (planeDistance(wp, pos, flat) > reach) break;
+ rt.index++;
+ }
+
+ const goal = rt.waypoints[rt.waypoints.length - 1];
+ const withinGoal = goal !== undefined && agent.arriveRadius > 0
+ && planeDistance(goal, pos, flat) <= agent.arriveRadius;
+ const target = rt.waypoints[rt.index];
+ if (rt.index >= rt.waypoints.length || withinGoal || target === undefined) {
+ rt.velocityA = 0;
+ rt.velocityB = 0;
+ return true;
+ }
+
+ const v = steerTo(target, pos, agent, rt, flat, neighbours, nav);
+ rt.velocityA = v.a;
+ rt.velocityB = v.b;
+ pos.x += v.a * dt;
+ if (flat) pos.y += v.b * dt;
+ else pos.z += v.b * dt;
+ // The axis the route carries is the ground's, followed rather than steered on.
+ const height = flat ? target.z : target.y;
+ const own = flat ? pos.z : pos.y;
+ const lifted = own + (height - own) * Math.min(1, dt * HEIGHT_FOLLOW);
+ if (flat) pos.z = lifted; else pos.y = lifted;
+ world.set(entity, Transform, tf);
+ return false;
+}
+
+/** How fast a steered agent settles onto the height of the ground it is over. */
+const HEIGHT_FOLLOW = 8;
+
+/** Distance in the two axes an agent steers on. */
+function planeDistance(a: Vec3, b: Vec3, flat: boolean): number {
+ return Math.hypot(a.x - b.x, (flat ? a.y : a.z) - (flat ? b.y : b.z));
+}
+
/**
* Point a character controller at the next waypoint and let its own solver move
* it. Only the horizontal axes are written — the vertical one is the world's, and
@@ -307,6 +464,7 @@ function walkTransform(
function steerCharacter(
world: NavWorldView, entity: Entity, agent: ComponentData<typeof NavAgent>,
rt: AgentRuntime, character: ComponentData<typeof CharacterController3D>, dt: number,
+ flat: boolean, neighbours: AvoidanceNeighbour[] | null, nav: Navigation,
): boolean {
const tf = world.get(entity, Transform);
const pos = tf.position;
@@ -329,12 +487,14 @@ function steerCharacter(
if (done) {
character.velocity.x = 0;
character.velocity.z = 0;
+ rt.velocityA = 0;
+ rt.velocityB = 0;
} else {
- const dx = target.x - pos.x;
- const dz = target.z - pos.z;
- const length = Math.hypot(dx, dz);
- character.velocity.x = length > 0 ? (dx / length) * agent.speed : 0;
- character.velocity.z = length > 0 ? (dz / length) * agent.speed : 0;
+ const v = steerTo(target, pos, agent, rt, flat, neighbours, nav);
+ character.velocity.x = v.a;
+ character.velocity.z = v.b;
+ rt.velocityA = v.a;
+ rt.velocityB = v.b;
}
world.set(entity, CharacterController3D, character);
return done;
diff --git a/sdk/src/ai/nav/NavSurface.ts b/sdk/src/ai/nav/NavSurface.ts
index 8bdfd267a..51876a92a 100644
--- a/sdk/src/ai/nav/NavSurface.ts
+++ b/sdk/src/ai/nav/NavSurface.ts
@@ -69,6 +69,13 @@ export interface NavSurface {
*/
findWorldPath(from: NavPoint, to: NavPoint, opts?: NavQueryOptions): Vec3[] | null;
+ /**
+ * Whether an agent may stand at a world point. Asked of a place the agent has
+ * not been routed to — a step sideways to get round another body — which is
+ * why it is not the same question as "is there a route here".
+ */
+ isNavigable(p: NavPoint): boolean;
+
/** Hand the shape of the walkable world to a drawer. */
describe(sink: NavSurfaceSink): void;
}
diff --git a/sdk/src/ai/nav/avoidance.ts b/sdk/src/ai/nav/avoidance.ts
new file mode 100644
index 000000000..58ad8248c
--- /dev/null
+++ b/sdk/src/ai/nav/avoidance.ts
@@ -0,0 +1,165 @@
+// SPDX-License-Identifier: Apache-2.0
+// SPDX-FileCopyrightText: Copyright (c) 2024-present ESEngine Team
+/**
+ * @file avoidance.ts
+ * @brief Steering round the bodies a route knows nothing about — the other
+ * agents walking it.
+ *
+ * A route is planned against the world, which does not move. Agents do, and a
+ * dozen of them sent to one place all plan the same route and walk it as one
+ * body. What each of them needs is a velocity that still heads for the next
+ * waypoint and does not run into anyone: sampled rather than solved, because a
+ * sample can be REJECTED for leaving the navigable world, and a velocity solved
+ * from constraints has no natural place to ask that question.
+ *
+ * Reciprocal: every agent assumes the others are doing the same thing and each
+ * takes half the avoidance, so two meeting head-on part rather than both dodging
+ * the same way and meeting again.
+ *
+ * Pure and two-dimensional, in the ground plane — the axis an agent's own solver
+ * carries is not one it steers on.
+ */
+
+/** What one agent needs to know about another to avoid it. */
+export interface AvoidanceNeighbour {
+ x: number;
+ z: number;
+ vx: number;
+ vz: number;
+ radius: number;
+}
+
+export interface AvoidanceOptions {
+ /** How far ahead a collision is worth steering round, in seconds. */
+ horizon: number;
+ /** The agent's own top speed — candidates are sampled against it. */
+ maxSpeed: number;
+ /** Whether the agent may stand somewhere. A candidate that leaves the
+ * navigable world is refused however clear of other bodies it is. */
+ canStand?: (x: number, z: number) => boolean;
+}
+
+/** How far ahead a rejected sample is tested for leaving the world, in seconds. */
+const LOOKAHEAD = 0.4;
+/** Rings of candidate velocities around the desired one, and samples per ring. */
+const RINGS = 2;
+const DIVISIONS = 8;
+/** How much a candidate is penalised for not being the velocity that was wanted. */
+const WEIGHT_DESIRED = 2;
+/** …for turning away from the direction already being travelled. */
+const WEIGHT_CURRENT = 0.75;
+/** …for running into somebody soon. */
+const WEIGHT_IMPACT = 2.5;
+/**
+ * …for passing on the wrong side. Two agents meeting head-on are a mirror of
+ * each other, so every symmetric rule picks the same dodge for both and they meet
+ * again: what parts them is a convention, and the convention is keep right.
+ */
+const WEIGHT_SIDE = 0.6;
+
+/**
+ * The velocity to travel at: `desired` if nothing is in the way, otherwise the
+ * best of a spread of alternatives around it.
+ */
+export function avoidVelocity(
+ self: { x: number; z: number; vx: number; vz: number; radius: number },
+ desired: { x: number; z: number },
+ neighbours: readonly AvoidanceNeighbour[],
+ opts: AvoidanceOptions,
+): { x: number; z: number } {
+ if (neighbours.length === 0) return { x: desired.x, z: desired.z };
+
+ let bestX = desired.x;
+ let bestZ = desired.z;
+ let bestPenalty = Infinity;
+ const speed = Math.max(opts.maxSpeed, 1e-3);
+
+ for (let ring = 0; ring <= RINGS; ring++) {
+ const count = ring === 0 ? 1 : DIVISIONS;
+ const spread = (ring / RINGS) * speed;
+ for (let i = 0; i < count; i++) {
+ const angle = (i / count) * Math.PI * 2;
+ const cx = desired.x + Math.cos(angle) * spread;
+ const cz = desired.z + Math.sin(angle) * spread;
+ const length = Math.hypot(cx, cz);
+ // Nothing is gained by sampling faster than the agent can go.
+ const scale = length > speed ? speed / length : 1;
+ const vx = cx * scale;
+ const vz = cz * scale;
+ if (opts.canStand && !opts.canStand(self.x + vx * LOOKAHEAD, self.z + vz * LOOKAHEAD)) {
+ continue;
+ }
+ const penalty = score(self, desired, vx, vz, neighbours, opts, speed);
+ if (penalty >= bestPenalty) continue;
+ bestPenalty = penalty;
+ bestX = vx;
+ bestZ = vz;
+ }
+ }
+ return { x: bestX, z: bestZ };
+}
+
+function score(
+ self: { x: number; z: number; vx: number; vz: number; radius: number },
+ desired: { x: number; z: number },
+ vx: number, vz: number,
+ neighbours: readonly AvoidanceNeighbour[],
+ opts: AvoidanceOptions,
+ speed: number,
+): number {
+ let earliest = opts.horizon;
+ let side = 0;
+ const length = Math.hypot(vx, vz);
+ for (const other of neighbours) {
+ // Each side takes half the avoidance, so the closing velocity is read as
+ // twice this candidate less what the two are already doing.
+ const rvx = vx * 2 - self.vx - other.vx;
+ const rvz = vz * 2 - self.vz - other.vz;
+ const hit = sweep(self.x, self.z, self.radius + other.radius,
+ rvx, rvz, other.x, other.z);
+ if (hit !== null && hit < earliest) earliest = hit;
+
+ const dx = other.x - self.x;
+ const dz = other.z - self.z;
+ const toward = Math.hypot(dx, dz);
+ if (toward < 1e-6 || length < 1e-6) continue;
+ // Right of the line to them, in the ground plane. Which of the two is
+ // "right" is a convention; that both agents use the SAME one is the point.
+ const rightX = dz / toward;
+ const rightZ = -dx / toward;
+ side += 0.5 - (vx * rightX + vz * rightZ) / (length * 2);
+ }
+ const deviation = Math.hypot(vx - desired.x, vz - desired.z) / speed;
+ const turn = Math.hypot(vx - self.vx, vz - self.vz) / speed;
+ // The nearer the impact, the sharper the cost — a collision a long way off is
+ // not worth walking round.
+ const impact = 1 / (0.1 + earliest / opts.horizon);
+ return WEIGHT_DESIRED * deviation + WEIGHT_CURRENT * turn + WEIGHT_IMPACT * impact
+ + WEIGHT_SIDE * (side / neighbours.length);
+}
+
+/**
+ * When two circles closing at `(rvx, rvz)` touch, or null if they never do. For a
+ * pair that overlaps the touch has happened, so the answer is negative: half of
+ * it, made positive, is what such a pair scores with, so the way out that leaves
+ * fastest reads as the safest.
+ */
+function sweep(
+ ax: number, az: number, radius: number,
+ rvx: number, rvz: number,
+ bx: number, bz: number,
+): number | null {
+ const dx = bx - ax;
+ const dz = bz - az;
+ const c = dx * dx + dz * dz - radius * radius;
+ const a = rvx * rvx + rvz * rvz;
+ if (a < 1e-8) return c <= 0 ? 0 : null;
+ const b = dx * rvx + dz * rvz;
+ const discriminant = b * b - a * c;
+ if (discriminant <= 0) return null;
+ const root = Math.sqrt(discriminant);
+ const tmin = (b - root) / a;
+ const tmax = (b + root) / a;
+ if (tmin < 0 && tmax > 0) return -tmin * 0.5;
+ return tmin >= 0 ? tmin : null;
+}
diff --git a/sdk/tests/nav-avoidance.test.ts b/sdk/tests/nav-avoidance.test.ts
new file mode 100644
index 000000000..7767df45c
--- /dev/null
+++ b/sdk/tests/nav-avoidance.test.ts
@@ -0,0 +1,107 @@
+// SPDX-License-Identifier: Apache-2.0
+// SPDX-FileCopyrightText: Copyright (c) 2024-present ESEngine Team
+/**
+ * @file Steering round the bodies a route knows nothing about.
+ *
+ * A route is planned against a world that does not move; agents do. The claims
+ * are about the velocity that comes out: it is the one that was wanted when
+ * nothing is in the way, it turns when something is, and it never turns onto
+ * ground the agent may not stand on.
+ */
+import { describe, it, expect } from 'vitest';
+import { avoidVelocity, type AvoidanceNeighbour } from '../src/ai/nav/avoidance';
+
+const OPTS = { horizon: 2, maxSpeed: 100 };
+const me = (over: Partial<{ x: number; z: number; vx: number; vz: number; radius: number }> = {}) => ({
+ x: 0, z: 0, vx: 100, vz: 0, radius: 30, ...over,
+});
+const other = (over: Partial<AvoidanceNeighbour> = {}): AvoidanceNeighbour => ({
+ x: 200, z: 0, vx: -100, vz: 0, radius: 30, ...over,
+});
+
+/** How far two agents get from each other by walking their chosen velocities. */
+function simulate(steps: number, dt = 1 / 30): { closest: number; swapped: boolean } {
+ let a = { x: -200, z: 0, vx: 0, vz: 0, radius: 30 };
+ let b = { x: 200, z: 0, vx: 0, vz: 0, radius: 30 };
+ let closest = Infinity;
+ for (let i = 0; i < steps; i++) {
+ const wantA = { x: 100, z: 0 };
+ const wantB = { x: -100, z: 0 };
+ const va = avoidVelocity(a, wantA, [{ ...b }], OPTS);
+ const vb = avoidVelocity(b, wantB, [{ ...a }], OPTS);
+ a = { ...a, x: a.x + va.x * dt, z: a.z + va.z * dt, vx: va.x, vz: va.z };
+ b = { ...b, x: b.x + vb.x * dt, z: b.z + vb.z * dt, vx: vb.x, vz: vb.z };
+ closest = Math.min(closest, Math.hypot(a.x - b.x, a.z - b.z));
+ }
+ return { closest, swapped: a.x > 0 && b.x < 0 };
+}
+
+describe('avoidVelocity', () => {
+ it('leaves an agent with nobody near it alone', () => {
+ expect(avoidVelocity(me(), { x: 100, z: 0 }, [], OPTS)).toEqual({ x: 100, z: 0 });
+ });
+
+ it('leaves an agent alone when the other is walking away', () => {
+ const away = avoidVelocity(me(), { x: 100, z: 0 }, [other({ vx: 100 })], OPTS);
+ expect(away.x).toBeCloseTo(100, 1);
+ expect(away.z).toBeCloseTo(0, 1);
+ });
+
+ // Head on, straight ahead is exactly what neither of them may keep doing.
+ it('turns away from a body it is about to walk into', () => {
+ const v = avoidVelocity(me(), { x: 100, z: 0 }, [other()], OPTS);
+ expect(Math.abs(v.z)).toBeGreaterThan(20);
+ });
+
+ // Two agents each assuming the other will also give way: they part, pass, and
+ // end up where the other one started rather than shoving in the middle.
+ it('lets two agents meeting head-on swap places without touching', () => {
+ const run = simulate(200);
+ expect(run.closest).toBeGreaterThan(50); // their radii sum to 60
+ expect(run.swapped).toBe(true);
+ });
+
+ // Steering is not a licence to leave the world: a corridor with a body coming
+ // the other way is a wait, not a walk through the wall.
+ it('never steers onto ground the agent may not stand on', () => {
+ const wide = Math.abs(avoidVelocity(me(), { x: 100, z: 0 }, [other()], OPTS).z);
+ const inCorridor = { ...OPTS, canStand: (_x: number, z: number) => Math.abs(z) < 5 };
+ const v = avoidVelocity(me(), { x: 100, z: 0 }, [other()], inCorridor);
+ // The corridor is narrower than the dodge it would otherwise have taken.
+ expect(wide * 0.4).toBeGreaterThan(5);
+ expect(inCorridor.canStand(0, v.z * 0.4)).toBe(true);
+ });
+
+ // Everyone gives way, so nobody has to give way twice. Reading a stationary
+ // body as something only THIS agent can get round makes standing still the
+ // safest thing to do, which is how a crowd stops in front of a lamp post.
+ it('walks round a body standing in the way rather than stopping in front of it', () => {
+ const v = avoidVelocity(me(), { x: 100, z: 0 }, [other({ x: 80, vx: 0 })], OPTS);
+ expect(Math.hypot(v.x, v.z)).toBeGreaterThan(40);
+ });
+
+ // Swept over many arrangements, because the scoring only rarely PREFERS a
+ // velocity faster than the agent has — and rarely is not never.
+ it('never asks for more speed than the agent has', () => {
+ let seed = 99;
+ const rnd = (): number => (seed = (seed * 1103515245 + 12345) & 0x7fffffff) / 0x7fffffff;
+ for (let t = 0; t < 500; t++) {
+ const self = me({
+ vx: (rnd() * 2 - 1) * 100, vz: (rnd() * 2 - 1) * 100, radius: 20 + rnd() * 30,
+ });
+ const want = { x: (rnd() * 2 - 1) * 100, z: (rnd() * 2 - 1) * 100 };
+ const crowd = Array.from({ length: 1 + Math.floor(rnd() * 3) }, () => other({
+ x: (rnd() * 2 - 1) * 250, z: (rnd() * 2 - 1) * 250,
+ vx: (rnd() * 2 - 1) * 100, vz: (rnd() * 2 - 1) * 100, radius: 20 + rnd() * 30,
+ }));
+ const v = avoidVelocity(self, want, crowd, OPTS);
+ expect(Math.hypot(v.x, v.z)).toBeLessThanOrEqual(OPTS.maxSpeed + 1e-6);
+ }
+ });
+
+ it('does something at once about a body already inside it', () => {
+ const overlapping = other({ x: 10, z: 0, vx: 0 });
+ const v = avoidVelocity(me({ vx: 0 }), { x: 0, z: 0 }, [overlapping], OPTS);
+ expect(Math.hypot(v.x, v.z)).toBeGreaterThan(10);
+ });
+});
diff --git a/sdk/tests/nav-step.test.ts b/sdk/tests/nav-step.test.ts
index a96d65f06..203795267 100644
--- a/sdk/tests/nav-step.test.ts
+++ b/sdk/tests/nav-step.test.ts
@@ -335,3 +335,144 @@ describe('a replan that finds nothing', () => {
expect(world.pos(entity).x).toBe(0);
});
});
+
+/**
+ * A route is planned against a world that does not move. A dozen agents sent to
+ * one place all plan the same route and walk it as one body unless each of them
+ * gives way, which is what declaring a RADIUS asks for.
+ */
+describe('agents that are bodies', () => {
+ const room = (): Navigation => {
+ const nav = new Navigation();
+ nav.setSurface(new NavGrid({ width: 30, height: 30, cellSize: 10 }));
+ return nav;
+ };
+
+ it('walk past each other rather than through', () => {
+ const world = new FakeWorld();
+ const a = world.spawnAgent(20, 150, {
+ speed: 100, radius: 25, arriveRadius: 20, hasTarget: true, targetX: 270, targetY: 150,
+ });
+ const b = world.spawnAgent(270, 150, {
+ speed: 100, radius: 25, arriveRadius: 20, hasTarget: true, targetX: 20, targetY: 150,
+ });
+ const nav = room();
+ const runtimes = new Map<Entity, AgentRuntime>();
+ let closest = Infinity;
+ for (let i = 0; i < 200; i++) {
+ stepNavigation(world, nav, 1 / 30, runtimes);
+ const pa = world.pos(a);
+ const pb = world.pos(b);
+ closest = Math.min(closest, Math.hypot(pa.x - pb.x, pa.y - pb.y));
+ }
+ expect(closest).toBeGreaterThan(40); // their radii sum to 50, less the step
+ expect(world.pos(a).x).toBeGreaterThan(200);
+ expect(world.pos(b).x).toBeLessThan(90);
+ });
+
+ // An agent routed as a point declares no body: it neither gives way nor is
+ // given way to, and it walks the route it was handed exactly as before.
+ it('leave an agent routed as a point walking its route exactly', () => {
+ const world = new FakeWorld();
+ const point = world.spawnAgent(20, 150, {
+ speed: 100, radius: 0, arriveRadius: 0, hasTarget: true, targetX: 270, targetY: 150,
+ });
+ world.spawnAgent(150, 150, { speed: 0, radius: 25 });
+ const nav = room();
+ const runtimes = new Map<Entity, AgentRuntime>();
+ for (let i = 0; i < 60; i++) stepNavigation(world, nav, 1 / 30, runtimes);
+ // Straight down the row it was planned along, through where the body is.
+ expect(world.pos(point).y).toBeCloseTo(150, 1);
+ });
+
+ // Something standing in the way is still something to get round, and getting
+ // round it means keeping moving: an agent that reads a stationary body as
+ // "only I can avoid this" finds standing still the safest thing to do.
+ it('walk round a body standing in the way', () => {
+ const world = new FakeWorld();
+ const mover = world.spawnAgent(20, 150, {
+ speed: 100, radius: 25, arriveRadius: 20, hasTarget: true, targetX: 270, targetY: 150,
+ });
+ const still = world.spawnAgent(150, 150, { speed: 0, radius: 25 });
+ const nav = room();
+ const runtimes = new Map<Entity, AgentRuntime>();
+ let closest = Infinity;
+ for (let i = 0; i < 200; i++) {
+ stepNavigation(world, nav, 1 / 30, runtimes);
+ const pa = world.pos(mover);
+ const pb = world.pos(still);
+ closest = Math.min(closest, Math.hypot(pa.x - pb.x, pa.y - pb.y));
+ }
+ expect(world.pos(mover).x).toBeGreaterThan(200);
+ expect(closest).toBeGreaterThan(40);
+ });
+
+ // A point declares no body, so it is nothing to walk round either.
+ it('walk straight through an agent routed as a point', () => {
+ const world = new FakeWorld();
+ const mover = world.spawnAgent(20, 150, {
+ speed: 100, radius: 25, arriveRadius: 20, hasTarget: true, targetX: 270, targetY: 150,
+ });
+ world.spawnAgent(150, 150, { speed: 0, radius: 0 });
+ const nav = room();
+ const runtimes = new Map<Entity, AgentRuntime>();
+ for (let i = 0; i < 60; i++) stepNavigation(world, nav, 1 / 30, runtimes);
+ expect(world.pos(mover).y).toBeCloseTo(150, 0);
+ });
+
+ // What each of them is already doing is half of what the others are steering
+ // around, so the runtime carries it from frame to frame.
+ it('remember what they travelled at', () => {
+ const world = new FakeWorld();
+ const a = world.spawnAgent(20, 150, {
+ speed: 100, radius: 25, arriveRadius: 20, hasTarget: true, targetX: 270, targetY: 150,
+ });
+ // Near enough to be a neighbour, nowhere near its path: the velocity to
+ // remember is then the one it wanted, which is what makes it readable.
+ world.spawnAgent(150, 280, { speed: 0, radius: 25 });
+ const runtimes = new Map<Entity, AgentRuntime>();
+ stepNavigation(world, room(), 1 / 30, runtimes);
+ const rt = runtimes.get(a)!;
+ expect(Math.hypot(rt.velocityA, rt.velocityB)).toBeCloseTo(100, 0);
+ });
+
+ // Steering is not a licence to leave the world. A corridor that fits one body
+ // does not fit two abreast, so two meeting in one do not get past — and an
+ // agent that got past anyway got past through the wall.
+ it('will not squeeze past each other through a wall', () => {
+ const grid = new NavGrid({ width: 30, height: 9, cellSize: 10 });
+ for (let x = 0; x < 30; x++) {
+ for (let y = 0; y < 9; y++) if (y < 2 || y > 6) grid.setWalkable(x, y, false);
+ }
+ const nav = new Navigation();
+ nav.setSurface(grid);
+ const world = new FakeWorld();
+ const a = world.spawnAgent(20, 40, {
+ speed: 100, radius: 20, arriveRadius: 15, hasTarget: true, targetX: 270, targetY: 40,
+ });
+ const b = world.spawnAgent(270, 40, {
+ speed: 100, radius: 20, arriveRadius: 15, hasTarget: true, targetX: 20, targetY: 40,
+ });
+ const runtimes = new Map<Entity, AgentRuntime>();
+ for (let i = 0; i < 300; i++) {
+ stepNavigation(world, nav, 1 / 30, runtimes);
+ for (const e of [a, b]) {
+ const p = world.pos(e);
+ expect(grid.isNavigable({ x: p.x, y: p.y, z: 0 })).toBe(true);
+ }
+ }
+ expect(world.pos(a).x).toBeLessThan(200);
+ expect(world.pos(b).x).toBeGreaterThan(90);
+ });
+
+ it('are not slowed by an empty room', () => {
+ const world = new FakeWorld();
+ const alone = world.spawnAgent(20, 150, {
+ speed: 100, radius: 25, arriveRadius: 0, hasTarget: true, targetX: 270, targetY: 150,
+ });
+ const nav = room();
+ const runtimes = new Map<Entity, AgentRuntime>();
+ for (let i = 0; i < 30; i++) stepNavigation(world, nav, 1 / 30, runtimes);
+ expect(world.pos(alone).x).toBeCloseTo(120, 0);
+ });
+});