Skip to main content

rayengine_core/pathfinding/
follow.rs

1use super::GridLayout;
2use crate::collision::{Body2D, Body3D};
3use glam::{UVec2, Vec2};
4
5/// Steers a character through world-space waypoints.
6///
7/// The follower only chooses velocities; collision stays with
8/// [`Body2D::move_and_slide`] or [`Body3D::move_and_slide`]. A waypoint is
9/// reached within `arrive_radius`. Each step is shortened so it ends on the
10/// current waypoint instead of jumping past it, keeping the character close
11/// to the path's segments, which [`smooth_path`](super::smooth_path) checked
12/// for clearance. Collision moves along each axis in turn, so keep steps
13/// (`speed * dt`) well under a cell. Any waypoint but the last also counts once the character is
14/// beyond it along its segment and within `arrive_radius` of that segment's
15/// line: the incoming segment, or the outgoing one for the first waypoint, so
16/// a character on the path ahead of its starting cell's center does not step
17/// back. A character pushed farther aside returns to the waypoint first.
18///
19/// ```
20/// use rayengine_core::collision::Body2D;
21/// use rayengine_core::glam::{UVec2, Vec2};
22/// use rayengine_core::pathfinding::{GridLayout, PathFollower};
23///
24/// let layout = GridLayout::new(Vec2::ZERO, Vec2::ONE);
25/// let mut follower = PathFollower::new(0.05);
26/// follower.set_cells(&layout, &[UVec2::new(0, 0), UVec2::new(3, 0)]);
27/// let mut body = Body2D::new(Vec2::splat(0.5), Vec2::splat(0.6));
28/// for _ in 0..120 {
29///     follower.steer_body_2d(&mut body, 4.0, 1.0 / 60.0);
30///     body.move_and_slide(1.0 / 60.0, &[]);
31/// }
32/// assert!(follower.is_finished());
33/// assert!((body.position - Vec2::new(3.5, 0.5)).length() < 0.05);
34/// ```
35#[derive(Clone, Debug, PartialEq)]
36pub struct PathFollower {
37    waypoints: Vec<Vec2>,
38    next: usize,
39    arrive_radius: f32,
40}
41
42impl PathFollower {
43    /// Creates a finished follower. Panics unless `arrive_radius` is finite
44    /// and positive.
45    pub fn new(arrive_radius: f32) -> Self {
46        assert!(arrive_radius.is_finite() && arrive_radius > 0.0);
47        Self {
48            waypoints: Vec::new(),
49            next: 0,
50            arrive_radius,
51        }
52    }
53
54    /// Follows the centers of `cells`, reusing waypoint capacity.
55    pub fn set_cells(&mut self, layout: &GridLayout, cells: &[UVec2]) {
56        self.set_waypoints(cells.iter().map(|&cell| layout.cell_center(cell)));
57    }
58
59    /// Follows world-space points, reusing waypoint capacity. Panics for a
60    /// nonfinite point.
61    pub fn set_waypoints(&mut self, points: impl IntoIterator<Item = Vec2>) {
62        self.waypoints.clear();
63        self.waypoints.extend(points);
64        assert!(self.waypoints.iter().all(|point| point.is_finite()));
65        self.next = 0;
66    }
67
68    /// Stops following and forgets the waypoints.
69    pub fn clear(&mut self) {
70        self.waypoints.clear();
71        self.next = 0;
72    }
73
74    /// Whether every waypoint has been reached (or none were given).
75    pub fn is_finished(&self) -> bool {
76        self.next >= self.waypoints.len()
77    }
78
79    /// All waypoints of the current path.
80    pub fn waypoints(&self) -> &[Vec2] {
81        &self.waypoints
82    }
83
84    /// Waypoints not reached yet, starting with the current target.
85    pub fn remaining(&self) -> &[Vec2] {
86        &self.waypoints[self.next.min(self.waypoints.len())..]
87    }
88
89    /// Current target waypoint.
90    pub fn next_waypoint(&self) -> Option<Vec2> {
91        self.remaining().first().copied()
92    }
93
94    /// Velocity toward the current waypoint at `speed` units per second.
95    ///
96    /// Advances past reached waypoints first, returns zero once finished, and
97    /// slows down so one step of `dt` seconds does not overshoot the current
98    /// waypoint. Panics for a nonfinite position, a negative or nonfinite
99    /// speed, or a negative or nonfinite `dt`.
100    pub fn velocity(&mut self, position: Vec2, speed: f32, dt: f32) -> Vec2 {
101        assert!(position.is_finite() && speed.is_finite() && speed >= 0.0);
102        assert!(dt.is_finite() && dt >= 0.0);
103        let last = self.waypoints.len().saturating_sub(1);
104        while let Some(&target) = self.waypoints.get(self.next) {
105            let offset = position - target;
106            let reached = offset.length() <= self.arrive_radius;
107            let segment = match self.next {
108                next if next == last => Vec2::ZERO,
109                0 => self.waypoints[1] - target,
110                next => target - self.waypoints[next - 1],
111            };
112            let direction = segment.normalize_or_zero();
113            let passed = direction.dot(offset) > 0.0
114                && direction.perp_dot(offset).abs() <= self.arrive_radius;
115            if !(reached || passed) {
116                break;
117            }
118            self.next += 1;
119        }
120        let Some(target) = self.next_waypoint() else {
121            return Vec2::ZERO;
122        };
123        let offset = target - position;
124        let distance = offset.length();
125        let speed = if dt > 0.0 {
126            speed.min(distance / dt)
127        } else {
128            speed
129        };
130        offset / distance * speed
131    }
132
133    /// Sets a top-down body's velocity toward the path; see
134    /// [`velocity`](Self::velocity).
135    pub fn steer_body_2d(&mut self, body: &mut Body2D, speed: f32, dt: f32) {
136        body.velocity = self.velocity(body.position, speed, dt);
137    }
138
139    /// Sets a 3D body's horizontal velocity, reading waypoints as world
140    /// `(x, z)`. Vertical velocity (gravity, jumps) is kept.
141    pub fn steer_body_3d(&mut self, body: &mut Body3D, speed: f32, dt: f32) {
142        let velocity = self.velocity(Vec2::new(body.position.x, body.position.z), speed, dt);
143        body.velocity.x = velocity.x;
144        body.velocity.z = velocity.y;
145    }
146}