veloren_common/
path.rs

1use crate::{
2    astar::{Astar, PathResult},
3    terrain::Block,
4    vol::{BaseVol, ReadVol},
5};
6use common_base::span;
7use fxhash::FxBuildHasher;
8#[cfg(feature = "rrt_pathfinding")]
9use hashbrown::HashMap;
10#[cfg(feature = "rrt_pathfinding")]
11use kiddo::{SquaredEuclidean, float::kdtree::KdTree, nearest_neighbour::NearestNeighbour}; /* For RRT paths (disabled for now) */
12use rand::{Rng, thread_rng};
13#[cfg(feature = "rrt_pathfinding")]
14use rand::{
15    distributions::{Distribution, Uniform},
16    prelude::IteratorRandom,
17};
18#[cfg(feature = "rrt_pathfinding")]
19use std::f32::consts::PI;
20use std::iter::FromIterator;
21use vek::*;
22
23// Path
24
25#[derive(Clone, Debug)]
26pub struct Path<T> {
27    pub nodes: Vec<T>,
28}
29
30impl<T> Default for Path<T> {
31    fn default() -> Self {
32        Self {
33            nodes: Vec::default(),
34        }
35    }
36}
37
38impl<T> FromIterator<T> for Path<T> {
39    fn from_iter<I: IntoIterator<Item = T>>(iter: I) -> Self {
40        Self {
41            nodes: iter.into_iter().collect(),
42        }
43    }
44}
45
46impl<T> IntoIterator for Path<T> {
47    type IntoIter = std::vec::IntoIter<T>;
48    type Item = T;
49
50    fn into_iter(self) -> Self::IntoIter { self.nodes.into_iter() }
51}
52
53impl<T> Path<T> {
54    pub fn is_empty(&self) -> bool { self.nodes.is_empty() }
55
56    pub fn len(&self) -> usize { self.nodes.len() }
57
58    pub fn iter(&self) -> impl Iterator<Item = &T> { self.nodes.iter() }
59
60    pub fn start(&self) -> Option<&T> { self.nodes.first() }
61
62    pub fn end(&self) -> Option<&T> { self.nodes.last() }
63
64    pub fn nodes(&self) -> &[T] { &self.nodes }
65}
66
67// Route: A path that can be progressed along
68
69#[derive(Default, Clone, Debug)]
70pub struct Route {
71    path: Path<Vec3<i32>>,
72    next_idx: usize,
73}
74
75impl Route {
76    pub fn get_path(&self) -> &Path<Vec3<i32>> { &self.path }
77
78    pub fn next_idx(&self) -> usize { self.next_idx }
79}
80
81impl From<Path<Vec3<i32>>> for Route {
82    fn from(path: Path<Vec3<i32>>) -> Self { Self { path, next_idx: 0 } }
83}
84
85pub struct TraversalConfig {
86    /// The distance to a node at which node is considered visited.
87    pub node_tolerance: f32,
88    /// The slowdown factor when following corners.
89    /// 0.0 = no slowdown on corners, 1.0 = total slowdown on corners.
90    pub slow_factor: f32,
91    /// Whether the agent is currently on the ground.
92    pub on_ground: bool,
93    /// Whether the agent is currently in water.
94    pub in_liquid: bool,
95    /// The distance to the target below which it is considered reached.
96    pub min_tgt_dist: f32,
97    /// Whether the agent can climb.
98    pub can_climb: bool,
99    /// Whether the agent can fly.
100    pub can_fly: bool,
101    /// Whether the agent has vectored propulsion.
102    pub vectored_propulsion: bool,
103    /// Whether chunk containing target position is currently loaded
104    pub is_target_loaded: bool,
105}
106
107const DIAGONALS: [Vec2<i32>; 8] = [
108    Vec2::new(1, 0),
109    Vec2::new(1, 1),
110    Vec2::new(0, 1),
111    Vec2::new(-1, 1),
112    Vec2::new(-1, 0),
113    Vec2::new(-1, -1),
114    Vec2::new(0, -1),
115    Vec2::new(1, -1),
116];
117
118impl Route {
119    pub fn path(&self) -> &Path<Vec3<i32>> { &self.path }
120
121    pub fn next(&self, i: usize) -> Option<Vec3<i32>> {
122        self.path.nodes.get(self.next_idx + i).copied()
123    }
124
125    pub fn is_finished(&self) -> bool { self.next(0).is_none() }
126
127    pub fn traverse<V>(
128        &mut self,
129        vol: &V,
130        pos: Vec3<f32>,
131        vel: Vec3<f32>,
132        traversal_cfg: &TraversalConfig,
133    ) -> Option<(Vec3<f32>, f32)>
134    where
135        V: BaseVol<Vox = Block> + ReadVol,
136    {
137        let (next0, next1, next_tgt, be_precise) = loop {
138            // If we've reached the end of the path, stop
139            let next0 = self.next(0)?;
140            let next1 = self.next(1).unwrap_or(next0);
141
142            // Stop using obstructed paths
143            if !walkable(vol, next0) || !walkable(vol, next1) {
144                return None;
145            }
146
147            // If, in any direction, there is a column of open air of several blocks
148            let open_space_nearby = DIAGONALS.iter().any(|pos| {
149                (-2..2).all(|z| {
150                    vol.get(next0 + Vec3::new(pos.x, pos.y, z))
151                        .map(|b| !b.is_solid())
152                        .unwrap_or(false)
153                })
154            });
155
156            // If, in any direction, there is a solid wall
157            let wall_nearby = DIAGONALS.iter().any(|pos| {
158                vol.get(next0 + Vec3::new(pos.x, pos.y, 1))
159                    .map(|b| b.is_solid())
160                    .unwrap_or(true)
161            });
162
163            // Unwalkable obstacles, such as walls or open space can affect path-finding
164            let be_precise = open_space_nearby || wall_nearby;
165
166            // Map position of node to middle of block
167            let next_tgt = next0.map(|e| e as f32) + Vec3::new(0.5, 0.5, 0.0);
168            let closest_tgt = next_tgt
169                .map2(pos, |tgt, pos| pos.clamped(tgt.floor(), tgt.ceil()))
170                .xy()
171                .with_z(next_tgt.z);
172            // Determine whether we're close enough to the next to to consider it completed
173            let dist_sqrd = pos.xy().distance_squared(closest_tgt.xy());
174            if dist_sqrd
175                < (traversal_cfg.node_tolerance
176                    * if be_precise {
177                        0.5
178                    } else if traversal_cfg.in_liquid {
179                        2.5
180                    } else {
181                        1.0
182                    })
183                .powi(2)
184                && ((pos.z - 0.75 - closest_tgt.z).abs() < 1.0
185                    || (traversal_cfg.in_liquid
186                        && pos.z < closest_tgt.z + 0.8
187                        && pos.z > closest_tgt.z))
188            {
189                // Node completed, move on to the next one
190                self.next_idx += 1;
191            } else {
192                // The next node hasn't been reached yet, use it as a target
193                break (next0, next1, next_tgt, be_precise);
194            }
195        };
196
197        fn gradient(line: LineSegment2<f32>) -> f32 {
198            let r = (line.start.y - line.end.y) / (line.start.x - line.end.x);
199            if r.is_nan() { 100000.0 } else { r }
200        }
201
202        fn intersect(a: LineSegment2<f32>, b: LineSegment2<f32>) -> Option<Vec2<f32>> {
203            let ma = gradient(a);
204            let mb = gradient(b);
205
206            let ca = a.start.y - ma * a.start.x;
207            let cb = b.start.y - mb * b.start.x;
208
209            if (ma - mb).abs() < 0.0001 || (ca - cb).abs() < 0.0001 {
210                None
211            } else {
212                let x = (cb - ca) / (ma - mb);
213                let y = ma * x + ca;
214
215                Some(Vec2::new(x, y))
216            }
217        }
218
219        // We don't always want to aim for the centre of block since this can create
220        // jerky zig-zag movement. This function attempts to find a position
221        // inside a target block's area that aligned nicely with our velocity.
222        // This has a twofold benefit:
223        //
224        // 1. Entities can move at any angle when
225        // running on a flat surface
226        //
227        // 2. We don't have to search diagonals when
228        // pathfinding - cartesian positions are enough since this code will
229        // make the entity move smoothly along them
230        let corners = [
231            Vec2::new(0, 0),
232            Vec2::new(1, 0),
233            Vec2::new(1, 1),
234            Vec2::new(0, 1),
235            Vec2::new(0, 0), // Repeated start
236        ];
237
238        let vel_line = LineSegment2 {
239            start: pos.xy(),
240            end: pos.xy() + vel.xy() * 100.0,
241        };
242
243        let align = |block_pos: Vec3<i32>, precision: f32| {
244            let lerp_block =
245                |x, precision| Lerp::lerp(x, block_pos.xy().map(|e| e as f32), precision);
246
247            (0..4)
248                .filter_map(|i| {
249                    let edge_line = LineSegment2 {
250                        start: lerp_block(
251                            (block_pos.xy() + corners[i]).map(|e| e as f32),
252                            precision,
253                        ),
254                        end: lerp_block(
255                            (block_pos.xy() + corners[i + 1]).map(|e| e as f32),
256                            precision,
257                        ),
258                    };
259                    intersect(vel_line, edge_line).filter(|intersect| {
260                        intersect
261                            .clamped(
262                                block_pos.xy().map(|e| e as f32),
263                                block_pos.xy().map(|e| e as f32 + 1.0),
264                            )
265                            .distance_squared(*intersect)
266                            < 0.001
267                    })
268                })
269                .min_by_key(|intersect: &Vec2<f32>| {
270                    (intersect.distance_squared(vel_line.end) * 1000.0) as i32
271                })
272                .unwrap_or_else(|| {
273                    (0..2)
274                        .flat_map(|i| (0..2).map(move |j| Vec2::new(i, j)))
275                        .map(|rpos| block_pos + rpos)
276                        .map(|block_pos| {
277                            let block_posf = block_pos.xy().map(|e| e as f32);
278                            let proj = vel_line.projected_point(block_posf);
279                            let clamped = lerp_block(
280                                proj.clamped(
281                                    block_pos.xy().map(|e| e as f32),
282                                    block_pos.xy().map(|e| e as f32),
283                                ),
284                                precision,
285                            );
286
287                            (proj.distance_squared(clamped), clamped)
288                        })
289                        .min_by_key(|(d2, _)| (d2 * 1000.0) as i32)
290                        .unwrap()
291                        .1
292                })
293        };
294
295        let bez = CubicBezier2 {
296            start: pos.xy(),
297            ctrl0: pos.xy() + vel.xy().try_normalized().unwrap_or_default() * 1.0,
298            ctrl1: align(next0, 1.0),
299            end: align(next1, 1.0),
300        };
301
302        // Use a cubic spline of the next few targets to come up with a sensible target
303        // position. We want to use a position that gives smooth movement but is
304        // also accurate enough to avoid the agent getting stuck under ledges or
305        // falling off walls.
306        let next_dir = bez
307            .evaluate_derivative(0.85)
308            .try_normalized()
309            .unwrap_or_default();
310        let straight_factor = next_dir
311            .dot(vel.xy().try_normalized().unwrap_or(next_dir))
312            .max(0.0)
313            .powi(2);
314
315        let bez = CubicBezier2 {
316            start: pos.xy(),
317            ctrl0: pos.xy() + vel.xy().try_normalized().unwrap_or_default() * 1.0,
318            ctrl1: align(
319                next0,
320                (1.0 - if (next0.z as f32 - pos.z).abs() < 0.25 && !be_precise {
321                    straight_factor
322                } else {
323                    0.0
324                })
325                .max(0.1),
326            ),
327            end: align(next1, 1.0),
328        };
329
330        let tgt2d = bez.evaluate(if (next0.z as f32 - pos.z).abs() < 0.25 {
331            0.25
332        } else {
333            0.5
334        });
335        let tgt = if be_precise {
336            next_tgt
337        } else {
338            Vec3::from(tgt2d) + Vec3::unit_z() * next_tgt.z
339        };
340
341        Some((
342            tgt - pos,
343            // Control the entity's speed to hopefully stop us falling off walls on sharp
344            // corners. This code is very imperfect: it does its best but it
345            // can still fail for particularly fast entities.
346            1.0 - (traversal_cfg.slow_factor * (1.0 - straight_factor)).min(0.9),
347        ))
348        .filter(|(bearing, _)| bearing.z < 2.1)
349    }
350}
351
352/// A self-contained system that attempts to chase a moving target, only
353/// performing pathfinding if necessary
354#[derive(Default, Clone, Debug)]
355pub struct Chaser {
356    last_search_tgt: Option<Vec3<f32>>,
357    /// `bool` indicates whether the Route is a complete route to the target
358    route: Option<(Route, bool)>,
359    /// We use this hasher (FxHash) because:
360    /// (1) we don't care about DDOS attacks (We can use FxHash);
361    /// (2) we want this to be constant across compiles because of hot-reloading
362    /// (Ruling out AAHash);
363    astar: Option<Astar<Node, FxBuildHasher>>,
364}
365
366impl Chaser {
367    /// Returns bearing and speed
368    /// Bearing is a `Vec3<f32>` dictating the direction of movement
369    /// Speed is an f32 between 0.0 and 1.0
370    pub fn chase<V>(
371        &mut self,
372        vol: &V,
373        pos: Vec3<f32>,
374        vel: Vec3<f32>,
375        tgt: Vec3<f32>,
376        traversal_cfg: TraversalConfig,
377    ) -> Option<(Vec3<f32>, f32)>
378    where
379        V: BaseVol<Vox = Block> + ReadVol,
380    {
381        span!(_guard, "chase", "Chaser::chase");
382        let pos_to_tgt = pos.distance(tgt);
383
384        // If we're already close to the target then there's nothing to do
385        let end = self
386            .route
387            .as_ref()
388            .and_then(|(r, _)| r.path.end().copied())
389            .map(|e| e.map(|e| e as f32 + 0.5))
390            .unwrap_or(tgt);
391        if ((pos - end) * Vec3::new(1.0, 1.0, 2.0)).magnitude_squared()
392            < traversal_cfg.min_tgt_dist.powi(2)
393        {
394            self.route = None;
395            return None;
396        }
397
398        let bearing = if let Some((end, complete)) = self
399            .route
400            .as_ref()
401            .and_then(|(r, complete)| Some((r.path().end().copied()?, *complete)))
402        {
403            let end_to_tgt = end.map(|e| e as f32).distance(tgt);
404            // If the target has moved significantly since the path was generated then it's
405            // time to search for a new path. Also, do this randomly from time
406            // to time to avoid any edge cases that cause us to get stuck. In
407            // theory this shouldn't happen, but in practice the world is full
408            // of unpredictable obstacles that are more than willing to mess up
409            // our day. TODO: Come up with a better heuristic for this
410            if end_to_tgt > pos_to_tgt * 0.3 + 5.0 && complete && traversal_cfg.is_target_loaded {
411                self.astar = None;
412                None
413            } else if vel.magnitude_squared() < 0.2f32.powi(2)
414                && thread_rng().gen::<f32>() < 0.0025
415                && complete
416            {
417                self.route = None;
418                None
419            } else {
420                self.route
421                    .as_mut()
422                    .and_then(|(r, _)| r.traverse(vol, pos, vel, &traversal_cfg))
423            }
424        } else {
425            // There is no route found yet
426            None
427        };
428
429        // If a bearing has already been determined, use that
430        if let Some((bearing, speed)) = bearing {
431            Some((bearing, speed))
432        } else {
433            // Since no bearing has been determined yet, a new route will be
434            // calculated if the target has moved, pathfinding is not complete,
435            // or there is no route
436            let tgt_dir = (tgt - pos).xy().try_normalized().unwrap_or_default();
437
438            // Only search for a path if the target has moved from their last position. We
439            // don't want to be thrashing the pathfinding code for targets that
440            // we're unable to access!
441            if self
442                .last_search_tgt
443                .map(|last_tgt| last_tgt.distance(tgt) > pos_to_tgt * 0.15 + 5.0)
444                .unwrap_or(true)
445                || self.astar.is_some()
446                || self.route.is_none()
447                || !traversal_cfg.is_target_loaded
448            {
449                self.last_search_tgt = Some(tgt);
450
451                // NOTE: Enable air paths when air braking has been figured out
452                let (path, complete) = /*if cfg!(feature = "rrt_pathfinding") && traversal_cfg.can_fly {
453                    find_air_path(vol, pos, tgt, &traversal_cfg)
454                } else */{
455                    find_path(&mut self.astar, vol, pos, tgt, &traversal_cfg)
456                };
457
458                self.route = path.map(|path| {
459                    let start_index = path
460                        .iter()
461                        .enumerate()
462                        .min_by_key(|(_, node)| {
463                            node.map(|e| e as f32).distance_squared(pos + tgt_dir) as i32
464                        })
465                        .map(|(idx, _)| idx);
466
467                    (
468                        Route {
469                            path,
470                            next_idx: start_index.unwrap_or(0),
471                        },
472                        complete,
473                    )
474                });
475            }
476            // Start traversing the new route if it exists
477            if let Some(bearing) = self
478                .route
479                .as_mut()
480                .and_then(|(r, _)| r.traverse(vol, pos, vel, &traversal_cfg))
481            {
482                Some(bearing)
483            } else {
484                // At this point no route is available and no bearing
485                // has been determined, so we start sampling terrain.
486                // Check for falling off walls and try moving straight
487                // towards the target if falling is not a danger
488                let walking_towards_edge = (-8..2).all(|z| {
489                    vol.get(
490                        (pos + Vec3::<f32>::from(tgt_dir) * 2.5).map(|e| e as i32)
491                            + Vec3::unit_z() * z,
492                    )
493                    .map(|b| b.is_air())
494                    .unwrap_or(false)
495                });
496
497                // Enable when airbraking/flight is figured out
498                /*if traversal_cfg.can_fly {
499                    Some(((tgt - pos) , 1.0))
500                } else */
501                if traversal_cfg.can_fly {
502                    Some(((tgt - pos) * Vec3::new(1.0, 1.0, 0.5), 1.0))
503                } else if !walking_towards_edge {
504                    Some(((tgt - pos) * Vec3::new(1.0, 1.0, 0.0), 1.0))
505                } else {
506                    // This is unfortunately where an NPC will stare blankly
507                    // into space. No route has been found and no temporary
508                    // bearing would suffice. Hopefully a route will be found
509                    // in the coming ticks.
510                    None
511                }
512            }
513        }
514    }
515
516    pub fn get_route(&self) -> Option<&Route> { self.route.as_ref().map(|(r, _)| r) }
517
518    pub fn last_target(&self) -> Option<Vec3<f32>> { self.last_search_tgt }
519}
520
521fn walkable<V>(vol: &V, pos: Vec3<i32>) -> bool
522where
523    V: BaseVol<Vox = Block> + ReadVol,
524{
525    let below = vol
526        .get(pos - Vec3::unit_z())
527        .ok()
528        .copied()
529        .unwrap_or_else(Block::empty);
530    let a = vol.get(pos).ok().copied().unwrap_or_else(Block::empty);
531    let b = vol
532        .get(pos + Vec3::unit_z())
533        .ok()
534        .copied()
535        .unwrap_or_else(Block::empty);
536
537    let on_ground = below.is_filled();
538    let in_liquid = a.is_liquid();
539    (on_ground || in_liquid) && !a.is_solid() && !b.is_solid()
540}
541
542#[derive(Copy, Clone, PartialEq, Eq, Hash, Debug)]
543pub struct Node {
544    pos: Vec3<i32>,
545    last_dir: Vec2<i32>,
546    last_dir_count: u32,
547}
548
549/// Attempt to search for a path to a target, returning the path (if one was
550/// found) and whether it is complete (reaches the target)
551fn find_path<V>(
552    astar: &mut Option<Astar<Node, FxBuildHasher>>,
553    vol: &V,
554    startf: Vec3<f32>,
555    endf: Vec3<f32>,
556    traversal_cfg: &TraversalConfig,
557) -> (Option<Path<Vec3<i32>>>, bool)
558where
559    V: BaseVol<Vox = Block> + ReadVol,
560{
561    let is_walkable = |pos: &Vec3<i32>| walkable(vol, *pos);
562    let get_walkable_z = |pos| {
563        let mut z_incr = 0;
564        for _ in 0..32 {
565            let test_pos = pos + Vec3::unit_z() * z_incr;
566            if is_walkable(&test_pos) {
567                return Some(test_pos);
568            }
569            z_incr = -z_incr + i32::from(z_incr <= 0);
570        }
571        None
572    };
573
574    let (start, end) = match (
575        get_walkable_z(startf.map(|e| e.floor() as i32)),
576        get_walkable_z(endf.map(|e| e.floor() as i32)),
577    ) {
578        (Some(start), Some(end)) => (start, end),
579
580        // Special case for partially loaded path finding
581        (Some(start), None) if !traversal_cfg.is_target_loaded => {
582            (start, endf.map(|e| e.floor() as i32))
583        },
584
585        _ => return (None, false),
586    };
587
588    let heuristic = |node: &Node| node.pos.as_().distance(end.as_());
589    let transition = |a: Node, b: Node| {
590        1.0
591            // Discourage travelling in the same direction for too long: this encourages
592            // turns to be spread out along a path, more closely approximating a straight
593            // line toward the target.
594            + b.last_dir_count as f32 * 0.01
595            // Penalise jumping
596            + (b.pos.z - a.pos.z).max(0) as f32 * 2.0
597    };
598    let neighbors = |node: &Node| {
599        let node = *node;
600        let pos = node.pos;
601        const DIRS: [Vec3<i32>; 17] = [
602            Vec3::new(0, 1, 0),   // Forward
603            Vec3::new(0, 1, 1),   // Forward upward
604            Vec3::new(0, 1, -1),  // Forward downward
605            Vec3::new(0, 1, -2),  // Forward downwardx2
606            Vec3::new(1, 0, 0),   // Right
607            Vec3::new(1, 0, 1),   // Right upward
608            Vec3::new(1, 0, -1),  // Right downward
609            Vec3::new(1, 0, -2),  // Right downwardx2
610            Vec3::new(0, -1, 0),  // Backwards
611            Vec3::new(0, -1, 1),  // Backward Upward
612            Vec3::new(0, -1, -1), // Backward downward
613            Vec3::new(0, -1, -2), // Backward downwardx2
614            Vec3::new(-1, 0, 0),  // Left
615            Vec3::new(-1, 0, 1),  // Left upward
616            Vec3::new(-1, 0, -1), // Left downward
617            Vec3::new(-1, 0, -2), // Left downwardx2
618            Vec3::new(0, 0, -1),  // Downwards
619        ];
620
621        const JUMPS: [Vec3<i32>; 4] = [
622            Vec3::new(0, 1, 2),  // Forward Upwardx2
623            Vec3::new(1, 0, 2),  // Right Upwardx2
624            Vec3::new(0, -1, 2), // Backward Upwardx2
625            Vec3::new(-1, 0, 2), // Left Upwardx2
626        ];
627
628        // let walkable = [
629        //     is_walkable(&(pos + Vec3::new(1, 0, 0))),
630        //     is_walkable(&(pos + Vec3::new(-1, 0, 0))),
631        //     is_walkable(&(pos + Vec3::new(0, 1, 0))),
632        //     is_walkable(&(pos + Vec3::new(0, -1, 0))),
633        // ];
634
635        // const DIAGONALS: [(Vec3<i32>, [usize; 2]); 8] = [
636        //     (Vec3::new(1, 1, 0), [0, 2]),
637        //     (Vec3::new(-1, 1, 0), [1, 2]),
638        //     (Vec3::new(1, -1, 0), [0, 3]),
639        //     (Vec3::new(-1, -1, 0), [1, 3]),
640        //     (Vec3::new(1, 1, 1), [0, 2]),
641        //     (Vec3::new(-1, 1, 1), [1, 2]),
642        //     (Vec3::new(1, -1, 1), [0, 3]),
643        //     (Vec3::new(-1, -1, 1), [1, 3]),
644        // ];
645
646        DIRS.iter()
647            .chain(
648                Some(JUMPS.iter())
649                    .filter(|_| {
650                        vol.get(pos - Vec3::unit_z())
651                            .map(|b| !b.is_liquid())
652                            .unwrap_or(traversal_cfg.is_target_loaded)
653                            || traversal_cfg.can_climb
654                            || traversal_cfg.can_fly
655                    })
656                    .into_iter()
657                    .flatten(),
658            )
659            .map(move |dir| (pos, dir))
660            .filter(move |(pos, dir)| {
661                (traversal_cfg.can_fly || is_walkable(pos) && is_walkable(&(*pos + **dir)))
662                    && ((dir.z < 1
663                        || vol
664                            .get(pos + Vec3::unit_z() * 2)
665                            .map(|b| !b.is_solid())
666                            .unwrap_or(traversal_cfg.is_target_loaded))
667                        && (dir.z < 2
668                            || vol
669                                .get(pos + Vec3::unit_z() * 3)
670                                .map(|b| !b.is_solid())
671                                .unwrap_or(traversal_cfg.is_target_loaded))
672                        && (dir.z >= 0
673                            || vol
674                                .get(pos + *dir + Vec3::unit_z() * 2)
675                                .map(|b| !b.is_solid())
676                                .unwrap_or(traversal_cfg.is_target_loaded)))
677            })
678            .map(move |(pos, dir)| {
679                let next_node = Node {
680                    pos: pos + dir,
681                    last_dir: dir.xy(),
682                    last_dir_count: if node.last_dir == dir.xy() {
683                        node.last_dir_count + 1
684                    } else {
685                        0
686                    },
687                };
688
689                (next_node, transition(node, next_node))
690            })
691        // .chain(
692        //     DIAGONALS
693        //         .iter()
694        //         .filter(move |(dir, [a, b])| {
695        //             is_walkable(&(pos + *dir)) && walkable[*a] &&
696        // walkable[*b]         })
697        //         .map(move |(dir, _)| pos + *dir),
698        // )
699    };
700
701    let satisfied = |node: &Node| node.pos == end;
702
703    let mut new_astar = match astar.take() {
704        None => Astar::new(
705            if traversal_cfg.is_target_loaded {
706                // Normal mode
707                50_000
708            } else {
709                // Most of the times we would need to plot within current chunk,
710                // so half of intra-site limit should be enough in most cases
711                500
712            },
713            Node {
714                pos: start,
715                last_dir: Vec2::zero(),
716                last_dir_count: 0,
717            },
718            FxBuildHasher::default(),
719        ),
720        Some(astar) => astar,
721    };
722
723    let path_result = new_astar.poll(250, heuristic, neighbors, satisfied);
724
725    let (path, finished) = match path_result {
726        PathResult::Path(path, _cost) => (Some(path), true),
727        PathResult::None(path) => (Some(path), false),
728        PathResult::Exhausted(path) => (Some(path), false),
729
730        PathResult::Pending => {
731            // Keep astar for the next iteration
732            *astar = Some(new_astar);
733
734            (None, false)
735        },
736    };
737    (
738        path.map(|path| path.nodes.into_iter().map(|n| n.pos).collect()),
739        finished,
740    )
741}
742
743// Enable when airbraking/sensible flight is a thing
744#[cfg(feature = "rrt_pathfinding")]
745fn find_air_path<V>(
746    vol: &V,
747    startf: Vec3<f32>,
748    endf: Vec3<f32>,
749    traversal_cfg: &TraversalConfig,
750) -> (Option<Path<Vec3<i32>>>, bool)
751where
752    V: BaseVol<Vox = Block> + ReadVol,
753{
754    let radius = traversal_cfg.node_tolerance;
755    let total_dist_sqrd = startf.distance_squared(endf);
756    // First check if a straight line path works
757    if vol
758        .ray(startf + Vec3::unit_z(), endf + Vec3::unit_z())
759        .until(Block::is_opaque)
760        .cast()
761        .0
762        .powi(2)
763        >= total_dist_sqrd
764    {
765        let mut path = Vec::new();
766        path.push(endf.map(|e| e.floor() as i32));
767        let connect = true;
768        (Some(path.into_iter().collect()), connect)
769    // Else use RRTs
770    } else {
771        let is_traversable = |start: &Vec3<f32>, end: &Vec3<f32>| {
772            vol.ray(*start, *end)
773                .until(Block::is_solid)
774                .cast()
775                .0
776                .powi(2)
777                > (*start).distance_squared(*end)
778            //vol.get(*pos).ok().copied().unwrap_or_else(Block::empty).
779            // is_fluid();
780        };
781        informed_rrt_connect(vol, startf, endf, is_traversable, radius)
782    }
783}
784
785/// Attempts to find a path from a start to the end using an informed
786/// RRT-Connect algorithm. A point is sampled from a bounding spheroid
787/// between the start and end. Two separate rapidly exploring random
788/// trees extend toward the sampled point. Nodes are stored in k-d trees
789/// for quicker nearest node calculations. Points are sampled until the
790/// trees connect. A final path is then reconstructed from the nodes.
791/// This pathfinding algorithm is more appropriate for 3D pathfinding
792/// with wider gaps, such as flying through a forest than for terrain
793/// with narrow gaps, such as navigating a maze.
794/// Returns a path and whether that path is complete or not.
795#[cfg(feature = "rrt_pathfinding")]
796fn informed_rrt_connect<V>(
797    vol: &V,
798    startf: Vec3<f32>,
799    endf: Vec3<f32>,
800    is_valid_edge: impl Fn(&Vec3<f32>, &Vec3<f32>) -> bool,
801    radius: f32,
802) -> (Option<Path<Vec3<i32>>>, bool)
803where
804    V: BaseVol<Vox = Block> + ReadVol,
805{
806    const MAX_POINTS: usize = 7000;
807    let mut path = Vec::new();
808
809    // Each tree has a vector of nodes
810    let mut node_index1: usize = 0;
811    let mut node_index2: usize = 0;
812    let mut nodes1 = Vec::new();
813    let mut nodes2 = Vec::new();
814
815    // The parents hashmap stores nodes and their parent nodes as pairs to
816    // retrace the complete path once the two RRTs connect
817    let mut parents1 = HashMap::new();
818    let mut parents2 = HashMap::new();
819
820    // The path vector stores the path from the appropriate terminal to the
821    // connecting node or vice versa
822    let mut path1 = Vec::new();
823    let mut path2 = Vec::new();
824
825    // K-d trees are used to find the closest nodes rapidly
826    let mut kdtree1: KdTree<f32, usize, 3, 32, u32> = KdTree::with_capacity(MAX_POINTS);
827    let mut kdtree2: KdTree<f32, usize, 3, 32, u32> = KdTree::with_capacity(MAX_POINTS);
828
829    // Add the start as the first node of the first k-d tree
830    kdtree1.add(&[startf.x, startf.y, startf.z], node_index1);
831    nodes1.push(startf);
832    node_index1 += 1;
833
834    // Add the end as the first node of the second k-d tree
835    kdtree2.add(&[endf.x, endf.y, endf.z], node_index2);
836    nodes2.push(endf);
837    node_index2 += 1;
838
839    let mut connection1_idx = 0;
840    let mut connection2_idx = 0;
841
842    let mut connect = false;
843
844    // Scalar non-dimensional value that is proportional to the size of the
845    // sample spheroid volume. This increases in value until a path is found.
846    let mut search_parameter = 0.01;
847
848    // Maximum of MAX_POINTS iterations
849    for _i in 0..MAX_POINTS {
850        if connect {
851            break;
852        }
853
854        // Sample a point on the bounding spheroid
855        let (sampled_point1, sampled_point2) = {
856            let point = point_on_prolate_spheroid(startf, endf, search_parameter);
857            (point, point)
858        };
859
860        // Find the nearest nodes to the the sampled point
861        let nearest_index1 = kdtree1
862            .nearest_one::<SquaredEuclidean>(&[
863                sampled_point1.x,
864                sampled_point1.y,
865                sampled_point1.z,
866            ])
867            .item;
868        let nearest_index2 = kdtree2
869            .nearest_one::<SquaredEuclidean>(&[
870                sampled_point2.x,
871                sampled_point2.y,
872                sampled_point2.z,
873            ])
874            .item;
875        let nearest1 = nodes1[nearest_index1];
876        let nearest2 = nodes2[nearest_index2];
877
878        // Extend toward the sampled point from the nearest node of each tree
879        let new_point1 = nearest1 + (sampled_point1 - nearest1).normalized().map(|a| a * radius);
880        let new_point2 = nearest2 + (sampled_point2 - nearest2).normalized().map(|a| a * radius);
881
882        // Ensure the new nodes are valid/traversable
883        if is_valid_edge(&nearest1, &new_point1) {
884            kdtree1.add(&[new_point1.x, new_point1.y, new_point1.z], node_index1);
885            nodes1.push(new_point1);
886            parents1.insert(node_index1, nearest_index1);
887            node_index1 += 1;
888            // Check if the trees connect
889            let NearestNeighbour {
890                distance: check,
891                item: index,
892            } = kdtree2.nearest_one::<SquaredEuclidean>(&[
893                new_point1.x,
894                new_point1.y,
895                new_point1.z,
896            ]);
897            if check < radius {
898                let connection = nodes2[index];
899                connection2_idx = index;
900                nodes1.push(connection);
901                connection1_idx = nodes1.len() - 1;
902                parents1.insert(node_index1, node_index1 - 1);
903                connect = true;
904            }
905        }
906
907        // Repeat the validity check for the second tree
908        if is_valid_edge(&nearest2, &new_point2) {
909            kdtree2.add(&[new_point2.x, new_point2.y, new_point1.z], node_index2);
910            nodes2.push(new_point2);
911            parents2.insert(node_index2, nearest_index2);
912            node_index2 += 1;
913            // Again check for a connection
914            let NearestNeighbour {
915                distance: check,
916                item: index,
917            } = kdtree1.nearest_one::<SquaredEuclidean>(&[
918                new_point2.x,
919                new_point2.y,
920                new_point1.z,
921            ]);
922            if check < radius {
923                let connection = nodes1[index];
924                connection1_idx = index;
925                nodes2.push(connection);
926                connection2_idx = nodes2.len() - 1;
927                parents2.insert(node_index2, node_index2 - 1);
928                connect = true;
929            }
930        }
931        // Increase the search parameter to widen the sample volume
932        search_parameter += 0.02;
933    }
934
935    if connect {
936        // Construct paths from the connection node to the start and end
937        let mut current_node_index1 = connection1_idx;
938        while current_node_index1 > 0 {
939            current_node_index1 = *parents1.get(&current_node_index1).unwrap_or(&0);
940            path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32));
941        }
942        let mut current_node_index2 = connection2_idx;
943        while current_node_index2 > 0 {
944            current_node_index2 = *parents2.get(&current_node_index2).unwrap_or(&0);
945            path2.push(nodes2[current_node_index2].map(|e| e.floor() as i32));
946        }
947        // Join the two paths together in the proper order and remove duplicates
948        path1.pop();
949        path1.reverse();
950        path.append(&mut path1);
951        path.append(&mut path2);
952        path.dedup();
953    } else {
954        // If the trees did not connect, construct a path from the start to
955        // the closest node to the end
956        let mut current_node_index1 = kdtree1
957            .nearest_one::<SquaredEuclidean>(&[endf.x, endf.y, endf.z])
958            .item;
959        // Attempt to pick a node other than the start node
960        for _i in 0..3 {
961            if current_node_index1 == 0
962                || nodes1[current_node_index1].distance_squared(startf) < 4.0
963            {
964                if let Some(index) = parents1.values().into_iter().choose(&mut thread_rng()) {
965                    current_node_index1 = *index;
966                } else {
967                    break;
968                }
969            } else {
970                break;
971            }
972        }
973        path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32));
974        // Construct the path
975        while current_node_index1 != 0 && nodes1[current_node_index1].distance_squared(startf) > 4.0
976        {
977            current_node_index1 = *parents1.get(&current_node_index1).unwrap_or(&0);
978            path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32));
979        }
980
981        path1.reverse();
982        path.append(&mut path1);
983    }
984    let mut new_path = Vec::new();
985    let mut node = path[0];
986    new_path.push(node);
987    let mut node_idx = 0;
988    let num_nodes = path.len();
989    let end = path[num_nodes - 1];
990    while node != end {
991        let next_idx = if node_idx + 4 > num_nodes - 1 {
992            num_nodes - 1
993        } else {
994            node_idx + 4
995        };
996        let next_node = path[next_idx];
997        let start_pos = node.map(|e| e as f32 + 0.5);
998        let end_pos = next_node.map(|e| e as f32 + 0.5);
999        if vol
1000            .ray(start_pos, end_pos)
1001            .until(Block::is_solid)
1002            .cast()
1003            .0
1004            .powi(2)
1005            > (start_pos).distance_squared(end_pos)
1006        {
1007            node_idx = next_idx;
1008            new_path.push(next_node);
1009        } else {
1010            node_idx += 1;
1011        }
1012        node = path[node_idx];
1013    }
1014    path = new_path;
1015    (Some(path.into_iter().collect()), connect)
1016}
1017
1018/// Returns a random point within a radially symmetrical ellipsoid with given
1019/// foci and a `search parameter` to determine the size of the ellipse beyond
1020/// the foci. Technically the point is within a prolate spheroid translated and
1021/// rotated to the proper place in cartesian space.
1022/// The search_parameter is a float that relates to the length of the string for
1023/// a two dimensional ellipse or the size of the ellipse beyond the foci. In
1024/// this case that analogy still holds as the ellipse is radially symmetrical
1025/// along the axis between the foci. The value of the search parameter must be
1026/// greater than zero. In order to increase the sample area, the
1027/// search_parameter should be increased linearly as the search continues.
1028#[cfg(feature = "rrt_pathfinding")]
1029pub fn point_on_prolate_spheroid(
1030    focus1: Vec3<f32>,
1031    focus2: Vec3<f32>,
1032    search_parameter: f32,
1033) -> Vec3<f32> {
1034    let mut rng = thread_rng();
1035    // Uniform distribution
1036    let range = Uniform::from(0.0..1.0);
1037
1038    // Midpoint is used as the local origin
1039    let midpoint = 0.5 * (focus1 + focus2);
1040    // Radius between the start and end of the path
1041    let radius: f32 = focus1.distance(focus2);
1042    // The linear eccentricity of an ellipse is the distance from the origin to a
1043    // focus A prolate spheroid is a half-ellipse rotated for a full revolution
1044    // which is why ellipse variables are used frequently in this function
1045    let linear_eccentricity: f32 = 0.5 * radius;
1046
1047    // For an ellipsoid, three variables determine the shape: a, b, and c.
1048    // These are the distance from the center/origin to the surface on the
1049    // x, y, and z axes, respectively.
1050    // For a prolate spheroid a and b are equal.
1051    // c is determined by adding the search parameter to the linear eccentricity.
1052    // As the search parameter increases the size of the spheroid increases
1053    let c: f32 = linear_eccentricity + search_parameter;
1054    // The width is calculated to prioritize increasing width over length of
1055    // the ellipsoid
1056    let a: f32 = (c.powi(2) - linear_eccentricity.powi(2)).powf(0.5);
1057    // The width should be the same in both the x and y directions
1058    let b: f32 = a;
1059
1060    // The parametric spherical equation for an ellipsoid measuring from the
1061    // center point is as follows:
1062    // x = a * cos(theta) * cos(lambda)
1063    // y = b * cos(theta) * sin(lambda)
1064    // z = c * sin(theta)
1065    //
1066    // where     -0.5 * PI <= theta <= 0.5 * PI
1067    // and       0.0 <= lambda < 2.0 * PI
1068    //
1069    // Select these two angles using the uniform distribution defined at the
1070    // beginning of the function from 0.0 to 1.0
1071    let rtheta: f32 = PI * range.sample(&mut rng) - 0.5 * PI;
1072    let lambda: f32 = 2.0 * PI * range.sample(&mut rng);
1073    // Select a point on the surface of the ellipsoid
1074    let point = Vec3::new(
1075        a * rtheta.cos() * lambda.cos(),
1076        b * rtheta.cos() * lambda.sin(),
1077        c * rtheta.sin(),
1078    );
1079    // NOTE: Theoretically we should sample a point within the spheroid
1080    // requiring selecting a point along the radius. In my tests selecting
1081    // a point *on the surface* of the spheroid results in sampling that is
1082    // "good enough". The following code is commented out to reduce expense.
1083    //let surface_point = Vec3::new(a * rtheta.cos() * lambda.cos(), b *
1084    // rtheta.cos() * lambda.sin(), c * rtheta.sin()); let magnitude =
1085    // surface_point.magnitude(); let direction = surface_point.normalized();
1086    //// Randomly select a point along the vector to the previously selected surface
1087    //// point using the uniform distribution
1088    //let point = magnitude * range.sample(&mut rng) * direction;
1089
1090    // Now that a point has been selected in local space, it must be rotated and
1091    // translated into global coordinates
1092    // NOTE: Don't rotate about the z axis as the point is already randomly
1093    // selected about the z axis
1094    //let dx = focus2.x - focus1.x;
1095    //let dy = focus2.y - focus1.y;
1096    let dz = focus2.z - focus1.z;
1097    // Phi and theta are the angles from the x axis in the x-y plane and from
1098    // the z axis, respectively. (As found in spherical coordinates)
1099    // These angles are used to rotate the random point in the spheroid about
1100    // the local origin
1101    //
1102    // Rotate about z axis by phi
1103    //let phi: f32 = if dx.abs() > 0.0 {
1104    //    (dy / dx).atan()
1105    //} else {
1106    //    0.5 * PI
1107    //};
1108    // This is unnecessary as rtheta is randomly selected between 0.0 and 2.0 * PI
1109    // let rot_z_mat = Mat3::new(phi.cos(), -1.0 * phi.sin(), 0.0, phi.sin(),
1110    // phi.cos(), 0.0, 0.0, 0.0, 1.0);
1111
1112    // Rotate about perpendicular vector in the xy plane by theta
1113    let theta: f32 = if radius > 0.0 {
1114        (dz / radius).acos()
1115    } else {
1116        0.0
1117    };
1118    // Vector from focus1 to focus2
1119    let r_vec = focus2 - focus1;
1120    // Perpendicular vector in xy plane
1121    let perp_vec = Vec3::new(-1.0 * r_vec.y, r_vec.x, 0.0).normalized();
1122    let l = perp_vec.x;
1123    let m = perp_vec.y;
1124    let n = perp_vec.z;
1125    // Rotation matrix for rotation about a vector
1126    let rot_2_mat = Mat3::new(
1127        l * l * (1.0 - theta.cos()),
1128        m * l * (1.0 - theta.cos()) - n * theta.sin(),
1129        n * l * (1.0 - theta.cos()) + m * theta.sin(),
1130        l * m * (1.0 - theta.cos()) + n * theta.sin(),
1131        m * m * (1.0 - theta.cos()) + theta.cos(),
1132        n * m * (1.0 - theta.cos()) - l * theta.sin(),
1133        l * n * (1.0 - theta.cos()) - m * theta.sin(),
1134        m * n * (1.0 - theta.cos()) + l * theta.sin(),
1135        n * n * (1.0 - theta.cos()) + theta.cos(),
1136    );
1137
1138    // Get the global coordinates of the point by rotating and adding the origin
1139    // rot_z_mat is unneeded due to the random rotation defined by lambda
1140    // let global_coords = midpoint + rot_2_mat * (rot_z_mat * point);
1141    midpoint + rot_2_mat * point
1142}