repame_nav/
agent.rs

1//! [`NavigationAgent`]: path following with seek plus arrival, and an
2//! avoidance hook that resolves the desired velocity against nearby agents.
3
4use crate::avoid::{resolve, AvoidanceConfig, Neighbor2};
5use crate::vec::NavVec;
6
7/// Another agent seen by the avoidance resolver.
8#[derive(Clone, Copy, Debug)]
9pub struct AgentNeighbor<V> {
10    /// Agent position.
11    pub position: V,
12    /// Agent velocity.
13    pub velocity: V,
14    /// Agent radius.
15    pub radius: f32,
16}
17
18/// Follows a path returned by [`NavMesh::find_path`](crate::NavMesh::find_path)
19/// or [`NavMap::find_path`](crate::NavMap::find_path).
20///
21/// `desired_velocity` seeks the current waypoint and slows into the final one
22/// by `arrival_radius`. `avoidance_velocity` then feeds that desired velocity
23/// through the reciprocal velocity obstacle resolver.
24#[derive(Clone, Debug)]
25pub struct NavigationAgent<V: NavVec> {
26    /// Current path, first point nearest the agent.
27    pub path: Vec<V>,
28    /// Index of the waypoint being sought.
29    pub waypoint: usize,
30    /// Desired speed toward a waypoint.
31    pub max_speed: f32,
32    /// Distance to the final waypoint within which the agent stops.
33    pub arrival_radius: f32,
34    /// Distance within which an intermediate waypoint counts as reached.
35    pub waypoint_advance: f32,
36    /// Last velocity produced by [`NavigationAgent::desired_velocity`].
37    pub desired_velocity: V,
38    /// Avoidance tuning.
39    pub avoidance: AvoidanceConfig,
40    /// When false, avoidance is a pass-through.
41    pub avoidance_enabled: bool,
42}
43
44impl<V: NavVec> NavigationAgent<V> {
45    /// New agent with no path.
46    pub fn new(max_speed: f32) -> Self {
47        Self {
48            path: Vec::new(),
49            waypoint: 0,
50            max_speed,
51            arrival_radius: 0.5,
52            waypoint_advance: 0.3,
53            desired_velocity: V::ZERO,
54            avoidance: AvoidanceConfig::default(),
55            avoidance_enabled: false,
56        }
57    }
58
59    /// Replace the path and reset following to its first point.
60    pub fn set_path(&mut self, path: Vec<V>) {
61        self.waypoint = 0;
62        self.path = path;
63        self.desired_velocity = V::ZERO;
64    }
65
66    /// Drop the path.
67    pub fn clear_path(&mut self) {
68        self.path.clear();
69        self.waypoint = 0;
70        self.desired_velocity = V::ZERO;
71    }
72
73    /// The final waypoint, if any.
74    pub fn target(&self) -> Option<V> {
75        self.path.last().copied()
76    }
77
78    /// Whether the path is empty or the final waypoint is within
79    /// [`NavigationAgent::arrival_radius`].
80    pub fn is_finished(&self, position: V) -> bool {
81        match self.target() {
82            Some(target) => position.distance(target) <= self.arrival_radius,
83            None => true,
84        }
85    }
86
87    /// Advance past every waypoint the agent has already reached.
88    pub fn advance(&mut self, position: V) {
89        while self.waypoint + 1 < self.path.len()
90            && position.distance(self.path[self.waypoint]) <= self.waypoint_advance
91        {
92            self.waypoint += 1;
93        }
94    }
95
96    /// Seek the current waypoint, slowing into the final one. Caches the result
97    /// in [`NavigationAgent::desired_velocity`].
98    pub fn desired_velocity(&mut self, position: V) -> V {
99        self.advance(position);
100        if self.path.is_empty() {
101            self.desired_velocity = V::ZERO;
102            return V::ZERO;
103        }
104        let waypoint = self.path[self.waypoint];
105        let distance = position.distance(waypoint);
106        let is_final = self.waypoint + 1 >= self.path.len();
107        if is_final && distance <= self.arrival_radius {
108            self.desired_velocity = V::ZERO;
109            return V::ZERO;
110        }
111        let direction = waypoint.sub(position).normalized();
112        let speed = if is_final {
113            let span = (distance - self.arrival_radius).max(0.0) / self.arrival_radius.max(1e-4);
114            self.max_speed * span.min(1.0)
115        } else {
116            self.max_speed
117        };
118        self.desired_velocity = direction.mul(speed);
119        self.desired_velocity
120    }
121
122    /// Apply avoidance to the cached desired velocity.
123    ///
124    /// Avoidance runs in the navigation plane, so a 3D agent dodges on the
125    /// ground plane while keeping the vertical component of its desired
126    /// velocity (the slope). Deterministic for a given neighbour list.
127    pub fn avoidance_velocity(&self, position: V, neighbors: &[AgentNeighbor<V>]) -> V {
128        if !self.avoidance_enabled || neighbors.is_empty() {
129            return self.desired_velocity;
130        }
131        let plane: Vec<Neighbor2> = neighbors
132            .iter()
133            .map(|n| {
134                let p = n.position.nav2();
135                let v = n.velocity.nav2();
136                Neighbor2 {
137                    position: glam::Vec2::new(p[0], p[1]),
138                    velocity: glam::Vec2::new(v[0], v[1]),
139                    radius: n.radius,
140                }
141            })
142            .collect();
143        let desired = self.desired_velocity.nav2();
144        let resolved = resolve(
145            glam::Vec2::new(desired[0], desired[1]),
146            glam::Vec2::new(position.nav2()[0], position.nav2()[1]),
147            &plane,
148            &self.avoidance,
149        );
150        V::from_nav2([resolved.x, resolved.y], self.desired_velocity.height())
151    }
152}
repame_nav/agent.rs

repame_nav/
agent.rs
