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

233
src/pathfinding.rs
View File

@ -1,5 +1,3 @@
use std::marker::PhantomData;
use bevy::{
ecs::entity::Entities,
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);
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) {
let mut cost = tile.2 * 100.;
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;
}
}
@ -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,
start: Transform,
destination: Destination,
map: Map<D>,
cost_map: &Option<CostMap>,
query_pipeline: QueryPipeline,
collider_set: ColliderSet,
@ -111,36 +109,42 @@ fn find_path_for_shape<D: 'static + Clone + Default + Send + Sync>(
&start.i32(),
|p| {
let mut successors: Vec<((i32, i32), u32)> = vec![];
if let Some(tile) = map.at(p.0 as usize, p.1 as usize) {
if tile.is_walkable() {
for tile in map.get_available_exits(p.0 as usize, p.1 as usize) {
let mut should_push = true;
let shape_pos = Isometry2::new(
Vector2::new(tile.0 as f32 + 0.5, tile.1 as f32 + 0.5),
0.,
);
query_pipeline.intersections_with_shape(
&rigid_body_set,
&collider_set,
&shape_pos,
&*shape.raw,
bevy_rapier2d::rapier::pipeline::QueryFilter::new()
.predicate(&|h, _c| h != initiator),
|_handle| {
should_push = false;
false
},
);
if should_push {
let mut cost = tile.2 * 100.;
if let Some(cost_map) = cost_map {
if let Some(modifier) = cost_map.modifier(tile.0, tile.1) {
cost *= modifier;
}
}
successors.push(((tile.0 as i32, tile.1 as i32), cost as u32));
let x = p.0 as i32;
let y = p.1 as i32;
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 shape_pos =
Isometry2::new(Vector2::new(exit.0 .0 as f32, exit.0 .1 as f32), 0.);
query_pipeline.intersections_with_shape(
&rigid_body_set,
&collider_set,
&shape_pos,
&*shape.raw,
bevy_rapier2d::rapier::pipeline::QueryFilter::new()
.predicate(&|h, _c| h != initiator),
|_handle| {
should_push = false;
false
},
);
if should_push {
let mut cost = exit.1 * 100.;
if let Some(cost_map) = cost_map {
if let Some(modifier) = cost_map.modifier(exit.0 .0, exit.0 .1) {
cost *= modifier;
}
}
successors.push((exit.0, cost as u32));
}
}
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,
rapier_context: Res<RapierContext>,
obstructions: Query<&RapierColliderHandle, With<MapObstruction>>,
@ -170,7 +174,6 @@ fn calculate_path<D: 'static + Clone + Default + Send + Sync>(
),
Changed<Destination>,
>,
map: Query<&Map<D>>,
) {
for (entity, handle, destination, coordinates, shape, cost_map) in &query {
if coordinates.i32() == **destination {
@ -183,48 +186,50 @@ fn calculate_path<D: 'static + Clone + Default + Send + Sync>(
.remove::<Speed>();
continue;
}
if let Ok(map) = map.get_single() {
let coordinates_clone = *coordinates;
let destination_clone = *destination;
let query_pipeline = rapier_context.query_pipeline.clone();
let map_clone = map.clone();
let cost_map_clone = cost_map.cloned();
let handle_clone = *handle;
let mut collider_set = rapier_context.colliders.clone();
let mut to_remove = vec![];
for handle in collider_set.iter() {
if !obstructions.iter().map(|v| v.0).any(|x| x == handle.0) {
to_remove.push(handle.0);
}
let coordinates_clone = *coordinates;
let destination_clone = *destination;
let query_pipeline = rapier_context.query_pipeline.clone();
let cost_map_clone = cost_map.cloned();
let handle_clone = *handle;
let mut collider_set = rapier_context.colliders.clone();
let mut to_remove = vec![];
for handle in collider_set.iter() {
if !obstructions.iter().map(|v| v.0).any(|x| x == handle.0) {
to_remove.push(handle.0);
}
let mut bodies = rapier_context.bodies.clone();
if !to_remove.is_empty() {
let mut islands = rapier_context.islands.clone();
for handle in to_remove {
collider_set.remove(handle, &mut islands, &mut bodies, false);
}
}
let shape_clone = (*shape).clone();
let pool = AsyncComputeTaskPool::get();
let task = pool.spawn(async move {
find_path_for_shape(
handle_clone.0,
coordinates_clone,
destination_clone,
map_clone,
&cost_map_clone,
query_pipeline,
collider_set,
bodies,
shape_clone,
)
});
commands.run_if_exists(entity, |mut entity| {
entity.insert(Calculating(task));
entity.remove::<Path>();
entity.remove::<NoPath>();
});
}
let mut bodies = rapier_context.bodies.clone();
if !to_remove.is_empty() {
let mut islands = rapier_context.islands.clone();
for handle in to_remove {
collider_set.remove(handle, &mut islands, &mut bodies, false);
}
}
let shape_clone = (*shape).clone();
trace!(
"{:?}: path: calculating from {:?} to {:?}",
entity,
coordinates.i32(),
destination
);
let pool = AsyncComputeTaskPool::get();
let task = pool.spawn(async move {
find_path_for_shape(
handle_clone.0,
coordinates_clone,
destination_clone,
&cost_map_clone,
query_pipeline,
collider_set,
bodies,
shape_clone,
)
});
commands.run_if_exists(entity, |mut entity| {
entity.insert(Calculating(task));
entity.remove::<Path>();
entity.remove::<NoPath>();
});
}
}
@ -232,8 +237,10 @@ fn poll_tasks(mut commands: Commands, mut query: Query<(Entity, &mut Calculating
for (entity, mut calculating) in query.iter_mut() {
if let Some(result) = future::block_on(future::poll_once(&mut **calculating)) {
if let Some(path) = result {
trace!("{:?}: path: result: {:?}", entity, path);
commands.entity(entity).insert(path);
} else {
trace!("{:?}: path: no path", entity);
commands.entity(entity).insert(NoPath);
}
commands.entity(entity).remove::<Calculating>();
@ -264,53 +271,63 @@ fn negotiate_path(
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 mut new_path = path
.iter()
.cloned()
.skip_while(|v| *v == start_i32)
.collect::<Vec<(i32, i32)>>();
new_path.retain(|v| *v != start_i32);
**path = new_path;
if let Some(next) = path.first() {
let start = Vec2::new(transform.translation.x, transform.translation.y);
let next = Vec2::new(next.0 as f32 + 0.5, next.1 as f32 + 0.5);
let mut direction = next - start;
direction = direction.normalize();
direction *= **speed;
velocity.linvel = direction;
if rotation_speed.is_some() {
let v = next - start;
let angle = v.y.atan2(v.x);
transform.rotation = Quat::from_rotation_z(angle);
let mut cleanup = false;
if let Some(last) = path.last() {
if last == &start_i32 {
trace!("{:?}: path: at destination", entity);
cleanup = true;
}
continue;
} else {
}
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() {
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 mut direction = next - start;
direction = direction.normalize();
trace!("{:?}: path: direction: {:?}", entity, direction);
direction *= **speed;
velocity.linvel = direction;
trace!("{:?}: path: velocity: {:?}", entity, velocity.linvel);
if rotation_speed.is_some() {
let v = next - start;
let angle = v.y.atan2(v.x);
transform.rotation = Quat::from_rotation_z(angle);
}
} else {
cleanup = true;
}
}
if cleanup {
commands
.entity(entity)
.remove::<Path>()
.remove::<NoPath>()
.remove::<Destination>()
.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> {
fn default() -> Self {
Self(Default::default())
}
}
impl<D: 'static + Clone + Default + Send + Sync> Plugin for PathfindingPlugin<D> {
impl Plugin for PathfindingPlugin {
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(poll_tasks)
.add_system(negotiate_path.after(crate::navigation::limit_speed));
.add_system_to_stage(CoreStage::PostUpdate, negotiate_path);
}
}