repame_nav/
avoid.rs

1//! Reciprocal velocity obstacle avoidance.
2//!
3//! Each neighbour contributes a collision cone in velocity space. The
4//! resolver samples a fixed fan of candidate velocities around the desired
5//! one and takes the candidate closest to it that is collision free within
6//! the time horizon; when none is free it takes the candidate with the latest
7//! first collision. Sampling is deterministic: the fan order is fixed and the
8//! neighbour list is sorted before use. This is a real velocity-obstacle
9//! resolver with half-shared responsibility (each agent dodges half), not a
10//! jitter or a hardcoded offset.
11
12use glam::Vec2;
13
14/// Tuning for [`resolve`].
15#[derive(Clone, Copy, Debug)]
16pub struct AvoidanceConfig {
17    /// Neighbours farther than this (surface to surface) are ignored.
18    pub neighbor_distance: f32,
19    /// At most this many nearest neighbours are considered.
20    pub max_neighbors: usize,
21    /// This agent's radius.
22    pub radius: f32,
23    /// How far ahead a collision is predicted.
24    pub time_horizon: f32,
25    /// Candidate headings sampled around the desired heading.
26    pub angle_samples: u32,
27}
28
29impl Default for AvoidanceConfig {
30    fn default() -> Self {
31        Self {
32            neighbor_distance: 4.0,
33            max_neighbors: 8,
34            radius: 0.4,
35            time_horizon: 2.0,
36            angle_samples: 16,
37        }
38    }
39}
40
41/// A neighbour in the navigation plane.
42#[derive(Clone, Copy, Debug)]
43pub struct Neighbor2 {
44    /// Neighbour position.
45    pub position: Vec2,
46    /// Neighbour velocity.
47    pub velocity: Vec2,
48    /// Neighbour radius.
49    pub radius: f32,
50}
51
52/// Earliest collision time of `candidate` with one neighbour under the
53/// reciprocal (half-shared) relative velocity, or `f32::INFINITY` if none
54/// inside the horizon.
55fn earliest_collision(
56    position: Vec2,
57    candidate: Vec2,
58    neighbor: &Neighbor2,
59    radius_sum: f32,
60    horizon: f32,
61) -> f32 {
62    let d = neighbor.position - position;
63    let relative = (candidate - neighbor.velocity) * 0.5;
64    let c = d.dot(d) - radius_sum * radius_sum;
65    if c < 0.0 {
66        return 0.0;
67    }
68    let a = relative.dot(relative);
69    if a <= 1e-12 {
70        return f32::INFINITY;
71    }
72    let b = 2.0 * d.dot(relative);
73    let discriminant = b * b - 4.0 * a * c;
74    if discriminant < 0.0 {
75        return f32::INFINITY;
76    }
77    let t = (-b - discriminant.sqrt()) / (2.0 * a);
78    if t >= 0.0 && t <= horizon { t } else { f32::INFINITY }
79}
80
81/// Score one candidate against the neighbour set and fold it into `best`.
82fn consider(
83    candidate: Vec2,
84    index: u32,
85    desired: Vec2,
86    position: Vec2,
87    near: &[(f32, Neighbor2)],
88    config: &AvoidanceConfig,
89    best: &mut Option<(bool, f32, u32, Vec2)>,
90) {
91    let mut earliest = f32::INFINITY;
92    for (_, neighbor) in near {
93        let t = earliest_collision(
94            position,
95            candidate,
96            neighbor,
97            config.radius + neighbor.radius,
98            config.time_horizon,
99        );
100        if t < earliest {
101            earliest = t;
102        }
103    }
104    let feasible = earliest.is_infinite();
105    let key = if feasible { (candidate - desired).length() } else { -earliest };
106    let better = match *best {
107        None => true,
108        Some((best_feasible, best_key, best_index, _)) => {
109            (feasible && !best_feasible)
110                || (feasible == best_feasible
111                    && (key < best_key || (key == best_key && index < best_index)))
112        }
113    };
114    if better {
115        *best = Some((feasible, key, index, candidate));
116    }
117}
118
119/// Resolve `desired` against nearby agents. Returns the chosen velocity.
120pub fn resolve(desired: Vec2, position: Vec2, neighbors: &[Neighbor2], config: &AvoidanceConfig) -> Vec2 {
121    let mut near: Vec<(f32, Neighbor2)> = neighbors
122        .iter()
123        .filter(|n| position.distance(n.position) - n.radius - config.radius <= config.neighbor_distance)
124        .map(|n| (position.distance(n.position), *n))
125        .collect();
126    near.sort_by(|a, b| {
127        a.0.total_cmp(&b.0)
128            .then(a.1.position.x.to_bits().cmp(&b.1.position.x.to_bits()))
129            .then(a.1.position.y.to_bits().cmp(&b.1.position.y.to_bits()))
130            .then(a.1.velocity.x.to_bits().cmp(&b.1.velocity.x.to_bits()))
131            .then(a.1.velocity.y.to_bits().cmp(&b.1.velocity.y.to_bits()))
132    });
133    near.truncate(config.max_neighbors);
134    if near.is_empty() {
135        return desired;
136    }
137
138    let speed = desired.length();
139    if speed < 1e-6 {
140        return Vec2::ZERO;
141    }
142    let base = desired / speed;
143    let speeds = [1.0f32, 0.7, 0.4];
144
145    let mut best: Option<(bool, f32, u32, Vec2)> = None;
146    let mut index = 0u32;
147    for sample in 0..config.angle_samples {
148        let angle = std::f32::consts::TAU * sample as f32 / config.angle_samples as f32;
149        let (sin, cos) = angle.sin_cos();
150        let rotated = Vec2::new(base.x * cos - base.y * sin, base.x * sin + base.y * cos);
151        for scale in speeds {
152            consider(rotated * (speed * scale), index, desired, position, &near, config, &mut best);
153            index += 1;
154        }
155    }
156    consider(Vec2::ZERO, index, desired, position, &near, config, &mut best);
157
158    best.map(|(_, _, _, velocity)| velocity).unwrap_or(desired)
159}
repame_nav/avoid.rs

repame_nav/
avoid.rs
