From 786393a9032af32638cd1670fb79280ae52bce76 Mon Sep 17 00:00:00 2001 From: Francois Date: Tue, 7 Oct 2025 14:50:42 +0900 Subject: [PATCH] feat: add RVO crowd steering --- PROJECT_DESCRIPTION.md | 4 +- README.md | 2 +- docs/index.d.ts | 22 +++++ examples/rvo.ts | 32 +++++++ src/ai/rvo.ts | 189 +++++++++++++++++++++++++++++++++++++++++ src/index.ts | 1 + src/types.ts | 6 ++ tests/rvo.test.ts | 70 +++++++++++++++ 8 files changed, 323 insertions(+), 3 deletions(-) create mode 100644 examples/rvo.ts create mode 100644 src/ai/rvo.ts create mode 100644 tests/rvo.test.ts diff --git a/PROJECT_DESCRIPTION.md b/PROJECT_DESCRIPTION.md index fa6a9c1..4ca0dcf 100644 --- a/PROJECT_DESCRIPTION.md +++ b/PROJECT_DESCRIPTION.md @@ -42,7 +42,7 @@ npm run build | Data transforms | Diff (LCS) / Deep clone / Group by | `data/*.ts` | | Graph traversal | BFS distances / DFS callbacks / Topological sort | `graph/traversal.ts` | | Geometry & visuals | Convex hull / Segment intersection / Point-in-poly / Bezier / Easings | `geometry/*.ts`, `visual/*.ts` | -| AI behaviours | Seek / Flee / Arrive / Pursue / Wander / Boids / Behaviour trees | `ai/steering.ts`, `ai/boids.ts`, `ai/behaviorTree.ts` | +| AI behaviours | Seek / Flee / Arrive / Pursue / Wander / Boids / Behaviour trees / RVO crowd steering | `ai/steering.ts`, `ai/boids.ts`, `ai/behaviorTree.ts`, `ai/rvo.ts` | --- @@ -95,7 +95,7 @@ Consistency between runtime code, documentation, and TypeScript declarations kee - **Data tools:** Diff operations (LCS), deep clone, groupBy. - **Graph:** BFS distance map, DFS traversal, topological sort. - **Geometry & visuals:** Convex hull, line intersection, point-in-polygon, easing presets, quadratic/cubic Bezier evaluation. -- **AI behaviours:** Steering behaviours (seek, flee, arrive, pursue, wander). +- **AI behaviours:** Steering behaviours (seek, flee, arrive, pursue, wander), boids, behaviour trees, RVO crowd steering. --- diff --git a/README.md b/README.md index d676b08..6122d92 100644 --- a/README.md +++ b/README.md @@ -29,7 +29,7 @@ CDN usage: - **Web performance:** Debounce, throttle, LRU cache, memoize, request deduplication helper, virtual scrolling range calculator - **Graph:** BFS distance map, DFS traversal, topological sort - **Visual & Geometry:** Convex hull, line intersection, point-in-polygon, easing presets, Bezier helpers -- **AI Behaviours:** Steering behaviours (seek, flee, arrive, pursue, wander), boids flocking update, behaviour trees +- **AI Behaviours:** Steering behaviours (seek, flee, arrive, pursue, wander), boids flocking update, behaviour trees, reciprocal velocity obstacles (RVO) ## Scripts ```bash diff --git a/docs/index.d.ts b/docs/index.d.ts index 156f849..819bfc2 100644 --- a/docs/index.d.ts +++ b/docs/index.d.ts @@ -526,6 +526,28 @@ export interface BoidOptions { } export function updateBoids(boids: Boid[], options: BoidOptions): void; +/** + * Reciprocal velocity obstacles (RVO) step for multi-agent avoidance. + * Use for: crowd steering, swarms, dense navigation. + * Performance: O(n × log n) with neighbor filtering. + * Import: ai/rvo.ts + */ +export interface RvoAgent extends Agent { + id?: string; + radius: number; + preferredVelocity: Vector2D; +} +export interface RvoOptions { + timeHorizon?: number; + maxNeighbors?: number; + avoidanceStrength?: number; +} +export interface RvoResult { + id?: string; + velocity: Vector2D; +} +export function rvoStep(agents: ReadonlyArray, options?: RvoOptions): RvoResult[]; + /** * Behavior tree orchestrator for AI decision making. * Use for: hierarchical NPC logic, modular behaviour scripting, goal selection. diff --git a/examples/rvo.ts b/examples/rvo.ts new file mode 100644 index 0000000..7003282 --- /dev/null +++ b/examples/rvo.ts @@ -0,0 +1,32 @@ +import { rvoStep } from '../src/index.js'; +import type { RvoAgent } from '../src/types.js'; + +const agents: RvoAgent[] = [ + { + id: 'alpha', + position: { x: -2, y: 0 }, + velocity: { x: 1, y: 0 }, + preferredVelocity: { x: 1, y: 0 }, + radius: 0.4, + maxSpeed: 1.5, + }, + { + id: 'beta', + position: { x: 2, y: 0.2 }, + velocity: { x: -1, y: 0 }, + preferredVelocity: { x: -1, y: 0 }, + radius: 0.4, + maxSpeed: 1.5, + }, + { + id: 'gamma', + position: { x: 0, y: 1.5 }, + velocity: { x: 0, y: -0.8 }, + preferredVelocity: { x: 0, y: -0.8 }, + radius: 0.4, + maxSpeed: 1.2, + }, +]; + +const results = rvoStep(agents, { timeHorizon: 3 }); +console.log(results); diff --git a/src/ai/rvo.ts b/src/ai/rvo.ts new file mode 100644 index 0000000..28df2c4 --- /dev/null +++ b/src/ai/rvo.ts @@ -0,0 +1,189 @@ +import type { RvoAgent, Vector2D } from '../types.js'; + +const EPSILON = 1e-6; + +export interface RvoOptions { + timeHorizon?: number; + maxNeighbors?: number; + avoidanceStrength?: number; +} + +export interface RvoResult { + id?: string; + velocity: Vector2D; +} + +/** + * Computes collision-avoiding agent velocities using reciprocal velocity obstacles (RVO). + * Useful for: crowd steering, swarm navigation, multi-agent avoidance. + */ +export function rvoStep( + agents: ReadonlyArray, + options: RvoOptions = {} +): RvoResult[] { + if (!Array.isArray(agents)) { + throw new TypeError('agents must be an array'); + } + + const timeHorizon = options.timeHorizon ?? 2; + const maxNeighbors = options.maxNeighbors ?? agents.length; + const avoidanceStrength = options.avoidanceStrength ?? 0.6; + + if (timeHorizon <= 0) { + throw new RangeError('timeHorizon must be greater than zero'); + } + if (maxNeighbors <= 0) { + throw new RangeError('maxNeighbors must be greater than zero'); + } + if (avoidanceStrength < 0) { + throw new RangeError('avoidanceStrength must be non-negative'); + } + + return agents.map((agent: RvoAgent, index: number) => { + validateAgent(agent, index); + + const neighborEntries: Array<{ other: RvoAgent; distanceSq: number }> = agents + .map((other: RvoAgent, otherIndex: number) => ({ + other, + distanceSq: squaredDistance(agent.position, other.position), + otherIndex, + })) + .filter((entry) => entry.otherIndex !== index) + .sort((a, b) => a.distanceSq - b.distanceSq) + .slice(0, maxNeighbors) + .map(({ other, distanceSq }) => ({ other, distanceSq })); + + let adjusted = { ...agent.preferredVelocity }; + + for (const { other } of neighborEntries) { + const avoidance = computeAvoidance(agent, other, adjusted, timeHorizon, avoidanceStrength); + adjusted = { + x: adjusted.x + avoidance.x, + y: adjusted.y + avoidance.y, + }; + } + + const speed = length(adjusted); + if (speed > agent.maxSpeed) { + adjusted = scale(adjusted, agent.maxSpeed / (speed || 1)); + } + + return { id: agent.id, velocity: adjusted }; + }); +} + +function validateAgent(agent: RvoAgent, index: number): void { + if (!agent) { + throw new TypeError(`agents[${index}] is undefined`); + } + if (!isFinite(agent.position.x) || !isFinite(agent.position.y)) { + throw new TypeError(`agents[${index}].position must contain finite numbers`); + } + if (!isFinite(agent.velocity.x) || !isFinite(agent.velocity.y)) { + throw new TypeError(`agents[${index}].velocity must contain finite numbers`); + } + if (!isFinite(agent.preferredVelocity.x) || !isFinite(agent.preferredVelocity.y)) { + throw new TypeError(`agents[${index}].preferredVelocity must contain finite numbers`); + } + if (!isFinite(agent.radius) || agent.radius < 0) { + throw new RangeError(`agents[${index}].radius must be a non-negative number`); + } + if (!isFinite(agent.maxSpeed) || agent.maxSpeed <= 0) { + throw new RangeError(`agents[${index}].maxSpeed must be a positive number`); + } +} + +function computeAvoidance( + agent: RvoAgent, + other: RvoAgent, + candidateVelocity: Vector2D, + timeHorizon: number, + avoidanceStrength: number +): Vector2D { + const relPos = subtract(other.position, agent.position); + const relVel = subtract(other.velocity, candidateVelocity); + const combinedRadius = agent.radius + other.radius; + const distSq = dot(relPos, relPos); + + if (distSq < EPSILON) { + const direction = normalize(subtract(agent.position, other.position)); + return scale(direction, avoidanceStrength * agent.maxSpeed); + } + + const timeToCollision = computeTimeToCollision(relPos, relVel, combinedRadius); + + if (!Number.isFinite(timeToCollision) || timeToCollision > timeHorizon) { + return { x: 0, y: 0 }; + } + + const weight = Math.max(0, (timeHorizon - timeToCollision) / timeHorizon); + const separation = Math.sqrt(distSq); + + if (separation <= combinedRadius) { + const away = normalize(subtract(agent.position, other.position)); + return scale(away, weight * avoidanceStrength * agent.maxSpeed); + } + + const normal = normalize(relPos); + const tangent = { x: -normal.y, y: normal.x }; + const directionSign = dot(relVel, tangent) >= 0 ? 1 : -1; + const slide = scale(tangent, directionSign * weight * avoidanceStrength * agent.maxSpeed); + return slide; +} + +function computeTimeToCollision(relPos: Vector2D, relVel: Vector2D, combinedRadius: number): number { + const a = dot(relVel, relVel); + const b = 2 * dot(relPos, relVel); + const c = dot(relPos, relPos) - combinedRadius * combinedRadius; + + if (a < EPSILON) { + return Number.POSITIVE_INFINITY; + } + + const discriminant = b * b - 4 * a * c; + if (discriminant < 0) { + return Number.POSITIVE_INFINITY; + } + + const sqrtDisc = Math.sqrt(discriminant); + const t1 = (-b - sqrtDisc) / (2 * a); + const t2 = (-b + sqrtDisc) / (2 * a); + + if (t1 > EPSILON) { + return t1; + } + if (t2 > EPSILON) { + return t2; + } + return Number.POSITIVE_INFINITY; +} + +function subtract(a: Vector2D, b: Vector2D): Vector2D { + return { x: a.x - b.x, y: a.y - b.y }; +} + +function dot(a: Vector2D, b: Vector2D): number { + return a.x * b.x + a.y * b.y; +} + +function length(vec: Vector2D): number { + return Math.hypot(vec.x, vec.y); +} + +function squaredDistance(a: Vector2D, b: Vector2D): number { + return (a.x - b.x) ** 2 + (a.y - b.y) ** 2; +} + +function normalize(vec: Vector2D): Vector2D { + const len = length(vec); + if (len < EPSILON) { + return { x: 0, y: 0 }; + } + return { x: vec.x / len, y: vec.y / len }; +} + +function scale(vec: Vector2D, scalar: number): Vector2D { + return { x: vec.x * scalar, y: vec.y * scalar }; +} + +export const __internals = { computeTimeToCollision, subtract, dot, length, normalize, scale }; diff --git a/src/index.ts b/src/index.ts index 6c94570..bc40b43 100644 --- a/src/index.ts +++ b/src/index.ts @@ -50,6 +50,7 @@ export { quadraticBezier, cubicBezier } from './visual/bezier.js'; export { seek, flee, pursue, wander, arrive } from './ai/steering.js'; export { updateBoids } from './ai/boids.js'; +export { rvoStep } from './ai/rvo.js'; export { BehaviorTree, type BehaviorStatus, diff --git a/src/types.ts b/src/types.ts index f608cd7..39c09bb 100644 --- a/src/types.ts +++ b/src/types.ts @@ -50,3 +50,9 @@ export interface SteeringAgent extends Agent { export interface Boid extends SteeringAgent { acceleration: Vector2D; } + +export interface RvoAgent extends Agent { + id?: string; + radius: number; + preferredVelocity: Vector2D; +} diff --git a/tests/rvo.test.ts b/tests/rvo.test.ts new file mode 100644 index 0000000..349eac9 --- /dev/null +++ b/tests/rvo.test.ts @@ -0,0 +1,70 @@ +import { describe, expect, it } from 'vitest'; +import { rvoStep } from '../src/ai/rvo.js'; +import type { RvoAgent } from '../src/types.js'; + +describe('rvoStep', () => { + const baseAgents: RvoAgent[] = [ + { + id: 'left', + position: { x: -1, y: 0 }, + velocity: { x: 1, y: 0 }, + preferredVelocity: { x: 1, y: 0 }, + radius: 0.3, + maxSpeed: 2, + }, + { + id: 'right', + position: { x: 1, y: 0 }, + velocity: { x: -1, y: 0 }, + preferredVelocity: { x: -1, y: 0 }, + radius: 0.3, + maxSpeed: 2, + }, + ]; + + it('adjusts velocities to avoid impending collisions', () => { + const results = rvoStep(baseAgents, { timeHorizon: 3 }); + const left = results.find((entry) => entry.id === 'left'); + const right = results.find((entry) => entry.id === 'right'); + + expect(left).toBeDefined(); + expect(right).toBeDefined(); + + expect(Math.abs(left!.velocity.y)).toBeGreaterThan(0); + expect(Math.abs(right!.velocity.y)).toBeGreaterThan(0); + expect(left!.velocity.y * right!.velocity.y).toBeLessThan(0); + + expect(left!.velocity.x).toBeGreaterThan(0); + expect(right!.velocity.x).toBeLessThan(0); + }); + + it('respects max speed and leaves non-threatening agents unchanged', () => { + const agents: RvoAgent[] = [ + { + id: 'a', + position: { x: 0, y: 0 }, + velocity: { x: 0.2, y: 0 }, + preferredVelocity: { x: 1, y: 0 }, + radius: 0.2, + maxSpeed: 1, + }, + { + id: 'b', + position: { x: 0, y: 5 }, + velocity: { x: 0, y: -0.2 }, + preferredVelocity: { x: 0, y: -0.5 }, + radius: 0.2, + maxSpeed: 1, + }, + ]; + + const [first, second] = rvoStep(agents, { timeHorizon: 2 }); + expect(Math.hypot(first.velocity.x, first.velocity.y)).toBeLessThanOrEqual(1 + 1e-6); + expect(second.velocity).toEqual(agents[1].preferredVelocity); + }); + + it('validates inputs', () => { + expect(() => rvoStep(null as unknown as RvoAgent[])).toThrow(TypeError); + expect(() => rvoStep(baseAgents, { timeHorizon: 0 })).toThrow(RangeError); + }); +});