projects/blackout
1
0
Fork 0
Code Pull Requests Projects Releases Wiki Activity

Various pathfinding/navigation fixes.

Browse Source
This commit is contained in:
Nolan Darilek 2025-01-06 20:33:46 -05:00
parent b524fc39da
commit 08c91380ad
2 changed files with 56 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
src/navigation.rs
View File

@ -8,7 +8,7 @@ use bevy_tts::Tts;
use leafwing_input_manager::prelude::*; use leafwing_input_manager::prelude::*;
use crate::{ use crate::{
core::{CardinalDirection, GlobalTransformExt, Player, TransformExt, Zone}, core::{CardinalDirection, GlobalTransformExt, Player, Zone},
error::error_handler, error::error_handler,
log::Log, log::Log,
utils::target_and_other, utils::target_and_other,
@ -168,6 +168,7 @@ fn controls(
Option<&ForwardMovementFactor>, Option<&ForwardMovementFactor>,
Option<&StrafeMovementFactor>, Option<&StrafeMovementFactor>,
&mut Transform, &mut Transform,
&GlobalTransform,
&Collider, &Collider,
)>, )>,
) { ) {
@ -181,6 +182,7 @@ fn controls(
forward_movement_factor, forward_movement_factor,
strafe_movement_factor, strafe_movement_factor,
mut transform, mut transform,
global_transform,
collider, collider,
) in &mut query ) in &mut query
{ {
@ -221,11 +223,10 @@ fn controls(
if actions.axis_pair(&NavigationAction::Translate) != Vec2::ZERO { if actions.axis_pair(&NavigationAction::Translate) != Vec2::ZERO {
let pair = actions.axis_pair(&NavigationAction::Translate); let pair = actions.axis_pair(&NavigationAction::Translate);
let dir = Dir2::new_unchecked(pair.normalize()); let dir = Dir2::new_unchecked(pair.normalize());
let mut can_translate = true; let hit = spatial_query.cast_shape(
spatial_query.shape_hits_callback(
collider, collider,
transform.translation.truncate(), global_transform.translation().truncate(),
transform.yaw().as_radians(), global_transform.yaw().as_radians(),
dir, dir,
&ShapeCastConfig { &ShapeCastConfig {
max_distance: pair.length(), max_distance: pair.length(),
@ -233,23 +234,18 @@ fn controls(
..default() ..default()
}, },
&SpatialQueryFilter::from_excluded_entities(&sensors), &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.); transform.translation += pair.extend(0.);
actions.set_axis_pair(&NavigationAction::Translate, Vec2::ZERO);
} else { } 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 !snap_timers.contains_key(&entity) {
if let Some(rotation_speed) = rotation_speed { if let Some(rotation_speed) = rotation_speed {
let delta = let delta =
@ -258,8 +254,6 @@ fn controls(
} }
} }
if actions.value(&NavigationAction::SetAngularVelocity) != 0. { if actions.value(&NavigationAction::SetAngularVelocity) != 0. {
// velocity.angvel =
// actions.value(&NavigationAction::SetAngularVelocity);
transform.rotation *= Quat::from_rotation_z( transform.rotation *= Quat::from_rotation_z(
actions.value(&NavigationAction::SetAngularVelocity) * time.delta_secs(), actions.value(&NavigationAction::SetAngularVelocity) * time.delta_secs(),
); );
@ -390,8 +384,8 @@ impl Plugin for NavigationPlugin {
.register_type::<RotationSpeed>() .register_type::<RotationSpeed>()
.register_type::<Speed>() .register_type::<Speed>()
.add_plugins(InputManagerPlugin::<NavigationAction>::default()) .add_plugins(InputManagerPlugin::<NavigationAction>::default())
.add_systems(PreUpdate, (update_direction, add_speed)) .add_systems(FixedPreUpdate, (update_direction, add_speed))
.add_systems(Update, (snap, controls).chain().in_set(Movement)) .add_systems(FixedUpdate, (snap, controls).chain().in_set(Movement))
.add_systems( .add_systems(
FixedUpdate, FixedUpdate,
(tick_snap_timers, speak_direction.pipe(error_handler)), (tick_snap_timers, speak_direction.pipe(error_handler)),

85
src/pathfinding.rs
View File

@ -92,23 +92,30 @@ fn calculate_path(
), ),
Changed<Destination>, Changed<Destination>,
>, >,
obstructions: Query<Entity, With<Obstacle>>, obstacles: Query<Entity, With<Obstacle>>,
sensors: Query<Entity, With<Sensor>>, sensors: Query<Entity, With<Sensor>>,
) { ) {
for (entity, destination, transform, collider, cost_map, actions) in &mut query { for (entity, destination, transform, collider, cost_map, actions) in &mut query {
trace!("{entity}: destination: {destination:?}"); trace!("{entity}: destination: {destination:?}");
commands.entity(entity).remove::<Path>().remove::<NoPath>();
if transform.translation().truncate().as_ivec2() == **destination { if transform.translation().truncate().as_ivec2() == **destination {
commands commands.entity(entity).remove::<Destination>();
.entity(entity)
.remove::<Path>()
.remove::<NoPath>()
.remove::<Destination>();
continue; continue;
} }
commands.entity(entity).remove::<Path>().remove::<NoPath>();
let path = astar( let path = astar(
&transform.translation().truncate().as_ivec2(), &transform.translation().truncate().as_ivec2(),
|p| { |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 mut successors: Vec<(IVec2, u32)> = vec![];
let x = p.x; let x = p.x;
let y = p.y; let y = p.y;
@ -123,7 +130,6 @@ fn calculate_path(
(IVec2::new(x + 1, y + 1), 1.5), (IVec2::new(x + 1, y + 1), 1.5),
]; ];
for exit in &exits { for exit in &exits {
let mut should_push = true;
let mut check = exit.0.as_vec2(); let mut check = exit.0.as_vec2();
if check.x >= 0. { if check.x >= 0. {
check.x += 0.5; check.x += 0.5;
@ -135,19 +141,23 @@ fn calculate_path(
} else { } else {
check.y -= 0.5; 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, collider,
check, start,
transform.yaw().as_radians(), transform.yaw().as_radians(),
dir,
&ShapeCastConfig {
max_distance: delta,
ignore_origin_penetration: true,
..default()
},
&SpatialQueryFilter::from_excluded_entities(&sensors), &SpatialQueryFilter::from_excluded_entities(&sensors),
&|entity| obstacles.contains(entity),
); );
for entity in &hits { if hits.is_none() {
if obstructions.contains(*entity) {
should_push = false;
break;
}
}
if should_push {
let mut cost = exit.1 * 100.; let mut cost = exit.1 * 100.;
if let Some(cost_map) = cost_map { if let Some(cost_map) = cost_map {
if let Some(modifier) = cost_map.get(&exit.0) { if let Some(modifier) = cost_map.get(&exit.0) {
@ -199,7 +209,6 @@ fn negotiate_path(
&Speed, &Speed,
Option<&RotationSpeed>, Option<&RotationSpeed>,
)>, )>,
obstructions: Query<Entity, With<Obstacle>>,
sensors: Query<Entity, With<Sensor>>, sensors: Query<Entity, With<Sensor>>,
) { ) {
if physics_time.is_paused() { if physics_time.is_paused() {
@ -218,7 +227,7 @@ fn negotiate_path(
) in &mut query ) in &mut query
{ {
trace!("{entity:?}: negotiating path"); 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 { if path.len() > 0 && path[0] == start {
trace!("At start, removing"); trace!("At start, removing");
path.remove(0); path.remove(0);
@ -236,40 +245,30 @@ fn negotiate_path(
} else { } else {
next.y -= 0.5; next.y -= 0.5;
} }
let mut direction = next - start; let direction = (next - start).normalize();
direction = direction.normalize(); let delta = time.delta_secs() * **speed;
direction *= **speed; let dir = Dir2::new_unchecked(direction);
let mut hits = vec![]; let hit = spatial_query.cast_shape(
spatial_query.shape_hits_callback(
collider, collider,
start, start,
global_transform.yaw().as_radians(), global_transform.yaw().as_radians(),
Dir2::new_unchecked(direction.normalize()), dir,
&ShapeCastConfig { &ShapeCastConfig {
max_distance: time.delta_secs(), max_distance: delta,
ignore_origin_penetration: true, ignore_origin_penetration: true,
..default() ..default()
}, },
&SpatialQueryFilter::from_excluded_entities(&sensors), &SpatialQueryFilter::from_excluded_entities(&sensors),
|hit| {
if obstructions.contains(hit.entity) {
hits.push(hit.entity);
}
true
},
); );
if !hits.is_empty() { if hit.is_some() {
println!("{entity} is stuck"); // println!("{entity} is stuck");
for entity in hits {
commands.entity(entity).log_components();
}
// TODO: Remove when we have an actual character controller. // TODO: Remove when we have an actual character controller.
next.x = next.x.trunc(); next.x = next.x.trunc();
next.y = next.y.trunc(); next.y = next.y.trunc();
transform.translation = next.extend(0.); transform.translation = next.extend(0.);
} else { } else {
let delta = direction * time.delta_secs(); // println!("Translating: {:?}", direction * delta);
navigation_actions.set_axis_pair(&NavigationAction::Translate, delta); navigation_actions.set_axis_pair(&NavigationAction::Translate, direction * delta);
} }
if rotation_speed.is_some() { if rotation_speed.is_some() {
let angle = direction.y.atan2(direction.x); let angle = direction.y.atan2(direction.x);
@ -313,8 +312,7 @@ fn actions(
if let Some(mut current_dest) = destination { if let Some(mut current_dest) = destination {
trace!("Got a destination"); trace!("Got a destination");
if *current_dest != dest { if *current_dest != dest {
trace trace!("{entity:?}: New destination {dest:?} differs from {current_dest:?}, zeroing velocity");
!("{entity:?}: New destination {dest:?} differs from {current_dest:?}, zeroing velocity");
navigation_action navigation_action
.set_axis_pair(&NavigationAction::SetLinearVelocity, Vec2::ZERO); .set_axis_pair(&NavigationAction::SetLinearVelocity, Vec2::ZERO);
*current_dest = dest; *current_dest = dest;
@ -337,10 +335,9 @@ impl Plugin for PathfindingPlugin {
.register_type::<NoPath>() .register_type::<NoPath>()
.register_type::<Path>() .register_type::<Path>()
.register_type::<CostMap>() .register_type::<CostMap>()
.add_systems(PreUpdate, negotiate_path)
.add_systems( .add_systems(
PreUpdate, FixedPreUpdate,
(actions, calculate_path) (actions, calculate_path, negotiate_path)
.chain() .chain()
.after(InputManagerSystem::Tick), .after(InputManagerSystem::Tick),
) )