Truncate coordinates rather than using floor.

src/core.rs

@@ -298,11 +298,11 @@ pub trait PointLike {
     fn y(&self) -> f32;
 
     fn x_i32(&self) -> i32 {
-        self.x().round() as i32
+        self.x().trunc() as i32
     }
 
     fn y_i32(&self) -> i32 {
-        self.y().round() as i32
+        self.y().trunc() as i32
     }
 
     fn x_usize(&self) -> usize {
@@ -321,12 +321,12 @@ pub trait PointLike {
         (self.x_i32(), self.y_i32())
     }
 
-    fn floor(&self) -> (f32, f32) {
-        (self.x().floor(), self.y().floor())
+    fn trunc(&self) -> (f32, f32) {
+        (self.x().trunc(), self.y().trunc())
     }
 
     fn to_index(&self, width: usize) -> usize {
-        ((self.y_i32() * width as i32) + self.x_i32()) as usize
+        (self.y_usize() * width) + self.x_usize()
     }
 
     fn distance_squared(&self, other: &dyn PointLike) -> f32 {

src/exploration.rs

@@ -132,7 +132,7 @@ where
         let mut features = features
             .iter()
             .filter(|v| visible_entities.contains(&v.0))
-            .map(|v| (v.1.floor(), v.2))
+            .map(|v| (v.1.trunc(), v.2))
             .collect::<Vec<((f32, f32), &ExplorationType)>>();
         if features.is_empty() {
             tts.speak("Nothing visible.", true)?;
@@ -166,7 +166,7 @@ where
         if let Some((coordinates, _)) = target {
             commands
                 .entity(entity)
-                .insert(Exploring(coordinates.floor()));
+                .insert(Exploring(coordinates.trunc()));
         }
     }
     Ok(())
@@ -291,7 +291,7 @@ where
     MapData: 'static + Clone + Default + Send + Sync,
 {
     if let Ok((coordinates, exploring, viewshed)) = explorer.get_single() {
-        let coordinates = coordinates.floor();
+        let coordinates = coordinates.trunc();
         let point = **exploring;
         let shape = Collider::cuboid(0.5 - f32::EPSILON, 0.5 - f32::EPSILON);
         let (known, idx) = if let Ok((map, revealed_tiles)) = map.get_single() {

src/navigation.rs

@@ -119,12 +119,6 @@ fn controls(
             if let Some(pair) = actions.clamped_axis_pair(NavigationAction::Move) {
                 cleanup = true;
                 let mut direction = pair.xy();
-                if direction.x.abs() < 0.5 {
-                    direction.x = 0.;
-                }
-                if direction.y.abs() < 0.5 {
-                    direction.y = 0.;
-                }
                 let forward_movement_factor =
                     forward_movement_factor.map(|v| v.0).unwrap_or_else(|| 1.);
                 let backward_movement_factor =
@@ -161,6 +155,7 @@ fn controls(
                         .action_data_mut(NavigationAction::Translate)
                         .axis_pair = Some(DualAxisData::from_xy(translation));
                 } else {
+                    // println!("{entity:?}: SetLinearVelocity: {velocity:?}");
                     actions.press(NavigationAction::SetLinearVelocity);
                     actions
                         .action_data_mut(NavigationAction::SetLinearVelocity)
@@ -175,13 +170,14 @@ fn controls(
         }
         if actions.pressed(NavigationAction::SetLinearVelocity) {
             if let Some(pair) = actions.axis_pair(NavigationAction::SetLinearVelocity) {
-                trace!("{entity:?}: SetLinearVelocity: {pair:?}");
+                // println!("{entity:?}: SetLinearVelocity: {pair:?}");
                 velocity.linvel = pair.into();
             } else {
+                // println!("{entity:?}: SetLinearVelocity: 0");
                 velocity.linvel = Vec2::ZERO;
             }
         } else if actions.just_released(NavigationAction::SetLinearVelocity) {
-            trace!("{entity:?}: Released velocity");
+            // println!("{entity:?}: Released velocity");
             velocity.linvel = Vec2::ZERO;
             actions
                 .action_data_mut(NavigationAction::SetLinearVelocity)

src/pathfinding.rs

@@ -15,7 +15,7 @@ use pathfinding::prelude::*;
 
 use crate::{
     commands::RunIfExistsExt,
-    core::PointLike,
+    core::{PointLike, TransformExt},
     map::{Map, MapObstruction},
     navigation::{NavigationAction, RotationSpeed},
 };
@@ -123,22 +123,29 @@ fn find_path_for_shape(
                 ((x - 1, y + 1), 1.5),
                 ((x + 1, y + 1), 1.5),
             ];
+            let pos = Vector2::new(x as f32, y as f32);
+            let shape_pos = Isometry2::new(pos, 0.);
             for exit in &exits {
                 let mut should_push = true;
-                let shape_pos =
-                    Isometry2::new(Vector2::new(exit.0 .0 as f32, exit.0 .1 as f32), 0.);
-                query_pipeline.intersections_with_shape(
-                    &rigid_body_set,
-                    &collider_set,
-                    &shape_pos,
-                    &*shape.raw,
-                    bevy_rapier2d::rapier::pipeline::QueryFilter::new()
-                        .predicate(&|h, _c| h != initiator),
-                    |_handle| {
-                        should_push = false;
-                        false
-                    },
-                );
+                let dest = Vector2::new(exit.0 .0 as f32, exit.0 .1 as f32);
+                let shape_vel = dest - pos;
+                let max_toi = 1.;
+                if query_pipeline
+                    .cast_shape(
+                        &rigid_body_set,
+                        &collider_set,
+                        &shape_pos,
+                        &shape_vel,
+                        &*shape.raw,
+                        max_toi,
+                        true,
+                        bevy_rapier2d::rapier::pipeline::QueryFilter::new()
+                            .predicate(&|h, _c| h != initiator),
+                    )
+                    .is_some()
+                {
+                    should_push = false;
+                }
                 if should_push {
                     let mut cost = exit.1 * 100.;
                     if let Some(cost_map) = cost_map {
@@ -187,6 +194,10 @@ fn calculate_path(
                 .remove::<Destination>();
             continue;
         }
+        trace!(
+            "Calculating path from {:?}",
+            coordinates.translation.truncate().i32()
+        );
         let coordinates_clone = *coordinates;
         let destination_clone = *destination;
         let query_pipeline = rapier_context.query_pipeline.clone();
@@ -208,10 +219,8 @@ fn calculate_path(
         }
         let shape_clone = (*shape).clone();
         trace!(
-            "{:?}: path: calculating from {:?} to {:?}",
-            entity,
+            "{entity:?}: path: calculating from {:?} to {destination:?}",
             coordinates.i32(),
-            destination
         );
         let pool = AsyncComputeTaskPool::get();
         let task = pool.spawn(async move {
@@ -238,10 +247,10 @@ fn poll_tasks(mut commands: Commands, mut query: Query<(Entity, &mut Calculating
     for (entity, mut calculating) in query.iter_mut() {
         if let Some(result) = future::block_on(future::poll_once(&mut **calculating)) {
             if let Some(path) = result {
-                trace!("{:?}: path: result: {:?}", entity, path);
+                trace!("{entity:?}: Path: {path:?}");
                 commands.entity(entity).insert(path);
             } else {
-                trace!("{:?}: path: no path", entity);
+                trace!("{entity:?}: path: no path");
                 commands.entity(entity).insert(NoPath);
             }
             commands.entity(entity).remove::<Calculating>();
@@ -268,53 +277,74 @@ fn negotiate_path(
         &mut ActionState<NavigationAction>,
         &mut Path,
         &mut Transform,
+        &Collider,
         Option<&RotationSpeed>,
     )>,
+    rapier_context: Res<RapierContext>,
 ) {
-    for (entity, mut actions, mut path, mut transform, rotation_speed) in &mut query {
-        let start_i32 = (transform.translation.x, transform.translation.y).i32();
+    for (entity, mut actions, mut path, mut transform, collider, rotation_speed) in &mut query {
+        let start_i32 = transform.translation.truncate().i32();
         trace!(
             "{entity:?}: start pathfinding from {start_i32:?} to {:?}: {:?}",
             path.last(),
             transform.translation.truncate()
         );
-        let mut cleanup = false;
-        if let Some(last) = path.last() {
-            if last == &start_i32 {
-                trace!("{entity:?}: path: at destination");
-                cleanup = true;
+        if path.len() > 0 {
+            if path.len() > 1 && path[0] != start_i32 && path[1] != start_i32 {
+                trace!("Off path");
+            } else if path.len() > 1 && path[1] == start_i32 {
+                trace!("Next in path is start of path, shouldn't happen");
+            }
+            if path[0] == start_i32 {
+                trace!("At start, removing");
+                path.remove(0);
             }
         }
-        if !cleanup {
-            if let Some(position) = path.iter().position(|v| v == &start_i32) {
-                trace!("{entity:?}: Path contains start");
-                let (_, new_path) = path.split_at(position + 1);
-                **path = new_path.to_vec();
-            }
-            if let Some(next) = path.first() {
-                trace!("{entity:?}: path: moving from {start_i32:?} to {next:?}");
-                let start = Vec2::new(start_i32.x(), start_i32.y());
-                let next = Vec2::new(next.0 as f32, next.1 as f32);
-                let mut direction = next - start;
-                direction = direction.normalize();
-                trace!("{entity:?}: path: direction: {direction:?}");
-                actions.press(NavigationAction::Move);
-                actions.action_data_mut(NavigationAction::Move).axis_pair =
-                    Some(DualAxisData::from_xy(Vec2::new(-direction.y, direction.x)));
-                trace!(
-                    "{entity:?}: path: move: {:?}",
-                    Vec2::new(direction.y, -direction.x)
-                );
-                if rotation_speed.is_some() {
-                    let angle = direction.y.atan2(direction.x);
-                    transform.rotation = Quat::from_rotation_z(angle);
-                }
+        if let Some(next) = path.first() {
+            trace!("{entity:?}: path: moving from {start_i32:?} to {next:?}");
+            let start = transform.translation.truncate();
+            let mut next = Vec2::new(next.0 as f32, next.1 as f32);
+            if next.x > 0. {
+                next.x += 0.5;
             } else {
-                trace!("{entity:?}: empty path, cleaning");
-                cleanup = true;
+                next -= 0.5;
             }
-        }
-        if cleanup {
+            if next.y > 0. {
+                next.y += 0.5;
+            } else {
+                next.y -= 0.5;
+            }
+            let mut direction = next - start;
+            direction = direction.normalize();
+            trace!(
+                "{entity:?}: Direction: {direction:?}, Distance: {}",
+                (next - start).length()
+            );
+            if let Some((hit, toi)) = rapier_context.cast_shape(
+                start,
+                transform.yaw().radians(),
+                direction,
+                &*collider,
+                rapier_context.integration_parameters.dt,
+                QueryFilter::new()
+                    .exclude_sensors()
+                    .exclude_collider(entity),
+            ) {
+                trace!("{entity:?} is stuck, hit: {hit:?}, TOI: {toi:?}");
+                // TODO: Remove when we have an actual character controller.
+                transform.translation = next.extend(0.);
+                continue;
+            }
+            trace!("{entity:?}: path: direction: {direction:?}");
+            actions.press(NavigationAction::Move);
+            actions.action_data_mut(NavigationAction::Move).axis_pair =
+                Some(DualAxisData::from_xy(Vec2::new(-direction.y, direction.x)));
+            if rotation_speed.is_some() {
+                let angle = direction.y.atan2(direction.x);
+                transform.rotation = Quat::from_rotation_z(angle);
+            }
+        } else {
+            trace!("{entity:?}: empty path, cleaning");
             commands
                 .entity(entity)
                 .remove::<Path>()
@@ -330,24 +360,41 @@ fn actions(
     mut commands: Commands,
     mut query: Query<(
         Entity,
-        &ActionState<NegotiatePathAction>,
+        &mut ActionState<NegotiatePathAction>,
+        &mut ActionState<NavigationAction>,
         Option<&mut Destination>,
     )>,
 ) {
-    for (entity, actions, destination) in &mut query {
+    for (entity, mut actions, mut navigation_action, destination) in &mut query {
         if actions.pressed(NegotiatePathAction) {
             if let Some(pair) = actions.axis_pair(NegotiatePathAction) {
+                trace!("Negotiating path to {pair:?}");
                 let dest = Destination((pair.x() as i32, pair.y() as i32));
                 if let Some(mut current_dest) = destination {
                     if *current_dest != dest {
+                        trace!("{entity:?}: New destination, zeroing velocity");
+                        navigation_action.press(NavigationAction::SetLinearVelocity);
+                        navigation_action
+                            .action_data_mut(NavigationAction::SetLinearVelocity)
+                            .axis_pair = Some(DualAxisData::from_xy(Vec2::ZERO));
                         *current_dest = dest;
                     }
                 } else {
+                    trace!("{entity:?}: Adding destination, zeroing velocity");
+                    navigation_action.press(NavigationAction::SetLinearVelocity);
+                    navigation_action
+                        .action_data_mut(NavigationAction::SetLinearVelocity)
+                        .axis_pair = Some(DualAxisData::from_xy(Vec2::ZERO));
                     commands.entity(entity).insert(dest);
                 }
             } else if destination.is_some() {
-                commands.entity(entity).remove::<Destination>();
+                commands
+                    .entity(entity)
+                    .remove::<Destination>()
+                    .remove::<NoPath>();
             }
+            actions.release(NegotiatePathAction);
+            actions.action_data_mut(NegotiatePathAction).axis_pair = None;
         }
     }
 }
@@ -361,13 +408,18 @@ impl Plugin for PathfindingPlugin {
             .register_type::<NoPath>()
             .register_type::<Path>()
             .register_type::<CostMap>()
-            .add_system(calculate_path)
+            .add_system_to_stage(CoreStage::PreUpdate, calculate_path)
             .add_system_to_stage(CoreStage::PostUpdate, remove_destination)
-            .add_system(poll_tasks)
+            .add_system_to_stage(CoreStage::PreUpdate, poll_tasks)
             .add_system_to_stage(
                 CoreStage::PreUpdate,
                 negotiate_path.after(InputManagerSystem::Tick),
             )
-            .add_system(actions);
+            .add_system_to_stage(
+                CoreStage::PreUpdate,
+                actions
+                    .after(InputManagerSystem::Update)
+                    .before(calculate_path),
+            );
     }
 }