|
| 1 | +import type { RvoAgent, Vector2D } from '../types.js'; |
| 2 | + |
| 3 | +const EPSILON = 1e-6; |
| 4 | + |
| 5 | +export interface RvoOptions { |
| 6 | + timeHorizon?: number; |
| 7 | + maxNeighbors?: number; |
| 8 | + avoidanceStrength?: number; |
| 9 | +} |
| 10 | + |
| 11 | +export interface RvoResult { |
| 12 | + id?: string; |
| 13 | + velocity: Vector2D; |
| 14 | +} |
| 15 | + |
| 16 | +/** |
| 17 | + * Computes collision-avoiding agent velocities using reciprocal velocity obstacles (RVO). |
| 18 | + * Useful for: crowd steering, swarm navigation, multi-agent avoidance. |
| 19 | + */ |
| 20 | +export function rvoStep( |
| 21 | + agents: ReadonlyArray<RvoAgent>, |
| 22 | + options: RvoOptions = {} |
| 23 | +): RvoResult[] { |
| 24 | + if (!Array.isArray(agents)) { |
| 25 | + throw new TypeError('agents must be an array'); |
| 26 | + } |
| 27 | + |
| 28 | + const timeHorizon = options.timeHorizon ?? 2; |
| 29 | + const maxNeighbors = options.maxNeighbors ?? agents.length; |
| 30 | + const avoidanceStrength = options.avoidanceStrength ?? 0.6; |
| 31 | + |
| 32 | + if (timeHorizon <= 0) { |
| 33 | + throw new RangeError('timeHorizon must be greater than zero'); |
| 34 | + } |
| 35 | + if (maxNeighbors <= 0) { |
| 36 | + throw new RangeError('maxNeighbors must be greater than zero'); |
| 37 | + } |
| 38 | + if (avoidanceStrength < 0) { |
| 39 | + throw new RangeError('avoidanceStrength must be non-negative'); |
| 40 | + } |
| 41 | + |
| 42 | + return agents.map<RvoResult>((agent: RvoAgent, index: number) => { |
| 43 | + validateAgent(agent, index); |
| 44 | + |
| 45 | + const neighborEntries: Array<{ other: RvoAgent; distanceSq: number }> = agents |
| 46 | + .map((other: RvoAgent, otherIndex: number) => ({ |
| 47 | + other, |
| 48 | + distanceSq: squaredDistance(agent.position, other.position), |
| 49 | + otherIndex, |
| 50 | + })) |
| 51 | + .filter((entry) => entry.otherIndex !== index) |
| 52 | + .sort((a, b) => a.distanceSq - b.distanceSq) |
| 53 | + .slice(0, maxNeighbors) |
| 54 | + .map(({ other, distanceSq }) => ({ other, distanceSq })); |
| 55 | + |
| 56 | + let adjusted = { ...agent.preferredVelocity }; |
| 57 | + |
| 58 | + for (const { other } of neighborEntries) { |
| 59 | + const avoidance = computeAvoidance(agent, other, adjusted, timeHorizon, avoidanceStrength); |
| 60 | + adjusted = { |
| 61 | + x: adjusted.x + avoidance.x, |
| 62 | + y: adjusted.y + avoidance.y, |
| 63 | + }; |
| 64 | + } |
| 65 | + |
| 66 | + const speed = length(adjusted); |
| 67 | + if (speed > agent.maxSpeed) { |
| 68 | + adjusted = scale(adjusted, agent.maxSpeed / (speed || 1)); |
| 69 | + } |
| 70 | + |
| 71 | + return { id: agent.id, velocity: adjusted }; |
| 72 | + }); |
| 73 | +} |
| 74 | + |
| 75 | +function validateAgent(agent: RvoAgent, index: number): void { |
| 76 | + if (!agent) { |
| 77 | + throw new TypeError(`agents[${index}] is undefined`); |
| 78 | + } |
| 79 | + if (!isFinite(agent.position.x) || !isFinite(agent.position.y)) { |
| 80 | + throw new TypeError(`agents[${index}].position must contain finite numbers`); |
| 81 | + } |
| 82 | + if (!isFinite(agent.velocity.x) || !isFinite(agent.velocity.y)) { |
| 83 | + throw new TypeError(`agents[${index}].velocity must contain finite numbers`); |
| 84 | + } |
| 85 | + if (!isFinite(agent.preferredVelocity.x) || !isFinite(agent.preferredVelocity.y)) { |
| 86 | + throw new TypeError(`agents[${index}].preferredVelocity must contain finite numbers`); |
| 87 | + } |
| 88 | + if (!isFinite(agent.radius) || agent.radius < 0) { |
| 89 | + throw new RangeError(`agents[${index}].radius must be a non-negative number`); |
| 90 | + } |
| 91 | + if (!isFinite(agent.maxSpeed) || agent.maxSpeed <= 0) { |
| 92 | + throw new RangeError(`agents[${index}].maxSpeed must be a positive number`); |
| 93 | + } |
| 94 | +} |
| 95 | + |
| 96 | +function computeAvoidance( |
| 97 | + agent: RvoAgent, |
| 98 | + other: RvoAgent, |
| 99 | + candidateVelocity: Vector2D, |
| 100 | + timeHorizon: number, |
| 101 | + avoidanceStrength: number |
| 102 | +): Vector2D { |
| 103 | + const relPos = subtract(other.position, agent.position); |
| 104 | + const relVel = subtract(other.velocity, candidateVelocity); |
| 105 | + const combinedRadius = agent.radius + other.radius; |
| 106 | + const distSq = dot(relPos, relPos); |
| 107 | + |
| 108 | + if (distSq < EPSILON) { |
| 109 | + const direction = normalize(subtract(agent.position, other.position)); |
| 110 | + return scale(direction, avoidanceStrength * agent.maxSpeed); |
| 111 | + } |
| 112 | + |
| 113 | + const timeToCollision = computeTimeToCollision(relPos, relVel, combinedRadius); |
| 114 | + |
| 115 | + if (!Number.isFinite(timeToCollision) || timeToCollision > timeHorizon) { |
| 116 | + return { x: 0, y: 0 }; |
| 117 | + } |
| 118 | + |
| 119 | + const weight = Math.max(0, (timeHorizon - timeToCollision) / timeHorizon); |
| 120 | + const separation = Math.sqrt(distSq); |
| 121 | + |
| 122 | + if (separation <= combinedRadius) { |
| 123 | + const away = normalize(subtract(agent.position, other.position)); |
| 124 | + return scale(away, weight * avoidanceStrength * agent.maxSpeed); |
| 125 | + } |
| 126 | + |
| 127 | + const normal = normalize(relPos); |
| 128 | + const tangent = { x: -normal.y, y: normal.x }; |
| 129 | + const directionSign = dot(relVel, tangent) >= 0 ? 1 : -1; |
| 130 | + const slide = scale(tangent, directionSign * weight * avoidanceStrength * agent.maxSpeed); |
| 131 | + return slide; |
| 132 | +} |
| 133 | + |
| 134 | +function computeTimeToCollision(relPos: Vector2D, relVel: Vector2D, combinedRadius: number): number { |
| 135 | + const a = dot(relVel, relVel); |
| 136 | + const b = 2 * dot(relPos, relVel); |
| 137 | + const c = dot(relPos, relPos) - combinedRadius * combinedRadius; |
| 138 | + |
| 139 | + if (a < EPSILON) { |
| 140 | + return Number.POSITIVE_INFINITY; |
| 141 | + } |
| 142 | + |
| 143 | + const discriminant = b * b - 4 * a * c; |
| 144 | + if (discriminant < 0) { |
| 145 | + return Number.POSITIVE_INFINITY; |
| 146 | + } |
| 147 | + |
| 148 | + const sqrtDisc = Math.sqrt(discriminant); |
| 149 | + const t1 = (-b - sqrtDisc) / (2 * a); |
| 150 | + const t2 = (-b + sqrtDisc) / (2 * a); |
| 151 | + |
| 152 | + if (t1 > EPSILON) { |
| 153 | + return t1; |
| 154 | + } |
| 155 | + if (t2 > EPSILON) { |
| 156 | + return t2; |
| 157 | + } |
| 158 | + return Number.POSITIVE_INFINITY; |
| 159 | +} |
| 160 | + |
| 161 | +function subtract(a: Vector2D, b: Vector2D): Vector2D { |
| 162 | + return { x: a.x - b.x, y: a.y - b.y }; |
| 163 | +} |
| 164 | + |
| 165 | +function dot(a: Vector2D, b: Vector2D): number { |
| 166 | + return a.x * b.x + a.y * b.y; |
| 167 | +} |
| 168 | + |
| 169 | +function length(vec: Vector2D): number { |
| 170 | + return Math.hypot(vec.x, vec.y); |
| 171 | +} |
| 172 | + |
| 173 | +function squaredDistance(a: Vector2D, b: Vector2D): number { |
| 174 | + return (a.x - b.x) ** 2 + (a.y - b.y) ** 2; |
| 175 | +} |
| 176 | + |
| 177 | +function normalize(vec: Vector2D): Vector2D { |
| 178 | + const len = length(vec); |
| 179 | + if (len < EPSILON) { |
| 180 | + return { x: 0, y: 0 }; |
| 181 | + } |
| 182 | + return { x: vec.x / len, y: vec.y / len }; |
| 183 | +} |
| 184 | + |
| 185 | +function scale(vec: Vector2D, scalar: number): Vector2D { |
| 186 | + return { x: vec.x * scalar, y: vec.y * scalar }; |
| 187 | +} |
| 188 | + |
| 189 | +export const __internals = { computeTimeToCollision, subtract, dot, length, normalize, scale }; |
0 commit comments