veloren_rtsim/rule/npc_ai/
movement.rs

1use super::*;
2
3fn path_in_site(start: Vec2<i32>, end: Vec2<i32>, site: &site::Site) -> PathResult<Vec2<i32>> {
4    let heuristic = |tile: &Vec2<i32>| tile.as_::<f32>().distance(end.as_());
5    const ASTAR_ITERS: usize = 1000;
6    let mut astar = Astar::new(
7        ASTAR_ITERS,
8        start,
9        BuildHasherDefault::<FxHasher64>::default(),
10    );
11
12    let transition = |a: Vec2<i32>, b: Vec2<i32>| {
13        let distance = a.as_::<f32>().distance(b.as_());
14        let a_tile = site.tiles.get(a);
15        let b_tile = site.tiles.get(b);
16
17        let terrain = match &b_tile.kind {
18            TileKind::Empty => 3.0,
19            TileKind::Hazard(_) => 50.0,
20            TileKind::Field => 8.0,
21            TileKind::Plaza | TileKind::Road { .. } | TileKind::Path { .. } | TileKind::Bridge => {
22                1.0
23            },
24
25            TileKind::Building
26            | TileKind::Castle
27            | TileKind::Wall(_)
28            | TileKind::Tower(_)
29            | TileKind::Keep(_)
30            | TileKind::Gate
31            | TileKind::AdletStronghold
32            | TileKind::DwarvenMine
33            | TileKind::GnarlingFortification => 5.0,
34        };
35        let is_door_tile = |plot: Id<site::Plot>, tile: Vec2<i32>| {
36            site.plot(plot)
37                .kind()
38                .meta()
39                .is_some_and(|meta| meta.door_tile() == Some(tile))
40        };
41        let building = if a_tile.is_building() && b_tile.is_road() {
42            a_tile
43                .plot
44                .and_then(|plot| is_door_tile(plot, a).then_some(1.0))
45                .unwrap_or(10000.0)
46        } else if b_tile.is_building() && a_tile.is_road() {
47            b_tile
48                .plot
49                .and_then(|plot| is_door_tile(plot, b).then_some(1.0))
50                .unwrap_or(10000.0)
51        } else if (a_tile.is_building() || b_tile.is_building()) && a_tile.plot != b_tile.plot {
52            10000.0
53        } else {
54            1.0
55        };
56
57        distance * terrain + building
58    };
59
60    let neighbors = |tile: &Vec2<i32>| {
61        let tile = *tile;
62
63        const CARDINALS: &[Vec2<i32>] = &[
64            Vec2::new(1, 0),
65            Vec2::new(0, 1),
66            Vec2::new(-1, 0),
67            Vec2::new(0, -1),
68        ];
69
70        CARDINALS.iter().map(move |c| {
71            let n = tile + *c;
72            (n, transition(tile, n))
73        })
74    };
75
76    astar.poll(ASTAR_ITERS, heuristic, neighbors, |tile| *tile == end)
77}
78
79fn path_between_sites(
80    start: SiteId,
81    end: SiteId,
82    sites: &Sites,
83    world: &World,
84) -> PathResult<(Id<Track>, bool)> {
85    let world_site = |site_id: SiteId| {
86        let id = sites.get(site_id).and_then(|site| site.world_site)?;
87        world.civs().sites.recreate_id(id.id())
88    };
89
90    let start = if let Some(start) = world_site(start) {
91        start
92    } else {
93        return PathResult::Pending;
94    };
95    let end = if let Some(end) = world_site(end) {
96        end
97    } else {
98        return PathResult::Pending;
99    };
100
101    let get_site = |site: &Id<civ::Site>| world.civs().sites.get(*site);
102
103    let end_pos = get_site(&end).center.as_::<f32>();
104    let heuristic = |site: &Id<civ::Site>| get_site(site).center.as_().distance(end_pos);
105
106    let mut astar = Astar::new(250, start, BuildHasherDefault::<FxHasher64>::default());
107
108    let transition = |a: Id<civ::Site>, b: Id<civ::Site>| {
109        world
110            .civs()
111            .track_between(a, b)
112            .map(|(id, _)| world.civs().tracks.get(id).cost)
113            .unwrap_or(f32::INFINITY)
114    };
115    let neighbors = |site: &Id<civ::Site>| {
116        let site = *site;
117        world
118            .civs()
119            .neighbors(site)
120            .map(move |n| (n, transition(n, site)))
121    };
122
123    let path = astar.poll(250, heuristic, neighbors, |site| *site == end);
124
125    path.map(|path| {
126        let path = path
127            .into_iter()
128            .tuple_windows::<(_, _)>()
129            // Since we get a, b from neighbors, track_between shouldn't return None.
130            .filter_map(|(a, b)| world.civs().track_between(a, b))
131            .collect();
132        Path { nodes: path }
133    })
134}
135
136fn path_site(
137    start: Vec2<f32>,
138    end: Vec2<f32>,
139    site: Id<WorldSite>,
140    index: IndexRef,
141) -> Option<Vec<Vec2<f32>>> {
142    let site = index.sites.get(site);
143    let start = site.wpos_tile_pos(start.as_());
144
145    let end = site.wpos_tile_pos(end.as_());
146
147    let nodes = match path_in_site(start, end, site) {
148        PathResult::Path(p, _c) => p.nodes,
149        PathResult::Exhausted(p) => p.nodes,
150        PathResult::None(_) | PathResult::Pending => return None,
151    };
152
153    Some(
154        nodes
155            .into_iter()
156            .map(|tile| site.tile_center_wpos(tile).as_() + 0.5)
157            .collect(),
158    )
159}
160
161fn path_between_towns(
162    start: SiteId,
163    end: SiteId,
164    sites: &Sites,
165    world: &World,
166) -> Option<PathData<(Id<Track>, bool), SiteId>> {
167    match path_between_sites(start, end, sites, world) {
168        PathResult::Exhausted(p) => Some(PathData {
169            end,
170            path: p.nodes.into(),
171            repoll: true,
172        }),
173        PathResult::Path(p, _c) => Some(PathData {
174            end,
175            path: p.nodes.into(),
176            repoll: false,
177        }),
178        PathResult::Pending | PathResult::None(_) => None,
179    }
180}
181
182// Actions
183
184/// Try to walk toward a 3D position without caring for obstacles.
185pub fn goto<S: State>(wpos: Vec3<f32>, speed_factor: f32, goal_dist: f32) -> impl Action<S> {
186    const WAYPOINT_DIST: f32 = 12.0;
187
188    just(move |ctx, waypoint: &mut Option<Vec3<f32>>| {
189        // If we're close to the next waypoint, complete it
190        if waypoint.is_some_and(|waypoint: Vec3<f32>| {
191            ctx.npc.wpos.xy().distance_squared(waypoint.xy()) < WAYPOINT_DIST.powi(2)
192        }) {
193            *waypoint = None;
194        }
195
196        // Get the next waypoint on the route toward the goal
197        let waypoint = waypoint.get_or_insert_with(|| {
198            wpos.with_z(ctx.world.sim().get_surface_alt_approx(wpos.xy().as_()))
199        });
200
201        ctx.controller.do_goto(*waypoint, speed_factor);
202    })
203    .repeat()
204    .stop_if(move |ctx: &mut NpcCtx| {
205        ctx.npc.wpos.xy().distance_squared(wpos.xy()) < goal_dist.powi(2)
206    })
207    .with_state(None)
208    .debug(move || format!("goto {}, {}, {}", wpos.x, wpos.y, wpos.z))
209    .map(|_, _| {})
210}
211
212pub fn follow_actor<S: State>(actor: Actor, distance: f32) -> impl Action<S> {
213    // const STEP_DIST: f32 = 30.0;
214    just(move |ctx, _| {
215        if let Some(tgt_wpos) = util::locate_actor(ctx, actor)
216            && let dist_sqr = tgt_wpos.xy().distance_squared(ctx.npc.wpos.xy())
217            && dist_sqr > distance.powi(2)
218        {
219            // // Don't try to path too far in one go
220            // let tgt_wpos = if dist_sqr > STEP_DIST.powi(2) {
221            //     let tgt_wpos_2d = ctx.npc.wpos.xy() + (tgt_wpos -
222            // ctx.npc.wpos).xy().normalized() * STEP_DIST;     tgt_wpos_2d.
223            // with_z(ctx.world.sim().get_surface_alt_approx(tgt_wpos_2d.as_()))
224            // } else {
225            //     tgt_wpos
226            // };
227            ctx.controller.do_goto(
228                tgt_wpos,
229                ((dist_sqr.sqrt() - distance) * 0.2).clamp(0.25, 1.0),
230            );
231        } else {
232            ctx.controller.do_idle();
233        }
234    })
235    .repeat()
236    .debug(move || format!("Following actor {actor:?}"))
237    .map(|_, _| ())
238}
239
240pub fn goto_actor<S: State>(actor: Actor, distance: f32) -> impl Action<S> {
241    follow_actor(actor, distance)
242        .stop_if(move |ctx: &mut NpcCtx| {
243            if let Some(wpos) = util::locate_actor(ctx, actor) {
244                wpos.xy().distance_squared(ctx.npc.wpos.xy()) < distance.powi(2)
245            } else {
246                false
247            }
248        })
249        .map(|_, _| ())
250}
251
252/// Try to walk fly a 3D position following the terrain altitude at an offset
253/// without caring for obstacles.
254fn goto_flying<S: State>(
255    wpos: Vec3<f32>,
256    speed_factor: f32,
257    goal_dist: f32,
258    step_dist: f32,
259    waypoint_dist: f32,
260    height_offset: f32,
261) -> impl Action<S> {
262    just(move |ctx, waypoint: &mut Option<Vec3<f32>>| {
263        // If we're close to the next waypoint, complete it
264        if waypoint.is_some_and(|waypoint: Vec3<f32>| {
265            ctx.npc.wpos.distance_squared(waypoint) < waypoint_dist.powi(2)
266        }) {
267            *waypoint = None;
268        }
269
270        // Get the next waypoint on the route toward the goal
271        let waypoint = waypoint.get_or_insert_with(|| {
272            let rpos = wpos - ctx.npc.wpos;
273            let len = rpos.magnitude();
274            let wpos = ctx.npc.wpos + (rpos / len) * len.min(step_dist);
275
276            wpos.with_z(ctx.world.sim().get_surface_alt_approx(wpos.xy().as_()) + height_offset)
277        });
278
279        ctx.controller.do_goto(*waypoint, speed_factor);
280    })
281    .repeat()
282    .boxed()
283    .with_state(None)
284    .stop_if(move |ctx: &mut NpcCtx| {
285        ctx.npc.wpos.xy().distance_squared(wpos.xy()) < goal_dist.powi(2)
286    })
287    .debug(move || {
288        format!(
289            "goto flying ({}, {}, {}), goal dist {}",
290            wpos.x, wpos.y, wpos.z, goal_dist
291        )
292    })
293    .map(|_, _| {})
294}
295
296/// Try to walk toward a 2D position on the surface without caring for
297/// obstacles.
298pub fn goto_2d<S: State>(wpos2d: Vec2<f32>, speed_factor: f32, goal_dist: f32) -> impl Action<S> {
299    now(move |ctx, _| {
300        let wpos = wpos2d.with_z(ctx.world.sim().get_surface_alt_approx(wpos2d.as_()));
301        goto(wpos, speed_factor, goal_dist).debug(move || {
302            format!(
303                "goto 2d ({}, {}), z {}, goal dist {}",
304                wpos2d.x, wpos2d.y, wpos.z, goal_dist
305            )
306        })
307    })
308}
309
310/// Try to fly toward a 2D position following the terrain altitude at an offset
311/// without caring for obstacles.
312pub fn goto_2d_flying<S: State>(
313    wpos2d: Vec2<f32>,
314    speed_factor: f32,
315    goal_dist: f32,
316    step_dist: f32,
317    waypoint_dist: f32,
318    height_offset: f32,
319) -> impl Action<S> {
320    now(move |ctx, _| {
321        let wpos =
322            wpos2d.with_z(ctx.world.sim().get_surface_alt_approx(wpos2d.as_()) + height_offset);
323        goto_flying(
324            wpos,
325            speed_factor,
326            goal_dist,
327            step_dist,
328            waypoint_dist,
329            height_offset,
330        )
331        .debug(move || {
332            format!(
333                "goto 2d flying ({}, {}), goal dist {}",
334                wpos2d.x, wpos2d.y, goal_dist
335            )
336        })
337    })
338}
339
340fn traverse_points<S: State, F>(next_point: F, speed_factor: f32) -> impl Action<S>
341where
342    F: FnMut(&mut NpcCtx) -> Option<Vec2<f32>> + Clone + Send + Sync + 'static,
343{
344    until(move |ctx, next_point: &mut F| {
345        // Pick next waypoint, return if path ended
346        let Some(wpos) = next_point(ctx) else {
347            return ControlFlow::Break(());
348        };
349
350        let wpos_site = |wpos: Vec2<f32>| {
351            ctx.world
352                .sim()
353                .get_wpos(wpos.as_())
354                .and_then(|chunk| chunk.sites.first().copied())
355        };
356
357        let wpos_sites_contain = |wpos: Vec2<f32>, site: Id<world::site::Site>| {
358            ctx.world
359                .sim()
360                .get(wpos.as_().wpos_to_cpos())
361                .map(|chunk| chunk.sites.contains(&site))
362                .unwrap_or(false)
363        };
364
365        let npc_wpos = ctx.npc.wpos;
366
367        // If we're traversing within a site, do intra-site pathfinding
368        if let Some(site) = wpos_site(npc_wpos.xy()) {
369            let mut site_exit = wpos;
370            while let Some(next) = next_point(ctx).filter(|next| wpos_sites_contain(*next, site)) {
371                site_exit = next;
372            }
373
374            // Navigate through the site to the site exit
375            if let Some(path) = path_site(npc_wpos.xy(), site_exit, site, ctx.index) {
376                let path_len = path.len();
377                ControlFlow::Continue(Either::Left(
378                    seq(path.into_iter().map(move |wpos| goto_2d(wpos, 1.0, 8.0)))
379                        .then(goto_2d(site_exit, speed_factor, 8.0))
380                        .debug(move || {
381                            format!(
382                                "in site from ({}, {}) to ({}, {}), path length: {path_len}",
383                                npc_wpos.x, npc_wpos.y, site_exit.x, site_exit.y,
384                            )
385                        }),
386                ))
387            } else {
388                // No intra-site path found, just attempt to move towards the exit node
389                ControlFlow::Continue(Either::Right(
390                    goto_2d(site_exit, speed_factor, 8.0)
391                        .debug(move || {
392                            format!(
393                                "direct from {}, {}, ({}) to site exit at {}, {}",
394                                npc_wpos.x, npc_wpos.y, npc_wpos.z, site_exit.x, site_exit.y
395                            )
396                        })
397                        .boxed(),
398                ))
399            }
400        } else {
401            // We're in the middle of a road, just go to the next waypoint
402            ControlFlow::Continue(Either::Right(
403                goto_2d(wpos, speed_factor, 8.0)
404                    .debug(move || {
405                        format!(
406                            "from {}, {}, ({}) to the next waypoint at {}, {}",
407                            npc_wpos.x, npc_wpos.y, npc_wpos.z, wpos.x, wpos.y
408                        )
409                    })
410                    .boxed(),
411            ))
412        }
413    })
414    .with_state(next_point)
415    .debug(|| "traverse points")
416}
417
418/// Try to travel to a site. Where practical, paths will be taken.
419pub fn travel_to_point<S: State>(wpos: Vec2<f32>, speed_factor: f32) -> impl Action<S> {
420    now(move |ctx, _| {
421        const WAYPOINT: f32 = 48.0;
422        let start = ctx.npc.wpos.xy();
423        let diff = wpos - start;
424        let n = (diff.magnitude() / WAYPOINT).max(1.0);
425        let mut points = (1..n as usize + 1).map(move |i| start + diff * (i as f32 / n));
426        traverse_points(move |_| points.next(), speed_factor)
427    })
428    .debug(move || format!("travel to point {}, {}", wpos.x, wpos.y))
429}
430
431/// Try to travel to a site. Where practical, paths will be taken.
432pub fn travel_to_site<S: State>(tgt_site: SiteId, speed_factor: f32) -> impl Action<S> {
433    now(move |ctx, _| {
434        let sites = &ctx.state.data().sites;
435
436        let site_wpos = sites.get(tgt_site).map(|site| site.wpos.as_());
437
438        // If we're currently in a site, try to find a path to the target site via
439        // tracks
440        if let Some(current_site) = ctx.npc.current_site
441            && let Some(tracks) = path_between_towns(current_site, tgt_site, sites, ctx.world)
442        {
443
444            let mut path_nodes = tracks.path
445                .into_iter()
446                .flat_map(move |(track_id, reversed)| (0..)
447                    .map(move |node_idx| (node_idx, track_id, reversed)));
448
449            traverse_points(move |ctx| {
450                let (node_idx, track_id, reversed) = path_nodes.next()?;
451                let nodes = &ctx.world.civs().tracks.get(track_id).path().nodes;
452
453                // Handle the case where we walk paths backward
454                let idx = if reversed {
455                    nodes.len().checked_sub(node_idx + 1)
456                } else {
457                    Some(node_idx)
458                };
459
460                if let Some(node) = idx.and_then(|idx| nodes.get(idx)) {
461                    // Find the centre of the track node's chunk
462                    let node_chunk_wpos = TerrainChunkSize::center_wpos(*node);
463
464                    // Refine the node position a bit more based on local path information
465                    Some(ctx.world.sim()
466                        .get_nearest_path(node_chunk_wpos)
467                        .map_or(node_chunk_wpos, |(_, wpos, _, _)| wpos.as_())
468                        .as_::<f32>())
469                } else {
470                    None
471                }
472            }, speed_factor)
473                .boxed()
474        } else if let Some(site) = sites.get(tgt_site) {
475            // If all else fails, just walk toward the target site in a straight line
476            travel_to_point(site.wpos.map(|e| e as f32 + 0.5), speed_factor).debug(|| "travel to point fallback").boxed()
477        } else {
478            // If we can't find a way to get to the site at all, there's nothing more to be done
479            finish().boxed()
480        }
481            // Stop the NPC early if we're near the site to prevent huddling around the centre
482            .stop_if(move |ctx: &mut NpcCtx| site_wpos.is_some_and(|site_wpos| ctx.npc.wpos.xy().distance_squared(site_wpos) < 16f32.powi(2)))
483    })
484        .debug(move || format!("travel_to_site {:?}", tgt_site))
485        .map(|_, _| ())
486}