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

Action-based pathfinding.

Browse Source
This commit is contained in:
Nolan Darilek 2023-03-28 08:49:17 -05:00
parent 041d9ffe7d
commit 30250a4310
2 changed files with 117 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/navigation.rs
View File

@ -119,12 +119,6 @@ fn controls(
if let Some(pair) = actions.clamped_axis_pair(NavigationAction::Move) { if let Some(pair) = actions.clamped_axis_pair(NavigationAction::Move) {
cleanup = true; cleanup = true;
let mut direction = pair.xy(); 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 = let forward_movement_factor =
forward_movement_factor.map(|v| v.0).unwrap_or_else(|| 1.); forward_movement_factor.map(|v| v.0).unwrap_or_else(|| 1.);
let backward_movement_factor = let backward_movement_factor =
@ -161,6 +155,7 @@ fn controls(
.action_data_mut(NavigationAction::Translate) .action_data_mut(NavigationAction::Translate)
.axis_pair = Some(DualAxisData::from_xy(translation)); .axis_pair = Some(DualAxisData::from_xy(translation));
} else { } else {
// println!("{entity:?}: SetLinearVelocity: {velocity:?}");
actions.press(NavigationAction::SetLinearVelocity); actions.press(NavigationAction::SetLinearVelocity);
actions actions
.action_data_mut(NavigationAction::SetLinearVelocity) .action_data_mut(NavigationAction::SetLinearVelocity)
@ -175,13 +170,14 @@ fn controls(
} }
if actions.pressed(NavigationAction::SetLinearVelocity) { if actions.pressed(NavigationAction::SetLinearVelocity) {
if let Some(pair) = actions.axis_pair(NavigationAction::SetLinearVelocity) { if let Some(pair) = actions.axis_pair(NavigationAction::SetLinearVelocity) {
trace!("{entity:?}: SetLinearVelocity: {pair:?}"); // println!("{entity:?}: SetLinearVelocity: {pair:?}");
velocity.linvel = pair.into(); velocity.linvel = pair.into();
} else { } else {
// println!("{entity:?}: SetLinearVelocity: 0");
velocity.linvel = Vec2::ZERO; velocity.linvel = Vec2::ZERO;
} }
} else if actions.just_released(NavigationAction::SetLinearVelocity) { } else if actions.just_released(NavigationAction::SetLinearVelocity) {
trace!("{entity:?}: Released velocity"); // println!("{entity:?}: Released velocity");
velocity.linvel = Vec2::ZERO; velocity.linvel = Vec2::ZERO;
actions actions
.action_data_mut(NavigationAction::SetLinearVelocity) .action_data_mut(NavigationAction::SetLinearVelocity)

174
src/pathfinding.rs
View File

@ -15,7 +15,7 @@ use pathfinding::prelude::*;
use crate::{ use crate::{
commands::RunIfExistsExt, commands::RunIfExistsExt,
core::PointLike, core::{PointLike, TransformExt},
map::{Map, MapObstruction}, map::{Map, MapObstruction},
navigation::{NavigationAction, RotationSpeed}, navigation::{NavigationAction, RotationSpeed},
}; };
@ -123,22 +123,29 @@ fn find_path_for_shape(
((x - 1, y + 1), 1.5), ((x - 1, y + 1), 1.5),
((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 { for exit in &exits {
let mut should_push = true; let mut should_push = true;
let shape_pos = let dest = Vector2::new(exit.0 .0 as f32, exit.0 .1 as f32);
Isometry2::new(Vector2::new(exit.0 .0 as f32, exit.0 .1 as f32), 0.); let shape_vel = dest - pos;
query_pipeline.intersections_with_shape( let max_toi = 1.;
&rigid_body_set, if query_pipeline
&collider_set, .cast_shape(
&shape_pos, &rigid_body_set,
&*shape.raw, &collider_set,
bevy_rapier2d::rapier::pipeline::QueryFilter::new() &shape_pos,
.predicate(&|h, _c| h != initiator), &shape_vel,
|_handle| { &*shape.raw,
should_push = false; max_toi,
false true,
}, bevy_rapier2d::rapier::pipeline::QueryFilter::new()
); .predicate(&|h, _c| h != initiator),
)
.is_some()
{
should_push = false;
}
if should_push { 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 {
@ -187,6 +194,10 @@ fn calculate_path(
.remove::<Destination>(); .remove::<Destination>();
continue; continue;
} }
trace!(
"Calculating path from {:?}",
coordinates.translation.truncate().i32()
);
let coordinates_clone = *coordinates; let coordinates_clone = *coordinates;
let destination_clone = *destination; let destination_clone = *destination;
let query_pipeline = rapier_context.query_pipeline.clone(); let query_pipeline = rapier_context.query_pipeline.clone();
@ -208,10 +219,8 @@ fn calculate_path(
} }
let shape_clone = (*shape).clone(); let shape_clone = (*shape).clone();
trace!( trace!(
"{:?}: path: calculating from {:?} to {:?}", "{entity:?}: path: calculating from {:?} to {destination:?}",
entity,
coordinates.i32(), coordinates.i32(),
destination
); );
let pool = AsyncComputeTaskPool::get(); let pool = AsyncComputeTaskPool::get();
let task = pool.spawn(async move { 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() { for (entity, mut calculating) in query.iter_mut() {
if let Some(result) = future::block_on(future::poll_once(&mut **calculating)) { if let Some(result) = future::block_on(future::poll_once(&mut **calculating)) {
if let Some(path) = result { if let Some(path) = result {
trace!("{:?}: path: result: {:?}", entity, path); trace!("{entity:?}: Path: {path:?}");
commands.entity(entity).insert(path); commands.entity(entity).insert(path);
} else { } else {
trace!("{:?}: path: no path", entity); trace!("{entity:?}: path: no path");
commands.entity(entity).insert(NoPath); commands.entity(entity).insert(NoPath);
} }
commands.entity(entity).remove::<Calculating>(); commands.entity(entity).remove::<Calculating>();
@ -268,53 +277,74 @@ fn negotiate_path(
&mut ActionState<NavigationAction>, &mut ActionState<NavigationAction>,
&mut Path, &mut Path,
&mut Transform, &mut Transform,
&Collider,
Option<&RotationSpeed>, Option<&RotationSpeed>,
)>, )>,
rapier_context: Res<RapierContext>,
) { ) {
for (entity, mut actions, mut path, mut transform, rotation_speed) in &mut query { for (entity, mut actions, mut path, mut transform, collider, rotation_speed) in &mut query {
let start_i32 = (transform.translation.x, transform.translation.y).i32(); let start_i32 = transform.translation.truncate().i32();
trace!( trace!(
"{entity:?}: start pathfinding from {start_i32:?} to {:?}: {:?}", "{entity:?}: start pathfinding from {start_i32:?} to {:?}: {:?}",
path.last(), path.last(),
transform.translation.truncate() transform.translation.truncate()
); );
let mut cleanup = false; if path.len() > 0 {
if let Some(last) = path.last() { if path.len() > 1 && path[0] != start_i32 && path[1] != start_i32 {
if last == &start_i32 { trace!("Off path");
trace!("{entity:?}: path: at destination"); } else if path.len() > 1 && path[1] == start_i32 {
cleanup = true; 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(next) = path.first() {
if let Some(position) = path.iter().position(|v| v == &start_i32) { trace!("{entity:?}: path: moving from {start_i32:?} to {next:?}");
trace!("{entity:?}: Path contains start"); let start = transform.translation.truncate();
let (_, new_path) = path.split_at(position + 1); let mut next = Vec2::new(next.0 as f32, next.1 as f32);
**path = new_path.to_vec(); if next.x > 0. {
} next.x += 0.5;
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);
}
} else { } else {
trace!("{entity:?}: empty path, cleaning"); next -= 0.5;
cleanup = true;
} }
} if next.y > 0. {
if cleanup { 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 commands
.entity(entity) .entity(entity)
.remove::<Path>() .remove::<Path>()
@ -330,24 +360,41 @@ fn actions(
mut commands: Commands, mut commands: Commands,
mut query: Query<( mut query: Query<(
Entity, Entity,
&ActionState<NegotiatePathAction>, &mut ActionState<NegotiatePathAction>,
&mut ActionState<NavigationAction>,
Option<&mut Destination>, 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 actions.pressed(NegotiatePathAction) {
if let Some(pair) = actions.axis_pair(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)); let dest = Destination((pair.x() as i32, pair.y() as i32));
if let Some(mut current_dest) = destination { if let Some(mut current_dest) = destination {
if *current_dest != dest { 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; *current_dest = dest;
} }
} else { } 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); commands.entity(entity).insert(dest);
} }
} else if destination.is_some() { } else if destination.is_some() {
commands.entity(entity).remove::<Destination>(); commands
.entity(entity)
.remove::<Destination>()
.remove::<NoPath>();
} }
actions.release(NegotiatePathAction);
actions.action_data_mut(NegotiatePathAction).axis_pair = None;
} }
} }
} }
@ -361,13 +408,18 @@ impl Plugin for PathfindingPlugin {
.register_type::<NoPath>() .register_type::<NoPath>()
.register_type::<Path>() .register_type::<Path>()
.register_type::<CostMap>() .register_type::<CostMap>()
.add_system(calculate_path) .add_system_to_stage(CoreStage::PreUpdate, calculate_path)
.add_system_to_stage(CoreStage::PostUpdate, remove_destination) .add_system_to_stage(CoreStage::PostUpdate, remove_destination)
.add_system(poll_tasks) .add_system_to_stage(CoreStage::PreUpdate, poll_tasks)
.add_system_to_stage( .add_system_to_stage(
CoreStage::PreUpdate, CoreStage::PreUpdate,
negotiate_path.after(InputManagerSystem::Tick), negotiate_path.after(InputManagerSystem::Tick),
) )
.add_system(actions); .add_system_to_stage(
CoreStage::PreUpdate,
actions
.after(InputManagerSystem::Update)
.before(calculate_path),
);
} }
} }