Use physics for pathfinding.

This commit is contained in:
Nolan Darilek 2021-06-15 17:48:49 -05:00
parent 7545dd6bd3
commit 2e805b3b8b

View File

@ -1,7 +1,7 @@
use std::collections::HashMap; use std::collections::HashMap;
use bevy::{prelude::*, tasks::prelude::*}; use bevy::{prelude::*, tasks::prelude::*};
use bevy_rapier2d::prelude::*; use bevy_rapier2d::{na::UnitComplex, prelude::*};
use crossbeam_channel::{unbounded, Receiver}; use crossbeam_channel::{unbounded, Receiver};
use derive_more::{Deref, DerefMut}; use derive_more::{Deref, DerefMut};
use pathfinding::prelude::*; use pathfinding::prelude::*;
@ -41,43 +41,6 @@ pub fn find_path(
) )
} }
fn nearest_extreme(from: f32, to: i32) -> f32 {
let to = to as f32;
let range = to..=(to + 0.999);
if from <= *range.start() {
*range.start()
} else {
*range.end()
}
}
fn cheat_assign(
start: (f32, f32),
end: (i32, i32),
map_width: usize,
motion_blocked: Vec<bool>,
) -> Option<(f32, f32)> {
let x;
let y;
if start.0 as i32 == end.0 {
x = start.0;
} else {
x = nearest_extreme(start.0, end.0);
}
if start.1 as i32 == end.1 {
y = start.1;
} else {
y = nearest_extreme(start.1, end.1);
}
let point = (x, y);
let index = point.to_index(map_width);
if motion_blocked[index] {
None
} else {
Some(point)
}
}
fn calculate_path( fn calculate_path(
mut commands: Commands, mut commands: Commands,
pool: Res<AsyncComputeTaskPool>, pool: Res<AsyncComputeTaskPool>,
@ -117,93 +80,62 @@ fn calculate_path(
} }
} }
/*fn negotiate_path( fn negotiate_path(
mut commands: Commands, mut commands: Commands,
time: Res<Time>,
mut query: Query<( mut query: Query<(
Entity, Entity,
&mut Path, &mut Path,
&mut Coordinates, &mut RigidBodyPosition,
&mut RigidBodyVelocity, &mut RigidBodyVelocity,
&Speed, &Speed,
Option<&RotationSpeed>, Option<&RotationSpeed>,
&mut Transform,
)>, )>,
map: Query<&Map>,
) { ) {
for (entity, mut path, mut coordinates, mut velocity, speed, rotation_speed, mut transform) in for (entity, mut path, mut position, mut velocity, speed, rotation_speed) in query.iter_mut() {
query.iter_mut() let mut new_path = path.0.clone();
{ let start_i32 = (
for map in map.iter() { position.position.translation.x,
let mut new_path = path.0.clone(); position.position.translation.y,
let start_i32 = coordinates.i32(); )
let new_path_clone = new_path.clone(); .i32();
let mut iter = new_path_clone.split(|p| *p == start_i32); let new_path_clone = new_path.clone();
if iter.next().is_some() { let mut iter = new_path_clone.split(|p| *p == start_i32);
if let Some(upcoming) = iter.next() { if iter.next().is_some() {
new_path = vec![start_i32]; if let Some(upcoming) = iter.next() {
new_path.append(&mut upcoming.to_vec()); new_path = vec![start_i32];
} new_path.append(&mut upcoming.to_vec());
}
**path = new_path;
if path.len() >= 2 {
let start = **coordinates;
let start_index = start.to_index(map.width());
let start = Vec2::new(start.0, start.1);
let next = path[1];
if motion_blocked[next.to_index(map.width())] {
// TODO: Should probably handle.
}
let next = Vec2::new(next.0 as f32, next.1 as f32);
if rotation_speed.is_some() {
let start = start.floor();
let v = next - start;
let angle = v.y.atan2(v.x);
transform.rotation = Quat::from_rotation_z(angle);
}
let mut direction = next - start;
direction = direction.normalize();
direction *= speed.0;
let displacement = direction * time.delta_seconds();
let dest = start + displacement;
let dest = (dest.x, dest.y);
let index = dest.to_index(map.width());
if start_index != index && motion_blocked[index] {
let (normal_x, normal_y) = **coordinates;
let next = path[1];
if let Some((cheat_x, cheat_y)) =
cheat_assign(**coordinates, next, map.width(), motion_blocked.0.clone())
{
let index = (normal_x, cheat_y).to_index(map.width());
if !motion_blocked.0[index] {
**coordinates = (normal_x, cheat_y);
return;
}
let index = (cheat_x, normal_y).to_index(map.width());
if !motion_blocked.0[index] {
**coordinates = (cheat_x, normal_y);
return;
}
**coordinates = (cheat_x, cheat_y);
}
velocity.linvel = Vec2::ZERO.into();
} else {
velocity.linvel = direction.into();
}
} else {
commands.entity(entity).remove::<Path>();
commands.entity(entity).remove::<Destination>();
velocity.linvel = Vec2::ZERO.into();
} }
} }
**path = new_path;
if path.len() >= 2 {
let start = Vec2::new(
position.position.translation.x,
position.position.translation.y,
);
let next = path[1];
let next = Vec2::new(next.0 as f32, next.1 as f32);
if rotation_speed.is_some() {
let v = next - start;
let angle = v.y.atan2(v.x);
position.position.rotation = UnitComplex::new(angle);
}
let mut direction = next - start;
direction = direction.normalize();
direction *= speed.0;
velocity.linvel = direction.into();
} else {
commands.entity(entity).remove::<Path>();
commands.entity(entity).remove::<Destination>();
velocity.linvel = Vec2::ZERO.into();
}
} }
}*/ }
pub struct PathfindingPlugin; pub struct PathfindingPlugin;
impl Plugin for PathfindingPlugin { impl Plugin for PathfindingPlugin {
fn build(&self, app: &mut AppBuilder) { fn build(&self, app: &mut AppBuilder) {
app.add_system_to_stage(CoreStage::PostUpdate, calculate_path.system()); app.add_system_to_stage(CoreStage::PostUpdate, calculate_path.system())
//.add_system(negotiate_path.system()); .add_system(negotiate_path.system());
} }
} }