Get pathfinding working.

src/navigation.rs

@@ -136,10 +136,8 @@ fn snap(
             snap_timers.insert(entity, SnapTimer::default());
             transform.rotate(Quat::from_rotation_z(PI));
         } else if actions.just_pressed(&NavigationAction::SnapCardinal) {
-            println!("Direction: {direction:?}");
             let yaw: Rot2 = direction.into();
             let yaw = yaw.as_radians();
-            println!("Yaw: {yaw}");
             let rotation = Quat::from_rotation_z(yaw);
             if transform.rotation != rotation {
                 transform.rotation = rotation;
@@ -215,7 +213,6 @@ fn controls(
             let mut speed = **speed;
             speed *= movement_factor;
             let move_velocity = direction * speed;
-            // println!("{entity:?}: SetLinearVelocity: {velocity:?}");
             actions.set_axis_pair(&NavigationAction::SetLinearVelocity, move_velocity);
         }
         if velocity.0 != actions.axis_pair(&NavigationAction::SetLinearVelocity) {
@@ -224,18 +221,25 @@ fn controls(
         if actions.axis_pair(&NavigationAction::Translate) != Vec2::ZERO {
             let pair = actions.axis_pair(&NavigationAction::Translate);
             let dir = Dir2::new_unchecked(pair.normalize());
-            if spatial_query
+            let mut can_translate = true;
-                .cast_shape(
+            spatial_query.shape_hits_callback(
                 collider,
                 transform.translation.truncate(),
                 transform.yaw().as_radians(),
                 dir,
-                    1.,
+                pair.length(),
                 true,
                 SpatialQueryFilter::from_excluded_entities(&sensors),
-                )
+                |hit| {
-                .is_none()
+                    if hit.entity != entity {
-            {
+                        can_translate = false;
+                        false
+                    } else {
+                        true
+                    }
+                },
+            );
+            if can_translate {
                 transform.translation += pair.extend(0.);
             }
             actions.set_axis_pair(&NavigationAction::Translate, Vec2::ZERO);

src/pathfinding.rs

@@ -4,8 +4,8 @@ use leafwing_input_manager::{plugin::InputManagerSystem, prelude::*};
 use pathfinding::prelude::*;
 
 use crate::{
-    core::{GlobalTransformExt, TransformExt},
+    core::GlobalTransformExt,
-    map::Map,
+    map::{Map, MapObstruction},
     navigation::{NavigationAction, RotationSpeed, Speed},
 };
 
@@ -65,7 +65,8 @@ pub fn find_path<D: 'static + Clone + Default + Send + Sync>(
                                     cost *= modifier;
                                 }
                             }
-                            successors.push((IVec2::new(tile.0 as i32, tile.1 as i32), cost as u32));
+                            successors
+                                .push((IVec2::new(tile.0 as i32, tile.1 as i32), cost as u32));
                         }
                     }
                 }
@@ -91,11 +92,12 @@ fn calculate_path(
         ),
         Changed<Destination>,
     >,
+    obstructions: Query<Entity, With<MapObstruction>>,
+    sensors: Query<Entity, With<Sensor>>,
 ) {
     for (entity, destination, transform, collider, cost_map, actions) in &mut query {
         trace!("{entity}: destination: {destination:?}");
         if transform.translation().truncate().as_ivec2() == **destination {
-            trace!("{entity}: remove1");
             commands
                 .entity(entity)
                 .remove::<Path>()
@@ -103,10 +105,6 @@ fn calculate_path(
                 .remove::<Destination>();
             continue;
         }
-        trace!(
-            "{entity:?}: Calculating path from {:?} to {destination:?}",
-            transform.translation().truncate().as_ivec2()
-        );
         commands.entity(entity).remove::<Path>().remove::<NoPath>();
         let path = astar(
             &transform.translation().truncate().as_ivec2(),
@@ -126,16 +124,28 @@ fn calculate_path(
                 ];
                 for exit in &exits {
                     let mut should_push = true;
-                    if !spatial_query
+                    let mut check = exit.0.as_vec2();
-                        .shape_intersections(
+                    if check.x >= 0. {
+                        check.x += 0.5;
+                    } else {
+                        check.x -= 0.5;
+                    }
+                    if check.y >= 0. {
+                        check.y += 0.5;
+                    } else {
+                        check.y -= 0.5;
+                    }
+                    let hits = spatial_query.shape_intersections(
                         collider,
-                            transform.translation().truncate(),
+                        check,
                         transform.yaw().as_radians(),
-                            SpatialQueryFilter::default(),
+                        SpatialQueryFilter::from_excluded_entities(&sensors),
-                        )
+                    );
-                        .is_empty()
+                    for entity in &hits {
-                    {
+                        if obstructions.contains(*entity) {
                             should_push = false;
+                            break;
+                        }
                     }
                     if should_push {
                         let mut cost = exit.1 * 100.;
@@ -176,6 +186,7 @@ fn remove_destination(mut commands: Commands, mut removed: RemovedComponents<Des
 fn negotiate_path(
     mut commands: Commands,
     time: Res<Time>,
+    physics_time: Res<Time<Physics>>,
     spatial_query: SpatialQuery,
     mut query: Query<(
         Entity,
@@ -183,17 +194,24 @@ fn negotiate_path(
         &mut ActionState<NavigationAction>,
         &mut Path,
         &mut Transform,
+        &GlobalTransform,
         &Collider,
         &Speed,
         Option<&RotationSpeed>,
     )>,
+    obstructions: Query<Entity, With<MapObstruction>>,
+    sensors: Query<Entity, With<Sensor>>,
 ) {
+    if physics_time.is_paused() {
+        return;
+    }
     for (
         entity,
         actions,
         mut navigation_actions,
         mut path,
         mut transform,
+        global_transform,
         collider,
         speed,
         rotation_speed,
@@ -201,17 +219,12 @@ fn negotiate_path(
     {
         trace!("{entity:?}: negotiating path");
         let start = transform.translation.truncate().as_ivec2();
-        trace!(
-            "{entity:?}: start pathfinding from {start:?} to {:?}: at {:?}",
-            path.last(),
-            transform.translation.truncate()
-        );
         if path.len() > 0 && path[0] == start {
             trace!("At start, removing");
             path.remove(0);
         }
         if let Some(next) = path.first() {
-            let start = transform.translation.truncate();
+            let start = global_transform.translation().truncate();
             let mut next = next.as_vec2();
             if next.x >= 0. {
                 next.x += 0.5;
@@ -226,30 +239,30 @@ fn negotiate_path(
             let mut direction = next - start;
             direction = direction.normalize();
             direction *= **speed;
-            direction *= time.delta_seconds();
+            let mut hits = vec![];
-            trace!(
+            spatial_query.shape_hits_callback(
-                "{entity:?}: Direction: {direction:?}, Distance: {}",
-                (next - start).length()
-            );
-            if spatial_query
-                .cast_shape(
                 collider,
                 start,
-                    transform.yaw().as_radians(),
+                global_transform.yaw().as_radians(),
                 Dir2::new_unchecked(direction.normalize()),
-                    direction.length(),
+                time.delta_seconds(),
                 true,
-                    SpatialQueryFilter::default(),
+                SpatialQueryFilter::from_excluded_entities(&sensors),
-                )
+                |hit| {
-                .is_some()
+                    if obstructions.contains(hit.entity) {
-            {
+                        hits.push(hit.entity);
-                trace!("{entity:?} is stuck");
+                    }
+                    true
+                },
+            );
+            if !hits.is_empty() {
                 // TODO: Remove when we have an actual character controller.
                 next.x = next.x.trunc();
                 next.y = next.y.trunc();
                 transform.translation = next.extend(0.);
             } else {
-                navigation_actions.set_axis_pair(&NavigationAction::Translate, direction);
+                let delta = direction * time.delta_seconds();
+                navigation_actions.set_axis_pair(&NavigationAction::Translate, delta);
             }
             if rotation_speed.is_some() {
                 let angle = direction.y.atan2(direction.x);