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

Get pathfinding working.

Browse Source
This commit is contained in:
Nolan Darilek 2024-12-10 13:17:55 -06:00
parent 850c5a5cdd
commit 330d915267
2 changed files with 74 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/navigation.rs
View File

@ -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);

97
src/pathfinding.rs
View File

@ -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<D: 'static + Clone + Default + Send + Sync>(
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<Destination>,
>,
obstructions: Query<Entity, With<MapObstruction>>,
sensors: Query<Entity, With<Sensor>>,
) {
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::<Path>()
@ -103,10 +105,6 @@ fn calculate_path(
.remove::<Destination>();
continue;
}
trace!(
"{entity:?}: Calculating path from {:?} to {destination:?}",
transform.translation().truncate().as_ivec2()
);
commands.entity(entity).remove::<Path>().remove::<NoPath>();
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<Des
fn negotiate_path(
mut commands: Commands,
time: Res<Time>,
physics_time: Res<Time<Physics>>,
spatial_query: SpatialQuery,
mut query: Query<(
Entity,
@ -183,17 +194,24 @@ fn negotiate_path(
&mut ActionState<NavigationAction>,
&mut Path,
&mut Transform,
&GlobalTransform,
&Collider,
&Speed,
Option<&RotationSpeed>,
)>,
obstructions: Query<Entity, With<MapObstruction>>,
sensors: Query<Entity, With<Sensor>>,
) {
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);