From 08d4d7429cac544910d891e7f779d3370893ab4c Mon Sep 17 00:00:00 2001 From: Nolan Darilek Date: Mon, 12 Sep 2022 17:00:33 -0500 Subject: [PATCH] Simplify and fix various issues with pathfinding. --- src/pathfinding.rs | 233 ++++++++++++++++++++++++--------------------- 1 file changed, 125 insertions(+), 108 deletions(-) diff --git a/src/pathfinding.rs b/src/pathfinding.rs index c2f9093..662f6fc 100644 --- a/src/pathfinding.rs +++ b/src/pathfinding.rs @@ -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( 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( ) } -fn find_path_for_shape( +fn find_path_for_shape( initiator: ColliderHandle, start: Transform, destination: Destination, - map: Map, cost_map: &Option, query_pipeline: QueryPipeline, collider_set: ColliderSet, @@ -111,36 +109,42 @@ fn find_path_for_shape( &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( } } -fn calculate_path( +fn calculate_path( mut commands: Commands, rapier_context: Res, obstructions: Query<&RapierColliderHandle, With>, @@ -170,7 +174,6 @@ fn calculate_path( ), Changed, >, - map: Query<&Map>, ) { for (entity, handle, destination, coordinates, shape, cost_map) in &query { if coordinates.i32() == **destination { @@ -183,48 +186,50 @@ fn calculate_path( .remove::(); 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::(); - entity.remove::(); - }); } + 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::(); + entity.remove::(); + }); } } @@ -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::(); @@ -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::>(); - 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::() .remove::() .remove::() .remove::(); - velocity.linvel = Vec2::ZERO; } } } -pub struct PathfindingPlugin(PhantomData); +pub struct PathfindingPlugin; -impl Default for PathfindingPlugin { - fn default() -> Self { - Self(Default::default()) - } -} - -impl Plugin for PathfindingPlugin { +impl Plugin for PathfindingPlugin { fn build(&self, app: &mut App) { - app.add_system(calculate_path::) + 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); } }