use super::GridLayout;
use crate::collision::{Body2D, Body3D};
use glam::{UVec2, Vec2};
#[derive(Clone, Debug, PartialEq)]
pub struct PathFollower {
waypoints: Vec<Vec2>,
next: usize,
arrive_radius: f32,
}
impl PathFollower {
pub fn new(arrive_radius: f32) -> Self {
assert!(arrive_radius.is_finite() && arrive_radius > 0.0);
Self {
waypoints: Vec::new(),
next: 0,
arrive_radius,
}
}
pub fn set_cells(&mut self, layout: &GridLayout, cells: &[UVec2]) {
self.set_waypoints(cells.iter().map(|&cell| layout.cell_center(cell)));
}
pub fn set_waypoints(&mut self, points: impl IntoIterator<Item = Vec2>) {
self.waypoints.clear();
self.waypoints.extend(points);
assert!(self.waypoints.iter().all(|point| point.is_finite()));
self.next = 0;
}
pub fn clear(&mut self) {
self.waypoints.clear();
self.next = 0;
}
pub fn is_finished(&self) -> bool {
self.next >= self.waypoints.len()
}
pub fn waypoints(&self) -> &[Vec2] {
&self.waypoints
}
pub fn remaining(&self) -> &[Vec2] {
&self.waypoints[self.next.min(self.waypoints.len())..]
}
pub fn next_waypoint(&self) -> Option<Vec2> {
self.remaining().first().copied()
}
pub fn velocity(&mut self, position: Vec2, speed: f32, dt: f32) -> Vec2 {
assert!(position.is_finite() && speed.is_finite() && speed >= 0.0);
assert!(dt.is_finite() && dt >= 0.0);
let last = self.waypoints.len().saturating_sub(1);
while let Some(&target) = self.waypoints.get(self.next) {
let offset = position - target;
let reached = offset.length() <= self.arrive_radius;
let segment = match self.next {
next if next == last => Vec2::ZERO,
0 => self.waypoints[1] - target,
next => target - self.waypoints[next - 1],
};
let direction = segment.normalize_or_zero();
let passed = direction.dot(offset) > 0.0
&& direction.perp_dot(offset).abs() <= self.arrive_radius;
if !(reached || passed) {
break;
}
self.next += 1;
}
let Some(target) = self.next_waypoint() else {
return Vec2::ZERO;
};
let offset = target - position;
let distance = offset.length();
let speed = if dt > 0.0 {
speed.min(distance / dt)
} else {
speed
};
offset / distance * speed
}
pub fn steer_body_2d(&mut self, body: &mut Body2D, speed: f32, dt: f32) {
body.velocity = self.velocity(body.position, speed, dt);
}
pub fn steer_body_3d(&mut self, body: &mut Body3D, speed: f32, dt: f32) {
let velocity = self.velocity(Vec2::new(body.position.x, body.position.z), speed, dt);
body.velocity.x = velocity.x;
body.velocity.z = velocity.y;
}
}