From 7ddc2297280ed276d6cfe3b5e8edf8892a0e8b46 Mon Sep 17 00:00:00 2001 From: James Melkonian Date: Wed, 4 Aug 2021 23:25:29 -0700 Subject: [PATCH 1/4] Initial RRT flight pathfinding --- Cargo.lock | 1 + common/Cargo.toml | 1 + common/src/path.rs | 419 +++++++++++++++++++++++++++++++++++----- server/src/sys/agent.rs | 20 ++ 4 files changed, 395 insertions(+), 46 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index af7f0a0bd0..4bbd825ed2 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -5873,6 +5873,7 @@ dependencies = [ "fxhash", "hashbrown 0.11.2", "indexmap", + "kiddo", "lazy_static", "num-derive", "num-traits", diff --git a/common/Cargo.toml b/common/Cargo.toml index fed28470a3..f97c401f17 100644 --- a/common/Cargo.toml +++ b/common/Cargo.toml @@ -25,6 +25,7 @@ serde = { version = "1.0.110", features = ["derive", "rc"] } # Util enum-iterator = "0.6" vek = { version = "=0.14.1", features = ["serde"] } +kiddo = "0.1" # Strum strum = { version = "0.21", features = ["derive"] } diff --git a/common/src/path.rs b/common/src/path.rs index 6c6657cecd..3530971af1 100644 --- a/common/src/path.rs +++ b/common/src/path.rs @@ -4,9 +4,11 @@ use crate::{ vol::{BaseVol, ReadVol}, }; use common_base::span; -use hashbrown::hash_map::DefaultHashBuilder; -use rand::prelude::*; -use std::iter::FromIterator; +use hashbrown::{hash_map::DefaultHashBuilder, HashMap}; +use rand::{prelude::IteratorRandom, thread_rng, Rng, distributions::{Distribution, Uniform}}; +use std::{f32::consts::PI, iter::FromIterator}; +use kiddo::{distance::squared_euclidean, KdTree}; +use tracing::warn; use vek::*; // Path @@ -81,6 +83,8 @@ pub struct TraversalConfig { pub can_climb: bool, /// Whether the agent can fly. pub can_fly: bool, + /// Testing for rrt pathing + pub rrt_test: bool, } const DIAGONALS: [Vec2; 8] = [ @@ -123,24 +127,34 @@ impl Route { let next1 = self.next(1).unwrap_or(next0); // Stop using obstructed paths - if !walkable(vol, next1) { + if !walkable(vol, next1, traversal_cfg) { return None; } - let be_precise = DIAGONALS.iter().any(|pos| { + let be_precise = if traversal_cfg.can_fly { + false + } else { + DIAGONALS.iter().any(|pos| { (-1..2).all(|z| { vol.get(next0 + Vec3::new(pos.x, pos.y, z)) .map(|b| !b.is_solid()) .unwrap_or(false) }) - }); + })}; - let next_tgt = next0.map(|e| e as f32) + Vec3::new(0.5, 0.5, 0.0); + let next_tgt = if traversal_cfg.can_fly { + next0.map(|e| e as f32) + Vec3::new(0.5, 0.5, 0.5) + } else { + next0.map(|e| e as f32) + Vec3::new(0.5, 0.5, 0.0) + }; let closest_tgt = next_tgt.map2(pos, |tgt, pos| pos.clamped(tgt.floor(), tgt.ceil())); // Determine whether we're close enough to the next to to consider it completed let dist_sqrd = pos.xy().distance_squared(closest_tgt.xy()); - if dist_sqrd + // FIXME use PID controller to actually hit nodes + if traversal_cfg.can_fly && dist_sqrd < 2.0 + || (dist_sqrd + // FIXME: Clean up magic numbers < traversal_cfg.node_tolerance.powi(2) * if be_precise { 0.25 } else { 1.0 } && (((pos.z - closest_tgt.z > 1.2 || (pos.z - closest_tgt.z > -0.2 && traversal_cfg.on_ground)) && (pos.z - closest_tgt.z < 1.2 || (pos.z - closest_tgt.z < 2.9 && vel.z < -0.05)) @@ -155,7 +169,7 @@ impl Route { && self.next_idx < self.path.len()) || (traversal_cfg.in_liquid && pos.z < closest_tgt.z + 0.8 - && pos.z > closest_tgt.z)) + && pos.z > closest_tgt.z))) { // Node completed, move on to the next one self.next_idx += 1; @@ -304,20 +318,27 @@ impl Route { } else { 0.5 }); - let tgt = if be_precise { + let tgt = if be_precise || traversal_cfg.can_fly { next_tgt } else { Vec3::from(tgt2d) + Vec3::unit_z() * next_tgt.z }; - Some(( - tgt - pos, - // Control the entity's speed to hopefully stop us falling off walls on sharp corners. - // This code is very imperfect: it does its best but it can still fail for particularly - // fast entities. - straight_factor * traversal_cfg.slow_factor + (1.0 - traversal_cfg.slow_factor), - )) - .filter(|(bearing, _)| bearing.z < 2.1) + if traversal_cfg.can_fly { + Some(( + tgt - pos, + 1.0, + )) + } else { + Some(( + tgt - pos, + // Control the entity's speed to hopefully stop us falling off walls on sharp corners. + // This code is very imperfect: it does its best but it can still fail for particularly + // fast entities. + straight_factor * traversal_cfg.slow_factor + (1.0 - traversal_cfg.slow_factor), + )) + .filter(|(bearing, _)| bearing.z < 2.1) + } } } @@ -336,6 +357,9 @@ pub struct Chaser { } impl Chaser { + /// Returns bearing and speed + /// Bearing is a Vec3 dictating the direction of movement + /// Speed is an f32 between 0.0 and 1.0 pub fn chase( &mut self, vol: &V, @@ -386,6 +410,9 @@ impl Chaser { .and_then(|(r, _)| r.traverse(vol, pos, vel, &traversal_cfg)) } } else { + if traversal_cfg.can_fly { + warn!("I think no route?"); + } None }; @@ -406,7 +433,12 @@ impl Chaser { { self.last_search_tgt = Some(tgt); - let (path, complete) = find_path(&mut self.astar, vol, pos, tgt, &traversal_cfg); + let (path, complete) = if traversal_cfg.can_fly && traversal_cfg.rrt_test { + find_air_path(vol, pos, tgt, &traversal_cfg) + } else { + find_path(&mut self.astar, vol, pos, tgt, &traversal_cfg) + }; + //let (path, complete) = find_path(&mut self.astar, vol, pos, tgt, &traversal_cfg); self.route = path.map(|path| { let start_index = path @@ -430,17 +462,59 @@ impl Chaser { }); } - let walking_towards_edge = (-3..2).all(|z| { - vol.get( - (pos + Vec3::::from(tgt_dir) * 2.5).map(|e| e as i32) + Vec3::unit_z() * z, - ) - .map(|b| b.is_air()) - .unwrap_or(false) - }); + //let walking_towards_edge = (-3..2).all(|z| { + // vol.get( + // (pos + Vec3::::from(tgt_dir) * 2.5).map(|e| e as i32) + Vec3::unit_z() * z, + // ) + // .map(|b| b.is_air()) + // .unwrap_or(false) + //}); + + //if traversal_cfg.can_fly { + // Some(((tgt - pos) , 1.0)) + //} else if !walking_towards_edge { + // Some(((tgt - pos) * Vec3::new(1.0, 1.0, 0.0), 1.0)) + //} else { + //warn!("Hopelessly lost in the world, with no where to go"); + // None + //} + if let Some(bearing) = self.route.as_mut().and_then(|(r, _)| r.traverse(vol, pos, vel, &traversal_cfg)) { + if traversal_cfg.can_fly { + warn!("spin?"); + } + Some(bearing) - if !walking_towards_edge || traversal_cfg.can_fly { - Some(((tgt - pos) * Vec3::new(1.0, 1.0, 0.0), 1.0)) } else { + if traversal_cfg.can_fly { + warn!("welp"); + let (path, complete) = if traversal_cfg.can_fly && traversal_cfg.rrt_test { + find_air_path(vol, pos, tgt, &traversal_cfg) + } else { + find_path(&mut self.astar, vol, pos, tgt, &traversal_cfg) + }; + //let (path, complete) = find_path(&mut self.astar, vol, pos, tgt, &traversal_cfg); + + self.route = path.map(|path| { + let start_index = path + .iter() + .enumerate() + .min_by_key(|(_, node)| { + node.xy() + .map(|e| e as f32) + .distance_squared(pos.xy() + tgt_dir) + as i32 + }) + .map(|(idx, _)| idx); + + ( + Route { + path, + next_idx: start_index.unwrap_or(0), + }, + complete, + ) + }); + } None } } @@ -448,25 +522,29 @@ impl Chaser { } #[allow(clippy::float_cmp)] // TODO: Pending review in #587 -fn walkable(vol: &V, pos: Vec3) -> bool +fn walkable(vol: &V, pos: Vec3, traversal_cfg: &TraversalConfig) -> bool where V: BaseVol + ReadVol, { - let below = vol - .get(pos - Vec3::unit_z()) - .ok() - .copied() - .unwrap_or_else(Block::empty); - let a = vol.get(pos).ok().copied().unwrap_or_else(Block::empty); - let b = vol - .get(pos + Vec3::unit_z()) - .ok() - .copied() - .unwrap_or_else(Block::empty); + if traversal_cfg.can_fly { + vol.get(pos).ok().copied().unwrap_or_else(Block::empty).is_fluid() + } else { + let below = vol + .get(pos - Vec3::unit_z()) + .ok() + .copied() + .unwrap_or_else(Block::empty); + let a = vol.get(pos).ok().copied().unwrap_or_else(Block::empty); + let b = vol + .get(pos + Vec3::unit_z()) + .ok() + .copied() + .unwrap_or_else(Block::empty); - let on_ground = below.is_filled(); - let in_liquid = a.is_liquid(); - (on_ground || in_liquid) && !a.is_solid() && !b.is_solid() + let on_ground = below.is_filled(); + let in_liquid = a.is_liquid(); + (on_ground || in_liquid) && !a.is_solid() && !b.is_solid() + } } /// Attempt to search for a path to a target, returning the path (if one was @@ -481,7 +559,12 @@ fn find_path( where V: BaseVol + ReadVol, { - let is_walkable = |pos: &Vec3| walkable(vol, *pos); + let is_walkable = |pos: &Vec3| walkable(vol, *pos, traversal_cfg); + //let is_walkable = |pos: &Vec3| if traversal_cfg.can_fly && traversal_cfg.rrt_test { + // vol.get(*pos).ok().copied().unwrap_or_else(Block::empty).is_fluid() + //} else { + // walkable(vol, *pos) + //}; let get_walkable_z = |pos| { let mut z_incr = 0; for _ in 0..32 { @@ -558,14 +641,14 @@ where .map(|b| !b.is_liquid()) .unwrap_or(true) || traversal_cfg.can_climb - || traversal_cfg.can_fly + //|| traversal_cfg.can_fly }) .into_iter() .flatten(), ) .map(move |dir| (pos, dir)) .filter(move |(pos, dir)| { - (traversal_cfg.can_fly || is_walkable(pos) && is_walkable(&(*pos + **dir))) + (/*traversal_cfg.can_fly || */is_walkable(pos) && is_walkable(&(*pos + **dir))) && ((dir.z < 1 || vol .get(pos + Vec3::unit_z() * 2) @@ -631,3 +714,247 @@ where PathResult::Pending => (None, false), } } + +fn find_air_path( + vol: &V, + startf: Vec3, + endf: Vec3, + traversal_cfg: &TraversalConfig, +) -> (Option>>, bool) +where + V: BaseVol + ReadVol, +{ + let radius = 0.9; + let is_traversable = |start: &Vec3, end: &Vec3| { + vol + .ray(*start, *end) + .until(Block::is_solid) + .cast() + .0.powi(2) + > (*start).distance_squared(*end) + //vol.get(*pos).ok().copied().unwrap_or_else(Block::empty).is_fluid(); + }; + + let mut node_index1: usize = 0; + let mut node_index2: usize = 0; + + let mut nodes1 = Vec::new(); + let mut parents1 = HashMap::new(); + let mut path1 = Vec::new(); + let mut kdtree1 = KdTree::new(); + kdtree1.add(&[startf.x, startf.y, startf.z], node_index1).unwrap(); + nodes1.push(startf); + node_index1 += 1; + + let mut nodes2 = Vec::new(); + let mut parents2 = HashMap::new(); + let mut path2 = Vec::new(); + let mut kdtree2 = KdTree::new(); + kdtree2.add(&[endf.x, endf.y, endf.z], node_index2).unwrap(); + nodes2.push(endf); + node_index2 += 1; + + let mut connect = false; + let mut connection1_idx = 0; + let mut connection2_idx = 0; + + let mut search_parameter = 0.01; + + for _i in 0..7000 { + if connect { + break; + } + let (sampled_point1, sampled_point2) = { + let point = point_in_prolate_spheroid(startf, endf, search_parameter); + (point, point) + }; + + let nearest_index1 = *kdtree1.nearest_one(&[sampled_point1.x, sampled_point1.y, sampled_point1.z], &squared_euclidean).unwrap().1 as usize; + let nearest_index2 = *kdtree2.nearest_one(&[sampled_point2.x, sampled_point2.y, sampled_point2.z], &squared_euclidean).unwrap().1 as usize; + + let nearest1 = nodes1[nearest_index1]; + let nearest2 = nodes2[nearest_index2]; + let new_point1 = nearest1 + + (sampled_point1 - nearest1) + .normalized() + .map(|a| a * radius); + let new_point2 = nearest2 + + (sampled_point2 - nearest2) + .normalized() + .map(|a| a * radius); + + if is_traversable(&nearest1, &new_point1) { + kdtree1.add(&[new_point1.x, new_point1.y, new_point1.z], node_index1).unwrap(); + nodes1.push(new_point1); + parents1.insert(node_index1, nearest_index1); + node_index1 += 1; + let (check, index) = kdtree2.nearest_one(&[new_point1.x, new_point1.y, new_point1.z], &squared_euclidean).unwrap(); + if check < radius { + let connection = nodes2[*index]; + connection2_idx = *index; + nodes1.push(connection); + connection1_idx = nodes1.len() - 1; + parents1.insert(node_index1, node_index1 - 1); + connect = true; + } + } + + if is_traversable(&nearest2, &new_point2) { + kdtree2.add(&[new_point2.x, new_point2.y, new_point1.z], node_index2).unwrap(); + nodes2.push(new_point2); + parents2.insert(node_index2, nearest_index2); + node_index2 += 1; + let (check, index) = kdtree1.nearest_one(&[new_point2.x, new_point2.y, new_point1.z], &squared_euclidean).unwrap(); + if check < radius { + let connection = nodes1[*index]; + connection1_idx = *index; + nodes2.push(connection); + connection2_idx = nodes2.len() - 1; + parents2.insert(node_index2, node_index2 - 1); + connect = true; + } + } + search_parameter += 0.02; + } + + let mut path = Vec::new(); + if connect { + let mut current_node_index1 = connection1_idx; + while current_node_index1 > 0 { + current_node_index1 = *parents1.get(¤t_node_index1).unwrap(); + path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32)); + } + let mut current_node_index2 = connection2_idx; + while current_node_index2 > 0 { + current_node_index2 = *parents2.get(¤t_node_index2).unwrap(); + path2.push(nodes2[current_node_index2].map(|e| e.floor() as i32)); + } + path1.reverse(); + path.append(&mut path1); + path.append(&mut path2); + path.dedup(); + } else { + let mut current_node_index1 = kdtree1.nearest_one(&[endf.x, endf.y, endf.z], &squared_euclidean).unwrap().1; + for _i in 0..3 { + if *current_node_index1 == 0 || nodes1[*current_node_index1].distance_squared(startf) < 4.0 { + current_node_index1 = parents1.values().choose(&mut thread_rng()).unwrap(); + } else { + break; + } + } + path1.push(nodes1[*current_node_index1].map(|e| e.floor() as i32)); + while *current_node_index1 != 0 && nodes1[*current_node_index1].distance_squared(startf) > 4.0 { + current_node_index1 = parents1.get(¤t_node_index1).unwrap(); + path1.push(nodes1[*current_node_index1].map(|e| e.floor() as i32)); + } + + path1.reverse(); + path.append(&mut path1); + } + println!("path: {:?}", path); + (Some(path.into_iter().collect()), connect) +} + +/// Returns a random point within a radially symmetrical ellipsoid with given foci +/// and a `search parameter` to determine the size of the ellipse beyond the foci. +/// Technically the point is within a prolate spheroid translated and rotated to the +/// proper place in cartesian space. +/// The search_parameter is a float that relates to the length of the string for +/// a two dimensional ellipse or the size of the ellipse beyond the foci. In this +/// case that analogy still holds as the ellipse is radially symmetrical along the +/// axis between the foci. The value of the search parameter must be greater than zero. +/// In order to increase the sample area, the search_parameter should be increased +/// linearly as the search continues. +pub fn point_in_prolate_spheroid(focus1: Vec3, focus2: Vec3, search_parameter: f32) -> Vec3 { + + let mut rng = thread_rng(); + // Uniform distribution + let range = Uniform::from(0.0..1.0); + + // Midpoint is used as the local origin + let midpoint = 0.5 * (focus1 + focus2); + // Radius between the start and end of the path + let radius: f32 = focus1.distance(focus2); + // The linear eccentricity of an ellipse is the distance from the origin to a focus + // A prolate spheroid is a half-ellipse rotated for a full revolution which is why + // ellipse variables are used frequently in this function + let linear_eccentricity: f32 = 0.5 * radius; + + // For an ellipsoid, three variables determine the shape: a, b, and c. + // These are the distance from the center/origin to the surface on the + // x, y, and z axes, respectively. + // For a prolate spheroid a and b are equal. + // c is determined by adding the search parameter to the linear eccentricity. + // As the search parameter increases the size of the spheroid increases + let c: f32 = linear_eccentricity + search_parameter; + // The width is calculated to prioritize increasing width over length of + // the ellipsoid + let a: f32 = (c.powi(2) - linear_eccentricity.powi(2)).powf(0.5); + // The width should be the same in both the x and y directions + let b: f32 = a; + + // The parametric spherical equation for an ellipsoid measuring from the + // center point is as follows: + // x = a * cos(theta) * cos(lambda) + // y = b * cos(theta) * sin(lambda) + // z = c * sin(theta) + // + // where -0.5 * PI <= theta <= 0.5 * PI + // and 0.0 <= lambda < 2.0 * PI + // + // Select these two angles using the uniform distribution defined at the + // beginning of the function from 0.0 to 1.0 + let rtheta: f32 = PI * range.sample(&mut rng) - 0.5 * PI; + let lambda: f32 = 2.0 * PI * range.sample(&mut rng); + // Select a point on the surface of the ellipsoid + let point = Vec3::new(a * rtheta.cos() * lambda.cos(), b * rtheta.cos() * lambda.sin(), c * rtheta.sin()); + //let surface_point = Vec3::new(a * rtheta.cos() * lambda.cos(), b * rtheta.cos() * lambda.sin(), c * rtheta.sin()); + //let magnitude = surface_point.magnitude(); + //let direction = surface_point.normalized(); + //// Randomly select a point along the vector to the previously selected surface + //// point using the uniform distribution + //let point = magnitude * range.sample(&mut rng) * direction; + + + // Now that a point has been selected in local space, it must be rotated and + // translated into global coordinates + let dx = focus2.x - focus1.x; + let dy = focus2.y - focus1.y; + let dz = focus2.z - focus1.z; + // Phi and theta are the angles from the x axis in the x-y plane and from + // the z axis, respectively. (As found in spherical coordinates) + // These angles are used to rotate the random point in the spheroid about + // the local origin + // Rotate about z axis by phi + //let phi: f32 = if dx.abs() > 0.0 { + // (dy / dx).atan() + //} else { + // 0.5 * PI + //}; + // This is unnecessary as rtheta is randomly selected between 0.0 and 2.0 * PI + // let rot_z_mat = Mat3::new(phi.cos(), -1.0 * phi.sin(), 0.0, phi.sin(), phi.cos(), 0.0, 0.0, 0.0, 1.0); + + // rotate about perpendicular vector in the xy plane by theta + let theta: f32 = if radius > 0.0 { + (dz / radius).acos() + } else { + 0.0 + }; + // Vector from focus1 to focus2 + let r_vec = focus2 - focus1; + // Perpendicular vector in xy plane + let perp_vec = Vec3::new(-1.0 * r_vec.y, r_vec.x, 0.0).normalized(); + let l = perp_vec.x; + let m = perp_vec.y; + let n = perp_vec.z; + // Rotation matrix for rotation about a vector + let rot_2_mat = Mat3::new(l * l * (1.0 - theta.cos()), m * l * (1.0 - theta.cos()) - n * theta.sin(), n * l * (1.0 - theta.cos()) + m * theta.sin(), + l * m * (1.0 - theta.cos()) + n * theta.sin(), m * m * (1.0 - theta.cos()) + theta.cos(), n * m * (1.0 - theta.cos()) - l * theta.sin(), + l * n * (1.0 - theta.cos()) - m * theta.sin(), m * n * (1.0 - theta.cos()) + l * theta.sin(), n * n * (1.0 - theta.cos()) + theta.cos()); + + // get the global coordinates of the point by rotating and adding the origin + // rot_z_mat is unneeded due to the random rotation defined by lambda + // let global_coords = midpoint + rot_2_mat * (rot_z_mat * point); + let global_coords = midpoint + rot_2_mat * point; + global_coords +} diff --git a/server/src/sys/agent.rs b/server/src/sys/agent.rs index 9aaaaa94e9..8ea2254a21 100644 --- a/server/src/sys/agent.rs +++ b/server/src/sys/agent.rs @@ -305,6 +305,25 @@ impl<'a> System<'a> for Sys { // obstacles that smaller entities would not). let node_tolerance = scale * 1.5; let slow_factor = body.map_or(0.0, |b| b.base_accel() / 250.0).min(1.0); + let rrt_test = if let Some(target_info) = agent.target { + let Target { + target, hostile, .. + } = target_info; + if let Some(Alignment::Owned(uid)) = alignment { + if read_data.uids.get(target) == Some(&uid) { + controller + .actions + .push(ControlAction::basic_input(InputKind::Fly)); + true + } else { + false + } + } else { + false + } + } else { + false + }; let traversal_config = TraversalConfig { node_tolerance, slow_factor, @@ -313,6 +332,7 @@ impl<'a> System<'a> for Sys { min_tgt_dist: 1.0, can_climb: body.map_or(false, Body::can_climb), can_fly: body.map_or(false, |b| b.fly_thrust().is_some()), + rrt_test, }; let flees = alignment.map_or(true, |a| { From c2c442975062b38de2fa7d28f6035784562ab586 Mon Sep 17 00:00:00 2001 From: James Melkonian Date: Sun, 22 Aug 2021 21:03:14 -0700 Subject: [PATCH 2/4] Disable RRT pathfinding --- Cargo.lock | 1 - common/Cargo.toml | 3 +- common/src/path.rs | 652 +++++++++++++++++++++++----------------- server/src/sys/agent.rs | 20 -- 4 files changed, 379 insertions(+), 297 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 4bbd825ed2..af7f0a0bd0 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -5873,7 +5873,6 @@ dependencies = [ "fxhash", "hashbrown 0.11.2", "indexmap", - "kiddo", "lazy_static", "num-derive", "num-traits", diff --git a/common/Cargo.toml b/common/Cargo.toml index f97c401f17..e31ab8f47b 100644 --- a/common/Cargo.toml +++ b/common/Cargo.toml @@ -25,7 +25,8 @@ serde = { version = "1.0.110", features = ["derive", "rc"] } # Util enum-iterator = "0.6" vek = { version = "=0.14.1", features = ["serde"] } -kiddo = "0.1" +# Used for RRT pathfinding (disabled until flight controls are improved) +# kiddo = "0.1" # Strum strum = { version = "0.21", features = ["derive"] } diff --git a/common/src/path.rs b/common/src/path.rs index 3530971af1..bd93cde00c 100644 --- a/common/src/path.rs +++ b/common/src/path.rs @@ -4,11 +4,11 @@ use crate::{ vol::{BaseVol, ReadVol}, }; use common_base::span; -use hashbrown::{hash_map::DefaultHashBuilder, HashMap}; -use rand::{prelude::IteratorRandom, thread_rng, Rng, distributions::{Distribution, Uniform}}; -use std::{f32::consts::PI, iter::FromIterator}; -use kiddo::{distance::squared_euclidean, KdTree}; -use tracing::warn; +use hashbrown::hash_map::DefaultHashBuilder; +//use kiddo::{distance::squared_euclidean, KdTree}; // For RRT paths (disabled +// for now) +use rand::{thread_rng, Rng}; +use std::iter::FromIterator; use vek::*; // Path @@ -83,8 +83,6 @@ pub struct TraversalConfig { pub can_climb: bool, /// Whether the agent can fly. pub can_fly: bool, - /// Testing for rrt pathing - pub rrt_test: bool, } const DIAGONALS: [Vec2; 8] = [ @@ -127,34 +125,24 @@ impl Route { let next1 = self.next(1).unwrap_or(next0); // Stop using obstructed paths - if !walkable(vol, next1, traversal_cfg) { + if !walkable(vol, next1) { return None; } - let be_precise = if traversal_cfg.can_fly { - false - } else { - DIAGONALS.iter().any(|pos| { + let be_precise = DIAGONALS.iter().any(|pos| { (-1..2).all(|z| { vol.get(next0 + Vec3::new(pos.x, pos.y, z)) .map(|b| !b.is_solid()) .unwrap_or(false) }) - })}; + }); - let next_tgt = if traversal_cfg.can_fly { - next0.map(|e| e as f32) + Vec3::new(0.5, 0.5, 0.5) - } else { - next0.map(|e| e as f32) + Vec3::new(0.5, 0.5, 0.0) - }; + // Map position of node to middle of block + let next_tgt = next0.map(|e| e as f32) + Vec3::new(0.5, 0.5, 0.0); let closest_tgt = next_tgt.map2(pos, |tgt, pos| pos.clamped(tgt.floor(), tgt.ceil())); - // Determine whether we're close enough to the next to to consider it completed let dist_sqrd = pos.xy().distance_squared(closest_tgt.xy()); - // FIXME use PID controller to actually hit nodes - if traversal_cfg.can_fly && dist_sqrd < 2.0 - || (dist_sqrd - // FIXME: Clean up magic numbers + if dist_sqrd < traversal_cfg.node_tolerance.powi(2) * if be_precise { 0.25 } else { 1.0 } && (((pos.z - closest_tgt.z > 1.2 || (pos.z - closest_tgt.z > -0.2 && traversal_cfg.on_ground)) && (pos.z - closest_tgt.z < 1.2 || (pos.z - closest_tgt.z < 2.9 && vel.z < -0.05)) @@ -169,7 +157,7 @@ impl Route { && self.next_idx < self.path.len()) || (traversal_cfg.in_liquid && pos.z < closest_tgt.z + 0.8 - && pos.z > closest_tgt.z))) + && pos.z > closest_tgt.z)) { // Node completed, move on to the next one self.next_idx += 1; @@ -318,27 +306,20 @@ impl Route { } else { 0.5 }); - let tgt = if be_precise || traversal_cfg.can_fly { + let tgt = if be_precise { next_tgt } else { Vec3::from(tgt2d) + Vec3::unit_z() * next_tgt.z }; - if traversal_cfg.can_fly { - Some(( - tgt - pos, - 1.0, - )) - } else { - Some(( - tgt - pos, - // Control the entity's speed to hopefully stop us falling off walls on sharp corners. - // This code is very imperfect: it does its best but it can still fail for particularly - // fast entities. - straight_factor * traversal_cfg.slow_factor + (1.0 - traversal_cfg.slow_factor), - )) - .filter(|(bearing, _)| bearing.z < 2.1) - } + Some(( + tgt - pos, + // Control the entity's speed to hopefully stop us falling off walls on sharp + // corners. This code is very imperfect: it does its best but it + // can still fail for particularly fast entities. + straight_factor * traversal_cfg.slow_factor + (1.0 - traversal_cfg.slow_factor), + )) + .filter(|(bearing, _)| bearing.z < 2.1) } } @@ -410,15 +391,17 @@ impl Chaser { .and_then(|(r, _)| r.traverse(vol, pos, vel, &traversal_cfg)) } } else { - if traversal_cfg.can_fly { - warn!("I think no route?"); - } + // There is no route found yet None }; + // If a bearing has already been determined, use that if let Some((bearing, speed)) = bearing { Some((bearing, speed)) } else { + // Since no bearing has been determined yet, a new route will be + // calculated if the target has moved, pathfinding is not complete, + // or there is no route let tgt_dir = (tgt - pos).xy().try_normalized().unwrap_or_default(); // Only search for a path if the target has moved from their last position. We @@ -433,12 +416,12 @@ impl Chaser { { self.last_search_tgt = Some(tgt); - let (path, complete) = if traversal_cfg.can_fly && traversal_cfg.rrt_test { + let (path, complete) = /*if traversal_cfg.can_fly { find_air_path(vol, pos, tgt, &traversal_cfg) - } else { + } else */{ + // Enable air paths when air braking has been figured out find_path(&mut self.astar, vol, pos, tgt, &traversal_cfg) }; - //let (path, complete) = find_path(&mut self.astar, vol, pos, tgt, &traversal_cfg); self.route = path.map(|path| { let start_index = path @@ -461,90 +444,65 @@ impl Chaser { ) }); } - - //let walking_towards_edge = (-3..2).all(|z| { - // vol.get( - // (pos + Vec3::::from(tgt_dir) * 2.5).map(|e| e as i32) + Vec3::unit_z() * z, - // ) - // .map(|b| b.is_air()) - // .unwrap_or(false) - //}); - - //if traversal_cfg.can_fly { - // Some(((tgt - pos) , 1.0)) - //} else if !walking_towards_edge { - // Some(((tgt - pos) * Vec3::new(1.0, 1.0, 0.0), 1.0)) - //} else { - //warn!("Hopelessly lost in the world, with no where to go"); - // None - //} - if let Some(bearing) = self.route.as_mut().and_then(|(r, _)| r.traverse(vol, pos, vel, &traversal_cfg)) { - if traversal_cfg.can_fly { - warn!("spin?"); - } + // Start traversing the new route if it exists + if let Some(bearing) = self + .route + .as_mut() + .and_then(|(r, _)| r.traverse(vol, pos, vel, &traversal_cfg)) + { Some(bearing) - } else { - if traversal_cfg.can_fly { - warn!("welp"); - let (path, complete) = if traversal_cfg.can_fly && traversal_cfg.rrt_test { - find_air_path(vol, pos, tgt, &traversal_cfg) - } else { - find_path(&mut self.astar, vol, pos, tgt, &traversal_cfg) - }; - //let (path, complete) = find_path(&mut self.astar, vol, pos, tgt, &traversal_cfg); + // At this point no route is available and no bearing + // has been determined, so we start sampling terrain. + // Check for falling off walls and try moving straight + // towards the target if falling is not a danger + let walking_towards_edge = (-3..2).all(|z| { + vol.get( + (pos + Vec3::::from(tgt_dir) * 2.5).map(|e| e as i32) + + Vec3::unit_z() * z, + ) + .map(|b| b.is_air()) + .unwrap_or(false) + }); - self.route = path.map(|path| { - let start_index = path - .iter() - .enumerate() - .min_by_key(|(_, node)| { - node.xy() - .map(|e| e as f32) - .distance_squared(pos.xy() + tgt_dir) - as i32 - }) - .map(|(idx, _)| idx); - - ( - Route { - path, - next_idx: start_index.unwrap_or(0), - }, - complete, - ) - }); + // Enable when airbraking/flight is figured out + /*if traversal_cfg.can_fly { + Some(((tgt - pos) , 1.0)) + } else */ + if !walking_towards_edge || traversal_cfg.can_fly { + Some(((tgt - pos) * Vec3::new(1.0, 1.0, 0.0), 1.0)) + } else { + // This is unfortunately where an NPC will stare blankly + // into space. No route has been found and no temporary + // bearing would suffice. Hopefully a route will be found + // in the coming ticks. + None } - None } } } } #[allow(clippy::float_cmp)] // TODO: Pending review in #587 -fn walkable(vol: &V, pos: Vec3, traversal_cfg: &TraversalConfig) -> bool +fn walkable(vol: &V, pos: Vec3) -> bool where V: BaseVol + ReadVol, { - if traversal_cfg.can_fly { - vol.get(pos).ok().copied().unwrap_or_else(Block::empty).is_fluid() - } else { - let below = vol - .get(pos - Vec3::unit_z()) - .ok() - .copied() - .unwrap_or_else(Block::empty); - let a = vol.get(pos).ok().copied().unwrap_or_else(Block::empty); - let b = vol - .get(pos + Vec3::unit_z()) - .ok() - .copied() - .unwrap_or_else(Block::empty); + let below = vol + .get(pos - Vec3::unit_z()) + .ok() + .copied() + .unwrap_or_else(Block::empty); + let a = vol.get(pos).ok().copied().unwrap_or_else(Block::empty); + let b = vol + .get(pos + Vec3::unit_z()) + .ok() + .copied() + .unwrap_or_else(Block::empty); - let on_ground = below.is_filled(); - let in_liquid = a.is_liquid(); - (on_ground || in_liquid) && !a.is_solid() && !b.is_solid() - } + let on_ground = below.is_filled(); + let in_liquid = a.is_liquid(); + (on_ground || in_liquid) && !a.is_solid() && !b.is_solid() } /// Attempt to search for a path to a target, returning the path (if one was @@ -559,12 +517,7 @@ fn find_path( where V: BaseVol + ReadVol, { - let is_walkable = |pos: &Vec3| walkable(vol, *pos, traversal_cfg); - //let is_walkable = |pos: &Vec3| if traversal_cfg.can_fly && traversal_cfg.rrt_test { - // vol.get(*pos).ok().copied().unwrap_or_else(Block::empty).is_fluid() - //} else { - // walkable(vol, *pos) - //}; + let is_walkable = |pos: &Vec3| walkable(vol, *pos); let get_walkable_z = |pos| { let mut z_incr = 0; for _ in 0..32 { @@ -641,14 +594,14 @@ where .map(|b| !b.is_liquid()) .unwrap_or(true) || traversal_cfg.can_climb - //|| traversal_cfg.can_fly + || traversal_cfg.can_fly }) .into_iter() .flatten(), ) .map(move |dir| (pos, dir)) .filter(move |(pos, dir)| { - (/*traversal_cfg.can_fly || */is_walkable(pos) && is_walkable(&(*pos + **dir))) + (traversal_cfg.can_fly || is_walkable(pos) && is_walkable(&(*pos + **dir))) && ((dir.z < 1 || vol .get(pos + Vec3::unit_z() * 2) @@ -715,6 +668,18 @@ where } } +// Enable when airbraking/sensible flight is a thing +/* +/// Attempts to find a path from a start to the end using an informed +/// RRT-Connect algorithm. A point is sampled from a bounding spheroid +/// between the start and end. Two separate rapidly exploring random +/// trees extend toward the sampled point. Nodes are stored in k-d trees +/// for quicker nearest node calculations. Points are sampled until the +/// trees connect. A final path is then reconstructed from the nodes. +/// This pathfinding algorithm is more appropriate for 3D pathfinding +/// with wider gaps, such as flying through a forest than for terrain +/// with narrow gaps, such as navigating a maze. +/// Returns a path and whether that path is complete or not. fn find_air_path( vol: &V, startf: Vec3, @@ -724,149 +689,267 @@ fn find_air_path( where V: BaseVol + ReadVol, { - let radius = 0.9; - let is_traversable = |start: &Vec3, end: &Vec3| { - vol - .ray(*start, *end) - .until(Block::is_solid) - .cast() - .0.powi(2) - > (*start).distance_squared(*end) - //vol.get(*pos).ok().copied().unwrap_or_else(Block::empty).is_fluid(); - }; - - let mut node_index1: usize = 0; - let mut node_index2: usize = 0; - - let mut nodes1 = Vec::new(); - let mut parents1 = HashMap::new(); - let mut path1 = Vec::new(); - let mut kdtree1 = KdTree::new(); - kdtree1.add(&[startf.x, startf.y, startf.z], node_index1).unwrap(); - nodes1.push(startf); - node_index1 += 1; - - let mut nodes2 = Vec::new(); - let mut parents2 = HashMap::new(); - let mut path2 = Vec::new(); - let mut kdtree2 = KdTree::new(); - kdtree2.add(&[endf.x, endf.y, endf.z], node_index2).unwrap(); - nodes2.push(endf); - node_index2 += 1; - - let mut connect = false; - let mut connection1_idx = 0; - let mut connection2_idx = 0; - - let mut search_parameter = 0.01; - - for _i in 0..7000 { - if connect { - break; - } - let (sampled_point1, sampled_point2) = { - let point = point_in_prolate_spheroid(startf, endf, search_parameter); - (point, point) - }; - - let nearest_index1 = *kdtree1.nearest_one(&[sampled_point1.x, sampled_point1.y, sampled_point1.z], &squared_euclidean).unwrap().1 as usize; - let nearest_index2 = *kdtree2.nearest_one(&[sampled_point2.x, sampled_point2.y, sampled_point2.z], &squared_euclidean).unwrap().1 as usize; - - let nearest1 = nodes1[nearest_index1]; - let nearest2 = nodes2[nearest_index2]; - let new_point1 = nearest1 - + (sampled_point1 - nearest1) - .normalized() - .map(|a| a * radius); - let new_point2 = nearest2 - + (sampled_point2 - nearest2) - .normalized() - .map(|a| a * radius); - - if is_traversable(&nearest1, &new_point1) { - kdtree1.add(&[new_point1.x, new_point1.y, new_point1.z], node_index1).unwrap(); - nodes1.push(new_point1); - parents1.insert(node_index1, nearest_index1); - node_index1 += 1; - let (check, index) = kdtree2.nearest_one(&[new_point1.x, new_point1.y, new_point1.z], &squared_euclidean).unwrap(); - if check < radius { - let connection = nodes2[*index]; - connection2_idx = *index; - nodes1.push(connection); - connection1_idx = nodes1.len() - 1; - parents1.insert(node_index1, node_index1 - 1); - connect = true; - } - } - - if is_traversable(&nearest2, &new_point2) { - kdtree2.add(&[new_point2.x, new_point2.y, new_point1.z], node_index2).unwrap(); - nodes2.push(new_point2); - parents2.insert(node_index2, nearest_index2); - node_index2 += 1; - let (check, index) = kdtree1.nearest_one(&[new_point2.x, new_point2.y, new_point1.z], &squared_euclidean).unwrap(); - if check < radius { - let connection = nodes1[*index]; - connection1_idx = *index; - nodes2.push(connection); - connection2_idx = nodes2.len() - 1; - parents2.insert(node_index2, node_index2 - 1); - connect = true; - } - } - search_parameter += 0.02; - } - + let radius = traversal_cfg.node_tolerance; let mut path = Vec::new(); - if connect { - let mut current_node_index1 = connection1_idx; - while current_node_index1 > 0 { - current_node_index1 = *parents1.get(¤t_node_index1).unwrap(); - path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32)); - } - let mut current_node_index2 = connection2_idx; - while current_node_index2 > 0 { - current_node_index2 = *parents2.get(¤t_node_index2).unwrap(); - path2.push(nodes2[current_node_index2].map(|e| e.floor() as i32)); - } - path1.reverse(); - path.append(&mut path1); - path.append(&mut path2); - path.dedup(); + let mut connect = false; + let total_dist_sqrd = startf.distance_squared(endf); + // First check if a straight line path works + if vol + .ray(startf + Vec3::unit_z(), endf + Vec3::unit_z()) + .until(Block::is_opaque) + .cast() + .0.powi(2) + >= total_dist_sqrd { + //let step = (endf - startf).normalized().map(|a| a * radius); + //let mut node: Vec3; + //// Maximum of 500 steps + //for i in 1..500 { + // node = startf + step.map(|s| s * i as f32); + // path.push(endf.map(|e| e.floor() as i32)); + // if node.distance_squared(endf) < radius{ + // connect = true; + // break; + // } + //} + path.push(endf.map(|e| e.floor() as i32)); + connect = true; + // Else use RRTs } else { - let mut current_node_index1 = kdtree1.nearest_one(&[endf.x, endf.y, endf.z], &squared_euclidean).unwrap().1; - for _i in 0..3 { - if *current_node_index1 == 0 || nodes1[*current_node_index1].distance_squared(startf) < 4.0 { - current_node_index1 = parents1.values().choose(&mut thread_rng()).unwrap(); - } else { + let is_traversable = |start: &Vec3, end: &Vec3| { + vol.ray(*start, *end) + .until(Block::is_solid) + .cast() + .0 + .powi(2) + > (*start).distance_squared(*end) + //vol.get(*pos).ok().copied().unwrap_or_else(Block::empty).is_fluid(); + }; + let mut node_index1: usize = 0; + let mut node_index2: usize = 0; + + // Each tree has a vector of nodes + let mut nodes1 = Vec::new(); + let mut nodes2 = Vec::new(); + + // The parents hashmap stores nodes and their parent nodes as pairs to + // retrace the complete path once the two RRTs connect + let mut parents1 = HashMap::new(); + let mut parents2 = HashMap::new(); + + // The path vector stores the path from the appropriate terminal to the + // connecting node or vice versa + let mut path1 = Vec::new(); + let mut path2 = Vec::new(); + + // K-d trees are used to find the closest nodes rapidly + let mut kdtree1 = KdTree::new(); + let mut kdtree2 = KdTree::new(); + + // Add the start as the first node of the first k-d tree + kdtree1 + .add(&[startf.x, startf.y, startf.z], node_index1) + .unwrap_or_default(); + nodes1.push(startf); + node_index1 += 1; + + // Add the end as the first node of the second k-d tree + kdtree2 + .add(&[endf.x, endf.y, endf.z], node_index2) + .unwrap_or_default(); + nodes2.push(endf); + node_index2 += 1; + + let mut connection1_idx = 0; + let mut connection2_idx = 0; + + // Scalar non-dimensional value that is proportional to the size of the + // sample spheroid volume. This increases in value until a path is found. + let mut search_parameter = 0.01; + + // Maximum of 7000 iterations + for _i in 0..7000 { + if connect { break; } - } - path1.push(nodes1[*current_node_index1].map(|e| e.floor() as i32)); - while *current_node_index1 != 0 && nodes1[*current_node_index1].distance_squared(startf) > 4.0 { - current_node_index1 = parents1.get(¤t_node_index1).unwrap(); - path1.push(nodes1[*current_node_index1].map(|e| e.floor() as i32)); + + // Sample a point on the bounding spheroid + let (sampled_point1, sampled_point2) = { + let point = point_on_prolate_spheroid(startf, endf, search_parameter); + (point, point) + }; + + // Find the nearest nodes to the the sampled point + let nearest_index1 = kdtree1 + .nearest_one( + &[sampled_point1.x, sampled_point1.y, sampled_point1.z], + &squared_euclidean, + ) + .map_or(0, |n| *n.1); + let nearest_index2 = kdtree2 + .nearest_one( + &[sampled_point2.x, sampled_point2.y, sampled_point2.z], + &squared_euclidean, + ) + .map_or(0, |n| *n.1); + let nearest1 = nodes1[nearest_index1]; + let nearest2 = nodes2[nearest_index2]; + + // Extend toward the sampled point from the nearest node of each tree + let new_point1 = nearest1 + (sampled_point1 - nearest1).normalized().map(|a| a * radius); + let new_point2 = nearest2 + (sampled_point2 - nearest2).normalized().map(|a| a * radius); + + // Ensure the new nodes are valid/traversable + if is_traversable(&nearest1, &new_point1) { + kdtree1 + .add(&[new_point1.x, new_point1.y, new_point1.z], node_index1) + .unwrap_or_default(); + nodes1.push(new_point1); + parents1.insert(node_index1, nearest_index1); + node_index1 += 1; + // Check if the trees connect + if let Ok((check, index)) = kdtree2.nearest_one( + &[new_point1.x, new_point1.y, new_point1.z], + &squared_euclidean, + ) { + if check < radius { + let connection = nodes2[*index]; + connection2_idx = *index; + nodes1.push(connection); + connection1_idx = nodes1.len() - 1; + parents1.insert(node_index1, node_index1 - 1); + connect = true; + } + } + } + + // Repeat the validity check for the second tree + if is_traversable(&nearest2, &new_point2) { + kdtree2 + .add(&[new_point2.x, new_point2.y, new_point1.z], node_index2) + .unwrap_or_default(); + nodes2.push(new_point2); + parents2.insert(node_index2, nearest_index2); + node_index2 += 1; + // Again check for a connection + if let Ok((check, index)) = kdtree1.nearest_one( + &[new_point2.x, new_point2.y, new_point1.z], + &squared_euclidean, + ) { + if check < radius { + let connection = nodes1[*index]; + connection1_idx = *index; + nodes2.push(connection); + connection2_idx = nodes2.len() - 1; + parents2.insert(node_index2, node_index2 - 1); + connect = true; + } + } + } + // Increase the search parameter to widen the sample volume + search_parameter += 0.02; } - path1.reverse(); - path.append(&mut path1); + if connect { + // Construct paths from the connection node to the start and end + let mut current_node_index1 = connection1_idx; + while current_node_index1 > 0 { + current_node_index1 = *parents1.get(¤t_node_index1).unwrap_or(&0); + path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32)); + } + let mut current_node_index2 = connection2_idx; + while current_node_index2 > 0 { + current_node_index2 = *parents2.get(¤t_node_index2).unwrap_or(&0); + path2.push(nodes2[current_node_index2].map(|e| e.floor() as i32)); + } + // Join the two paths together in the proper order and remove duplicates + path1.pop(); + path1.reverse(); + path.append(&mut path1); + path.append(&mut path2); + path.dedup(); + } else { + // If the trees did not connect, construct a path from the start to + // the closest node to the end + let mut current_node_index1 = kdtree1 + .nearest_one(&[endf.x, endf.y, endf.z], &squared_euclidean) + .map_or(0, |c| *c.1); + // Attempt to pick a node other than the start node + for _i in 0..3 { + if current_node_index1 == 0 + || nodes1[current_node_index1].distance_squared(startf) < 4.0 + { + if let Some(index) = parents1.values().choose(&mut thread_rng()) { + current_node_index1 = *index; + } else { + break; + } + } else { + break; + } + } + path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32)); + // Construct the path + while current_node_index1 != 0 && nodes1[current_node_index1].distance_squared(startf) > 4.0 + { + current_node_index1 = *parents1.get(¤t_node_index1).unwrap_or(&0); + path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32)); + } + + path1.reverse(); + path.append(&mut path1); + } + let mut new_path = Vec::new(); + let mut node = path[0]; + new_path.push(node); + let mut node_idx = 0; + let num_nodes = path.len(); + let end = path[num_nodes - 1]; + while node != end { + let next_idx = if node_idx + 4 > num_nodes - 1 { + num_nodes - 1 + } else { + node_idx + 4 + }; + let next_node = path[next_idx]; + let start_pos = node.map(|e| e as f32 + 0.5); + let end_pos = next_node.map(|e| e as f32 + 0.5); + if vol.ray(start_pos, end_pos) + .until(Block::is_solid) + .cast() + .0 + .powi(2) + > (start_pos).distance_squared(end_pos) + { + node_idx = next_idx; + new_path.push(next_node); + } else { + node_idx += 1; + } + node = path[node_idx]; + } + path = new_path; } - println!("path: {:?}", path); (Some(path.into_iter().collect()), connect) } +*/ -/// Returns a random point within a radially symmetrical ellipsoid with given foci -/// and a `search parameter` to determine the size of the ellipse beyond the foci. -/// Technically the point is within a prolate spheroid translated and rotated to the -/// proper place in cartesian space. +/* +/// Returns a random point within a radially symmetrical ellipsoid with given +/// foci and a `search parameter` to determine the size of the ellipse beyond +/// the foci. Technically the point is within a prolate spheroid translated and +/// rotated to the proper place in cartesian space. /// The search_parameter is a float that relates to the length of the string for -/// a two dimensional ellipse or the size of the ellipse beyond the foci. In this -/// case that analogy still holds as the ellipse is radially symmetrical along the -/// axis between the foci. The value of the search parameter must be greater than zero. -/// In order to increase the sample area, the search_parameter should be increased -/// linearly as the search continues. -pub fn point_in_prolate_spheroid(focus1: Vec3, focus2: Vec3, search_parameter: f32) -> Vec3 { - +/// a two dimensional ellipse or the size of the ellipse beyond the foci. In +/// this case that analogy still holds as the ellipse is radially symmetrical +/// along the axis between the foci. The value of the search parameter must be +/// greater than zero. In order to increase the sample area, the +/// search_parameter should be increased linearly as the search continues. +#[allow(clippy::many_single_char_names)] +pub fn point_on_prolate_spheroid( + focus1: Vec3, + focus2: Vec3, + search_parameter: f32, +) -> Vec3 { let mut rng = thread_rng(); // Uniform distribution let range = Uniform::from(0.0..1.0); @@ -875,9 +958,9 @@ pub fn point_in_prolate_spheroid(focus1: Vec3, focus2: Vec3, search_pa let midpoint = 0.5 * (focus1 + focus2); // Radius between the start and end of the path let radius: f32 = focus1.distance(focus2); - // The linear eccentricity of an ellipse is the distance from the origin to a focus - // A prolate spheroid is a half-ellipse rotated for a full revolution which is why - // ellipse variables are used frequently in this function + // The linear eccentricity of an ellipse is the distance from the origin to a + // focus A prolate spheroid is a half-ellipse rotated for a full revolution + // which is why ellipse variables are used frequently in this function let linear_eccentricity: f32 = 0.5 * radius; // For an ellipsoid, three variables determine the shape: a, b, and c. @@ -901,30 +984,40 @@ pub fn point_in_prolate_spheroid(focus1: Vec3, focus2: Vec3, search_pa // // where -0.5 * PI <= theta <= 0.5 * PI // and 0.0 <= lambda < 2.0 * PI - // + // // Select these two angles using the uniform distribution defined at the // beginning of the function from 0.0 to 1.0 let rtheta: f32 = PI * range.sample(&mut rng) - 0.5 * PI; let lambda: f32 = 2.0 * PI * range.sample(&mut rng); // Select a point on the surface of the ellipsoid - let point = Vec3::new(a * rtheta.cos() * lambda.cos(), b * rtheta.cos() * lambda.sin(), c * rtheta.sin()); - //let surface_point = Vec3::new(a * rtheta.cos() * lambda.cos(), b * rtheta.cos() * lambda.sin(), c * rtheta.sin()); - //let magnitude = surface_point.magnitude(); - //let direction = surface_point.normalized(); + let point = Vec3::new( + a * rtheta.cos() * lambda.cos(), + b * rtheta.cos() * lambda.sin(), + c * rtheta.sin(), + ); + // NOTE: Theoretically we should sample a point within the spheroid + // requiring selecting a point along the radius. In my tests selecting + // a point *on the surface* of the spheroid results in sampling that is + // "good enough". The following code is commented out to reduce expense. + //let surface_point = Vec3::new(a * rtheta.cos() * lambda.cos(), b * + // rtheta.cos() * lambda.sin(), c * rtheta.sin()); let magnitude = + // surface_point.magnitude(); let direction = surface_point.normalized(); //// Randomly select a point along the vector to the previously selected surface //// point using the uniform distribution //let point = magnitude * range.sample(&mut rng) * direction; - // Now that a point has been selected in local space, it must be rotated and // translated into global coordinates - let dx = focus2.x - focus1.x; - let dy = focus2.y - focus1.y; + // NOTE: Don't rotate about the z axis as the point is already randomly + // selected about the z axis + //let dx = focus2.x - focus1.x; + //let dy = focus2.y - focus1.y; let dz = focus2.z - focus1.z; // Phi and theta are the angles from the x axis in the x-y plane and from // the z axis, respectively. (As found in spherical coordinates) // These angles are used to rotate the random point in the spheroid about // the local origin + // // Rotate about z axis by phi //let phi: f32 = if dx.abs() > 0.0 { // (dy / dx).atan() @@ -932,9 +1025,10 @@ pub fn point_in_prolate_spheroid(focus1: Vec3, focus2: Vec3, search_pa // 0.5 * PI //}; // This is unnecessary as rtheta is randomly selected between 0.0 and 2.0 * PI - // let rot_z_mat = Mat3::new(phi.cos(), -1.0 * phi.sin(), 0.0, phi.sin(), phi.cos(), 0.0, 0.0, 0.0, 1.0); + // let rot_z_mat = Mat3::new(phi.cos(), -1.0 * phi.sin(), 0.0, phi.sin(), + // phi.cos(), 0.0, 0.0, 0.0, 1.0); - // rotate about perpendicular vector in the xy plane by theta + // Rotate about perpendicular vector in the xy plane by theta let theta: f32 = if radius > 0.0 { (dz / radius).acos() } else { @@ -948,13 +1042,21 @@ pub fn point_in_prolate_spheroid(focus1: Vec3, focus2: Vec3, search_pa let m = perp_vec.y; let n = perp_vec.z; // Rotation matrix for rotation about a vector - let rot_2_mat = Mat3::new(l * l * (1.0 - theta.cos()), m * l * (1.0 - theta.cos()) - n * theta.sin(), n * l * (1.0 - theta.cos()) + m * theta.sin(), - l * m * (1.0 - theta.cos()) + n * theta.sin(), m * m * (1.0 - theta.cos()) + theta.cos(), n * m * (1.0 - theta.cos()) - l * theta.sin(), - l * n * (1.0 - theta.cos()) - m * theta.sin(), m * n * (1.0 - theta.cos()) + l * theta.sin(), n * n * (1.0 - theta.cos()) + theta.cos()); + let rot_2_mat = Mat3::new( + l * l * (1.0 - theta.cos()), + m * l * (1.0 - theta.cos()) - n * theta.sin(), + n * l * (1.0 - theta.cos()) + m * theta.sin(), + l * m * (1.0 - theta.cos()) + n * theta.sin(), + m * m * (1.0 - theta.cos()) + theta.cos(), + n * m * (1.0 - theta.cos()) - l * theta.sin(), + l * n * (1.0 - theta.cos()) - m * theta.sin(), + m * n * (1.0 - theta.cos()) + l * theta.sin(), + n * n * (1.0 - theta.cos()) + theta.cos(), + ); - // get the global coordinates of the point by rotating and adding the origin + // Get the global coordinates of the point by rotating and adding the origin // rot_z_mat is unneeded due to the random rotation defined by lambda // let global_coords = midpoint + rot_2_mat * (rot_z_mat * point); - let global_coords = midpoint + rot_2_mat * point; - global_coords + midpoint + rot_2_mat * point } +*/ diff --git a/server/src/sys/agent.rs b/server/src/sys/agent.rs index 8ea2254a21..9aaaaa94e9 100644 --- a/server/src/sys/agent.rs +++ b/server/src/sys/agent.rs @@ -305,25 +305,6 @@ impl<'a> System<'a> for Sys { // obstacles that smaller entities would not). let node_tolerance = scale * 1.5; let slow_factor = body.map_or(0.0, |b| b.base_accel() / 250.0).min(1.0); - let rrt_test = if let Some(target_info) = agent.target { - let Target { - target, hostile, .. - } = target_info; - if let Some(Alignment::Owned(uid)) = alignment { - if read_data.uids.get(target) == Some(&uid) { - controller - .actions - .push(ControlAction::basic_input(InputKind::Fly)); - true - } else { - false - } - } else { - false - } - } else { - false - }; let traversal_config = TraversalConfig { node_tolerance, slow_factor, @@ -332,7 +313,6 @@ impl<'a> System<'a> for Sys { min_tgt_dist: 1.0, can_climb: body.map_or(false, Body::can_climb), can_fly: body.map_or(false, |b| b.fly_thrust().is_some()), - rrt_test, }; let flees = alignment.map_or(true, |a| { From 9875a74640bd0d0837ceb85ac0114c13badc0ad0 Mon Sep 17 00:00:00 2001 From: James Melkonian Date: Fri, 27 Aug 2021 09:47:34 -0700 Subject: [PATCH 3/4] Make RRT pathfinding a cfg feature --- Cargo.lock | 1 + common/Cargo.toml | 7 ++++--- common/src/path.rs | 50 ++++++++++++++++++++++------------------------ 3 files changed, 29 insertions(+), 29 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index af7f0a0bd0..4bbd825ed2 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -5873,6 +5873,7 @@ dependencies = [ "fxhash", "hashbrown 0.11.2", "indexmap", + "kiddo", "lazy_static", "num-derive", "num-traits", diff --git a/common/Cargo.toml b/common/Cargo.toml index e31ab8f47b..2c18050994 100644 --- a/common/Cargo.toml +++ b/common/Cargo.toml @@ -11,6 +11,7 @@ simd = ["vek/platform_intrinsics"] bin_csv = ["ron", "csv", "structopt"] bin_graphviz = ["petgraph"] bin_cmd_doc_gen = [] +rrt_pathfinding = ["kiddo"] default = ["simd"] @@ -25,8 +26,6 @@ serde = { version = "1.0.110", features = ["derive", "rc"] } # Util enum-iterator = "0.6" vek = { version = "=0.14.1", features = ["serde"] } -# Used for RRT pathfinding (disabled until flight controls are improved) -# kiddo = "0.1" # Strum strum = { version = "0.21", features = ["derive"] } @@ -65,6 +64,8 @@ csv = { version = "1.1.3", optional = true } structopt = { version = "0.3.13", optional = true } # graphviz exporters petgraph = { version = "0.5.1", optional = true } +# K-d trees used for RRT pathfinding +kiddo = { version = "0.1", optional = true } # Data structures hashbrown = { version = "0.11", features = ["rayon", "serde", "nightly"] } @@ -105,4 +106,4 @@ required-features = ["bin_graphviz"] [[bin]] name = "cmd_doc_gen" -required-features = ["bin_cmd_doc_gen"] \ No newline at end of file +required-features = ["bin_cmd_doc_gen"] diff --git a/common/src/path.rs b/common/src/path.rs index bd93cde00c..e4c9ef86c9 100644 --- a/common/src/path.rs +++ b/common/src/path.rs @@ -5,9 +5,13 @@ use crate::{ }; use common_base::span; use hashbrown::hash_map::DefaultHashBuilder; -//use kiddo::{distance::squared_euclidean, KdTree}; // For RRT paths (disabled -// for now) +#[cfg(rrt_pathfinding)] use hashbrown::HashMap; +#[cfg(rrt_pathfinding)] +use kiddo::{distance::squared_euclidean, KdTree}; // For RRT paths (disabled for now) +#[cfg(rrt_pathfinding)] +use rand::distributions::Uniform; use rand::{thread_rng, Rng}; +#[cfg(rrt_pathfinding)] use std::f32::consts::PI; use std::iter::FromIterator; use vek::*; @@ -416,10 +420,10 @@ impl Chaser { { self.last_search_tgt = Some(tgt); - let (path, complete) = /*if traversal_cfg.can_fly { + // NOTE: Enable air paths when air braking has been figured out + let (path, complete) = /*if cfg!(rrt_pathfinding) && traversal_cfg.can_fly { find_air_path(vol, pos, tgt, &traversal_cfg) } else */{ - // Enable air paths when air braking has been figured out find_path(&mut self.astar, vol, pos, tgt, &traversal_cfg) }; @@ -669,7 +673,6 @@ where } // Enable when airbraking/sensible flight is a thing -/* /// Attempts to find a path from a start to the end using an informed /// RRT-Connect algorithm. A point is sampled from a bounding spheroid /// between the start and end. Two separate rapidly exploring random @@ -680,6 +683,7 @@ where /// with wider gaps, such as flying through a forest than for terrain /// with narrow gaps, such as navigating a maze. /// Returns a path and whether that path is complete or not. +#[cfg(rrt_pathfinding)] fn find_air_path( vol: &V, startf: Vec3, @@ -698,19 +702,10 @@ where .ray(startf + Vec3::unit_z(), endf + Vec3::unit_z()) .until(Block::is_opaque) .cast() - .0.powi(2) - >= total_dist_sqrd { - //let step = (endf - startf).normalized().map(|a| a * radius); - //let mut node: Vec3; - //// Maximum of 500 steps - //for i in 1..500 { - // node = startf + step.map(|s| s * i as f32); - // path.push(endf.map(|e| e.floor() as i32)); - // if node.distance_squared(endf) < radius{ - // connect = true; - // break; - // } - //} + .0 + .powi(2) + >= total_dist_sqrd + { path.push(endf.map(|e| e.floor() as i32)); connect = true; // Else use RRTs @@ -722,7 +717,8 @@ where .0 .powi(2) > (*start).distance_squared(*end) - //vol.get(*pos).ok().copied().unwrap_or_else(Block::empty).is_fluid(); + //vol.get(*pos).ok().copied().unwrap_or_else(Block::empty). + // is_fluid(); }; let mut node_index1: usize = 0; let mut node_index2: usize = 0; @@ -795,8 +791,10 @@ where let nearest2 = nodes2[nearest_index2]; // Extend toward the sampled point from the nearest node of each tree - let new_point1 = nearest1 + (sampled_point1 - nearest1).normalized().map(|a| a * radius); - let new_point2 = nearest2 + (sampled_point2 - nearest2).normalized().map(|a| a * radius); + let new_point1 = + nearest1 + (sampled_point1 - nearest1).normalized().map(|a| a * radius); + let new_point2 = + nearest2 + (sampled_point2 - nearest2).normalized().map(|a| a * radius); // Ensure the new nodes are valid/traversable if is_traversable(&nearest1, &new_point1) { @@ -889,7 +887,8 @@ where } path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32)); // Construct the path - while current_node_index1 != 0 && nodes1[current_node_index1].distance_squared(startf) > 4.0 + while current_node_index1 != 0 + && nodes1[current_node_index1].distance_squared(startf) > 4.0 { current_node_index1 = *parents1.get(¤t_node_index1).unwrap_or(&0); path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32)); @@ -913,7 +912,8 @@ where let next_node = path[next_idx]; let start_pos = node.map(|e| e as f32 + 0.5); let end_pos = next_node.map(|e| e as f32 + 0.5); - if vol.ray(start_pos, end_pos) + if vol + .ray(start_pos, end_pos) .until(Block::is_solid) .cast() .0 @@ -931,9 +931,7 @@ where } (Some(path.into_iter().collect()), connect) } -*/ -/* /// Returns a random point within a radially symmetrical ellipsoid with given /// foci and a `search parameter` to determine the size of the ellipse beyond /// the foci. Technically the point is within a prolate spheroid translated and @@ -945,6 +943,7 @@ where /// greater than zero. In order to increase the sample area, the /// search_parameter should be increased linearly as the search continues. #[allow(clippy::many_single_char_names)] +#[cfg(rrt_pathfinding)] pub fn point_on_prolate_spheroid( focus1: Vec3, focus2: Vec3, @@ -1059,4 +1058,3 @@ pub fn point_on_prolate_spheroid( // let global_coords = midpoint + rot_2_mat * (rot_z_mat * point); midpoint + rot_2_mat * point } -*/ From 36884d69198100dfd5aadc7e48ece31d432b9f65 Mon Sep 17 00:00:00 2001 From: James Melkonian Date: Fri, 17 Sep 2021 16:27:00 -0700 Subject: [PATCH 4/4] Move rrt algorithm into its own function --- common/src/path.rs | 407 +++++++++++++++++++++++---------------------- 1 file changed, 208 insertions(+), 199 deletions(-) diff --git a/common/src/path.rs b/common/src/path.rs index e4c9ef86c9..11a367ed0b 100644 --- a/common/src/path.rs +++ b/common/src/path.rs @@ -673,16 +673,6 @@ where } // Enable when airbraking/sensible flight is a thing -/// Attempts to find a path from a start to the end using an informed -/// RRT-Connect algorithm. A point is sampled from a bounding spheroid -/// between the start and end. Two separate rapidly exploring random -/// trees extend toward the sampled point. Nodes are stored in k-d trees -/// for quicker nearest node calculations. Points are sampled until the -/// trees connect. A final path is then reconstructed from the nodes. -/// This pathfinding algorithm is more appropriate for 3D pathfinding -/// with wider gaps, such as flying through a forest than for terrain -/// with narrow gaps, such as navigating a maze. -/// Returns a path and whether that path is complete or not. #[cfg(rrt_pathfinding)] fn find_air_path( vol: &V, @@ -694,7 +684,6 @@ where V: BaseVol + ReadVol, { let radius = traversal_cfg.node_tolerance; - let mut path = Vec::new(); let mut connect = false; let total_dist_sqrd = startf.distance_squared(endf); // First check if a straight line path works @@ -706,8 +695,10 @@ where .powi(2) >= total_dist_sqrd { + let mut path = Vec::new(); path.push(endf.map(|e| e.floor() as i32)); connect = true; + (Some(path.into_iter().collect()), connect) // Else use RRTs } else { let is_traversable = |start: &Vec3, end: &Vec3| { @@ -720,216 +711,234 @@ where //vol.get(*pos).ok().copied().unwrap_or_else(Block::empty). // is_fluid(); }; - let mut node_index1: usize = 0; - let mut node_index2: usize = 0; + informed_rrt_connect(start, end, is_traversable) + } +} - // Each tree has a vector of nodes - let mut nodes1 = Vec::new(); - let mut nodes2 = Vec::new(); +/// Attempts to find a path from a start to the end using an informed +/// RRT-Connect algorithm. A point is sampled from a bounding spheroid +/// between the start and end. Two separate rapidly exploring random +/// trees extend toward the sampled point. Nodes are stored in k-d trees +/// for quicker nearest node calculations. Points are sampled until the +/// trees connect. A final path is then reconstructed from the nodes. +/// This pathfinding algorithm is more appropriate for 3D pathfinding +/// with wider gaps, such as flying through a forest than for terrain +/// with narrow gaps, such as navigating a maze. +/// Returns a path and whether that path is complete or not. +#[cfg(rrt_pathfinding)] +fn informed_rrt_connect( + start: Vec3, + end: Vec3, + is_valid_edge: impl Fn(&Vec3, &Vec3) -> bool, +) -> (Option>>, bool) { + let mut path = Vec::new(); - // The parents hashmap stores nodes and their parent nodes as pairs to - // retrace the complete path once the two RRTs connect - let mut parents1 = HashMap::new(); - let mut parents2 = HashMap::new(); + // Each tree has a vector of nodes + let mut node_index1: usize = 0; + let mut node_index2: usize = 0; + let mut nodes1 = Vec::new(); + let mut nodes2 = Vec::new(); - // The path vector stores the path from the appropriate terminal to the - // connecting node or vice versa - let mut path1 = Vec::new(); - let mut path2 = Vec::new(); + // The parents hashmap stores nodes and their parent nodes as pairs to + // retrace the complete path once the two RRTs connect + let mut parents1 = HashMap::new(); + let mut parents2 = HashMap::new(); - // K-d trees are used to find the closest nodes rapidly - let mut kdtree1 = KdTree::new(); - let mut kdtree2 = KdTree::new(); + // The path vector stores the path from the appropriate terminal to the + // connecting node or vice versa + let mut path1 = Vec::new(); + let mut path2 = Vec::new(); - // Add the start as the first node of the first k-d tree - kdtree1 - .add(&[startf.x, startf.y, startf.z], node_index1) - .unwrap_or_default(); - nodes1.push(startf); - node_index1 += 1; + // K-d trees are used to find the closest nodes rapidly + let mut kdtree1 = KdTree::new(); + let mut kdtree2 = KdTree::new(); - // Add the end as the first node of the second k-d tree - kdtree2 - .add(&[endf.x, endf.y, endf.z], node_index2) - .unwrap_or_default(); - nodes2.push(endf); - node_index2 += 1; + // Add the start as the first node of the first k-d tree + kdtree1 + .add(&[startf.x, startf.y, startf.z], node_index1) + .unwrap_or_default(); + nodes1.push(startf); + node_index1 += 1; - let mut connection1_idx = 0; - let mut connection2_idx = 0; + // Add the end as the first node of the second k-d tree + kdtree2 + .add(&[endf.x, endf.y, endf.z], node_index2) + .unwrap_or_default(); + nodes2.push(endf); + node_index2 += 1; - // Scalar non-dimensional value that is proportional to the size of the - // sample spheroid volume. This increases in value until a path is found. - let mut search_parameter = 0.01; + let mut connection1_idx = 0; + let mut connection2_idx = 0; - // Maximum of 7000 iterations - for _i in 0..7000 { - if connect { - break; - } + let mut connect = false; - // Sample a point on the bounding spheroid - let (sampled_point1, sampled_point2) = { - let point = point_on_prolate_spheroid(startf, endf, search_parameter); - (point, point) - }; + // Scalar non-dimensional value that is proportional to the size of the + // sample spheroid volume. This increases in value until a path is found. + let mut search_parameter = 0.01; - // Find the nearest nodes to the the sampled point - let nearest_index1 = kdtree1 - .nearest_one( - &[sampled_point1.x, sampled_point1.y, sampled_point1.z], - &squared_euclidean, - ) - .map_or(0, |n| *n.1); - let nearest_index2 = kdtree2 - .nearest_one( - &[sampled_point2.x, sampled_point2.y, sampled_point2.z], - &squared_euclidean, - ) - .map_or(0, |n| *n.1); - let nearest1 = nodes1[nearest_index1]; - let nearest2 = nodes2[nearest_index2]; - - // Extend toward the sampled point from the nearest node of each tree - let new_point1 = - nearest1 + (sampled_point1 - nearest1).normalized().map(|a| a * radius); - let new_point2 = - nearest2 + (sampled_point2 - nearest2).normalized().map(|a| a * radius); - - // Ensure the new nodes are valid/traversable - if is_traversable(&nearest1, &new_point1) { - kdtree1 - .add(&[new_point1.x, new_point1.y, new_point1.z], node_index1) - .unwrap_or_default(); - nodes1.push(new_point1); - parents1.insert(node_index1, nearest_index1); - node_index1 += 1; - // Check if the trees connect - if let Ok((check, index)) = kdtree2.nearest_one( - &[new_point1.x, new_point1.y, new_point1.z], - &squared_euclidean, - ) { - if check < radius { - let connection = nodes2[*index]; - connection2_idx = *index; - nodes1.push(connection); - connection1_idx = nodes1.len() - 1; - parents1.insert(node_index1, node_index1 - 1); - connect = true; - } - } - } - - // Repeat the validity check for the second tree - if is_traversable(&nearest2, &new_point2) { - kdtree2 - .add(&[new_point2.x, new_point2.y, new_point1.z], node_index2) - .unwrap_or_default(); - nodes2.push(new_point2); - parents2.insert(node_index2, nearest_index2); - node_index2 += 1; - // Again check for a connection - if let Ok((check, index)) = kdtree1.nearest_one( - &[new_point2.x, new_point2.y, new_point1.z], - &squared_euclidean, - ) { - if check < radius { - let connection = nodes1[*index]; - connection1_idx = *index; - nodes2.push(connection); - connection2_idx = nodes2.len() - 1; - parents2.insert(node_index2, node_index2 - 1); - connect = true; - } - } - } - // Increase the search parameter to widen the sample volume - search_parameter += 0.02; + // Maximum of 7000 iterations + for _i in 0..7000 { + if connect { + break; } - if connect { - // Construct paths from the connection node to the start and end - let mut current_node_index1 = connection1_idx; - while current_node_index1 > 0 { - current_node_index1 = *parents1.get(¤t_node_index1).unwrap_or(&0); - path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32)); + // Sample a point on the bounding spheroid + let (sampled_point1, sampled_point2) = { + let point = point_on_prolate_spheroid(startf, endf, search_parameter); + (point, point) + }; + + // Find the nearest nodes to the the sampled point + let nearest_index1 = kdtree1 + .nearest_one( + &[sampled_point1.x, sampled_point1.y, sampled_point1.z], + &squared_euclidean, + ) + .map_or(0, |n| *n.1); + let nearest_index2 = kdtree2 + .nearest_one( + &[sampled_point2.x, sampled_point2.y, sampled_point2.z], + &squared_euclidean, + ) + .map_or(0, |n| *n.1); + let nearest1 = nodes1[nearest_index1]; + let nearest2 = nodes2[nearest_index2]; + + // Extend toward the sampled point from the nearest node of each tree + let new_point1 = nearest1 + (sampled_point1 - nearest1).normalized().map(|a| a * radius); + let new_point2 = nearest2 + (sampled_point2 - nearest2).normalized().map(|a| a * radius); + + // Ensure the new nodes are valid/traversable + if is_valid_edge(&nearest1, &new_point1) { + kdtree1 + .add(&[new_point1.x, new_point1.y, new_point1.z], node_index1) + .unwrap_or_default(); + nodes1.push(new_point1); + parents1.insert(node_index1, nearest_index1); + node_index1 += 1; + // Check if the trees connect + if let Ok((check, index)) = kdtree2.nearest_one( + &[new_point1.x, new_point1.y, new_point1.z], + &squared_euclidean, + ) { + if check < radius { + let connection = nodes2[*index]; + connection2_idx = *index; + nodes1.push(connection); + connection1_idx = nodes1.len() - 1; + parents1.insert(node_index1, node_index1 - 1); + connect = true; + } } - let mut current_node_index2 = connection2_idx; - while current_node_index2 > 0 { - current_node_index2 = *parents2.get(¤t_node_index2).unwrap_or(&0); - path2.push(nodes2[current_node_index2].map(|e| e.floor() as i32)); + } + + // Repeat the validity check for the second tree + if is_valid_edge(&nearest2, &new_point2) { + kdtree2 + .add(&[new_point2.x, new_point2.y, new_point1.z], node_index2) + .unwrap_or_default(); + nodes2.push(new_point2); + parents2.insert(node_index2, nearest_index2); + node_index2 += 1; + // Again check for a connection + if let Ok((check, index)) = kdtree1.nearest_one( + &[new_point2.x, new_point2.y, new_point1.z], + &squared_euclidean, + ) { + if check < radius { + let connection = nodes1[*index]; + connection1_idx = *index; + nodes2.push(connection); + connection2_idx = nodes2.len() - 1; + parents2.insert(node_index2, node_index2 - 1); + connect = true; + } } - // Join the two paths together in the proper order and remove duplicates - path1.pop(); - path1.reverse(); - path.append(&mut path1); - path.append(&mut path2); - path.dedup(); - } else { - // If the trees did not connect, construct a path from the start to - // the closest node to the end - let mut current_node_index1 = kdtree1 - .nearest_one(&[endf.x, endf.y, endf.z], &squared_euclidean) - .map_or(0, |c| *c.1); - // Attempt to pick a node other than the start node - for _i in 0..3 { - if current_node_index1 == 0 - || nodes1[current_node_index1].distance_squared(startf) < 4.0 - { - if let Some(index) = parents1.values().choose(&mut thread_rng()) { - current_node_index1 = *index; - } else { - break; - } + } + // Increase the search parameter to widen the sample volume + search_parameter += 0.02; + } + + if connect { + // Construct paths from the connection node to the start and end + let mut current_node_index1 = connection1_idx; + while current_node_index1 > 0 { + current_node_index1 = *parents1.get(¤t_node_index1).unwrap_or(&0); + path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32)); + } + let mut current_node_index2 = connection2_idx; + while current_node_index2 > 0 { + current_node_index2 = *parents2.get(¤t_node_index2).unwrap_or(&0); + path2.push(nodes2[current_node_index2].map(|e| e.floor() as i32)); + } + // Join the two paths together in the proper order and remove duplicates + path1.pop(); + path1.reverse(); + path.append(&mut path1); + path.append(&mut path2); + path.dedup(); + } else { + // If the trees did not connect, construct a path from the start to + // the closest node to the end + let mut current_node_index1 = kdtree1 + .nearest_one(&[endf.x, endf.y, endf.z], &squared_euclidean) + .map_or(0, |c| *c.1); + // Attempt to pick a node other than the start node + for _i in 0..3 { + if current_node_index1 == 0 + || nodes1[current_node_index1].distance_squared(startf) < 4.0 + { + if let Some(index) = parents1.values().choose(&mut thread_rng()) { + current_node_index1 = *index; } else { break; } + } else { + break; } + } + path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32)); + // Construct the path + while current_node_index1 != 0 && nodes1[current_node_index1].distance_squared(startf) > 4.0 + { + current_node_index1 = *parents1.get(¤t_node_index1).unwrap_or(&0); path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32)); - // Construct the path - while current_node_index1 != 0 - && nodes1[current_node_index1].distance_squared(startf) > 4.0 - { - current_node_index1 = *parents1.get(¤t_node_index1).unwrap_or(&0); - path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32)); - } + } - path1.reverse(); - path.append(&mut path1); - } - let mut new_path = Vec::new(); - let mut node = path[0]; - new_path.push(node); - let mut node_idx = 0; - let num_nodes = path.len(); - let end = path[num_nodes - 1]; - while node != end { - let next_idx = if node_idx + 4 > num_nodes - 1 { - num_nodes - 1 - } else { - node_idx + 4 - }; - let next_node = path[next_idx]; - let start_pos = node.map(|e| e as f32 + 0.5); - let end_pos = next_node.map(|e| e as f32 + 0.5); - if vol - .ray(start_pos, end_pos) - .until(Block::is_solid) - .cast() - .0 - .powi(2) - > (start_pos).distance_squared(end_pos) - { - node_idx = next_idx; - new_path.push(next_node); - } else { - node_idx += 1; - } - node = path[node_idx]; - } - path = new_path; + path1.reverse(); + path.append(&mut path1); } - (Some(path.into_iter().collect()), connect) + let mut new_path = Vec::new(); + let mut node = path[0]; + new_path.push(node); + let mut node_idx = 0; + let num_nodes = path.len(); + let end = path[num_nodes - 1]; + while node != end { + let next_idx = if node_idx + 4 > num_nodes - 1 { + num_nodes - 1 + } else { + node_idx + 4 + }; + let next_node = path[next_idx]; + let start_pos = node.map(|e| e as f32 + 0.5); + let end_pos = next_node.map(|e| e as f32 + 0.5); + if vol + .ray(start_pos, end_pos) + .until(Block::is_solid) + .cast() + .0 + .powi(2) + > (start_pos).distance_squared(end_pos) + { + node_idx = next_idx; + new_path.push(next_node); + } else { + node_idx += 1; + } + node = path[node_idx]; + } + path = new_path; } /// Returns a random point within a radially symmetrical ellipsoid with given