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}; use 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#[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#[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 pub node_tolerance: f32,
88 pub slow_factor: f32,
91 pub on_ground: bool,
93 pub in_liquid: bool,
95 pub min_tgt_dist: f32,
97 pub can_climb: bool,
99 pub can_fly: bool,
101 pub vectored_propulsion: bool,
103 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 let next0 = self.next(0)?;
140 let next1 = self.next(1).unwrap_or(next0);
141
142 if !walkable(vol, next0) || !walkable(vol, next1) {
144 return None;
145 }
146
147 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 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 let be_precise = open_space_nearby || wall_nearby;
165
166 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 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 self.next_idx += 1;
191 } else {
192 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 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), ];
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 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 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#[derive(Default, Clone, Debug)]
355pub struct Chaser {
356 last_search_tgt: Option<Vec3<f32>>,
357 route: Option<(Route, bool)>,
359 astar: Option<Astar<Node, FxBuildHasher>>,
364}
365
366impl Chaser {
367 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 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 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 None
427 };
428
429 if let Some((bearing, speed)) = bearing {
431 Some((bearing, speed))
432 } else {
433 let tgt_dir = (tgt - pos).xy().try_normalized().unwrap_or_default();
437
438 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 let (path, complete) = {
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 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 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 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 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
549fn 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 (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 + b.last_dir_count as f32 * 0.01
595 + (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), Vec3::new(0, 1, 1), Vec3::new(0, 1, -1), Vec3::new(0, 1, -2), Vec3::new(1, 0, 0), Vec3::new(1, 0, 1), Vec3::new(1, 0, -1), Vec3::new(1, 0, -2), Vec3::new(0, -1, 0), Vec3::new(0, -1, 1), Vec3::new(0, -1, -1), Vec3::new(0, -1, -2), Vec3::new(-1, 0, 0), Vec3::new(-1, 0, 1), Vec3::new(-1, 0, -1), Vec3::new(-1, 0, -2), Vec3::new(0, 0, -1), ];
620
621 const JUMPS: [Vec3<i32>; 4] = [
622 Vec3::new(0, 1, 2), Vec3::new(1, 0, 2), Vec3::new(0, -1, 2), Vec3::new(-1, 0, 2), ];
627
628 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 };
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 50_000
708 } else {
709 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 *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#[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 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 {
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 };
781 informed_rrt_connect(vol, startf, endf, is_traversable, radius)
782 }
783}
784
785#[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 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 let mut parents1 = HashMap::new();
818 let mut parents2 = HashMap::new();
819
820 let mut path1 = Vec::new();
823 let mut path2 = Vec::new();
824
825 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 kdtree1.add(&[startf.x, startf.y, startf.z], node_index1);
831 nodes1.push(startf);
832 node_index1 += 1;
833
834 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 let mut search_parameter = 0.01;
847
848 for _i in 0..MAX_POINTS {
850 if connect {
851 break;
852 }
853
854 let (sampled_point1, sampled_point2) = {
856 let point = point_on_prolate_spheroid(startf, endf, search_parameter);
857 (point, point)
858 };
859
860 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 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 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 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 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 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 search_parameter += 0.02;
933 }
934
935 if connect {
936 let mut current_node_index1 = connection1_idx;
938 while current_node_index1 > 0 {
939 current_node_index1 = *parents1.get(¤t_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(¤t_node_index2).unwrap_or(&0);
945 path2.push(nodes2[current_node_index2].map(|e| e.floor() as i32));
946 }
947 path1.pop();
949 path1.reverse();
950 path.append(&mut path1);
951 path.append(&mut path2);
952 path.dedup();
953 } else {
954 let mut current_node_index1 = kdtree1
957 .nearest_one::<SquaredEuclidean>(&[endf.x, endf.y, endf.z])
958 .item;
959 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 while current_node_index1 != 0 && nodes1[current_node_index1].distance_squared(startf) > 4.0
976 {
977 current_node_index1 = *parents1.get(¤t_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#[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 let range = Uniform::from(0.0..1.0);
1037
1038 let midpoint = 0.5 * (focus1 + focus2);
1040 let radius: f32 = focus1.distance(focus2);
1042 let linear_eccentricity: f32 = 0.5 * radius;
1046
1047 let c: f32 = linear_eccentricity + search_parameter;
1054 let a: f32 = (c.powi(2) - linear_eccentricity.powi(2)).powf(0.5);
1057 let b: f32 = a;
1059
1060 let rtheta: f32 = PI * range.sample(&mut rng) - 0.5 * PI;
1072 let lambda: f32 = 2.0 * PI * range.sample(&mut rng);
1073 let point = Vec3::new(
1075 a * rtheta.cos() * lambda.cos(),
1076 b * rtheta.cos() * lambda.sin(),
1077 c * rtheta.sin(),
1078 );
1079 let dz = focus2.z - focus1.z;
1097 let theta: f32 = if radius > 0.0 {
1114 (dz / radius).acos()
1115 } else {
1116 0.0
1117 };
1118 let r_vec = focus2 - focus1;
1120 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 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 midpoint + rot_2_mat * point
1142}