diff --git a/src/navigation.rs b/src/navigation.rs index 61de406..9325ec5 100644 --- a/src/navigation.rs +++ b/src/navigation.rs @@ -119,12 +119,6 @@ fn controls( if let Some(pair) = actions.clamped_axis_pair(NavigationAction::Move) { cleanup = true; let mut direction = pair.xy(); - if direction.x.abs() < 0.5 { - direction.x = 0.; - } - if direction.y.abs() < 0.5 { - direction.y = 0.; - } let forward_movement_factor = forward_movement_factor.map(|v| v.0).unwrap_or_else(|| 1.); let backward_movement_factor = @@ -161,6 +155,7 @@ fn controls( .action_data_mut(NavigationAction::Translate) .axis_pair = Some(DualAxisData::from_xy(translation)); } else { + // println!("{entity:?}: SetLinearVelocity: {velocity:?}"); actions.press(NavigationAction::SetLinearVelocity); actions .action_data_mut(NavigationAction::SetLinearVelocity) @@ -175,13 +170,14 @@ fn controls( } if actions.pressed(NavigationAction::SetLinearVelocity) { if let Some(pair) = actions.axis_pair(NavigationAction::SetLinearVelocity) { - trace!("{entity:?}: SetLinearVelocity: {pair:?}"); + // println!("{entity:?}: SetLinearVelocity: {pair:?}"); velocity.linvel = pair.into(); } else { + // println!("{entity:?}: SetLinearVelocity: 0"); velocity.linvel = Vec2::ZERO; } } else if actions.just_released(NavigationAction::SetLinearVelocity) { - trace!("{entity:?}: Released velocity"); + // println!("{entity:?}: Released velocity"); velocity.linvel = Vec2::ZERO; actions .action_data_mut(NavigationAction::SetLinearVelocity) diff --git a/src/pathfinding.rs b/src/pathfinding.rs index dd490a4..d9a9f2a 100644 --- a/src/pathfinding.rs +++ b/src/pathfinding.rs @@ -15,7 +15,7 @@ use pathfinding::prelude::*; use crate::{ commands::RunIfExistsExt, - core::PointLike, + core::{PointLike, TransformExt}, map::{Map, MapObstruction}, navigation::{NavigationAction, RotationSpeed}, }; @@ -123,22 +123,29 @@ fn find_path_for_shape( ((x - 1, y + 1), 1.5), ((x + 1, y + 1), 1.5), ]; + let pos = Vector2::new(x as f32, y as f32); + let shape_pos = Isometry2::new(pos, 0.); 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 - }, - ); + let dest = Vector2::new(exit.0 .0 as f32, exit.0 .1 as f32); + let shape_vel = dest - pos; + let max_toi = 1.; + if query_pipeline + .cast_shape( + &rigid_body_set, + &collider_set, + &shape_pos, + &shape_vel, + &*shape.raw, + max_toi, + true, + bevy_rapier2d::rapier::pipeline::QueryFilter::new() + .predicate(&|h, _c| h != initiator), + ) + .is_some() + { + should_push = false; + } if should_push { let mut cost = exit.1 * 100.; if let Some(cost_map) = cost_map { @@ -187,6 +194,10 @@ fn calculate_path( .remove::(); continue; } + trace!( + "Calculating path from {:?}", + coordinates.translation.truncate().i32() + ); let coordinates_clone = *coordinates; let destination_clone = *destination; let query_pipeline = rapier_context.query_pipeline.clone(); @@ -208,10 +219,8 @@ fn calculate_path( } let shape_clone = (*shape).clone(); trace!( - "{:?}: path: calculating from {:?} to {:?}", - entity, + "{entity:?}: path: calculating from {:?} to {destination:?}", coordinates.i32(), - destination ); let pool = AsyncComputeTaskPool::get(); let task = pool.spawn(async move { @@ -238,10 +247,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); + trace!("{entity:?}: Path: {path:?}"); commands.entity(entity).insert(path); } else { - trace!("{:?}: path: no path", entity); + trace!("{entity:?}: path: no path"); commands.entity(entity).insert(NoPath); } commands.entity(entity).remove::(); @@ -268,53 +277,74 @@ fn negotiate_path( &mut ActionState, &mut Path, &mut Transform, + &Collider, Option<&RotationSpeed>, )>, + rapier_context: Res, ) { - for (entity, mut actions, mut path, mut transform, rotation_speed) in &mut query { - let start_i32 = (transform.translation.x, transform.translation.y).i32(); + for (entity, mut actions, mut path, mut transform, collider, rotation_speed) in &mut query { + let start_i32 = transform.translation.truncate().i32(); trace!( "{entity:?}: start pathfinding from {start_i32:?} to {:?}: {:?}", path.last(), transform.translation.truncate() ); - let mut cleanup = false; - if let Some(last) = path.last() { - if last == &start_i32 { - trace!("{entity:?}: path: at destination"); - cleanup = true; + if path.len() > 0 { + if path.len() > 1 && path[0] != start_i32 && path[1] != start_i32 { + trace!("Off path"); + } else if path.len() > 1 && path[1] == start_i32 { + trace!("Next in path is start of path, shouldn't happen"); + } + if path[0] == start_i32 { + trace!("At start, removing"); + path.remove(0); } } - if !cleanup { - if let Some(position) = path.iter().position(|v| v == &start_i32) { - trace!("{entity:?}: Path contains start"); - let (_, new_path) = path.split_at(position + 1); - **path = new_path.to_vec(); - } - if let Some(next) = path.first() { - trace!("{entity:?}: path: moving from {start_i32:?} to {next:?}"); - let start = Vec2::new(start_i32.x(), start_i32.y()); - let next = Vec2::new(next.0 as f32, next.1 as f32); - let mut direction = next - start; - direction = direction.normalize(); - trace!("{entity:?}: path: direction: {direction:?}"); - actions.press(NavigationAction::Move); - actions.action_data_mut(NavigationAction::Move).axis_pair = - Some(DualAxisData::from_xy(Vec2::new(-direction.y, direction.x))); - trace!( - "{entity:?}: path: move: {:?}", - Vec2::new(direction.y, -direction.x) - ); - if rotation_speed.is_some() { - let angle = direction.y.atan2(direction.x); - transform.rotation = Quat::from_rotation_z(angle); - } + if let Some(next) = path.first() { + trace!("{entity:?}: path: moving from {start_i32:?} to {next:?}"); + let start = transform.translation.truncate(); + let mut next = Vec2::new(next.0 as f32, next.1 as f32); + if next.x > 0. { + next.x += 0.5; } else { - trace!("{entity:?}: empty path, cleaning"); - cleanup = true; + next -= 0.5; } - } - if cleanup { + if next.y > 0. { + next.y += 0.5; + } else { + next.y -= 0.5; + } + let mut direction = next - start; + direction = direction.normalize(); + trace!( + "{entity:?}: Direction: {direction:?}, Distance: {}", + (next - start).length() + ); + if let Some((hit, toi)) = rapier_context.cast_shape( + start, + transform.yaw().radians(), + direction, + &*collider, + rapier_context.integration_parameters.dt, + QueryFilter::new() + .exclude_sensors() + .exclude_collider(entity), + ) { + trace!("{entity:?} is stuck, hit: {hit:?}, TOI: {toi:?}"); + // TODO: Remove when we have an actual character controller. + transform.translation = next.extend(0.); + continue; + } + trace!("{entity:?}: path: direction: {direction:?}"); + actions.press(NavigationAction::Move); + actions.action_data_mut(NavigationAction::Move).axis_pair = + Some(DualAxisData::from_xy(Vec2::new(-direction.y, direction.x))); + if rotation_speed.is_some() { + let angle = direction.y.atan2(direction.x); + transform.rotation = Quat::from_rotation_z(angle); + } + } else { + trace!("{entity:?}: empty path, cleaning"); commands .entity(entity) .remove::() @@ -330,24 +360,41 @@ fn actions( mut commands: Commands, mut query: Query<( Entity, - &ActionState, + &mut ActionState, + &mut ActionState, Option<&mut Destination>, )>, ) { - for (entity, actions, destination) in &mut query { + for (entity, mut actions, mut navigation_action, destination) in &mut query { if actions.pressed(NegotiatePathAction) { if let Some(pair) = actions.axis_pair(NegotiatePathAction) { + trace!("Negotiating path to {pair:?}"); let dest = Destination((pair.x() as i32, pair.y() as i32)); if let Some(mut current_dest) = destination { if *current_dest != dest { + trace!("{entity:?}: New destination, zeroing velocity"); + navigation_action.press(NavigationAction::SetLinearVelocity); + navigation_action + .action_data_mut(NavigationAction::SetLinearVelocity) + .axis_pair = Some(DualAxisData::from_xy(Vec2::ZERO)); *current_dest = dest; } } else { + trace!("{entity:?}: Adding destination, zeroing velocity"); + navigation_action.press(NavigationAction::SetLinearVelocity); + navigation_action + .action_data_mut(NavigationAction::SetLinearVelocity) + .axis_pair = Some(DualAxisData::from_xy(Vec2::ZERO)); commands.entity(entity).insert(dest); } } else if destination.is_some() { - commands.entity(entity).remove::(); + commands + .entity(entity) + .remove::() + .remove::(); } + actions.release(NegotiatePathAction); + actions.action_data_mut(NegotiatePathAction).axis_pair = None; } } } @@ -361,13 +408,18 @@ impl Plugin for PathfindingPlugin { .register_type::() .register_type::() .register_type::() - .add_system(calculate_path) + .add_system_to_stage(CoreStage::PreUpdate, calculate_path) .add_system_to_stage(CoreStage::PostUpdate, remove_destination) - .add_system(poll_tasks) + .add_system_to_stage(CoreStage::PreUpdate, poll_tasks) .add_system_to_stage( CoreStage::PreUpdate, negotiate_path.after(InputManagerSystem::Tick), ) - .add_system(actions); + .add_system_to_stage( + CoreStage::PreUpdate, + actions + .after(InputManagerSystem::Update) + .before(calculate_path), + ); } }