From c2c442975062b38de2fa7d28f6035784562ab586 Mon Sep 17 00:00:00 2001
From: James Melkonian <jemelkonian@gmail.com>
Date: Sun, 22 Aug 2021 21:03:14 -0700
Subject: [PATCH] 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<i32>; 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::<f32>::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::<f32>::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<V>(vol: &V, pos: Vec3<i32>, traversal_cfg: &TraversalConfig) -> bool
+fn walkable<V>(vol: &V, pos: Vec3<i32>) -> bool
where
V: BaseVol<Vox = Block> + 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<V>(
where
V: BaseVol<Vox = Block> + ReadVol,
{
- let is_walkable = |pos: &Vec3<i32>| walkable(vol, *pos, traversal_cfg);
- //let is_walkable = |pos: &Vec3<i32>| 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<i32>| 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<V>(
vol: &V,
startf: Vec3<f32>,
@@ -724,149 +689,267 @@ fn find_air_path<V>(
where
V: BaseVol<Vox = Block> + ReadVol,
{
- let radius = 0.9;
- let is_traversable = |start: &Vec3<f32>, end: &Vec3<f32>| {
- 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<f32>;
+ //// 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<f32>, end: &Vec3<f32>| {
+ 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<f32>, focus2: Vec3<f32>, search_parameter: f32) -> Vec3<f32> {
-
+/// 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<f32>,
+ focus2: Vec3<f32>,
+ search_parameter: f32,
+) -> Vec3<f32> {
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<f32>, focus2: Vec3<f32>, 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<f32>, focus2: Vec3<f32>, 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<f32>, focus2: Vec3<f32>, 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<f32>, focus2: Vec3<f32>, 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| {