wde_terrain_navigation/navigation/
mod.rs

1use std::collections::{HashMap, VecDeque};
2use wde_logger::prelude::*;
3
4use bevy::{ecs::system::SystemParam, prelude::*};
5use wde_terrain_grid::prelude::*;
6
7use crate::{TerrainNavigator, navgrid::NavGrid};
8
9pub(crate) mod pathfinding;
10pub(crate) mod walkmap;
11
12pub use pathfinding::AIRLOCK_ROLE;
13use pathfinding::{NavWorld, Waypoint, find_path, formation_spots};
14use walkmap::WalkMap;
15
16/// Turning speed, in radians per second.
17const TURN_SPEED: f32 = 10.0;
18/// Distance at which a waypoint counts as reached.
19const WAYPOINT_REACHED: f32 = 0.05;
20/// Distance to the center of an airlock within which its cycle starts: several colonists cycle
21/// together rather than queueing for the exact center.
22const AIRLOCK_REACHED: f32 = 0.3;
23/// Time without moving after which a blocked walker looks for another path, in seconds.
24const STUCK_REPLAN: f32 = 1.0;
25/// Radius of entities without [`NavAgent`] when spreading a group.
26const DEFAULT_AGENT_RADIUS: f32 = 0.14;
27/// Spacing of group standing spots, in agent diameters.
28const FORMATION_SPACING: f32 = 1.2;
29/// How fast overlapping agents are pushed apart (fraction of the overlap per second).
30const SEPARATION_RATE: f32 = 8.0;
31/// Share of the push taken by a walking agent against a standing one: standing agents make way.
32const WALKER_YIELD: f32 = 0.25;
33/// Sidestep of walking agents, relative to the overlap, so that crossing agents pass each other.
34const SIDESTEP: f32 = 0.6;
35
36/// Walking speeds: a base speed scaled by a factor depending on the terrain walked on. The path
37/// search minimizes the walking time, so colonists prefer paths and tunnels over bare ground.
38#[derive(Resource, Clone, Debug)]
39pub struct WalkSpeeds {
40    /// Speed for a factor of 1, in world units per second (about two colonist heights).
41    pub base: f32,
42    /// Factor inside pressurized buildings and tunnels.
43    pub indoor: f32,
44    /// Factor on outdoor paths and paved accesses.
45    pub path: f32,
46    /// Factor on bare outdoor ground.
47    pub ground: f32,
48    /// Time spent cycling an airlock when going out or in, in seconds.
49    pub airlock_cycle: f32
50}
51impl Default for WalkSpeeds {
52    fn default() -> Self {
53        Self {
54            base: 1.5,
55            indoor: 1.2,
56            path: 1.0,
57            ground: 0.7,
58            airlock_cycle: 2.5
59        }
60    }
61}
62
63/// Body of a walking entity: agents keep `radius` away from each other.
64#[derive(Component, Clone, Copy, Debug)]
65pub struct NavAgent {
66    pub radius: f32,
67    /// Walking speed relative to [`WalkSpeeds`].
68    pub speed: f32,
69    /// Never walks through pressurized places (e.g. wild creatures).
70    pub outdoors_only: bool
71}
72impl Default for NavAgent {
73    fn default() -> Self {
74        Self {
75            radius: DEFAULT_AGENT_RADIUS,
76            speed: 1.0,
77            outdoors_only: false
78        }
79    }
80}
81
82/// Whether a walking entity is in a pressurized place. It flips once an airlock cycle is over,
83/// e.g. to swap a colonist between its suit and indoor models.
84#[derive(Component, Clone, Copy, PartialEq, Eq, Debug)]
85pub struct Indoors(pub bool);
86
87pub struct NavigationPlugin;
88impl Plugin for NavigationPlugin {
89    fn build(&self, app: &mut App) {
90        app.init_resource::<WalkSpeeds>()
91            .init_resource::<WalkMap>()
92            .add_systems(
93                Update,
94                (
95                    walkmap::rebuild_walk_map.after(crate::navgrid::rebuild_nav_grid),
96                    request_paths,
97                    replan_on_network_change,
98                    follow_paths,
99                    separate_agents
100                )
101                    .chain()
102            );
103    }
104}
105
106/// Path followed by a moving entity.
107#[derive(Component)]
108pub(crate) struct NavPath {
109    pub waypoints: VecDeque<Waypoint>,
110    pub destination: Vec3,
111    /// Remaining stop time at the waypoint just reached (airlock cycle).
112    pub waiting: f32,
113    /// Time spent unable to move toward the next waypoint.
114    pub stuck: f32
115}
116
117/// Everything the path search reads.
118#[derive(SystemParam)]
119struct NavWorldParam<'w, 's> {
120    grid: Res<'w, Grid>,
121    placed: Query<'w, 's, (Entity, &'static GridEntity), With<BuildingConnections>>,
122    walk: Res<'w, WalkMap>,
123    network: Res<'w, NavGrid>,
124    terrain: Query<'w, 's, &'static wde_terrain::prelude::Terrain>
125}
126impl NavWorldParam<'_, '_> {
127    fn world(&self) -> NavWorld<'_> {
128        NavWorld {
129            grid: &self.grid,
130            buildings: self.placed.iter().collect::<HashMap<_, _>>(),
131            walk: &self.walk,
132            network: &self.network,
133            terrain: self.terrain.single().ok()
134        }
135    }
136
137    fn is_changed(&self) -> bool {
138        self.walk.is_changed() || self.network.is_changed()
139    }
140}
141
142/// Matches agents to spots, closest pairs first. Agents left without a spot get `None`.
143fn assign_spots(agents: &[(Entity, Vec3)], spots: &[Vec3]) -> Vec<(Entity, Option<Vec3>)> {
144    let mut pairs: Vec<(f32, usize, usize)> = agents
145        .iter()
146        .enumerate()
147        .flat_map(|(a, (_, pos))| {
148            spots
149                .iter()
150                .enumerate()
151                .map(move |(s, spot)| (pos.xz().distance_squared(spot.xz()), a, s))
152        })
153        .collect();
154    pairs.sort_unstable_by(|x, y| x.0.total_cmp(&y.0));
155    let mut agent_spot = vec![None; agents.len()];
156    let mut taken = vec![false; spots.len()];
157    for (_, a, s) in pairs {
158        if agent_spot[a].is_none() && !taken[s] {
159            agent_spot[a] = Some(spots[s]);
160            taken[s] = true;
161        }
162    }
163    agents.iter().map(|(e, _)| *e).zip(agent_spot).collect()
164}
165
166/// Computes the paths requested through the [`TerrainNavigator`].
167fn request_paths(
168    mut commands: Commands,
169    mut navigator: ResMut<TerrainNavigator>,
170    transforms: Query<&Transform>,
171    agents: Query<&NavAgent>,
172    nav: NavWorldParam,
173    speeds: Res<WalkSpeeds>
174) {
175    if navigator.new_entities.is_empty()
176        && navigator.groups.is_empty()
177        && navigator.removed_entities.is_empty()
178    {
179        return;
180    }
181    let world = nav.world();
182
183    // Groups: spread their destination into one standing spot per entity
184    let mut requests: Vec<(Entity, Vec3)> = navigator.new_entities.drain(..).collect();
185    for (entities, destination) in std::mem::take(&mut navigator.groups) {
186        let members: Vec<(Entity, Vec3)> = entities
187            .iter()
188            .filter_map(|e| transforms.get(*e).ok().map(|t| (*e, t.translation)))
189            .collect();
190        let radius = members
191            .iter()
192            .map(|(e, _)| agents.get(*e).map_or(DEFAULT_AGENT_RADIUS, |a| a.radius))
193            .fold(0.0, f32::max);
194        let spacing = 2.0 * radius * FORMATION_SPACING;
195        let spots = formation_spots(&world, destination, members.len(), spacing);
196        requests.extend(
197            assign_spots(&members, &spots)
198                .into_iter()
199                .map(|(entity, spot)| (entity, spot.unwrap_or(destination)))
200        );
201    }
202
203    for (entity, destination) in requests {
204        let Ok(transform) = transforms.get(entity) else {
205            continue;
206        };
207        let outdoors_only = agents.get(entity).is_ok_and(|a| a.outdoors_only);
208        match find_path(
209            &world,
210            &speeds,
211            transform.translation,
212            destination,
213            outdoors_only
214        ) {
215            Some(waypoints) => {
216                commands.entity(entity).insert(NavPath {
217                    waypoints: waypoints.into(),
218                    destination,
219                    waiting: 0.0,
220                    stuck: 0.0
221                });
222            }
223            None => {
224                warn!("No path from {} to {}.", transform.translation, destination);
225                commands.entity(entity).remove::<NavPath>();
226            }
227        }
228    }
229    for entity in navigator.removed_entities.drain(..) {
230        commands.entity(entity).remove::<NavPath>();
231    }
232}
233
234/// Replans the moving entities when buildings are placed or removed.
235fn replan_on_network_change(
236    nav: NavWorldParam,
237    mut moving: Query<(&Transform, &mut NavPath, Option<&NavAgent>)>,
238    speeds: Res<WalkSpeeds>
239) {
240    if !nav.is_changed() || moving.is_empty() {
241        return;
242    }
243    let world = nav.world();
244    for (transform, mut path, agent) in &mut moving {
245        if path.waiting > 0.0 {
246            continue; // Let the airlock cycle finish
247        }
248        // Keep the current path when the destination became unreachable
249        let outdoors_only = agent.is_some_and(|a| a.outdoors_only);
250        if let Some(waypoints) = find_path(
251            &world,
252            &speeds,
253            transform.translation,
254            path.destination,
255            outdoors_only
256        ) {
257            path.waypoints = waypoints.into();
258        }
259    }
260}
261
262/// Moves entities along their path at the speed of the terrain, stopping at airlocks, and turning
263/// smoothly toward their heading.
264/// Where a step from `from` ends: the full step if it stays walkable, else its slide along the
265/// blocking axis, else `None`. Off the walkable area (e.g. pushed), steps are free so that the
266/// walker gets back on it.
267fn walk_step(world: &NavWorld, from: Vec3, step: Vec3) -> Option<Vec3> {
268    let Some(here) = world.locate(from.xz()) else {
269        return Some(from + step);
270    };
271    [
272        step,
273        step * Vec3::new(1.0, 1.0, 0.0),
274        step * Vec3::new(0.0, 1.0, 1.0)
275    ]
276    .into_iter()
277    .filter(|s| s.xz().length_squared() > 1e-10)
278    .map(|s| from + s)
279    .find(|to| {
280        world
281            .locate(to.xz())
282            .is_some_and(|there| world.can_cross(&here, &there))
283    })
284}
285
286/// Entity walking along a path.
287type Walker<'a> = (
288    Entity,
289    &'a mut Transform,
290    &'a mut NavPath,
291    Option<&'a Indoors>,
292    Option<&'a NavAgent>
293);
294
295fn follow_paths(
296    mut commands: Commands,
297    mut moving: Query<Walker>,
298    nav: NavWorldParam,
299    speeds: Res<WalkSpeeds>,
300    time: Res<Time>
301) {
302    let dt = time.delta_secs();
303    let world = nav.world();
304    for (entity, mut transform, mut path, indoors, agent) in &mut moving {
305        let mut time_left = dt;
306        let mut heading = None;
307        let mut environment = indoors.map(|i| i.0);
308        while time_left > 0.0 {
309            // Airlock cycle: stand still
310            if path.waiting > 0.0 {
311                let waited = path.waiting.min(time_left);
312                path.waiting -= waited;
313                time_left -= waited;
314                continue;
315            }
316            let Some(&waypoint) = path.waypoints.front() else {
317                break;
318            };
319            // Helmet on or off for the segment ahead (after any airlock cycle)
320            environment = Some(waypoint.indoor);
321
322            let speed = speeds.base * waypoint.speed * agent.map_or(1.0, |a| a.speed);
323            let to_target = waypoint.position - transform.translation;
324            let distance = to_target.length();
325            if waypoint.wait > 0.0 && distance <= AIRLOCK_REACHED {
326                path.waypoints.pop_front();
327                path.waiting = waypoint.wait;
328                continue;
329            }
330            let reach = speed * time_left;
331            if distance <= WAYPOINT_REACHED.max(reach) {
332                transform.translation = waypoint.position;
333                time_left -= distance / speed.max(1e-3);
334                path.waypoints.pop_front();
335                path.waiting = waypoint.wait;
336                continue;
337            }
338            // Never through walls, even when pushed off the planned line
339            match walk_step(&world, transform.translation, to_target / distance * reach) {
340                Some(mut next) => {
341                    // Outdoors, on the ground (straight segments may cross bumps)
342                    if let Some(here) = world.locate(next.xz())
343                        && here.building.is_none()
344                    {
345                        next.y = here.height;
346                    }
347                    transform.translation = next;
348                    heading = Some(to_target);
349                    path.stuck = 0.0;
350                }
351                None => path.stuck += time_left
352            }
353            time_left = 0.0;
354        }
355        if path.stuck > STUCK_REPLAN {
356            path.stuck = 0.0;
357            if let Some(waypoints) = find_path(
358                &world,
359                &speeds,
360                transform.translation,
361                path.destination,
362                agent.is_some_and(|a| a.outdoors_only)
363            ) {
364                path.waypoints = waypoints.into();
365            }
366        }
367
368        if let Some(indoor) = environment
369            && indoors.is_none_or(|i| i.0 != indoor)
370        {
371            commands.entity(entity).insert(Indoors(indoor));
372        }
373
374        // Models face +X: yaw so that +X follows the horizontal heading
375        if let Some(heading) = heading
376            && heading.xz().length_squared() > 1e-6
377        {
378            let target = Quat::from_rotation_y((-heading.z).atan2(heading.x));
379            let rotation = transform.rotation;
380            transform.rotation = rotation.slerp(target, (TURN_SPEED * dt).min(1.0));
381        }
382
383        if path.waypoints.is_empty() && path.waiting <= 0.0 {
384            commands.entity(entity).remove::<NavPath>();
385        }
386    }
387}
388
389/// Keeps agents from overlapping: overlapping agents are pushed apart, standing ones making way
390/// for walking ones, and walking agents step aside to pass each other. Pushes never cross walls.
391fn separate_agents(
392    mut agents: Query<(&NavAgent, &mut Transform, Option<&NavPath>)>,
393    nav: NavWorldParam,
394    time: Res<Time>
395) {
396    let rate = (time.delta_secs() * SEPARATION_RATE).min(1.0);
397    if rate <= 0.0 {
398        return;
399    }
400
401    // (position, radius, mobility, heading)
402    let bodies: Vec<(Vec2, f32, f32, Option<Vec2>)> = agents
403        .iter()
404        .map(|(agent, transform, path)| {
405            let position = transform.translation.xz();
406            let (mobility, heading) = match path {
407                // Cycling an airlock: stays put
408                Some(path) if path.waiting > 0.0 => (0.0, None),
409                Some(path) => (
410                    WALKER_YIELD,
411                    path.waypoints
412                        .front()
413                        .and_then(|w| (w.position.xz() - position).try_normalize())
414                ),
415                None => (1.0, None)
416            };
417            (position, agent.radius, mobility, heading)
418        })
419        .collect();
420    if bodies.len() < 2 {
421        return;
422    }
423
424    // Spatial hash, cells as wide as the largest agent
425    let cell = 2.0 * bodies.iter().map(|b| b.1).fold(0.0, f32::max).max(0.01);
426    let key = |p: Vec2| (p / cell).floor().as_ivec2();
427    let mut cells: HashMap<IVec2, Vec<usize>> = HashMap::new();
428    for (i, body) in bodies.iter().enumerate() {
429        cells.entry(key(body.0)).or_default().push(i);
430    }
431
432    let mut pushes = vec![Vec2::ZERO; bodies.len()];
433    for (i, &(pi, ri, mi, hi)) in bodies.iter().enumerate() {
434        let k = key(pi);
435        for dx in -1..=1 {
436            for dy in -1..=1 {
437                for &j in cells.get(&(k + IVec2::new(dx, dy))).into_iter().flatten() {
438                    if j <= i {
439                        continue;
440                    }
441                    let (pj, rj, mj, hj) = bodies[j];
442                    let overlap = ri + rj - pi.distance(pj);
443                    if overlap <= 0.0 || mi + mj <= 0.0 {
444                        continue;
445                    }
446                    // Agents on the same point: split them along an arbitrary, stable axis
447                    let normal = (pi - pj)
448                        .try_normalize()
449                        .unwrap_or_else(|| Vec2::from_angle(i as f32 * 2.399));
450                    pushes[i] += normal * overlap * mi / (mi + mj);
451                    pushes[j] -= normal * overlap * mj / (mi + mj);
452                    // Walkers keep to their right
453                    if let Some(h) = hi {
454                        pushes[i] += Vec2::new(-h.y, h.x) * overlap * SIDESTEP;
455                    }
456                    if let Some(h) = hj {
457                        pushes[j] += Vec2::new(-h.y, h.x) * overlap * SIDESTEP;
458                    }
459                }
460            }
461        }
462    }
463    if pushes.iter().all(|p| *p == Vec2::ZERO) {
464        return;
465    }
466
467    let world = nav.world();
468    for ((_, mut transform, _), push) in agents.iter_mut().zip(pushes) {
469        if push == Vec2::ZERO {
470            continue;
471        }
472        let from = transform.translation.xz();
473        let to = from + push * rate;
474        // Never pushed through a wall or into an obstacle
475        if let Some(here) = world.locate(from)
476            && let Some(there) = world.locate(to)
477            && world.can_cross(&here, &there)
478        {
479            transform.translation.x = to.x;
480            transform.translation.z = to.y;
481        }
482    }
483}
wde_terrain_navigation/navigation/mod.rs

wde_terrain_navigation/navigation/
mod.rs
