diff --git a/src/navigation.rs b/src/navigation.rs index 7e4a0ce..a2a0d4e 100644 --- a/src/navigation.rs +++ b/src/navigation.rs @@ -136,10 +136,8 @@ fn snap( snap_timers.insert(entity, SnapTimer::default()); transform.rotate(Quat::from_rotation_z(PI)); } else if actions.just_pressed(&NavigationAction::SnapCardinal) { - println!("Direction: {direction:?}"); let yaw: Rot2 = direction.into(); let yaw = yaw.as_radians(); - println!("Yaw: {yaw}"); let rotation = Quat::from_rotation_z(yaw); if transform.rotation != rotation { transform.rotation = rotation; @@ -215,7 +213,6 @@ fn controls( let mut speed = **speed; speed *= movement_factor; let move_velocity = direction * speed; - // println!("{entity:?}: SetLinearVelocity: {velocity:?}"); actions.set_axis_pair(&NavigationAction::SetLinearVelocity, move_velocity); } if velocity.0 != actions.axis_pair(&NavigationAction::SetLinearVelocity) { @@ -224,18 +221,25 @@ fn controls( if actions.axis_pair(&NavigationAction::Translate) != Vec2::ZERO { let pair = actions.axis_pair(&NavigationAction::Translate); let dir = Dir2::new_unchecked(pair.normalize()); - if spatial_query - .cast_shape( - collider, - transform.translation.truncate(), - transform.yaw().as_radians(), - dir, - 1., - true, - SpatialQueryFilter::from_excluded_entities(&sensors), - ) - .is_none() - { + let mut can_translate = true; + spatial_query.shape_hits_callback( + collider, + transform.translation.truncate(), + transform.yaw().as_radians(), + dir, + pair.length(), + true, + SpatialQueryFilter::from_excluded_entities(&sensors), + |hit| { + if hit.entity != entity { + can_translate = false; + false + } else { + true + } + }, + ); + if can_translate { transform.translation += pair.extend(0.); } actions.set_axis_pair(&NavigationAction::Translate, Vec2::ZERO); diff --git a/src/pathfinding.rs b/src/pathfinding.rs index 3de085d..d4cc5d9 100644 --- a/src/pathfinding.rs +++ b/src/pathfinding.rs @@ -4,8 +4,8 @@ use leafwing_input_manager::{plugin::InputManagerSystem, prelude::*}; use pathfinding::prelude::*; use crate::{ - core::{GlobalTransformExt, TransformExt}, - map::Map, + core::GlobalTransformExt, + map::{Map, MapObstruction}, navigation::{NavigationAction, RotationSpeed, Speed}, }; @@ -65,7 +65,8 @@ pub fn find_path( cost *= modifier; } } - successors.push((IVec2::new(tile.0 as i32, tile.1 as i32), cost as u32)); + successors + .push((IVec2::new(tile.0 as i32, tile.1 as i32), cost as u32)); } } } @@ -91,11 +92,12 @@ fn calculate_path( ), Changed, >, + obstructions: Query>, + sensors: Query>, ) { for (entity, destination, transform, collider, cost_map, actions) in &mut query { trace!("{entity}: destination: {destination:?}"); if transform.translation().truncate().as_ivec2() == **destination { - trace!("{entity}: remove1"); commands .entity(entity) .remove::() @@ -103,10 +105,6 @@ fn calculate_path( .remove::(); continue; } - trace!( - "{entity:?}: Calculating path from {:?} to {destination:?}", - transform.translation().truncate().as_ivec2() - ); commands.entity(entity).remove::().remove::(); let path = astar( &transform.translation().truncate().as_ivec2(), @@ -126,16 +124,28 @@ fn calculate_path( ]; for exit in &exits { let mut should_push = true; - if !spatial_query - .shape_intersections( - collider, - transform.translation().truncate(), - transform.yaw().as_radians(), - SpatialQueryFilter::default(), - ) - .is_empty() - { - should_push = false; + let mut check = exit.0.as_vec2(); + if check.x >= 0. { + check.x += 0.5; + } else { + check.x -= 0.5; + } + if check.y >= 0. { + check.y += 0.5; + } else { + check.y -= 0.5; + } + let hits = spatial_query.shape_intersections( + collider, + check, + transform.yaw().as_radians(), + SpatialQueryFilter::from_excluded_entities(&sensors), + ); + for entity in &hits { + if obstructions.contains(*entity) { + should_push = false; + break; + } } if should_push { let mut cost = exit.1 * 100.; @@ -176,6 +186,7 @@ fn remove_destination(mut commands: Commands, mut removed: RemovedComponents, + physics_time: Res>, spatial_query: SpatialQuery, mut query: Query<( Entity, @@ -183,17 +194,24 @@ fn negotiate_path( &mut ActionState, &mut Path, &mut Transform, + &GlobalTransform, &Collider, &Speed, Option<&RotationSpeed>, )>, + obstructions: Query>, + sensors: Query>, ) { + if physics_time.is_paused() { + return; + } for ( entity, actions, mut navigation_actions, mut path, mut transform, + global_transform, collider, speed, rotation_speed, @@ -201,17 +219,12 @@ fn negotiate_path( { trace!("{entity:?}: negotiating path"); let start = transform.translation.truncate().as_ivec2(); - trace!( - "{entity:?}: start pathfinding from {start:?} to {:?}: at {:?}", - path.last(), - transform.translation.truncate() - ); if path.len() > 0 && path[0] == start { trace!("At start, removing"); path.remove(0); } if let Some(next) = path.first() { - let start = transform.translation.truncate(); + let start = global_transform.translation().truncate(); let mut next = next.as_vec2(); if next.x >= 0. { next.x += 0.5; @@ -226,30 +239,30 @@ fn negotiate_path( let mut direction = next - start; direction = direction.normalize(); direction *= **speed; - direction *= time.delta_seconds(); - trace!( - "{entity:?}: Direction: {direction:?}, Distance: {}", - (next - start).length() + let mut hits = vec![]; + spatial_query.shape_hits_callback( + collider, + start, + global_transform.yaw().as_radians(), + Dir2::new_unchecked(direction.normalize()), + time.delta_seconds(), + true, + SpatialQueryFilter::from_excluded_entities(&sensors), + |hit| { + if obstructions.contains(hit.entity) { + hits.push(hit.entity); + } + true + }, ); - if spatial_query - .cast_shape( - collider, - start, - transform.yaw().as_radians(), - Dir2::new_unchecked(direction.normalize()), - direction.length(), - true, - SpatialQueryFilter::default(), - ) - .is_some() - { - trace!("{entity:?} is stuck"); + if !hits.is_empty() { // TODO: Remove when we have an actual character controller. next.x = next.x.trunc(); next.y = next.y.trunc(); transform.translation = next.extend(0.); } else { - navigation_actions.set_axis_pair(&NavigationAction::Translate, direction); + let delta = direction * time.delta_seconds(); + navigation_actions.set_axis_pair(&NavigationAction::Translate, delta); } if rotation_speed.is_some() { let angle = direction.y.atan2(direction.x);