Skip to main content

veloren_common/
path.rs

1use crate::{
2    astar::{Astar, PathResult},
3    comp::Body,
4    resources::Time,
5    terrain::Block,
6    vol::{BaseVol, ReadVol},
7};
8use common_base::span;
9use fxhash::FxBuildHasher;
10#[cfg(feature = "rrt_pathfinding")]
11use hashbrown::HashMap;
12#[cfg(feature = "rrt_pathfinding")]
13use kiddo::{SquaredEuclidean, float::kdtree::KdTree, nearest_neighbour::NearestNeighbour}; /* For RRT paths (disabled for now) */
14use rand::{RngExt, rng};
15#[cfg(feature = "rrt_pathfinding")]
16use rand::{
17    distr::{Distribution, Uniform},
18    prelude::IteratorRandom,
19};
20#[cfg(feature = "rrt_pathfinding")]
21use std::f32::consts::PI;
22use std::{collections::VecDeque, iter::FromIterator};
23use vek::*;
24
25// Path
26
27#[derive(Clone, Debug)]
28pub struct Path<T> {
29    pub nodes: Vec<T>,
30}
31
32impl<T> Default for Path<T> {
33    fn default() -> Self {
34        Self {
35            nodes: Vec::default(),
36        }
37    }
38}
39
40impl<T> FromIterator<T> for Path<T> {
41    fn from_iter<I: IntoIterator<Item = T>>(iter: I) -> Self {
42        Self {
43            nodes: iter.into_iter().collect(),
44        }
45    }
46}
47
48impl<T> IntoIterator for Path<T> {
49    type IntoIter = std::vec::IntoIter<T>;
50    type Item = T;
51
52    fn into_iter(self) -> Self::IntoIter { self.nodes.into_iter() }
53}
54
55impl<T> Path<T> {
56    pub fn is_empty(&self) -> bool { self.nodes.is_empty() }
57
58    pub fn len(&self) -> usize { self.nodes.len() }
59
60    pub fn iter(&self) -> impl Iterator<Item = &T> { self.nodes.iter() }
61
62    pub fn start(&self) -> Option<&T> { self.nodes.first() }
63
64    pub fn end(&self) -> Option<&T> { self.nodes.last() }
65
66    pub fn nodes(&self) -> &[T] { &self.nodes }
67}
68
69// Route: A path that can be progressed along
70
71#[derive(Default, Clone, Debug)]
72pub struct Route {
73    path: Path<Vec3<i32>>,
74    next_idx: usize,
75}
76
77impl Route {
78    pub fn get_path(&self) -> &Path<Vec3<i32>> { &self.path }
79
80    pub fn next_idx(&self) -> usize { self.next_idx }
81}
82
83impl From<Path<Vec3<i32>>> for Route {
84    fn from(path: Path<Vec3<i32>>) -> Self { Self { path, next_idx: 0 } }
85}
86
87pub struct TraversalConfig {
88    /// The distance to a node at which node is considered visited.
89    pub node_tolerance: f32,
90    /// The slowdown factor when following corners.
91    /// 0.0 = no slowdown on corners, 1.0 = total slowdown on corners.
92    pub slow_factor: f32,
93    /// Whether the agent is currently on the ground.
94    pub on_ground: bool,
95    /// Whether the agent is currently in water.
96    pub in_liquid: bool,
97    /// The distance to the target below which it is considered reached.
98    pub min_tgt_dist: f32,
99    /// The body that's moving.
100    pub moving_body: Option<Body>,
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
107impl TraversalConfig {
108    pub fn fly_thrust(&self) -> Option<f32> { self.moving_body.as_ref().and_then(Body::fly_thrust) }
109
110    pub fn can_fly(&self) -> bool { self.fly_thrust().is_some() }
111
112    pub fn can_climb(&self) -> bool { self.moving_body.as_ref().is_some_and(Body::can_climb) }
113
114    pub fn ground_accel(&self) -> Option<f32> {
115        self.moving_body.as_ref().and_then(Body::ground_accel)
116    }
117
118    pub fn swim_thrust(&self) -> Option<f32> {
119        self.moving_body.as_ref().and_then(Body::swim_thrust)
120    }
121}
122
123const DIAGONALS: [Vec2<i32>; 8] = [
124    Vec2::new(1, 0),
125    Vec2::new(1, 1),
126    Vec2::new(0, 1),
127    Vec2::new(-1, 1),
128    Vec2::new(-1, 0),
129    Vec2::new(-1, -1),
130    Vec2::new(0, -1),
131    Vec2::new(1, -1),
132];
133
134pub enum TraverseStop {
135    Done,
136    InvalidOutput,
137    InvalidPath,
138}
139
140impl Route {
141    pub fn path(&self) -> &Path<Vec3<i32>> { &self.path }
142
143    pub fn next(&self, i: usize) -> Option<Vec3<i32>> {
144        self.path.nodes.get(self.next_idx + i).copied()
145    }
146
147    pub fn is_finished(&self) -> bool { self.next(0).is_none() }
148
149    /// Handles moving along a path.
150    pub fn traverse<V>(
151        &mut self,
152        vol: &V,
153        pos: Vec3<f32>,
154        vel: Vec3<f32>,
155        traversal_cfg: &TraversalConfig,
156    ) -> Result<(Vec3<f32>, f32), TraverseStop>
157    where
158        V: BaseVol<Vox = Block> + ReadVol,
159    {
160        let (next0, next1, next_tgt, be_precise) = loop {
161            // If we've reached the end of the path, stop
162            let next0 = self.next(0).ok_or(TraverseStop::Done)?;
163            let next1 = self.next(1).unwrap_or(next0);
164
165            // Stop using obstructed paths
166            if !walkable(vol, next0, traversal_cfg) || !walkable(vol, next1, traversal_cfg) {
167                return Err(TraverseStop::InvalidPath);
168            }
169
170            // If, in any direction, there is a column of open air of several blocks
171            let open_space_nearby = DIAGONALS.iter().any(|pos| {
172                (-2..2).all(|z| {
173                    vol.get(next0 + Vec3::new(pos.x, pos.y, z))
174                        .map(|b| !b.is_solid())
175                        .unwrap_or(false)
176                })
177            });
178
179            // If, in any direction, there is a solid wall
180            let wall_nearby = DIAGONALS.iter().any(|pos| {
181                vol.get(next0 + Vec3::new(pos.x, pos.y, 1))
182                    .map(|b| b.is_solid())
183                    .unwrap_or(true)
184            });
185
186            // Unwalkable obstacles, such as walls or open space or stepping up blocks can
187            // affect path-finding
188            let be_precise =
189                open_space_nearby || wall_nearby || (pos.z - next0.z as f32).abs() > 1.0;
190
191            // If we're not being precise and the next next target is closer, go towards
192            // that instead.
193            if !be_precise
194                && next0.as_::<f32>().distance_squared(pos)
195                    > next1.as_::<f32>().distance_squared(pos)
196            {
197                self.next_idx += 1;
198                continue;
199            }
200
201            // Map position of node to middle of block
202            let next_tgt = next0.map(|e| e as f32) + Vec3::new(0.5, 0.5, 0.0);
203            let closest_tgt = next_tgt
204                .map2(pos, |tgt, pos| pos.clamped(tgt.floor(), tgt.ceil()))
205                .xy()
206                .with_z(next_tgt.z);
207            // Determine whether we're close enough to the next to to consider it completed
208            let dist_sqrd = pos.xy().distance_squared(closest_tgt.xy());
209            if dist_sqrd
210                < (traversal_cfg.node_tolerance
211                    * if be_precise {
212                        0.5
213                    } else if traversal_cfg.in_liquid {
214                        2.5
215                    } else {
216                        1.0
217                    })
218                .powi(2)
219                && ((-1.0..=2.25).contains(&(pos.z - closest_tgt.z))
220                    || (traversal_cfg.in_liquid
221                        && pos.z < closest_tgt.z + 0.8
222                        && pos.z > closest_tgt.z))
223            {
224                // Node completed, move on to the next one
225                self.next_idx += 1;
226            } else {
227                // The next node hasn't been reached yet, use it as a target
228                break (next0, next1, next_tgt, be_precise);
229            }
230        };
231
232        fn gradient(line: LineSegment2<f32>) -> f32 {
233            let r = (line.start.y - line.end.y) / (line.start.x - line.end.x);
234            if r.is_nan() { 100000.0 } else { r }
235        }
236
237        fn intersect(a: LineSegment2<f32>, b: LineSegment2<f32>) -> Option<Vec2<f32>> {
238            let ma = gradient(a);
239            let mb = gradient(b);
240
241            let ca = a.start.y - ma * a.start.x;
242            let cb = b.start.y - mb * b.start.x;
243
244            if (ma - mb).abs() < 0.0001 || (ca - cb).abs() < 0.0001 {
245                None
246            } else {
247                let x = (cb - ca) / (ma - mb);
248                let y = ma * x + ca;
249
250                Some(Vec2::new(x, y))
251            }
252        }
253
254        let line_segments = [
255            LineSegment3 {
256                start: self
257                    .next_idx
258                    .checked_sub(2)
259                    .and_then(|i| self.path().nodes().get(i))
260                    .unwrap_or(&next0)
261                    .as_()
262                    + 0.5,
263                end: self
264                    .next_idx
265                    .checked_sub(1)
266                    .and_then(|i| self.path().nodes().get(i))
267                    .unwrap_or(&next0)
268                    .as_()
269                    + 0.5,
270            },
271            LineSegment3 {
272                start: self
273                    .next_idx
274                    .checked_sub(1)
275                    .and_then(|i| self.path().nodes().get(i))
276                    .unwrap_or(&next0)
277                    .as_()
278                    + 0.5,
279                end: next0.as_() + 0.5,
280            },
281            LineSegment3 {
282                start: next0.as_() + 0.5,
283                end: next1.as_() + 0.5,
284            },
285        ];
286
287        if line_segments
288            .iter()
289            .map(|ls| {
290                if self.next_idx > 1 {
291                    ls.projected_point(pos).distance_squared(pos)
292                } else {
293                    LineSegment2 {
294                        start: ls.start.xy(),
295                        end: ls.end.xy(),
296                    }
297                    .projected_point(pos.xy())
298                    .distance_squared(pos.xy())
299                }
300            })
301            .reduce(|a, b| a.min(b))
302            .is_some_and(|d| {
303                d > if traversal_cfg.in_liquid {
304                    traversal_cfg.node_tolerance * 5.0
305                } else {
306                    traversal_cfg.node_tolerance * 2.0
307                }
308                .powi(2)
309            })
310        {
311            return Err(TraverseStop::InvalidPath);
312        }
313
314        // We don't always want to aim for the centre of block since this can create
315        // jerky zig-zag movement. This function attempts to find a position
316        // inside a target block's area that aligned nicely with our velocity.
317        // This has a twofold benefit:
318        //
319        // 1. Entities can move at any angle when
320        // running on a flat surface
321        //
322        // 2. We don't have to search diagonals when
323        // pathfinding - cartesian positions are enough since this code will
324        // make the entity move smoothly along them
325        let corners = [
326            Vec2::new(0, 0),
327            Vec2::new(1, 0),
328            Vec2::new(1, 1),
329            Vec2::new(0, 1),
330            Vec2::new(0, 0), // Repeated start
331        ];
332
333        let vel_line = LineSegment2 {
334            start: pos.xy(),
335            end: pos.xy() + vel.xy() * 100.0,
336        };
337
338        let align = |block_pos: Vec3<i32>, precision: f32| {
339            let lerp_block =
340                |x, precision| Lerp::lerp(x, block_pos.xy().map(|e| e as f32), precision);
341
342            (0..4)
343                .filter_map(|i| {
344                    let edge_line = LineSegment2 {
345                        start: lerp_block(
346                            (block_pos.xy() + corners[i]).map(|e| e as f32),
347                            precision,
348                        ),
349                        end: lerp_block(
350                            (block_pos.xy() + corners[i + 1]).map(|e| e as f32),
351                            precision,
352                        ),
353                    };
354                    intersect(vel_line, edge_line).filter(|intersect| {
355                        intersect
356                            .clamped(
357                                block_pos.xy().map(|e| e as f32),
358                                block_pos.xy().map(|e| e as f32 + 1.0),
359                            )
360                            .distance_squared(*intersect)
361                            < 0.001
362                    })
363                })
364                .min_by_key(|intersect: &Vec2<f32>| {
365                    (intersect.distance_squared(vel_line.end) * 1000.0) as i32
366                })
367                .unwrap_or_else(|| {
368                    (0..2)
369                        .flat_map(|i| (0..2).map(move |j| Vec2::new(i, j)))
370                        .map(|rpos| block_pos + rpos)
371                        .map(|block_pos| {
372                            let block_posf = block_pos.xy().map(|e| e as f32);
373                            let proj = vel_line.projected_point(block_posf);
374                            let clamped = lerp_block(
375                                proj.clamped(
376                                    block_pos.xy().map(|e| e as f32),
377                                    block_pos.xy().map(|e| e as f32),
378                                ),
379                                precision,
380                            );
381
382                            (proj.distance_squared(clamped), clamped)
383                        })
384                        .min_by_key(|(d2, _)| (d2 * 1000.0) as i32)
385                        .unwrap()
386                        .1
387                })
388        };
389
390        let bez = CubicBezier2 {
391            start: pos.xy(),
392            ctrl0: pos.xy() + vel.xy().try_normalized().unwrap_or_default() * 1.0,
393            ctrl1: align(next0, 1.0),
394            end: align(next1, 1.0),
395        };
396
397        // Use a cubic spline of the next few targets to come up with a sensible target
398        // position. We want to use a position that gives smooth movement but is
399        // also accurate enough to avoid the agent getting stuck under ledges or
400        // falling off walls.
401        let next_dir = bez
402            .evaluate_derivative(0.85)
403            .try_normalized()
404            .unwrap_or_default();
405        let straight_factor = next_dir
406            .dot(vel.xy().try_normalized().unwrap_or(next_dir))
407            .max(0.0)
408            .powi(2);
409
410        let bez = CubicBezier2 {
411            start: pos.xy(),
412            ctrl0: pos.xy() + vel.xy().try_normalized().unwrap_or_default() * 1.0,
413            ctrl1: align(
414                next0,
415                (1.0 - if (next0.z as f32 - pos.z).abs() < 0.25 && !be_precise {
416                    straight_factor
417                } else {
418                    0.0
419                })
420                .max(0.1),
421            ),
422            end: align(next1, 1.0),
423        };
424
425        let tgt2d = bez.evaluate(if (next0.z as f32 - pos.z).abs() < 0.25 {
426            0.25
427        } else {
428            0.5
429        });
430        let tgt = if be_precise {
431            next_tgt
432        } else {
433            Vec3::from(tgt2d) + Vec3::unit_z() * next_tgt.z
434        };
435
436        Some((
437            tgt - pos,
438            // Control the entity's speed to hopefully stop us falling off walls on sharp
439            // corners. This code is very imperfect: it does its best but it
440            // can still fail for particularly fast entities.
441            1.0 - (traversal_cfg.slow_factor * (1.0 - straight_factor)).min(0.9),
442        ))
443        .filter(|(bearing, _)| bearing.z < 2.1)
444        .ok_or(TraverseStop::InvalidOutput)
445    }
446}
447
448#[derive(Default, Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord)]
449/// How long the path we're trying to compute should be.
450pub enum PathLength {
451    #[default]
452    Small,
453    Medium,
454    Long,
455    Longest,
456}
457
458#[derive(Default, Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord)]
459pub enum PathState {
460    /// There is no path.
461    #[default]
462    None,
463    /// A non-complete path.
464    Exhausted,
465    /// In progress of computing a path.
466    Pending,
467    /// A complete path.
468    Path,
469}
470
471/// A self-contained system that attempts to chase a moving target, only
472/// performing pathfinding if necessary
473#[derive(Default, Clone, Debug)]
474pub struct Chaser {
475    last_search_tgt: Option<Vec3<f32>>,
476    /// `bool` indicates whether the Route is a complete route to the target
477    ///
478    /// `Vec3` is the target end pos
479    route: Option<(Route, bool, Vec3<f32>)>,
480    /// We use this hasher (FxHash) because:
481    /// (1) we don't care about DDOS attacks (We can use FxHash);
482    /// (2) we want this to be constant across compiles because of hot-reloading
483    /// (Ruling out AAHash);
484    ///
485    /// The Vec3 is the astar's start position.
486    astar: Option<(Astar<Node, FxBuildHasher>, Vec3<f32>)>,
487    flee_from: Option<Vec3<f32>>,
488    /// Whether to allow consideration of longer paths, npc will stand still
489    /// while doing this.
490    path_length: PathLength,
491
492    /// The current state of the path.
493    path_state: PathState,
494
495    /// The last time the `chase` method was called.
496    last_update_time: Option<Time>,
497
498    /// (position, requested walk dir)
499    recent_states: VecDeque<(Time, Vec3<f32>, Vec3<f32>)>,
500}
501
502impl Chaser {
503    fn stuck_check(
504        &mut self,
505        pos: Vec3<f32>,
506        bearing: Vec3<f32>,
507        speed: f32,
508        time: &Time,
509    ) -> (Vec3<f32>, f32, bool) {
510        /// The min amount of cached items.
511        const MIN_CACHED_STATES: usize = 3;
512        /// The max amount of cached items.
513        const MAX_CACHED_STATES: usize = 10;
514        /// Cache over 1 second.
515        const CACHED_TIME_SPAN: f64 = 1.0;
516        const TOLERANCE: f32 = 0.2;
517
518        // We pop the first until there is only one element which was over
519        // `CACHED_TIME_SPAN` seconds ago.
520        while self.recent_states.len() > MIN_CACHED_STATES
521            && self
522                .recent_states
523                .get(1)
524                .is_some_and(|(t, ..)| time.0 - t.0 > CACHED_TIME_SPAN)
525        {
526            self.recent_states.pop_front();
527        }
528
529        if self.recent_states.len() < MAX_CACHED_STATES {
530            self.recent_states.push_back((*time, pos, bearing * speed));
531
532            if self.recent_states.len() >= MIN_CACHED_STATES
533                && self
534                    .recent_states
535                    .front()
536                    .is_some_and(|(t, ..)| time.0 - t.0 > CACHED_TIME_SPAN)
537                && (bearing * speed).magnitude_squared() > 0.01
538            {
539                let average_pos = self
540                    .recent_states
541                    .iter()
542                    .map(|(_, pos, _)| *pos)
543                    .sum::<Vec3<f32>>()
544                    * (1.0 / self.recent_states.len() as f32);
545                let max_distance_sqr = self
546                    .recent_states
547                    .iter()
548                    .map(|(_, pos, _)| pos.distance_squared(average_pos))
549                    .reduce(|a, b| a.max(b));
550
551                let average_speed = self
552                    .recent_states
553                    .iter()
554                    .zip(self.recent_states.iter().skip(1).map(|(t, ..)| *t))
555                    .map(|((t0, _, bearing), t1)| {
556                        bearing.magnitude_squared() * (t1.0 - t0.0).powi(2) as f32
557                    })
558                    .sum::<f32>()
559                    * (1.0 / self.recent_states.len() as f32);
560
561                let is_stuck =
562                    max_distance_sqr.is_some_and(|d| d < (average_speed * TOLERANCE).powi(2));
563
564                let bearing = if is_stuck {
565                    match rng().random_range(0..100u32) {
566                        0..10 => -bearing,
567                        10..20 => Vec3::new(bearing.y, bearing.x, bearing.z),
568                        20..30 => Vec3::new(-bearing.y, bearing.x, bearing.z),
569                        30..50 => {
570                            if let Some((route, ..)) = &mut self.route {
571                                route.next_idx = route.next_idx.saturating_sub(1);
572                            }
573
574                            bearing
575                        },
576                        50..60 => {
577                            if let Some((route, ..)) = &mut self.route {
578                                route.next_idx = route.next_idx.saturating_sub(2);
579                            }
580
581                            bearing
582                        },
583                        _ => bearing,
584                    }
585                } else {
586                    bearing
587                };
588
589                return (bearing, speed, is_stuck);
590            }
591        }
592        (bearing, speed, false)
593    }
594
595    fn reset(&mut self) {
596        self.route = None;
597        self.astar = None;
598        self.last_search_tgt = None;
599        self.path_length = Default::default();
600        self.flee_from = None;
601    }
602
603    /// Returns bearing and speed
604    /// Bearing is a `Vec3<f32>` dictating the direction of movement
605    /// Speed is an f32 between 0.0 and 1.0
606    pub fn chase<V>(
607        &mut self,
608        vol: &V,
609        pos: Vec3<f32>,
610        vel: Vec3<f32>,
611        tgt: Vec3<f32>,
612        traversal_cfg: TraversalConfig,
613        time: &Time,
614    ) -> Option<(Vec3<f32>, f32, bool)>
615    where
616        V: BaseVol<Vox = Block> + ReadVol,
617    {
618        span!(_guard, "chase", "Chaser::chase");
619        self.last_update_time = Some(*time);
620        // If we're already close to the target then there's nothing to do
621        if ((pos - tgt) * Vec3::new(1.0, 1.0, 2.0)).magnitude_squared()
622            < traversal_cfg.min_tgt_dist.powi(2)
623        {
624            self.reset();
625            return None;
626        }
627
628        let d = tgt.distance_squared(pos);
629
630        // Check if the current route is no longer valid.
631        if let Some(end) = self.route.as_ref().map(|(_, _, end)| *end)
632            && self.flee_from.is_none()
633            && self.path_length < PathLength::Longest
634            && d < tgt.distance_squared(end)
635        {
636            self.path_length = Default::default();
637            self.route = None;
638        }
639
640        // If we're closer than the designated `flee_from` position, we ignore
641        // that.
642        if self.flee_from.is_some_and(|p| d < p.distance_squared(tgt)) {
643            self.route = None;
644            self.flee_from = None;
645            self.astar = None;
646            self.path_length = Default::default();
647        }
648
649        // Find a route if we don't have one.
650        if self.route.is_none() {
651            // Reset astar if last tgt is too far from tgt.
652            if self
653                .last_search_tgt
654                .is_some_and(|last_tgt| tgt.distance_squared(last_tgt) > 2.0)
655            {
656                self.astar = None;
657            }
658            match find_path(
659                &mut self.astar,
660                vol,
661                pos,
662                tgt,
663                &traversal_cfg,
664                self.path_length,
665                self.flee_from,
666            ) {
667                PathResult::Pending => {
668                    self.path_state = PathState::Pending;
669                },
670                PathResult::None(path) => {
671                    self.path_state = PathState::None;
672                    self.route = Some((Route { path, next_idx: 0 }, false, tgt));
673                },
674                PathResult::Exhausted(path) => {
675                    self.path_state = PathState::Exhausted;
676                    self.route = Some((Route { path, next_idx: 0 }, false, tgt));
677                },
678                PathResult::Path(path, _) => {
679                    self.flee_from = None;
680                    self.path_state = PathState::Path;
681                    self.path_length = Default::default();
682                    self.route = Some((Route { path, next_idx: 0 }, true, tgt));
683                },
684            }
685
686            self.last_search_tgt = Some(tgt);
687        }
688
689        if let Some((route, ..)) = &mut self.route {
690            let res = route.traverse(vol, pos, vel, &traversal_cfg);
691
692            // None either means we're done, or can't continue, either way we don't care
693            // about that route anymore.
694            if let Err(e) = &res {
695                self.route = None;
696                match e {
697                    TraverseStop::InvalidOutput => {
698                        return Some(self.stuck_check(
699                            pos,
700                            (tgt - pos).try_normalized().unwrap_or(Vec3::unit_x()),
701                            1.0,
702                            time,
703                        ));
704                    },
705                    TraverseStop::InvalidPath => {
706                        // If the path is invalid, blocks along the path have most likely changed,
707                        // so reset the astar.
708                        self.astar = None;
709                    },
710                    TraverseStop::Done => match self.path_state {
711                        PathState::None => {
712                            return Some(self.stuck_check(
713                                pos,
714                                (tgt - pos).try_normalized().unwrap_or_default(),
715                                1.0,
716                                time,
717                            ));
718                        },
719                        PathState::Exhausted => {
720                            // Upgrade path length if path is exhausted and we're at the same
721                            // position.
722                            if self.astar.as_ref().is_some_and(|(.., start)| {
723                                start.distance_squared(pos) < traversal_cfg.node_tolerance.powi(2)
724                            }) {
725                                match self.path_length {
726                                    PathLength::Small => {
727                                        self.path_length = PathLength::Medium;
728                                    },
729                                    PathLength::Medium => {
730                                        self.path_length = PathLength::Long;
731                                    },
732                                    PathLength::Long => {
733                                        self.path_length = PathLength::Longest;
734                                    },
735                                    PathLength::Longest => {
736                                        self.flee_from = Some(pos);
737                                        self.astar = None;
738                                    },
739                                }
740                            } else {
741                                self.astar = None;
742                            }
743                        },
744                        PathState::Pending | PathState::Path => {},
745                    },
746                }
747            }
748
749            let (bearing, speed) = res.ok()?;
750
751            return Some(self.stuck_check(pos, bearing, speed, time));
752        }
753
754        None
755    }
756
757    pub fn get_route(&self) -> Option<&Route> { self.route.as_ref().map(|(r, ..)| r) }
758
759    pub fn last_target(&self) -> Option<Vec3<f32>> { self.last_search_tgt }
760
761    pub fn state(&self) -> (PathLength, PathState) { (self.path_length, self.path_state) }
762
763    pub fn last_update_time(&self) -> Time {
764        self.last_update_time.unwrap_or(Time(f64::NEG_INFINITY))
765    }
766}
767
768fn walkable<V>(vol: &V, pos: Vec3<i32>, traversal_cfg: &TraversalConfig) -> bool
769where
770    V: BaseVol<Vox = Block> + ReadVol,
771{
772    let mut below_z = 1;
773    // We loop downwards
774    let below = loop {
775        if let Some(block) = vol.get(pos - Vec3::unit_z() * below_z).ok().copied() {
776            if block.is_solid() || block.is_liquid() {
777                break block;
778            }
779
780            below_z += 1;
781
782            if below_z > Block::MAX_HEIGHT.ceil() as i32 {
783                break Block::empty();
784            }
785        } else if traversal_cfg.is_target_loaded {
786            break Block::empty();
787        } else {
788            // If not loaded assume we can walk there.
789            break Block::new(crate::terrain::BlockKind::Misc, Default::default());
790        }
791    };
792
793    let a = vol.get(pos).ok().copied().unwrap_or_else(Block::empty);
794    let b = vol
795        .get(pos + Vec3::unit_z())
796        .ok()
797        .copied()
798        .unwrap_or_else(Block::empty);
799
800    let on_ground = (below_z == 1 && below.is_filled())
801        || below.get_sprite().is_some_and(|sprite| {
802            sprite
803                .solid_height()
804                .is_some_and(|h| ((below_z - 1) as f32) < h && h <= below_z as f32)
805        });
806    let in_liquid = a.is_liquid();
807    ((on_ground && traversal_cfg.ground_accel().is_some())
808        || (in_liquid && traversal_cfg.swim_thrust().is_some()))
809        && !a.is_solid()
810        && !b.is_solid()
811}
812
813#[derive(Copy, Clone, PartialEq, Eq, Hash, Debug)]
814pub struct Node {
815    pos: Vec3<i32>,
816    last_dir: Vec2<i32>,
817    last_dir_count: u32,
818}
819
820/// Attempt to search for a path to a target, returning the path (if one was
821/// found) and whether it is complete (reaches the target)
822///
823/// If `flee_from` is `Some` this will attempt to both walk away from that
824/// position and towards the target.
825fn find_path<V>(
826    astar: &mut Option<(Astar<Node, FxBuildHasher>, Vec3<f32>)>,
827    vol: &V,
828    startf: Vec3<f32>,
829    endf: Vec3<f32>,
830    traversal_cfg: &TraversalConfig,
831    path_length: PathLength,
832    flee_from: Option<Vec3<f32>>,
833) -> PathResult<Vec3<i32>>
834where
835    V: BaseVol<Vox = Block> + ReadVol,
836{
837    let is_walkable = |pos: &Vec3<i32>| walkable(vol, *pos, traversal_cfg);
838    let get_walkable_z = |pos| {
839        let mut z_incr = 0;
840        for _ in 0..32 {
841            let test_pos = pos + Vec3::unit_z() * z_incr;
842            if is_walkable(&test_pos) {
843                return Some(test_pos);
844            }
845            z_incr = -z_incr + i32::from(z_incr <= 0);
846        }
847        None
848    };
849
850    // Find walkable ground for start and end.
851    let (start, end) = match (
852        get_walkable_z(startf.map(|e| e.floor() as i32)),
853        get_walkable_z(endf.map(|e| e.floor() as i32)),
854    ) {
855        (Some(start), Some(end)) => (start, end),
856
857        // Special case for partially loaded path finding
858        (Some(start), None) if !traversal_cfg.is_target_loaded => {
859            (start, endf.map(|e| e.floor() as i32))
860        },
861
862        _ => return PathResult::None(Path::default()),
863    };
864
865    let heuristic = |node: &Node| {
866        let diff = end.as_::<f32>() - node.pos.as_::<f32>();
867        let d = diff.magnitude();
868
869        d - flee_from.map_or(0.0, |p| {
870            let ndiff = p - node.pos.as_::<f32>() - 0.5;
871            let nd = ndiff.magnitude();
872            nd.sqrt() * ((diff / d).dot(ndiff / nd) + 0.1).max(0.0) * 10.0
873        })
874    };
875    let transition = |a: Node, b: Node| {
876        1.0
877            // Discourage travelling in the same direction for too long: this encourages
878            // turns to be spread out along a path, more closely approximating a straight
879            // line toward the target.
880            + b.last_dir_count as f32 * 0.01
881            // Penalise jumping
882            + (b.pos.z - a.pos.z + 1).max(0) as f32 * 2.0
883    };
884    let neighbors = |node: &Node| {
885        let node = *node;
886        let pos = node.pos;
887        const DIRS: [Vec3<i32>; 9] = [
888            Vec3::new(0, 1, 0), // Forward
889            Vec3::new(0, 1, 1), // Forward upward
890            // Vec3::new(0, 1, -1),  // Forward downward
891            // Vec3::new(0, 1, -2),  // Forward downwardx2
892            Vec3::new(1, 0, 0), // Right
893            Vec3::new(1, 0, 1), // Right upward
894            // Vec3::new(1, 0, -1),  // Right downward
895            // Vec3::new(1, 0, -2),  // Right downwardx2
896            Vec3::new(0, -1, 0), // Backwards
897            Vec3::new(0, -1, 1), // Backward Upward
898            // Vec3::new(0, -1, -1), // Backward downward
899            // Vec3::new(0, -1, -2), // Backward downwardx2
900            Vec3::new(-1, 0, 0), // Left
901            Vec3::new(-1, 0, 1), // Left upward
902            // Vec3::new(-1, 0, -1), // Left downward
903            // Vec3::new(-1, 0, -2), // Left downwardx2
904            Vec3::new(0, 0, -1), // Downwards
905        ];
906
907        const JUMPS: [Vec3<i32>; 4] = [
908            Vec3::new(0, 1, 2),  // Forward Upwardx2
909            Vec3::new(1, 0, 2),  // Right Upwardx2
910            Vec3::new(0, -1, 2), // Backward Upwardx2
911            Vec3::new(-1, 0, 2), // Left Upwardx2
912        ];
913
914        /// The cost of falling a block.
915        const FALL_COST: f32 = 1.5;
916
917        let walkable = [
918            (is_walkable(&(pos + Vec3::new(1, 0, 0))), Vec3::new(1, 0, 0)),
919            (
920                is_walkable(&(pos + Vec3::new(-1, 0, 0))),
921                Vec3::new(-1, 0, 0),
922            ),
923            (is_walkable(&(pos + Vec3::new(0, 1, 0))), Vec3::new(0, 1, 0)),
924            (
925                is_walkable(&(pos + Vec3::new(0, -1, 0))),
926                Vec3::new(0, -1, 0),
927            ),
928        ];
929
930        // Discourage walking alog walls/edges.
931        let edge_cost = if path_length < PathLength::Medium {
932            walkable.iter().any(|(w, _)| !*w) as i32 as f32
933        } else {
934            0.0
935        };
936
937        // const DIAGONALS: [(Vec3<i32>, [usize; 2]); 8] = [
938        //     (Vec3::new(1, 1, 0), [0, 2]),
939        //     (Vec3::new(-1, 1, 0), [1, 2]),
940        //     (Vec3::new(1, -1, 0), [0, 3]),
941        //     (Vec3::new(-1, -1, 0), [1, 3]),
942        //     (Vec3::new(1, 1, 1), [0, 2]),
943        //     (Vec3::new(-1, 1, 1), [1, 2]),
944        //     (Vec3::new(1, -1, 1), [0, 3]),
945        //     (Vec3::new(-1, -1, 1), [1, 3]),
946        // ];
947
948        DIRS.iter()
949            .chain(
950                (vol.get(pos - Vec3::unit_z())
951                    .map(|b| !b.is_liquid())
952                    .unwrap_or(traversal_cfg.is_target_loaded)
953                    || traversal_cfg.can_climb()
954                    || traversal_cfg.can_fly()).then_some(JUMPS.iter())
955                    .into_iter().flatten()
956            )
957            .map(move |dir| (pos, dir))
958            .filter(move |(pos, dir)| {
959                (traversal_cfg.can_fly() || is_walkable(pos) && is_walkable(&(*pos + **dir)))
960                    && ((dir.z < 1
961                        || vol
962                            .get(pos + Vec3::unit_z() * 2)
963                            .map(|b| !b.is_solid())
964                            .unwrap_or(traversal_cfg.is_target_loaded))
965                        && (dir.z < 2
966                            || vol
967                                .get(pos + Vec3::unit_z() * 3)
968                                .map(|b| !b.is_solid())
969                                .unwrap_or(traversal_cfg.is_target_loaded))
970                        && (dir.z >= 0
971                            || vol
972                                .get(pos + *dir + Vec3::unit_z() * 2)
973                                .map(|b| !b.is_solid())
974                                .unwrap_or(traversal_cfg.is_target_loaded)))
975            })
976            .map(move |(pos, dir)| {
977                let next_node = Node {
978                    pos: pos + dir,
979                    last_dir: dir.xy(),
980                    last_dir_count: if node.last_dir == dir.xy() {
981                        node.last_dir_count + 1
982                    } else {
983                        0
984                    },
985                };
986
987                (
988                    next_node,
989                    transition(node, next_node) + if dir.z == 0 { edge_cost } else { 0.0 },
990                )
991            })
992            // Falls
993            .chain(walkable.into_iter().filter_map(move |(w, dir)| {
994                let pos = pos + dir;
995                if w ||
996                    vol.get(pos).map(|b| b.is_solid()).unwrap_or(true) ||
997                    vol.get(pos + Vec3::unit_z()).map(|b| b.is_solid()).unwrap_or(true) {
998                    return None;
999                }
1000
1001                let down = (1..12).find(|i| is_walkable(&(pos - Vec3::unit_z() * *i)))?;
1002
1003                let next_node = Node {
1004                    pos: pos - Vec3::unit_z() * down,
1005                    last_dir: dir.xy(),
1006                    last_dir_count: 0,
1007                };
1008
1009                // Falling costs a lot.
1010                Some((next_node, match down {
1011                    1..=2 => {
1012                        transition(node, next_node)
1013                    }
1014                    _ => FALL_COST * (down - 2) as f32,
1015                }))
1016            }))
1017        // .chain(
1018        //     DIAGONALS
1019        //         .iter()
1020        //         .filter(move |(dir, [a, b])| {
1021        //             is_walkable(&(pos + *dir)) && walkable[*a] &&
1022        // walkable[*b]         })
1023        //         .map(move |(dir, _)| pos + *dir),
1024        // )
1025    };
1026
1027    let satisfied = |node: &Node| node.pos == end;
1028
1029    if astar
1030        .as_ref()
1031        .is_some_and(|(_, start)| start.distance_squared(startf) > 4.0)
1032    {
1033        *astar = None;
1034    }
1035    let max_iters = match path_length {
1036        PathLength::Small => 500,
1037        PathLength::Medium => 5000,
1038        PathLength::Long => 25_000,
1039        PathLength::Longest => 75_000,
1040    };
1041
1042    let (astar, _) = astar.get_or_insert_with(|| {
1043        (
1044            Astar::new(
1045                max_iters,
1046                Node {
1047                    pos: start,
1048                    last_dir: Vec2::zero(),
1049                    last_dir_count: 0,
1050                },
1051                FxBuildHasher::default(),
1052            ),
1053            startf,
1054        )
1055    });
1056
1057    astar.set_max_iters(max_iters);
1058
1059    let path_result = astar.poll(
1060        match path_length {
1061            PathLength::Small => 250,
1062            PathLength::Medium => 400,
1063            PathLength::Long => 500,
1064            PathLength::Longest => 750,
1065        },
1066        heuristic,
1067        neighbors,
1068        satisfied,
1069    );
1070
1071    path_result.map(|path| path.nodes.into_iter().map(|n| n.pos).collect())
1072}
1073// Enable when airbraking/sensible flight is a thing
1074#[cfg(feature = "rrt_pathfinding")]
1075fn find_air_path<V>(
1076    vol: &V,
1077    startf: Vec3<f32>,
1078    endf: Vec3<f32>,
1079    traversal_cfg: &TraversalConfig,
1080) -> (Option<Path<Vec3<i32>>>, bool)
1081where
1082    V: BaseVol<Vox = Block> + ReadVol,
1083{
1084    let radius = traversal_cfg.node_tolerance;
1085    let total_dist_sqrd = startf.distance_squared(endf);
1086    // First check if a straight line path works
1087    if vol
1088        .ray(startf + Vec3::unit_z(), endf + Vec3::unit_z())
1089        .until(Block::is_opaque)
1090        .cast()
1091        .0
1092        .powi(2)
1093        >= total_dist_sqrd
1094    {
1095        let path = vec![endf.map(|e| e.floor() as i32)];
1096        let connect = true;
1097        (Some(path.into_iter().collect()), connect)
1098    // Else use RRTs
1099    } else {
1100        let is_traversable = |start: &Vec3<f32>, end: &Vec3<f32>| {
1101            vol.ray(*start, *end)
1102                .until(Block::is_solid)
1103                .cast()
1104                .0
1105                .powi(2)
1106                > (*start).distance_squared(*end)
1107            //vol.get(*pos).ok().copied().unwrap_or_else(Block::empty).
1108            // is_fluid();
1109        };
1110        informed_rrt_connect(vol, startf, endf, is_traversable, radius)
1111    }
1112}
1113
1114/// Attempts to find a path from a start to the end using an informed
1115/// RRT-Connect algorithm. A point is sampled from a bounding spheroid
1116/// between the start and end. Two separate rapidly exploring random
1117/// trees extend toward the sampled point. Nodes are stored in k-d trees
1118/// for quicker nearest node calculations. Points are sampled until the
1119/// trees connect. A final path is then reconstructed from the nodes.
1120/// This pathfinding algorithm is more appropriate for 3D pathfinding
1121/// with wider gaps, such as flying through a forest than for terrain
1122/// with narrow gaps, such as navigating a maze.
1123/// Returns a path and whether that path is complete or not.
1124#[cfg(feature = "rrt_pathfinding")]
1125fn informed_rrt_connect<V>(
1126    vol: &V,
1127    startf: Vec3<f32>,
1128    endf: Vec3<f32>,
1129    is_valid_edge: impl Fn(&Vec3<f32>, &Vec3<f32>) -> bool,
1130    radius: f32,
1131) -> (Option<Path<Vec3<i32>>>, bool)
1132where
1133    V: BaseVol<Vox = Block> + ReadVol,
1134{
1135    const MAX_POINTS: usize = 7000;
1136    let mut path = Vec::new();
1137
1138    // Each tree has a vector of nodes
1139    let mut node_index1: usize = 0;
1140    let mut node_index2: usize = 0;
1141    let mut nodes1 = Vec::new();
1142    let mut nodes2 = Vec::new();
1143
1144    // The parents hashmap stores nodes and their parent nodes as pairs to
1145    // retrace the complete path once the two RRTs connect
1146    let mut parents1 = HashMap::new();
1147    let mut parents2 = HashMap::new();
1148
1149    // The path vector stores the path from the appropriate terminal to the
1150    // connecting node or vice versa
1151    let mut path1 = Vec::new();
1152    let mut path2 = Vec::new();
1153
1154    // K-d trees are used to find the closest nodes rapidly
1155    let mut kdtree1: KdTree<f32, usize, 3, 32, u32> = KdTree::with_capacity(MAX_POINTS);
1156    let mut kdtree2: KdTree<f32, usize, 3, 32, u32> = KdTree::with_capacity(MAX_POINTS);
1157
1158    // Add the start as the first node of the first k-d tree
1159    kdtree1.add(&[startf.x, startf.y, startf.z], node_index1);
1160    nodes1.push(startf);
1161    node_index1 += 1;
1162
1163    // Add the end as the first node of the second k-d tree
1164    kdtree2.add(&[endf.x, endf.y, endf.z], node_index2);
1165    nodes2.push(endf);
1166    node_index2 += 1;
1167
1168    let mut connection1_idx = 0;
1169    let mut connection2_idx = 0;
1170
1171    let mut connect = false;
1172
1173    // Scalar non-dimensional value that is proportional to the size of the
1174    // sample spheroid volume. This increases in value until a path is found.
1175    let mut search_parameter = 0.01;
1176
1177    // Maximum of MAX_POINTS iterations
1178    for _i in 0..MAX_POINTS {
1179        if connect {
1180            break;
1181        }
1182
1183        // Sample a point on the bounding spheroid
1184        let (sampled_point1, sampled_point2) = {
1185            let point = point_on_prolate_spheroid(startf, endf, search_parameter);
1186            (point, point)
1187        };
1188
1189        // Find the nearest nodes to the the sampled point
1190        let nearest_index1 = kdtree1
1191            .nearest_one::<SquaredEuclidean>(&[
1192                sampled_point1.x,
1193                sampled_point1.y,
1194                sampled_point1.z,
1195            ])
1196            .item;
1197        let nearest_index2 = kdtree2
1198            .nearest_one::<SquaredEuclidean>(&[
1199                sampled_point2.x,
1200                sampled_point2.y,
1201                sampled_point2.z,
1202            ])
1203            .item;
1204        let nearest1 = nodes1[nearest_index1];
1205        let nearest2 = nodes2[nearest_index2];
1206
1207        // Extend toward the sampled point from the nearest node of each tree
1208        let new_point1 = nearest1 + (sampled_point1 - nearest1).normalized().map(|a| a * radius);
1209        let new_point2 = nearest2 + (sampled_point2 - nearest2).normalized().map(|a| a * radius);
1210
1211        // Ensure the new nodes are valid/traversable
1212        if is_valid_edge(&nearest1, &new_point1) {
1213            kdtree1.add(&[new_point1.x, new_point1.y, new_point1.z], node_index1);
1214            nodes1.push(new_point1);
1215            parents1.insert(node_index1, nearest_index1);
1216            node_index1 += 1;
1217            // Check if the trees connect
1218            let NearestNeighbour {
1219                distance: check,
1220                item: index,
1221            } = kdtree2.nearest_one::<SquaredEuclidean>(&[
1222                new_point1.x,
1223                new_point1.y,
1224                new_point1.z,
1225            ]);
1226            if check < radius {
1227                let connection = nodes2[index];
1228                connection2_idx = index;
1229                nodes1.push(connection);
1230                connection1_idx = nodes1.len() - 1;
1231                parents1.insert(node_index1, node_index1 - 1);
1232                connect = true;
1233            }
1234        }
1235
1236        // Repeat the validity check for the second tree
1237        if is_valid_edge(&nearest2, &new_point2) {
1238            kdtree2.add(&[new_point2.x, new_point2.y, new_point1.z], node_index2);
1239            nodes2.push(new_point2);
1240            parents2.insert(node_index2, nearest_index2);
1241            node_index2 += 1;
1242            // Again check for a connection
1243            let NearestNeighbour {
1244                distance: check,
1245                item: index,
1246            } = kdtree1.nearest_one::<SquaredEuclidean>(&[
1247                new_point2.x,
1248                new_point2.y,
1249                new_point1.z,
1250            ]);
1251            if check < radius {
1252                let connection = nodes1[index];
1253                connection1_idx = index;
1254                nodes2.push(connection);
1255                connection2_idx = nodes2.len() - 1;
1256                parents2.insert(node_index2, node_index2 - 1);
1257                connect = true;
1258            }
1259        }
1260        // Increase the search parameter to widen the sample volume
1261        search_parameter += 0.02;
1262    }
1263
1264    if connect {
1265        // Construct paths from the connection node to the start and end
1266        let mut current_node_index1 = connection1_idx;
1267        while current_node_index1 > 0 {
1268            current_node_index1 = *parents1.get(&current_node_index1).unwrap_or(&0);
1269            path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32));
1270        }
1271        let mut current_node_index2 = connection2_idx;
1272        while current_node_index2 > 0 {
1273            current_node_index2 = *parents2.get(&current_node_index2).unwrap_or(&0);
1274            path2.push(nodes2[current_node_index2].map(|e| e.floor() as i32));
1275        }
1276        // Join the two paths together in the proper order and remove duplicates
1277        path1.pop();
1278        path1.reverse();
1279        path.append(&mut path1);
1280        path.append(&mut path2);
1281        path.dedup();
1282    } else {
1283        // If the trees did not connect, construct a path from the start to
1284        // the closest node to the end
1285        let mut current_node_index1 = kdtree1
1286            .nearest_one::<SquaredEuclidean>(&[endf.x, endf.y, endf.z])
1287            .item;
1288        // Attempt to pick a node other than the start node
1289        for _i in 0..3 {
1290            if current_node_index1 == 0
1291                || nodes1[current_node_index1].distance_squared(startf) < 4.0
1292            {
1293                if let Some(index) = parents1.values().choose(&mut rng()) {
1294                    current_node_index1 = *index;
1295                } else {
1296                    break;
1297                }
1298            } else {
1299                break;
1300            }
1301        }
1302        path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32));
1303        // Construct the path
1304        while current_node_index1 != 0 && nodes1[current_node_index1].distance_squared(startf) > 4.0
1305        {
1306            current_node_index1 = *parents1.get(&current_node_index1).unwrap_or(&0);
1307            path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32));
1308        }
1309
1310        path1.reverse();
1311        path.append(&mut path1);
1312    }
1313    let mut new_path = Vec::new();
1314    let mut node = path[0];
1315    new_path.push(node);
1316    let mut node_idx = 0;
1317    let num_nodes = path.len();
1318    let end = path[num_nodes - 1];
1319    while node != end {
1320        let next_idx = if node_idx + 4 > num_nodes - 1 {
1321            num_nodes - 1
1322        } else {
1323            node_idx + 4
1324        };
1325        let next_node = path[next_idx];
1326        let start_pos = node.map(|e| e as f32 + 0.5);
1327        let end_pos = next_node.map(|e| e as f32 + 0.5);
1328        if vol
1329            .ray(start_pos, end_pos)
1330            .until(Block::is_solid)
1331            .cast()
1332            .0
1333            .powi(2)
1334            > (start_pos).distance_squared(end_pos)
1335        {
1336            node_idx = next_idx;
1337            new_path.push(next_node);
1338        } else {
1339            node_idx += 1;
1340        }
1341        node = path[node_idx];
1342    }
1343    path = new_path;
1344    (Some(path.into_iter().collect()), connect)
1345}
1346
1347/// Returns a random point within a radially symmetrical ellipsoid with given
1348/// foci and a `search parameter` to determine the size of the ellipse beyond
1349/// the foci. Technically the point is within a prolate spheroid translated and
1350/// rotated to the proper place in cartesian space.
1351/// The search_parameter is a float that relates to the length of the string for
1352/// a two dimensional ellipse or the size of the ellipse beyond the foci. In
1353/// this case that analogy still holds as the ellipse is radially symmetrical
1354/// along the axis between the foci. The value of the search parameter must be
1355/// greater than zero. In order to increase the sample area, the
1356/// search_parameter should be increased linearly as the search continues.
1357#[cfg(feature = "rrt_pathfinding")]
1358pub fn point_on_prolate_spheroid(
1359    focus1: Vec3<f32>,
1360    focus2: Vec3<f32>,
1361    search_parameter: f32,
1362) -> Vec3<f32> {
1363    let mut rng = rng();
1364    // Uniform distribution
1365    let range = Uniform::new(0.0, 1.0).unwrap();
1366
1367    // Midpoint is used as the local origin
1368    let midpoint = 0.5 * (focus1 + focus2);
1369    // Radius between the start and end of the path
1370    let radius: f32 = focus1.distance(focus2);
1371    // The linear eccentricity of an ellipse is the distance from the origin to a
1372    // focus A prolate spheroid is a half-ellipse rotated for a full revolution
1373    // which is why ellipse variables are used frequently in this function
1374    let linear_eccentricity: f32 = 0.5 * radius;
1375
1376    // For an ellipsoid, three variables determine the shape: a, b, and c.
1377    // These are the distance from the center/origin to the surface on the
1378    // x, y, and z axes, respectively.
1379    // For a prolate spheroid a and b are equal.
1380    // c is determined by adding the search parameter to the linear eccentricity.
1381    // As the search parameter increases the size of the spheroid increases
1382    let c: f32 = linear_eccentricity + search_parameter;
1383    // The width is calculated to prioritize increasing width over length of
1384    // the ellipsoid
1385    let a: f32 = (c.powi(2) - linear_eccentricity.powi(2)).powf(0.5);
1386    // The width should be the same in both the x and y directions
1387    let b: f32 = a;
1388
1389    // The parametric spherical equation for an ellipsoid measuring from the
1390    // center point is as follows:
1391    // x = a * cos(theta) * cos(lambda)
1392    // y = b * cos(theta) * sin(lambda)
1393    // z = c * sin(theta)
1394    //
1395    // where     -0.5 * PI <= theta <= 0.5 * PI
1396    // and       0.0 <= lambda < 2.0 * PI
1397    //
1398    // Select these two angles using the uniform distribution defined at the
1399    // beginning of the function from 0.0 to 1.0
1400    let rtheta: f32 = PI * range.sample(&mut rng) - 0.5 * PI;
1401    let lambda: f32 = 2.0 * PI * range.sample(&mut rng);
1402    // Select a point on the surface of the ellipsoid
1403    let point = Vec3::new(
1404        a * rtheta.cos() * lambda.cos(),
1405        b * rtheta.cos() * lambda.sin(),
1406        c * rtheta.sin(),
1407    );
1408    // NOTE: Theoretically we should sample a point within the spheroid
1409    // requiring selecting a point along the radius. In my tests selecting
1410    // a point *on the surface* of the spheroid results in sampling that is
1411    // "good enough". The following code is commented out to reduce expense.
1412    //let surface_point = Vec3::new(a * rtheta.cos() * lambda.cos(), b *
1413    // rtheta.cos() * lambda.sin(), c * rtheta.sin()); let magnitude =
1414    // surface_point.magnitude(); let direction = surface_point.normalized();
1415    //// Randomly select a point along the vector to the previously selected surface
1416    //// point using the uniform distribution
1417    //let point = magnitude * range.sample(&mut rng) * direction;
1418
1419    // Now that a point has been selected in local space, it must be rotated and
1420    // translated into global coordinates
1421    // NOTE: Don't rotate about the z axis as the point is already randomly
1422    // selected about the z axis
1423    //let dx = focus2.x - focus1.x;
1424    //let dy = focus2.y - focus1.y;
1425    let dz = focus2.z - focus1.z;
1426    // Phi and theta are the angles from the x axis in the x-y plane and from
1427    // the z axis, respectively. (As found in spherical coordinates)
1428    // These angles are used to rotate the random point in the spheroid about
1429    // the local origin
1430    //
1431    // Rotate about z axis by phi
1432    //let phi: f32 = if dx.abs() > 0.0 {
1433    //    (dy / dx).atan()
1434    //} else {
1435    //    0.5 * PI
1436    //};
1437    // This is unnecessary as rtheta is randomly selected between 0.0 and 2.0 * PI
1438    // let rot_z_mat = Mat3::new(phi.cos(), -1.0 * phi.sin(), 0.0, phi.sin(),
1439    // phi.cos(), 0.0, 0.0, 0.0, 1.0);
1440
1441    // Rotate about perpendicular vector in the xy plane by theta
1442    let theta: f32 = if radius > 0.0 {
1443        (dz / radius).acos()
1444    } else {
1445        0.0
1446    };
1447    // Vector from focus1 to focus2
1448    let r_vec = focus2 - focus1;
1449    // Perpendicular vector in xy plane
1450    let perp_vec = Vec3::new(-1.0 * r_vec.y, r_vec.x, 0.0).normalized();
1451    let l = perp_vec.x;
1452    let m = perp_vec.y;
1453    let n = perp_vec.z;
1454    // Rotation matrix for rotation about a vector
1455    let rot_2_mat = Mat3::new(
1456        l * l * (1.0 - theta.cos()),
1457        m * l * (1.0 - theta.cos()) - n * theta.sin(),
1458        n * l * (1.0 - theta.cos()) + m * theta.sin(),
1459        l * m * (1.0 - theta.cos()) + n * theta.sin(),
1460        m * m * (1.0 - theta.cos()) + theta.cos(),
1461        n * m * (1.0 - theta.cos()) - l * theta.sin(),
1462        l * n * (1.0 - theta.cos()) - m * theta.sin(),
1463        m * n * (1.0 - theta.cos()) + l * theta.sin(),
1464        n * n * (1.0 - theta.cos()) + theta.cos(),
1465    );
1466
1467    // Get the global coordinates of the point by rotating and adding the origin
1468    // rot_z_mat is unneeded due to the random rotation defined by lambda
1469    // let global_coords = midpoint + rot_2_mat * (rot_z_mat * point);
1470    midpoint + rot_2_mat * point
1471}