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 bevy::{prelude::*, tasks::prelude::*};
use bevy_rapier2d::prelude::*;
use bevy_rapier2d::{na::UnitComplex, prelude::*};
use crossbeam_channel::{unbounded, Receiver};
use derive_more::{Deref, DerefMut};
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(
mut commands: Commands,
pool: Res<AsyncComputeTaskPool>,
@ -117,26 +80,24 @@ fn calculate_path(
}
}
/*fn negotiate_path(
fn negotiate_path(
mut commands: Commands,
time: Res<Time>,
mut query: Query<(
Entity,
&mut Path,
&mut Coordinates,
&mut RigidBodyPosition,
&mut RigidBodyVelocity,
&Speed,
Option<&RotationSpeed>,
&mut Transform,
)>,
map: Query<&Map>,
) {
for (entity, mut path, mut coordinates, mut velocity, speed, rotation_speed, mut transform) in
query.iter_mut()
{
for map in map.iter() {
for (entity, mut path, mut position, mut velocity, speed, rotation_speed) in query.iter_mut() {
let mut new_path = path.0.clone();
let start_i32 = coordinates.i32();
let start_i32 = (
position.position.translation.x,
position.position.translation.y,
)
.i32();
let new_path_clone = new_path.clone();
let mut iter = new_path_clone.split(|p| *p == start_i32);
if iter.next().is_some() {
@ -147,63 +108,34 @@ fn calculate_path(
}
**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 start = Vec2::new(
position.position.translation.x,
position.position.translation.y,
);
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);
position.position.rotation = UnitComplex::new(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();
}
}
}
}*/
}
pub struct PathfindingPlugin;
impl Plugin for PathfindingPlugin {
fn build(&self, app: &mut AppBuilder) {
app.add_system_to_stage(CoreStage::PostUpdate, calculate_path.system());
//.add_system(negotiate_path.system());
app.add_system_to_stage(CoreStage::PostUpdate, calculate_path.system())
.add_system(negotiate_path.system());
}
}