Mirrored_Repos/veloren
1
0
Fork 0
mirror of https://gitlab.com/veloren/veloren.git synced 2024-08-30 18:12:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

Disable RRT pathfinding

Browse Source
This commit is contained in:
James Melkonian
2021-08-22 21:03:14 -07:00
parent 7ddc229728
commit c2c4429750
4 changed files with 379 additions and 297 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
Cargo.lock generated
View File

@ -5873,7 +5873,6 @@ dependencies = [
"fxhash", "fxhash",
"hashbrown 0.11.2", "hashbrown 0.11.2",
"indexmap", "indexmap",
"kiddo",
"lazy_static", "lazy_static",
"num-derive", "num-derive",
"num-traits", "num-traits",

3
common/Cargo.toml
View File

@ -25,7 +25,8 @@ serde = { version = "1.0.110", features = ["derive", "rc"] }
# Util # Util
enum-iterator = "0.6" enum-iterator = "0.6"
vek = { version = "=0.14.1", features = ["serde"] } 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
strum = { version = "0.21", features = ["derive"] } strum = { version = "0.21", features = ["derive"] }

652
common/src/path.rs
View File

@ -4,11 +4,11 @@ use crate::{
vol::{BaseVol, ReadVol}, vol::{BaseVol, ReadVol},
}; };
use common_base::span; use common_base::span;
use hashbrown::{hash_map::DefaultHashBuilder, HashMap}; use hashbrown::hash_map::DefaultHashBuilder;
use rand::{prelude::IteratorRandom, thread_rng, Rng, distributions::{Distribution, Uniform}}; //use kiddo::{distance::squared_euclidean, KdTree}; // For RRT paths (disabled
use std::{f32::consts::PI, iter::FromIterator}; // for now)
use kiddo::{distance::squared_euclidean, KdTree}; use rand::{thread_rng, Rng};
use tracing::warn; use std::iter::FromIterator;
use vek::*; use vek::*;
// Path // Path
@ -83,8 +83,6 @@ pub struct TraversalConfig {
pub can_climb: bool, pub can_climb: bool,
/// Whether the agent can fly. /// Whether the agent can fly.
pub can_fly: bool, pub can_fly: bool,
/// Testing for rrt pathing
pub rrt_test: bool,
} }
const DIAGONALS: [Vec2<i32>; 8] = [ const DIAGONALS: [Vec2<i32>; 8] = [
@ -127,34 +125,24 @@ impl Route {
let next1 = self.next(1).unwrap_or(next0); let next1 = self.next(1).unwrap_or(next0);
// Stop using obstructed paths // Stop using obstructed paths
if !walkable(vol, next1, traversal_cfg) { if !walkable(vol, next1) {
return None; return None;
} }
let be_precise = if traversal_cfg.can_fly { let be_precise = DIAGONALS.iter().any(|pos| {
false
} else {
DIAGONALS.iter().any(|pos| {
(-1..2).all(|z| { (-1..2).all(|z| {
vol.get(next0 + Vec3::new(pos.x, pos.y, z)) vol.get(next0 + Vec3::new(pos.x, pos.y, z))
.map(|b| !b.is_solid()) .map(|b| !b.is_solid())
.unwrap_or(false) .unwrap_or(false)
}) })
})}; });
let next_tgt = if traversal_cfg.can_fly { // Map position of node to middle of block
next0.map(|e| e as f32) + Vec3::new(0.5, 0.5, 0.5) let next_tgt = next0.map(|e| e as f32) + Vec3::new(0.5, 0.5, 0.0);
} 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())); 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 // Determine whether we're close enough to the next to to consider it completed
let dist_sqrd = pos.xy().distance_squared(closest_tgt.xy()); let dist_sqrd = pos.xy().distance_squared(closest_tgt.xy());
// FIXME use PID controller to actually hit nodes if dist_sqrd
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 } < 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 > -0.2 && traversal_cfg.on_ground))
&& (pos.z - closest_tgt.z < 1.2 || (pos.z - closest_tgt.z < 2.9 && vel.z < -0.05)) && (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()) && self.next_idx < self.path.len())
|| (traversal_cfg.in_liquid || (traversal_cfg.in_liquid
&& pos.z < closest_tgt.z + 0.8 && 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 // Node completed, move on to the next one
self.next_idx += 1; self.next_idx += 1;
@ -318,27 +306,20 @@ impl Route {
} else { } else {
0.5 0.5
}); });
let tgt = if be_precise || traversal_cfg.can_fly { let tgt = if be_precise {
next_tgt next_tgt
} else { } else {
Vec3::from(tgt2d) + Vec3::unit_z() * next_tgt.z Vec3::from(tgt2d) + Vec3::unit_z() * next_tgt.z
}; };
if traversal_cfg.can_fly { Some((
Some(( tgt - pos,
tgt - pos, // Control the entity's speed to hopefully stop us falling off walls on sharp
1.0, // corners. This code is very imperfect: it does its best but it
)) // can still fail for particularly fast entities.
} else { straight_factor * traversal_cfg.slow_factor + (1.0 - traversal_cfg.slow_factor),
Some(( ))
tgt - pos, .filter(|(bearing, _)| bearing.z < 2.1)
// 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)) .and_then(|(r, _)| r.traverse(vol, pos, vel, &traversal_cfg))
} }
} else { } else {
if traversal_cfg.can_fly { // There is no route found yet
warn!("I think no route?");
}
None None
}; };
// If a bearing has already been determined, use that
if let Some((bearing, speed)) = bearing { if let Some((bearing, speed)) = bearing {
Some((bearing, speed)) Some((bearing, speed))
} else { } 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(); 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 // 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); 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) 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) 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| { self.route = path.map(|path| {
let start_index = path let start_index = path
@ -461,90 +444,65 @@ impl Chaser {
) )
}); });
} }
// Start traversing the new route if it exists
//let walking_towards_edge = (-3..2).all(|z| { if let Some(bearing) = self
// vol.get( .route
// (pos + Vec3::<f32>::from(tgt_dir) * 2.5).map(|e| e as i32) + Vec3::unit_z() * z, .as_mut()
// ) .and_then(|(r, _)| r.traverse(vol, pos, vel, &traversal_cfg))
// .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) Some(bearing)
} else { } else {
if traversal_cfg.can_fly { // At this point no route is available and no bearing
warn!("welp"); // has been determined, so we start sampling terrain.
let (path, complete) = if traversal_cfg.can_fly && traversal_cfg.rrt_test { // Check for falling off walls and try moving straight
find_air_path(vol, pos, tgt, &traversal_cfg) // towards the target if falling is not a danger
} else { let walking_towards_edge = (-3..2).all(|z| {
find_path(&mut self.astar, vol, pos, tgt, &traversal_cfg) vol.get(
}; (pos + Vec3::<f32>::from(tgt_dir) * 2.5).map(|e| e as i32)
//let (path, complete) = find_path(&mut self.astar, vol, pos, tgt, &traversal_cfg); + Vec3::unit_z() * z,
)
.map(|b| b.is_air())
.unwrap_or(false)
});
self.route = path.map(|path| { // Enable when airbraking/flight is figured out
let start_index = path /*if traversal_cfg.can_fly {
.iter() Some(((tgt - pos) , 1.0))
.enumerate() } else */
.min_by_key(|(_, node)| { if !walking_towards_edge || traversal_cfg.can_fly {
node.xy() Some(((tgt - pos) * Vec3::new(1.0, 1.0, 0.0), 1.0))
.map(|e| e as f32) } else {
.distance_squared(pos.xy() + tgt_dir) // This is unfortunately where an NPC will stare blankly
as i32 // into space. No route has been found and no temporary
}) // bearing would suffice. Hopefully a route will be found
.map(|(idx, _)| idx); // in the coming ticks.
None
(
Route {
path,
next_idx: start_index.unwrap_or(0),
},
complete,
)
});
} }
None
} }
} }
} }
} }
#[allow(clippy::float_cmp)] // TODO: Pending review in #587 #[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 where
V: BaseVol<Vox = Block> + ReadVol, V: BaseVol<Vox = Block> + ReadVol,
{ {
if traversal_cfg.can_fly { let below = vol
vol.get(pos).ok().copied().unwrap_or_else(Block::empty).is_fluid() .get(pos - Vec3::unit_z())
} else { .ok()
let below = vol .copied()
.get(pos - Vec3::unit_z()) .unwrap_or_else(Block::empty);
.ok() let a = vol.get(pos).ok().copied().unwrap_or_else(Block::empty);
.copied() let b = vol
.unwrap_or_else(Block::empty); .get(pos + Vec3::unit_z())
let a = vol.get(pos).ok().copied().unwrap_or_else(Block::empty); .ok()
let b = vol .copied()
.get(pos + Vec3::unit_z()) .unwrap_or_else(Block::empty);
.ok()
.copied()
.unwrap_or_else(Block::empty);
let on_ground = below.is_filled(); let on_ground = below.is_filled();
let in_liquid = a.is_liquid(); let in_liquid = a.is_liquid();
(on_ground || in_liquid) && !a.is_solid() && !b.is_solid() (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 /// Attempt to search for a path to a target, returning the path (if one was
@ -559,12 +517,7 @@ fn find_path<V>(
where where
V: BaseVol<Vox = Block> + ReadVol, V: BaseVol<Vox = Block> + ReadVol,
{ {
let is_walkable = |pos: &Vec3<i32>| walkable(vol, *pos, traversal_cfg); let is_walkable = |pos: &Vec3<i32>| walkable(vol, *pos);
//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 get_walkable_z = |pos| { let get_walkable_z = |pos| {
let mut z_incr = 0; let mut z_incr = 0;
for _ in 0..32 { for _ in 0..32 {
@ -641,14 +594,14 @@ where
.map(|b| !b.is_liquid()) .map(|b| !b.is_liquid())
.unwrap_or(true) .unwrap_or(true)
|| traversal_cfg.can_climb || traversal_cfg.can_climb
//|| traversal_cfg.can_fly || traversal_cfg.can_fly
}) })
.into_iter() .into_iter()
.flatten(), .flatten(),
) )
.map(move |dir| (pos, dir)) .map(move |dir| (pos, dir))
.filter(move |(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 && ((dir.z < 1
|| vol || vol
.get(pos + Vec3::unit_z() * 2) .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>( fn find_air_path<V>(
vol: &V, vol: &V,
startf: Vec3<f32>, startf: Vec3<f32>,
@ -724,149 +689,267 @@ fn find_air_path<V>(
where where
V: BaseVol<Vox = Block> + ReadVol, V: BaseVol<Vox = Block> + ReadVol,
{ {
let radius = 0.9; let radius = traversal_cfg.node_tolerance;
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 mut path = Vec::new(); let mut path = Vec::new();
if connect { let mut connect = false;
let mut current_node_index1 = connection1_idx; let total_dist_sqrd = startf.distance_squared(endf);
while current_node_index1 > 0 { // First check if a straight line path works
current_node_index1 = *parents1.get(&current_node_index1).unwrap(); if vol
path1.push(nodes1[current_node_index1].map(|e| e.floor() as i32)); .ray(startf + Vec3::unit_z(), endf + Vec3::unit_z())
} .until(Block::is_opaque)
let mut current_node_index2 = connection2_idx; .cast()
while current_node_index2 > 0 { .0.powi(2)
current_node_index2 = *parents2.get(&current_node_index2).unwrap(); >= total_dist_sqrd {
path2.push(nodes2[current_node_index2].map(|e| e.floor() as i32)); //let step = (endf - startf).normalized().map(|a| a * radius);
} //let mut node: Vec3<f32>;
path1.reverse(); //// Maximum of 500 steps
path.append(&mut path1); //for i in 1..500 {
path.append(&mut path2); // node = startf + step.map(|s| s * i as f32);
path.dedup(); // 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 { } else {
let mut current_node_index1 = kdtree1.nearest_one(&[endf.x, endf.y, endf.z], &squared_euclidean).unwrap().1; let is_traversable = |start: &Vec3<f32>, end: &Vec3<f32>| {
for _i in 0..3 { vol.ray(*start, *end)
if *current_node_index1 == 0 || nodes1[*current_node_index1].distance_squared(startf) < 4.0 { .until(Block::is_solid)
current_node_index1 = parents1.values().choose(&mut thread_rng()).unwrap(); .cast()
} else { .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; break;
} }
}
path1.push(nodes1[*current_node_index1].map(|e| e.floor() as i32)); // Sample a point on the bounding spheroid
while *current_node_index1 != 0 && nodes1[*current_node_index1].distance_squared(startf) > 4.0 { let (sampled_point1, sampled_point2) = {
current_node_index1 = parents1.get(&current_node_index1).unwrap(); let point = point_on_prolate_spheroid(startf, endf, search_parameter);
path1.push(nodes1[*current_node_index1].map(|e| e.floor() as i32)); (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(); if connect {
path.append(&mut path1); // 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(&current_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(&current_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(&current_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) (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. /// Returns a random point within a radially symmetrical ellipsoid with given
/// Technically the point is within a prolate spheroid translated and rotated to the /// foci and a `search parameter` to determine the size of the ellipse beyond
/// proper place in cartesian space. /// 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 /// 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 /// a two dimensional ellipse or the size of the ellipse beyond the foci. In
/// case that analogy still holds as the ellipse is radially symmetrical along the /// this case that analogy still holds as the ellipse is radially symmetrical
/// axis between the foci. The value of the search parameter must be greater than zero. /// along the axis between the foci. The value of the search parameter must be
/// In order to increase the sample area, the search_parameter should be increased /// greater than zero. In order to increase the sample area, the
/// linearly as the search continues. /// 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> { #[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(); let mut rng = thread_rng();
// Uniform distribution // Uniform distribution
let range = Uniform::from(0.0..1.0); 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); let midpoint = 0.5 * (focus1 + focus2);
// Radius between the start and end of the path // Radius between the start and end of the path
let radius: f32 = focus1.distance(focus2); let radius: f32 = focus1.distance(focus2);
// The linear eccentricity of an ellipse is the distance from the origin to a focus // The linear eccentricity of an ellipse is the distance from the origin to a
// A prolate spheroid is a half-ellipse rotated for a full revolution which is why // focus A prolate spheroid is a half-ellipse rotated for a full revolution
// ellipse variables are used frequently in this function // which is why ellipse variables are used frequently in this function
let linear_eccentricity: f32 = 0.5 * radius; let linear_eccentricity: f32 = 0.5 * radius;
// For an ellipsoid, three variables determine the shape: a, b, and c. // 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 // where -0.5 * PI <= theta <= 0.5 * PI
// and 0.0 <= lambda < 2.0 * PI // and 0.0 <= lambda < 2.0 * PI
// //
// Select these two angles using the uniform distribution defined at the // Select these two angles using the uniform distribution defined at the
// beginning of the function from 0.0 to 1.0 // beginning of the function from 0.0 to 1.0
let rtheta: f32 = PI * range.sample(&mut rng) - 0.5 * PI; let rtheta: f32 = PI * range.sample(&mut rng) - 0.5 * PI;
let lambda: f32 = 2.0 * PI * range.sample(&mut rng); let lambda: f32 = 2.0 * PI * range.sample(&mut rng);
// Select a point on the surface of the ellipsoid // 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 point = Vec3::new(
//let surface_point = Vec3::new(a * rtheta.cos() * lambda.cos(), b * rtheta.cos() * lambda.sin(), c * rtheta.sin()); a * rtheta.cos() * lambda.cos(),
//let magnitude = surface_point.magnitude(); b * rtheta.cos() * lambda.sin(),
//let direction = surface_point.normalized(); 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 //// Randomly select a point along the vector to the previously selected surface
//// point using the uniform distribution //// point using the uniform distribution
//let point = magnitude * range.sample(&mut rng) * direction; //let point = magnitude * range.sample(&mut rng) * direction;
// Now that a point has been selected in local space, it must be rotated and // Now that a point has been selected in local space, it must be rotated and
// translated into global coordinates // translated into global coordinates
let dx = focus2.x - focus1.x; // NOTE: Don't rotate about the z axis as the point is already randomly
let dy = focus2.y - focus1.y; // selected about the z axis
//let dx = focus2.x - focus1.x;
//let dy = focus2.y - focus1.y;
let dz = focus2.z - focus1.z; let dz = focus2.z - focus1.z;
// Phi and theta are the angles from the x axis in the x-y plane and from // 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) // the z axis, respectively. (As found in spherical coordinates)
// These angles are used to rotate the random point in the spheroid about // These angles are used to rotate the random point in the spheroid about
// the local origin // the local origin
//
// Rotate about z axis by phi // Rotate about z axis by phi
//let phi: f32 = if dx.abs() > 0.0 { //let phi: f32 = if dx.abs() > 0.0 {
// (dy / dx).atan() // (dy / dx).atan()
@ -932,9 +1025,10 @@ pub fn point_in_prolate_spheroid(focus1: Vec3<f32>, focus2: Vec3<f32>, search_pa
// 0.5 * PI // 0.5 * PI
//}; //};
// This is unnecessary as rtheta is randomly selected between 0.0 and 2.0 * 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 { let theta: f32 = if radius > 0.0 {
(dz / radius).acos() (dz / radius).acos()
} else { } 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 m = perp_vec.y;
let n = perp_vec.z; let n = perp_vec.z;
// Rotation matrix for rotation about a vector // 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(), let rot_2_mat = Mat3::new(
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 * l * (1.0 - theta.cos()),
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()); 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 // 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 * (rot_z_mat * point);
let global_coords = midpoint + rot_2_mat * point; midpoint + rot_2_mat * point
global_coords
} }
*/

20
server/src/sys/agent.rs
View File

@ -305,25 +305,6 @@ impl<'a> System<'a> for Sys {
// obstacles that smaller entities would not). // obstacles that smaller entities would not).
let node_tolerance = scale * 1.5; let node_tolerance = scale * 1.5;
let slow_factor = body.map_or(0.0, |b| b.base_accel() / 250.0).min(1.0); 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 { let traversal_config = TraversalConfig {
node_tolerance, node_tolerance,
slow_factor, slow_factor,
@ -332,7 +313,6 @@ impl<'a> System<'a> for Sys {
min_tgt_dist: 1.0, min_tgt_dist: 1.0,
can_climb: body.map_or(false, Body::can_climb), can_climb: body.map_or(false, Body::can_climb),
can_fly: body.map_or(false, |b| b.fly_thrust().is_some()), can_fly: body.map_or(false, |b| b.fly_thrust().is_some()),
rrt_test,
}; };
let flees = alignment.map_or(true, |a| { let flees = alignment.map_or(true, |a| {