diff --git a/src/navigation.rs b/src/navigation.rs index 97d88c1..9aa3eea 100644 --- a/src/navigation.rs +++ b/src/navigation.rs @@ -8,7 +8,7 @@ use bevy_tts::Tts; use leafwing_input_manager::prelude::*; use crate::{ - core::{CardinalDirection, GlobalTransformExt, Player, TransformExt, Zone}, + core::{CardinalDirection, GlobalTransformExt, Player, Zone}, error::error_handler, log::Log, utils::target_and_other, @@ -168,6 +168,7 @@ fn controls( Option<&ForwardMovementFactor>, Option<&StrafeMovementFactor>, &mut Transform, + &GlobalTransform, &Collider, )>, ) { @@ -181,6 +182,7 @@ fn controls( forward_movement_factor, strafe_movement_factor, mut transform, + global_transform, collider, ) in &mut query { @@ -221,11 +223,10 @@ fn controls( if actions.axis_pair(&NavigationAction::Translate) != Vec2::ZERO { let pair = actions.axis_pair(&NavigationAction::Translate); let dir = Dir2::new_unchecked(pair.normalize()); - let mut can_translate = true; - spatial_query.shape_hits_callback( + let hit = spatial_query.cast_shape( collider, - transform.translation.truncate(), - transform.yaw().as_radians(), + global_transform.translation().truncate(), + global_transform.yaw().as_radians(), dir, &ShapeCastConfig { max_distance: pair.length(), @@ -233,22 +234,17 @@ fn controls( ..default() }, &SpatialQueryFilter::from_excluded_entities(&sensors), - |hit| { - if hit.entity != entity { - commands.entity(entity).log_components(); - println!("Hit {}: can't translate", hit.entity); - can_translate = false; - false - } else { - true - } - }, ); - if can_translate { + if hit.is_none() { transform.translation += pair.extend(0.); - actions.set_axis_pair(&NavigationAction::Translate, Vec2::ZERO); } else { + // println!("Can't translate: {:?}", pair.extend(0.)); + // println!("Delta: {}", pair.length()); + if let Some(hit) = hit { + commands.entity(hit.entity).log_components(); + } } + actions.set_axis_pair(&NavigationAction::Translate, Vec2::ZERO); } if !snap_timers.contains_key(&entity) { if let Some(rotation_speed) = rotation_speed { @@ -258,8 +254,6 @@ fn controls( } } if actions.value(&NavigationAction::SetAngularVelocity) != 0. { - // velocity.angvel = - // actions.value(&NavigationAction::SetAngularVelocity); transform.rotation *= Quat::from_rotation_z( actions.value(&NavigationAction::SetAngularVelocity) * time.delta_secs(), ); @@ -390,8 +384,8 @@ impl Plugin for NavigationPlugin { .register_type::() .register_type::() .add_plugins(InputManagerPlugin::::default()) - .add_systems(PreUpdate, (update_direction, add_speed)) - .add_systems(Update, (snap, controls).chain().in_set(Movement)) + .add_systems(FixedPreUpdate, (update_direction, add_speed)) + .add_systems(FixedUpdate, (snap, controls).chain().in_set(Movement)) .add_systems( FixedUpdate, (tick_snap_timers, speak_direction.pipe(error_handler)), diff --git a/src/pathfinding.rs b/src/pathfinding.rs index 97e5eba..e6e8a08 100644 --- a/src/pathfinding.rs +++ b/src/pathfinding.rs @@ -92,23 +92,30 @@ fn calculate_path( ), Changed, >, - obstructions: Query>, + obstacles: Query>, sensors: Query>, ) { for (entity, destination, transform, collider, cost_map, actions) in &mut query { trace!("{entity}: destination: {destination:?}"); + commands.entity(entity).remove::().remove::(); if transform.translation().truncate().as_ivec2() == **destination { - commands - .entity(entity) - .remove::() - .remove::() - .remove::(); + commands.entity(entity).remove::(); continue; } - commands.entity(entity).remove::().remove::(); let path = astar( &transform.translation().truncate().as_ivec2(), |p| { + let mut start = Vec2::new(p.x as f32, p.y as f32); + if start.x >= 0. { + start.x += 0.5; + } else { + start.x -= 0.5; + } + if start.y >= 0. { + start.y += 0.5; + } else { + start.y -= 0.5; + } let mut successors: Vec<(IVec2, u32)> = vec![]; let x = p.x; let y = p.y; @@ -123,7 +130,6 @@ fn calculate_path( (IVec2::new(x + 1, y + 1), 1.5), ]; for exit in &exits { - let mut should_push = true; let mut check = exit.0.as_vec2(); if check.x >= 0. { check.x += 0.5; @@ -135,19 +141,23 @@ fn calculate_path( } else { check.y -= 0.5; } - let hits = spatial_query.shape_intersections( + let dir = (check - start).normalize(); + let dir = Dir2::new_unchecked(dir); + let delta = (check - start).length(); + let hits = spatial_query.cast_shape_predicate( collider, - check, + start, transform.yaw().as_radians(), + dir, + &ShapeCastConfig { + max_distance: delta, + ignore_origin_penetration: true, + ..default() + }, &SpatialQueryFilter::from_excluded_entities(&sensors), + &|entity| obstacles.contains(entity), ); - for entity in &hits { - if obstructions.contains(*entity) { - should_push = false; - break; - } - } - if should_push { + if hits.is_none() { let mut cost = exit.1 * 100.; if let Some(cost_map) = cost_map { if let Some(modifier) = cost_map.get(&exit.0) { @@ -199,7 +209,6 @@ fn negotiate_path( &Speed, Option<&RotationSpeed>, )>, - obstructions: Query>, sensors: Query>, ) { if physics_time.is_paused() { @@ -218,7 +227,7 @@ fn negotiate_path( ) in &mut query { trace!("{entity:?}: negotiating path"); - let start = transform.translation.truncate().as_ivec2(); + let start = global_transform.translation().truncate().as_ivec2(); if path.len() > 0 && path[0] == start { trace!("At start, removing"); path.remove(0); @@ -236,40 +245,30 @@ fn negotiate_path( } else { next.y -= 0.5; } - let mut direction = next - start; - direction = direction.normalize(); - direction *= **speed; - let mut hits = vec![]; - spatial_query.shape_hits_callback( + let direction = (next - start).normalize(); + let delta = time.delta_secs() * **speed; + let dir = Dir2::new_unchecked(direction); + let hit = spatial_query.cast_shape( collider, start, global_transform.yaw().as_radians(), - Dir2::new_unchecked(direction.normalize()), + dir, &ShapeCastConfig { - max_distance: time.delta_secs(), + max_distance: delta, ignore_origin_penetration: true, ..default() }, &SpatialQueryFilter::from_excluded_entities(&sensors), - |hit| { - if obstructions.contains(hit.entity) { - hits.push(hit.entity); - } - true - }, ); - if !hits.is_empty() { - println!("{entity} is stuck"); - for entity in hits { - commands.entity(entity).log_components(); - } + if hit.is_some() { + // println!("{entity} is stuck"); // 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 { - let delta = direction * time.delta_secs(); - navigation_actions.set_axis_pair(&NavigationAction::Translate, delta); + // println!("Translating: {:?}", direction * delta); + navigation_actions.set_axis_pair(&NavigationAction::Translate, direction * delta); } if rotation_speed.is_some() { let angle = direction.y.atan2(direction.x); @@ -313,8 +312,7 @@ fn actions( if let Some(mut current_dest) = destination { trace!("Got a destination"); if *current_dest != dest { - trace -!("{entity:?}: New destination {dest:?} differs from {current_dest:?}, zeroing velocity"); + trace!("{entity:?}: New destination {dest:?} differs from {current_dest:?}, zeroing velocity"); navigation_action .set_axis_pair(&NavigationAction::SetLinearVelocity, Vec2::ZERO); *current_dest = dest; @@ -337,10 +335,9 @@ impl Plugin for PathfindingPlugin { .register_type::() .register_type::() .register_type::() - .add_systems(PreUpdate, negotiate_path) .add_systems( - PreUpdate, - (actions, calculate_path) + FixedPreUpdate, + (actions, calculate_path, negotiate_path) .chain() .after(InputManagerSystem::Tick), )