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

174
src/pathfinding.rs
View File

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