419 lines
14 KiB
Rust
419 lines
14 KiB
Rust
use bevy::{
|
|
ecs::entity::Entities,
|
|
prelude::*,
|
|
tasks::{futures_lite::future, prelude::*, Task},
|
|
utils::HashMap,
|
|
};
|
|
use bevy_rapier2d::{
|
|
na::{Isometry2, Vector2},
|
|
prelude::*,
|
|
rapier::prelude::{ColliderHandle, ColliderSet, QueryPipeline, RigidBodySet},
|
|
};
|
|
use leafwing_input_manager::{axislike::DualAxisData, plugin::InputManagerSystem, prelude::*};
|
|
use pathfinding::prelude::*;
|
|
|
|
use crate::{
|
|
core::{PointLike, TransformExt},
|
|
map::{Map, MapObstruction},
|
|
navigation::{NavigationAction, RotationSpeed, Speed},
|
|
};
|
|
|
|
#[derive(PartialEq, Eq, Clone, Copy, Hash, Debug, Reflect)]
|
|
pub struct NegotiatePathAction;
|
|
|
|
impl Actionlike for NegotiatePathAction {}
|
|
|
|
#[derive(Component, Debug, Deref, DerefMut)]
|
|
struct Calculating(Task<Option<Path>>);
|
|
|
|
#[derive(Component, Clone, Copy, Debug, Default, Deref, DerefMut, Eq, Hash, PartialEq, Reflect)]
|
|
#[reflect(Component)]
|
|
pub struct Destination(pub (i32, i32));
|
|
|
|
impl_pointlike_for_tuple_component!(Destination);
|
|
impl_pointlike_for_tuple_component!(&Destination);
|
|
|
|
#[derive(Component, Clone, Debug, Default, Reflect)]
|
|
#[reflect(Component)]
|
|
pub struct NoPath;
|
|
|
|
#[derive(Component, Clone, Debug, Default, Deref, DerefMut, Reflect)]
|
|
#[reflect(Component)]
|
|
pub struct Path(pub Vec<(i32, i32)>);
|
|
|
|
#[derive(Component, Clone, Debug, Default, Reflect, Deref, DerefMut)]
|
|
#[reflect(Component)]
|
|
pub struct CostMap(pub HashMap<(i32, i32), f32>);
|
|
|
|
pub fn find_path<D: 'static + Clone + Default + Send + Sync>(
|
|
start: &dyn PointLike,
|
|
destination: &dyn PointLike,
|
|
map: &Map<D>,
|
|
cost_map: &Option<CostMap>,
|
|
) -> Option<(Vec<(i32, i32)>, u32)> {
|
|
astar(
|
|
&start.into(),
|
|
|p| {
|
|
let mut successors: Vec<((i32, i32), u32)> = vec![];
|
|
if p.0 >= 0 && p.1 >= 0 {
|
|
if let Some(tile) = map.at(p.0 as usize, p.1 as usize) {
|
|
if tile.is_walkable() {
|
|
for tile in map.get_available_exits(p.0 as usize, p.1 as usize) {
|
|
let mut cost = tile.2 * 100.;
|
|
if let Some(cost_map) = cost_map {
|
|
if let Some(modifier) =
|
|
cost_map.get(&(tile.0 as i32, tile.1 as i32))
|
|
{
|
|
cost *= modifier;
|
|
}
|
|
}
|
|
successors.push(((tile.0 as i32, tile.1 as i32), cost as u32));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
successors
|
|
},
|
|
|p| (p.distance_squared(destination) * 100.) as u32,
|
|
|p| *p == destination.into(),
|
|
)
|
|
}
|
|
|
|
fn find_path_for_shape(
|
|
initiator: ColliderHandle,
|
|
start: Transform,
|
|
destination: Destination,
|
|
cost_map: &Option<CostMap>,
|
|
query_pipeline: QueryPipeline,
|
|
collider_set: ColliderSet,
|
|
rigid_body_set: RigidBodySet,
|
|
shape: Collider,
|
|
) -> Option<Path> {
|
|
let path = astar(
|
|
&start.i32(),
|
|
|p| {
|
|
let mut successors: Vec<((i32, i32), u32)> = vec![];
|
|
let x = p.0;
|
|
let y = p.1;
|
|
let exits = vec![
|
|
((x - 1, y), 1.),
|
|
((x + 1, y), 1.),
|
|
((x, y - 1), 1.),
|
|
((x, y + 1), 1.),
|
|
((x - 1, y - 1), 1.5),
|
|
((x + 1, y - 1), 1.5),
|
|
((x - 1, y + 1), 1.5),
|
|
((x + 1, y + 1), 1.5),
|
|
];
|
|
for exit in &exits {
|
|
let mut should_push = true;
|
|
let dest = Isometry2::new(Vector2::new(exit.0 .0 as f32, exit.0 .1 as f32), 0.);
|
|
if query_pipeline
|
|
.intersection_with_shape(
|
|
&rigid_body_set,
|
|
&collider_set,
|
|
&dest,
|
|
&*shape.raw,
|
|
bevy_rapier2d::rapier::pipeline::QueryFilter::new()
|
|
.exclude_sensors()
|
|
.exclude_collider(initiator),
|
|
)
|
|
.is_some()
|
|
{
|
|
should_push = false;
|
|
}
|
|
if should_push {
|
|
let mut cost = exit.1 * 100.;
|
|
if let Some(cost_map) = cost_map {
|
|
if let Some(modifier) = cost_map.get(&(exit.0 .0, exit.0 .1)) {
|
|
cost *= modifier;
|
|
}
|
|
}
|
|
successors.push((exit.0, cost as u32));
|
|
}
|
|
}
|
|
successors
|
|
},
|
|
|p| (p.distance_squared(&destination) * 100.) as u32,
|
|
|p| *p == destination.i32(),
|
|
);
|
|
if let Some(path) = path {
|
|
Some(Path(path.0))
|
|
} else {
|
|
None
|
|
}
|
|
}
|
|
|
|
fn calculate_path(
|
|
mut commands: Commands,
|
|
rapier_context: Res<RapierContext>,
|
|
obstructions: Query<&RapierColliderHandle, With<MapObstruction>>,
|
|
query: Query<
|
|
(
|
|
Entity,
|
|
&RapierColliderHandle,
|
|
&Destination,
|
|
&Transform,
|
|
&Collider,
|
|
Option<&CostMap>,
|
|
),
|
|
Changed<Destination>,
|
|
>,
|
|
) {
|
|
for (entity, handle, destination, coordinates, shape, cost_map) in &query {
|
|
if coordinates.i32() == **destination {
|
|
commands
|
|
.entity(entity)
|
|
.remove::<Path>()
|
|
.remove::<NoPath>()
|
|
.remove::<Calculating>()
|
|
.remove::<Destination>();
|
|
continue;
|
|
}
|
|
trace!(
|
|
"{entity:?}: Calculating path from {:?} to {destination:?}",
|
|
coordinates.translation.truncate().i32()
|
|
);
|
|
let coordinates_clone = *coordinates;
|
|
let destination_clone = *destination;
|
|
let query_pipeline = rapier_context.query_pipeline.clone();
|
|
let cost_map_clone = cost_map.cloned();
|
|
let handle_clone = *handle;
|
|
let mut collider_set = rapier_context.colliders.clone();
|
|
let mut to_remove = vec![handle.0];
|
|
for handle in collider_set.iter() {
|
|
if !obstructions.iter().map(|v| v.0).any(|x| x == handle.0) {
|
|
to_remove.push(handle.0);
|
|
}
|
|
}
|
|
let mut bodies = rapier_context.bodies.clone();
|
|
if !to_remove.is_empty() {
|
|
let mut islands = rapier_context.islands.clone();
|
|
for handle in to_remove {
|
|
collider_set.remove(handle, &mut islands, &mut bodies, false);
|
|
}
|
|
}
|
|
let shape_clone = (*shape).clone();
|
|
trace!(
|
|
"{entity:?}: path: calculating from {:?} to {destination:?}",
|
|
coordinates.i32(),
|
|
);
|
|
let pool = AsyncComputeTaskPool::get();
|
|
let task = pool.spawn(async move {
|
|
find_path_for_shape(
|
|
handle_clone.0,
|
|
coordinates_clone,
|
|
destination_clone,
|
|
&cost_map_clone,
|
|
query_pipeline,
|
|
collider_set,
|
|
bodies,
|
|
shape_clone,
|
|
)
|
|
});
|
|
commands
|
|
.entity(entity)
|
|
.insert(Calculating(task))
|
|
.remove::<Path>()
|
|
.remove::<NoPath>();
|
|
}
|
|
}
|
|
|
|
fn poll_tasks(
|
|
mut commands: Commands,
|
|
mut query: Query<(Entity, &mut Calculating, &Transform, &Destination)>,
|
|
) {
|
|
for (entity, mut calculating, transform, destination) in &mut query {
|
|
if let Some(result) = future::block_on(future::poll_once(&mut **calculating)) {
|
|
if let Some(path) = result {
|
|
trace!("{entity:?}: Path: {path:?}");
|
|
commands.entity(entity).insert(path);
|
|
} else {
|
|
trace!(
|
|
"{entity:?}: path: no path from {:?} to {:?}",
|
|
transform.translation.truncate().i32(),
|
|
**destination
|
|
);
|
|
commands.entity(entity).insert(NoPath);
|
|
}
|
|
commands.entity(entity).remove::<Calculating>();
|
|
}
|
|
}
|
|
}
|
|
|
|
fn remove_destination(
|
|
mut commands: Commands,
|
|
entities: &Entities,
|
|
mut removed: RemovedComponents<Destination>,
|
|
) {
|
|
for entity in removed.read() {
|
|
if entities.contains(entity) {
|
|
commands.entity(entity).remove::<Calculating>();
|
|
}
|
|
}
|
|
}
|
|
|
|
fn negotiate_path(
|
|
mut commands: Commands,
|
|
time: Res<Time>,
|
|
physics_config: Res<RapierConfiguration>,
|
|
mut query: Query<(
|
|
Entity,
|
|
&mut ActionState<NavigationAction>,
|
|
&mut Path,
|
|
&mut Transform,
|
|
&Collider,
|
|
&Speed,
|
|
Option<&RotationSpeed>,
|
|
)>,
|
|
rapier_context: Res<RapierContext>,
|
|
) {
|
|
if !physics_config.physics_pipeline_active || !physics_config.query_pipeline_active {
|
|
return;
|
|
}
|
|
for (entity, mut actions, mut path, mut transform, collider, speed, rotation_speed) in
|
|
&mut query
|
|
{
|
|
let start_i32 = transform.translation.truncate().i32();
|
|
trace!(
|
|
"{entity:?}: start pathfinding from {start_i32:?} to {:?}: at {:?}",
|
|
path.last(),
|
|
transform.translation.truncate().i32()
|
|
);
|
|
if path.len() > 0 && path[0] == start_i32 {
|
|
trace!("At start, removing");
|
|
path.remove(0);
|
|
}
|
|
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 {
|
|
next.x -= 0.5;
|
|
}
|
|
if next.y >= 0. {
|
|
next.y += 0.5;
|
|
} else {
|
|
next.y -= 0.5;
|
|
}
|
|
let mut direction = next - start;
|
|
direction = direction.normalize();
|
|
direction *= **speed;
|
|
direction *= time.delta_seconds();
|
|
trace!(
|
|
"{entity:?}: Direction: {direction:?}, Distance: {}",
|
|
(next - start).length()
|
|
);
|
|
if rapier_context
|
|
.cast_shape(
|
|
start,
|
|
transform.yaw().radians(),
|
|
direction,
|
|
collider,
|
|
1.,
|
|
true,
|
|
QueryFilter::new()
|
|
.exclude_sensors()
|
|
.exclude_collider(entity),
|
|
)
|
|
.is_some()
|
|
{
|
|
// 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::Translate);
|
|
actions
|
|
.action_data_mut_or_default(&NavigationAction::Translate)
|
|
.axis_pair = Some(DualAxisData::from_xy(direction));
|
|
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>()
|
|
.remove::<NoPath>()
|
|
.remove::<Destination>();
|
|
trace!("{entity:?}: pathfinding: cleaned up");
|
|
}
|
|
}
|
|
}
|
|
|
|
fn actions(
|
|
mut commands: Commands,
|
|
mut query: Query<(
|
|
Entity,
|
|
&mut ActionState<NegotiatePathAction>,
|
|
&mut ActionState<NavigationAction>,
|
|
Option<&mut Destination>,
|
|
)>,
|
|
) {
|
|
for (entity, mut actions, mut navigation_action, destination) in &mut query {
|
|
if actions.pressed(&NegotiatePathAction) {
|
|
if let Some(pair) = actions.axis_pair(&NegotiatePathAction) {
|
|
trace!("{entity:?}: Negotiating path to {pair:?}");
|
|
let dest = Destination(pair.xy().i32());
|
|
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");
|
|
navigation_action.press(&NavigationAction::SetLinearVelocity);
|
|
navigation_action
|
|
.action_data_mut_or_default(&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_or_default(&NavigationAction::SetLinearVelocity)
|
|
.axis_pair = Some(DualAxisData::from_xy(Vec2::ZERO));
|
|
commands.entity(entity).insert(dest);
|
|
}
|
|
} else if destination.is_some() {
|
|
trace!("No value, resetting");
|
|
commands
|
|
.entity(entity)
|
|
.remove::<Destination>()
|
|
.remove::<Path>()
|
|
.remove::<NoPath>();
|
|
}
|
|
actions.release(&NegotiatePathAction);
|
|
actions
|
|
.action_data_mut_or_default(&NegotiatePathAction)
|
|
.axis_pair = None;
|
|
}
|
|
}
|
|
}
|
|
|
|
pub struct PathfindingPlugin;
|
|
|
|
impl Plugin for PathfindingPlugin {
|
|
fn build(&self, app: &mut App) {
|
|
app.add_plugins(InputManagerPlugin::<NegotiatePathAction>::default())
|
|
.register_type::<Destination>()
|
|
.register_type::<NoPath>()
|
|
.register_type::<Path>()
|
|
.register_type::<CostMap>()
|
|
.add_systems(
|
|
PreUpdate,
|
|
(poll_tasks, apply_deferred, negotiate_path).chain(),
|
|
)
|
|
.add_systems(
|
|
PreUpdate,
|
|
(actions, apply_deferred, calculate_path)
|
|
.chain()
|
|
.after(InputManagerSystem::Tick),
|
|
)
|
|
.add_systems(PostUpdate, remove_destination);
|
|
}
|
|
}
|