Pathfinding takes physics into account.

Cargo.toml

@@ -23,7 +23,7 @@ features = [
 backtrace = "0.3"
 bevy_input_actionmap = { path = "../bevy_input_actionmap" }
 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" }
 coord_2d = "0.3"
 crossbeam-channel = "0.5"

src/core.rs

@@ -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) {
     fn x(&self) -> f32 {
         self.0 as f32

src/map.rs

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

src/pathfinding.rs

@@ -1,14 +1,18 @@
 use std::collections::HashMap;
 
 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 pathfinding::prelude::*;
 
 use crate::{
     core::{Coordinates, PointLike},
-    map::Map,
+    map::{Map, MapObstruction},
     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(
     mut commands: Commands,
     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>,
     map: Query<&Map>,
 ) {
     let calculating_clone = calculating.clone();
     for (entity, rx) in calculating_clone.iter() {
         if destinations.get(*entity).is_ok() {
-            if let Ok(path) = rx.try_recv() {
+            if let Ok(msg) = rx.try_recv() {
+                if let Some(path) = msg {
                     commands.entity(*entity).insert(path);
-                calculating.remove(&entity);
                 } else {
                     commands.entity(*entity).remove::<Destination>();
                 }
+                calculating.remove(&entity);
+            }
         } else {
             calculating.remove(&entity);
         }
     }
-    for (entity, destination, coordinates) in query.iter() {
+    for (entity, destination, coordinates, shape) in query.iter() {
         if !calculating.contains_key(&entity) {
             let (tx, rx) = unbounded();
             calculating.insert(entity, rx);
             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,
+                );
             }
         }
     }