projects/blackout
1
0
Fork 0
Code Pull Requests Projects Releases Wiki Activity

Simplify and fix various issues with pathfinding.
All checks were successful
continuous-integration/drone/push Build is passing
Details

Browse Source
This commit is contained in:
Nolan Darilek 2022-09-12 17:00:33 -05:00
parent 1108e3e75e
commit 08d4d7429c
1 changed files with 125 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

107
src/pathfinding.rs
View File

@ -1,5 +1,3 @@
use std::marker::PhantomData;
use bevy::{ use bevy::{
ecs::entity::Entities, ecs::entity::Entities,
prelude::*, prelude::*,
@ -55,7 +53,7 @@ impl CostMap {
} }
} }
pub fn modifier(&self, x: usize, y: usize) -> Option<&f32> { pub fn modifier(&self, x: i32, y: i32) -> Option<&f32> {
let idx = (y as usize) * self.width + (x as usize); let idx = (y as usize) * self.width + (x as usize);
self.costs.get(idx) self.costs.get(idx)
} }
@ -81,7 +79,8 @@ pub fn find_path<D: 'static + Clone + Default + Send + Sync>(
for tile in map.get_available_exits(p.0 as usize, p.1 as usize) { for tile in map.get_available_exits(p.0 as usize, p.1 as usize) {
let mut cost = tile.2 * 100.; let mut cost = tile.2 * 100.;
if let Some(cost_map) = cost_map { if let Some(cost_map) = cost_map {
if let Some(modifier) = cost_map.modifier(tile.0, tile.1) { if let Some(modifier) = cost_map.modifier(tile.0 as i32, tile.1 as i32)
{
cost *= modifier; cost *= modifier;
} }
} }
@ -96,11 +95,10 @@ pub fn find_path<D: 'static + Clone + Default + Send + Sync>(
) )
} }
fn find_path_for_shape<D: 'static + Clone + Default + Send + Sync>( fn find_path_for_shape(
initiator: ColliderHandle, initiator: ColliderHandle,
start: Transform, start: Transform,
destination: Destination, destination: Destination,
map: Map<D>,
cost_map: &Option<CostMap>, cost_map: &Option<CostMap>,
query_pipeline: QueryPipeline, query_pipeline: QueryPipeline,
collider_set: ColliderSet, collider_set: ColliderSet,
@ -111,14 +109,22 @@ fn find_path_for_shape<D: 'static + Clone + Default + Send + Sync>(
&start.i32(), &start.i32(),
|p| { |p| {
let mut successors: Vec<((i32, i32), u32)> = vec![]; let mut successors: Vec<((i32, i32), u32)> = vec![];
if let Some(tile) = map.at(p.0 as usize, p.1 as usize) { let x = p.0 as i32;
if tile.is_walkable() { let y = p.1 as i32;
for tile in map.get_available_exits(p.0 as usize, p.1 as usize) { let exits = vec![
((x - 1, y), 1.),
((x + 1, y), 1.),
((x, y - 1), 1.),
((x, y + 1), 1.),
((x - 1, y - 1), 1.5),
((x + 1, y - 1), 1.5),
((x - 1, y + 1), 1.5),
((x + 1, y + 1), 1.5),
];
for exit in &exits {
let mut should_push = true; let mut should_push = true;
let shape_pos = Isometry2::new( let shape_pos =
Vector2::new(tile.0 as f32 + 0.5, tile.1 as f32 + 0.5), Isometry2::new(Vector2::new(exit.0 .0 as f32, exit.0 .1 as f32), 0.);
0.,
);
query_pipeline.intersections_with_shape( query_pipeline.intersections_with_shape(
&rigid_body_set, &rigid_body_set,
&collider_set, &collider_set,
@ -132,15 +138,13 @@ fn find_path_for_shape<D: 'static + Clone + Default + Send + Sync>(
}, },
); );
if should_push { if should_push {
let mut cost = tile.2 * 100.; let mut cost = exit.1 * 100.;
if let Some(cost_map) = cost_map { if let Some(cost_map) = cost_map {
if let Some(modifier) = cost_map.modifier(tile.0, tile.1) { if let Some(modifier) = cost_map.modifier(exit.0 .0, exit.0 .1) {
cost *= modifier; cost *= modifier;
} }
} }
successors.push(((tile.0 as i32, tile.1 as i32), cost as u32)); successors.push((exit.0, cost as u32));
}
}
} }
} }
successors successors
@ -155,7 +159,7 @@ fn find_path_for_shape<D: 'static + Clone + Default + Send + Sync>(
} }
} }
fn calculate_path<D: 'static + Clone + Default + Send + Sync>( fn calculate_path(
mut commands: Commands, mut commands: Commands,
rapier_context: Res<RapierContext>, rapier_context: Res<RapierContext>,
obstructions: Query<&RapierColliderHandle, With<MapObstruction>>, obstructions: Query<&RapierColliderHandle, With<MapObstruction>>,
@ -170,7 +174,6 @@ fn calculate_path<D: 'static + Clone + Default + Send + Sync>(
), ),
Changed<Destination>, Changed<Destination>,
>, >,
map: Query<&Map<D>>,
) { ) {
for (entity, handle, destination, coordinates, shape, cost_map) in &query { for (entity, handle, destination, coordinates, shape, cost_map) in &query {
if coordinates.i32() == **destination { if coordinates.i32() == **destination {
@ -183,11 +186,9 @@ fn calculate_path<D: 'static + Clone + Default + Send + Sync>(
.remove::<Speed>(); .remove::<Speed>();
continue; continue;
} }
if let Ok(map) = map.get_single() {
let coordinates_clone = *coordinates; let coordinates_clone = *coordinates;
let destination_clone = *destination; let destination_clone = *destination;
let query_pipeline = rapier_context.query_pipeline.clone(); let query_pipeline = rapier_context.query_pipeline.clone();
let map_clone = map.clone();
let cost_map_clone = cost_map.cloned(); let cost_map_clone = cost_map.cloned();
let handle_clone = *handle; let handle_clone = *handle;
let mut collider_set = rapier_context.colliders.clone(); let mut collider_set = rapier_context.colliders.clone();
@ -205,13 +206,18 @@ fn calculate_path<D: 'static + Clone + Default + Send + Sync>(
} }
} }
let shape_clone = (*shape).clone(); let shape_clone = (*shape).clone();
trace!(
"{:?}: path: calculating from {:?} to {:?}",
entity,
coordinates.i32(),
destination
);
let pool = AsyncComputeTaskPool::get(); let pool = AsyncComputeTaskPool::get();
let task = pool.spawn(async move { let task = pool.spawn(async move {
find_path_for_shape( find_path_for_shape(
handle_clone.0, handle_clone.0,
coordinates_clone, coordinates_clone,
destination_clone, destination_clone,
map_clone,
&cost_map_clone, &cost_map_clone,
query_pipeline, query_pipeline,
collider_set, collider_set,
@ -226,14 +232,15 @@ fn calculate_path<D: 'static + Clone + Default + Send + Sync>(
}); });
} }
} }
}
fn poll_tasks(mut commands: Commands, mut query: Query<(Entity, &mut Calculating)>) { fn poll_tasks(mut commands: Commands, mut query: Query<(Entity, &mut Calculating)>) {
for (entity, mut calculating) in query.iter_mut() { for (entity, mut calculating) in query.iter_mut() {
if let Some(result) = future::block_on(future::poll_once(&mut **calculating)) { if let Some(result) = future::block_on(future::poll_once(&mut **calculating)) {
if let Some(path) = result { if let Some(path) = result {
trace!("{:?}: path: result: {:?}", entity, path);
commands.entity(entity).insert(path); commands.entity(entity).insert(path);
} else { } else {
trace!("{:?}: path: no path", entity);
commands.entity(entity).insert(NoPath); commands.entity(entity).insert(NoPath);
} }
commands.entity(entity).remove::<Calculating>(); commands.entity(entity).remove::<Calculating>();
@ -264,53 +271,63 @@ fn negotiate_path(
Option<&RotationSpeed>, Option<&RotationSpeed>,
)>, )>,
) { ) {
for (entity, mut path, mut transform, mut velocity, speed, rotation_speed) in query.iter_mut() { for (entity, mut path, mut transform, mut velocity, speed, rotation_speed) in &mut query {
let start_i32 = (transform.translation.x, transform.translation.y).i32(); let start_i32 = (transform.translation.x, transform.translation.y).i32();
let mut new_path = path let mut cleanup = false;
.iter() if let Some(last) = path.last() {
.cloned() if last == &start_i32 {
.skip_while(|v| *v == start_i32) trace!("{:?}: path: at destination", entity);
.collect::<Vec<(i32, i32)>>(); cleanup = true;
new_path.retain(|v| *v != start_i32); }
**path = new_path; }
if !cleanup {
if let Some(position) = path.iter().position(|v| v == &start_i32) {
trace!("{:?}: Path contains start", entity);
let (_, new_path) = path.split_at(position + 1);
**path = new_path.to_vec();
}
if let Some(next) = path.first() { if let Some(next) = path.first() {
let start = Vec2::new(transform.translation.x, transform.translation.y); trace!(
"{:?}: path: moving from {:?} to {:?}",
entity,
start_i32,
next
);
let start = transform.translation.truncate();
let next = Vec2::new(next.0 as f32 + 0.5, next.1 as f32 + 0.5); let next = Vec2::new(next.0 as f32 + 0.5, next.1 as f32 + 0.5);
let mut direction = next - start; let mut direction = next - start;
direction = direction.normalize(); direction = direction.normalize();
trace!("{:?}: path: direction: {:?}", entity, direction);
direction *= **speed; direction *= **speed;
velocity.linvel = direction; velocity.linvel = direction;
trace!("{:?}: path: velocity: {:?}", entity, velocity.linvel);
if rotation_speed.is_some() { if rotation_speed.is_some() {
let v = next - start; let v = next - start;
let angle = v.y.atan2(v.x); let angle = v.y.atan2(v.x);
transform.rotation = Quat::from_rotation_z(angle); transform.rotation = Quat::from_rotation_z(angle);
} }
continue;
} else { } else {
cleanup = true;
}
}
if cleanup {
commands commands
.entity(entity) .entity(entity)
.remove::<Path>() .remove::<Path>()
.remove::<NoPath>() .remove::<NoPath>()
.remove::<Destination>() .remove::<Destination>()
.remove::<Speed>(); .remove::<Speed>();
velocity.linvel = Vec2::ZERO;
} }
} }
} }
pub struct PathfindingPlugin<D: 'static + Clone + Default + Send + Sync>(PhantomData<D>); pub struct PathfindingPlugin;
impl<D: 'static + Clone + Default + Send + Sync> Default for PathfindingPlugin<D> { impl Plugin for PathfindingPlugin {
fn default() -> Self {
Self(Default::default())
}
}
impl<D: 'static + Clone + Default + Send + Sync> Plugin for PathfindingPlugin<D> {
fn build(&self, app: &mut App) { fn build(&self, app: &mut App) {
app.add_system(calculate_path::<D>) app.add_system(calculate_path)
.add_system_to_stage(CoreStage::PostUpdate, remove_destination) .add_system_to_stage(CoreStage::PostUpdate, remove_destination)
.add_system(poll_tasks) .add_system(poll_tasks)
.add_system(negotiate_path.after(crate::navigation::limit_speed)); .add_system_to_stage(CoreStage::PostUpdate, negotiate_path);
} }
} }