Pathfinding takes physics into account.

This commit is contained in:
Nolan Darilek 2021-06-28 10:04:53 -05:00
parent 64499a6278
commit a5fdea3292
4 changed files with 189 additions and 25 deletions

View File

@ -23,7 +23,7 @@ features = [
backtrace = "0.3" backtrace = "0.3"
bevy_input_actionmap = { path = "../bevy_input_actionmap" } bevy_input_actionmap = { path = "../bevy_input_actionmap" }
bevy_openal = { path = "../bevy_openal" } bevy_openal = { path = "../bevy_openal" }
bevy_rapier2d = { git = "https://github.com/ndarilek/bevy_rapier", features = ["enhanced-determinism", "serde-serialize"] } bevy_rapier2d = { git = "https://github.com/ndarilek/bevy_rapier", features = ["serde-serialize"] }
bevy_tts = { path = "../bevy_tts" } bevy_tts = { path = "../bevy_tts" }
coord_2d = "0.3" coord_2d = "0.3"
crossbeam-channel = "0.5" crossbeam-channel = "0.5"

View File

@ -429,6 +429,16 @@ impl PointLike for Transform {
} }
} }
impl PointLike for Vec2 {
fn x(&self) -> f32 {
self.x
}
fn y(&self) -> f32 {
self.y
}
}
impl PointLike for (i32, i32) { impl PointLike for (i32, i32) {
fn x(&self) -> f32 { fn x(&self) -> f32 {
self.0 as f32 self.0 as f32

View File

@ -20,9 +20,14 @@ impl From<mapgen::geometry::Point> for Coordinates {
Self((point.x as f32, point.y as f32)) Self((point.x as f32, point.y as f32))
} }
} }
#[derive(Clone, Debug, Default, Deref, DerefMut)] #[derive(Clone, Debug, Default, Deref, DerefMut)]
pub struct Areas(pub Vec<Area>); pub struct Areas(pub Vec<Area>);
#[derive(Clone, Debug, Default, Reflect)]
#[reflect(Component)]
pub struct MapObstruction;
#[derive(Clone, Debug, Default, Reflect)] #[derive(Clone, Debug, Default, Reflect)]
#[reflect(Component)] #[reflect(Component)]
pub struct Portal; pub struct Portal;
@ -184,8 +189,7 @@ fn add_map_colliders(mut commands: Commands, maps: Query<(Entity, &Map), Added<M
let tile = map.at(x, y); let tile = map.at(x, y);
if tile.blocks_motion() { if tile.blocks_motion() {
let id = commands let id = commands
.spawn() .spawn_bundle(ColliderBundle {
.insert_bundle(ColliderBundle {
shape: ColliderShape::cuboid(0.5, 0.5), shape: ColliderShape::cuboid(0.5, 0.5),
..Default::default() ..Default::default()
}) })
@ -193,6 +197,7 @@ fn add_map_colliders(mut commands: Commands, maps: Query<(Entity, &Map), Added<M
handle: rigid_body_entity.handle(), handle: rigid_body_entity.handle(),
pos_wrt_parent: Vec2::new(x as f32 + 0.5, y as f32 + 0.5).into(), pos_wrt_parent: Vec2::new(x as f32 + 0.5, y as f32 + 0.5).into(),
}) })
.insert(MapObstruction)
.id(); .id();
if tile.blocks_visibility() { if tile.blocks_visibility() {
commands.entity(id).insert(BlocksVisibility); commands.entity(id).insert(BlocksVisibility);
@ -247,8 +252,7 @@ fn portal_spawner(
let y = portal.1 as f32; let y = portal.1 as f32;
let coordinates = Coordinates((x, y)); let coordinates = Coordinates((x, y));
let portal = commands let portal = commands
.spawn() .spawn_bundle(PortalBundle {
.insert_bundle(PortalBundle {
coordinates, coordinates,
..Default::default() ..Default::default()
}) })

View File

@ -1,14 +1,18 @@
use std::collections::HashMap; use std::collections::HashMap;
use bevy::{prelude::*, tasks::prelude::*}; use bevy::{prelude::*, tasks::prelude::*};
use bevy_rapier2d::{na::UnitComplex, prelude::*}; use bevy_rapier2d::{
use crossbeam_channel::{unbounded, Receiver}; na::UnitComplex,
prelude::*,
rapier::data::{ComponentSet, ComponentSetOption, Index},
};
use crossbeam_channel::{unbounded, Receiver, Sender};
use derive_more::{Deref, DerefMut}; use derive_more::{Deref, DerefMut};
use pathfinding::prelude::*; use pathfinding::prelude::*;
use crate::{ use crate::{
core::{Coordinates, PointLike}, core::{Coordinates, PointLike},
map::Map, map::{Map, MapObstruction},
navigation::{RotationSpeed, Speed}, navigation::{RotationSpeed, Speed},
}; };
@ -41,42 +45,188 @@ pub fn find_path(
) )
} }
struct StaticColliderComponentsSet(
HashMap<Entity, (ColliderPosition, ColliderShape, ColliderFlags)>,
);
impl
From<(
&Query<'_, (Entity, &ColliderPosition, &SharedShape, &ColliderFlags)>,
&Query<'_, &MapObstruction>,
)> for StaticColliderComponentsSet
{
fn from(
query: (
&Query<(Entity, &ColliderPosition, &SharedShape, &ColliderFlags)>,
&Query<&MapObstruction>,
),
) -> Self {
let entries = query
.0
.iter()
.filter(|(a, _, _, _)| query.1.get(*a).is_ok())
.map(|(a, b, c, d)| (a, b.clone(), c.clone(), d.clone()))
.collect::<Vec<(Entity, ColliderPosition, ColliderShape, ColliderFlags)>>();
let mut m = HashMap::new();
for (e, a, b, c) in entries {
m.insert(e, (a, b, c));
}
Self(m)
}
}
impl ComponentSet<SharedShape> for StaticColliderComponentsSet {
fn size_hint(&self) -> usize {
self.0.len()
}
fn for_each(&self, _f: impl FnMut(Index, &SharedShape)) {
unimplemented!()
}
}
impl ComponentSetOption<SharedShape> for StaticColliderComponentsSet {
fn get(&self, index: Index) -> Option<&SharedShape> {
self.0.get(&index.entity()).map(|v| &v.1)
}
}
impl ComponentSet<ColliderFlags> for StaticColliderComponentsSet {
fn size_hint(&self) -> usize {
self.0.len()
}
fn for_each(&self, _f: impl FnMut(Index, &ColliderFlags)) {
unimplemented!()
}
}
impl ComponentSetOption<ColliderFlags> for StaticColliderComponentsSet {
fn get(&self, index: Index) -> Option<&ColliderFlags> {
self.0.get(&index.entity()).map(|v| &v.2)
}
}
impl ComponentSet<ColliderPosition> for StaticColliderComponentsSet {
fn size_hint(&self) -> usize {
self.0.len()
}
fn for_each(&self, _f: impl FnMut(Index, &ColliderPosition)) {
unimplemented!()
}
}
impl ComponentSetOption<ColliderPosition> for StaticColliderComponentsSet {
fn get(&self, index: Index) -> Option<&ColliderPosition> {
let v = self.0.get(&index.entity()).map(|v| &v.0);
v
}
}
fn find_path_for_shape(
pool: &AsyncComputeTaskPool,
query_pipeline: QueryPipeline,
initiator: Entity,
start: &dyn PointLike,
destination: &dyn PointLike,
map: &Map,
collider_query: &QueryPipelineColliderComponentsQuery,
obstructions: &Query<&MapObstruction>,
shape: &SharedShape,
channel: &Sender<Option<Path>>,
) {
let collider_set: StaticColliderComponentsSet = (collider_query, obstructions).into();
let start = start.i32();
let destination = destination.i32();
let map_clone = map.clone();
let shape_clone = shape.clone();
let channel_clone = channel.clone();
pool.spawn(async move {
let path = astar(
&start,
|p| {
let mut successors: Vec<((i32, i32), u32)> = vec![];
for tile in map_clone.get_available_exits(p.0 as usize, p.1 as usize) {
let mut should_push = true;
let shape_pos = Isometry::new(vector![tile.0 as f32, tile.1 as f32], 0.);
query_pipeline.intersections_with_shape(
&collider_set,
&shape_pos,
&*shape_clone,
InteractionGroups::all(),
Some(&|v| v.entity() != initiator),
|_handle| {
should_push = false;
false
},
);
if should_push {
successors.push(((tile.0 as i32, tile.1 as i32), (tile.2 * 100.) as u32));
}
}
successors
},
|p| (p.distance_squared(&destination) * 100.) as u32,
|p| *p == destination.into(),
);
channel_clone
.send(if let Some(path) = path {
Some(Path(path.0))
} else {
None
})
.expect("Channel should exist");
})
.detach();
}
fn calculate_path( fn calculate_path(
mut commands: Commands, mut commands: Commands,
pool: Res<AsyncComputeTaskPool>, pool: Res<AsyncComputeTaskPool>,
mut calculating: Local<HashMap<Entity, Receiver<Path>>>, query_pipeline: Res<QueryPipeline>,
query: Query<(Entity, &Destination, &Coordinates), Or<(Without<Path>, Changed<Destination>)>>, obstructions: Query<&MapObstruction>,
collider_query: QueryPipelineColliderComponentsQuery,
mut calculating: Local<HashMap<Entity, Receiver<Option<Path>>>>,
query: Query<
(Entity, &Destination, &Coordinates, &ColliderShape),
Or<(Without<Path>, Changed<Destination>)>,
>,
destinations: Query<&Destination>, destinations: Query<&Destination>,
map: Query<&Map>, map: Query<&Map>,
) { ) {
let calculating_clone = calculating.clone(); let calculating_clone = calculating.clone();
for (entity, rx) in calculating_clone.iter() { for (entity, rx) in calculating_clone.iter() {
if destinations.get(*entity).is_ok() { if destinations.get(*entity).is_ok() {
if let Ok(path) = rx.try_recv() { if let Ok(msg) = rx.try_recv() {
commands.entity(*entity).insert(path); if let Some(path) = msg {
commands.entity(*entity).insert(path);
} else {
commands.entity(*entity).remove::<Destination>();
}
calculating.remove(&entity); calculating.remove(&entity);
} else {
commands.entity(*entity).remove::<Destination>();
} }
} else { } else {
calculating.remove(&entity); calculating.remove(&entity);
} }
} }
for (entity, destination, coordinates) in query.iter() { for (entity, destination, coordinates, shape) in query.iter() {
if !calculating.contains_key(&entity) { if !calculating.contains_key(&entity) {
let (tx, rx) = unbounded(); let (tx, rx) = unbounded();
calculating.insert(entity, rx); calculating.insert(entity, rx);
for map in map.iter() { for map in map.iter() {
let start_clone = *coordinates; find_path_for_shape(
let destination_clone = *destination; &*pool,
let map_clone = map.clone(); query_pipeline.clone(),
let tx_clone = tx.clone(); entity,
pool.spawn(async move { coordinates,
if let Some(result) = find_path(&start_clone, &destination_clone, &map_clone) { destination,
tx_clone.send(Path(result.0)).expect("Channel should exist"); &map,
} &collider_query,
}) &obstructions,
.detach(); shape,
&tx,
);
} }
} }
} }