src/core.rs

@@ -298,11 +298,11 @@ pub trait PointLike {
     fn y(&self) -> f32;
 
     fn x_i32(&self) -> i32 {
-        self.x().trunc() as i32
+        self.x().round() as i32
     }
 
     fn y_i32(&self) -> i32 {
-        self.y().trunc() as i32
+        self.y().round() as i32
     }
 
     fn x_usize(&self) -> usize {
@@ -321,12 +321,12 @@ pub trait PointLike {
         (self.x_i32(), self.y_i32())
     }
 
-    fn trunc(&self) -> (f32, f32) {
-        (self.x().trunc(), self.y().trunc())
+    fn floor(&self) -> (f32, f32) {
+        (self.x().floor(), self.y().floor())
     }
 
     fn to_index(&self, width: usize) -> usize {
-        (self.y_usize() * width) + self.x_usize()
+        ((self.y_i32() * width as i32) + self.x_i32()) as 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.trunc(), v.2))
+            .map(|v| (v.1.floor(), 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.trunc()));
+                .insert(Exploring(coordinates.floor()));
         }
     }
     Ok(())
@@ -291,7 +291,7 @@ where
     MapData: 'static + Clone + Default + Send + Sync,
 {
     if let Ok((coordinates, exploring, viewshed)) = explorer.get_single() {
-        let coordinates = coordinates.trunc();
+        let coordinates = coordinates.floor();
         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,6 +119,12 @@ 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 =
@@ -155,7 +161,6 @@ 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)
@@ -170,14 +175,13 @@ fn controls(
         }
         if actions.pressed(NavigationAction::SetLinearVelocity) {
             if let Some(pair) = actions.axis_pair(NavigationAction::SetLinearVelocity) {
-                // println!("{entity:?}: SetLinearVelocity: {pair:?}");
+                trace!("{entity:?}: SetLinearVelocity: {pair:?}");
                 velocity.linvel = pair.into();
             } else {
-                // println!("{entity:?}: SetLinearVelocity: 0");
                 velocity.linvel = Vec2::ZERO;
             }
         } else if actions.just_released(NavigationAction::SetLinearVelocity) {
-            // println!("{entity:?}: Released velocity");
+            trace!("{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, TransformExt},
+    core::PointLike,
     map::{Map, MapObstruction},
     navigation::{NavigationAction, RotationSpeed},
 };
@@ -123,29 +123,22 @@ 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 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;
-                }
+                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
+                    },
+                );
                 if should_push {
                     let mut cost = exit.1 * 100.;
                     if let Some(cost_map) = cost_map {
@@ -194,10 +187,6 @@ 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();
@@ -219,8 +208,10 @@ fn calculate_path(
         }
         let shape_clone = (*shape).clone();
         trace!(
-            "{entity:?}: path: calculating from {:?} to {destination:?}",
+            "{:?}: path: calculating from {:?} to {:?}",
+            entity,
             coordinates.i32(),
+            destination
         );
         let pool = AsyncComputeTaskPool::get();
         let task = pool.spawn(async move {
@@ -247,10 +238,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!("{entity:?}: Path: {path:?}");
+                trace!("{:?}: path: result: {:?}", entity, path);
                 commands.entity(entity).insert(path);
             } else {
-                trace!("{entity:?}: path: no path");
+                trace!("{:?}: path: no path", entity);
                 commands.entity(entity).insert(NoPath);
             }
             commands.entity(entity).remove::<Calculating>();
@@ -277,74 +268,53 @@ 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, collider, rotation_speed) in &mut query {
-        let start_i32 = transform.translation.truncate().i32();
+    for (entity, mut actions, mut path, mut transform, rotation_speed) in &mut query {
+        let start_i32 = (transform.translation.x, transform.translation.y).i32();
         trace!(
             "{entity:?}: start pathfinding from {start_i32:?} to {:?}: {:?}",
             path.last(),
             transform.translation.truncate()
         );
-        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);
+        let mut cleanup = false;
+        if let Some(last) = path.last() {
+            if last == &start_i32 {
+                trace!("{entity:?}: path: at destination");
+                cleanup = true;
             }
         }
-        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;
+        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);
+                }
             } else {
-                next -= 0.5;
+                trace!("{entity:?}: empty path, cleaning");
+                cleanup = true;
             }
-            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");
+        }
+        if cleanup {
             commands
                 .entity(entity)
                 .remove::<Path>()
@@ -360,41 +330,24 @@ fn actions(
     mut commands: Commands,
     mut query: Query<(
         Entity,
-        &mut ActionState<NegotiatePathAction>,
-        &mut ActionState<NavigationAction>,
+        &ActionState<NegotiatePathAction>,
         Option<&mut Destination>,
     )>,
 ) {
-    for (entity, mut actions, mut navigation_action, destination) in &mut query {
+    for (entity, actions, 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>()
-                    .remove::<NoPath>();
+                commands.entity(entity).remove::<Destination>();
             }
-            actions.release(NegotiatePathAction);
-            actions.action_data_mut(NegotiatePathAction).axis_pair = None;
         }
     }
 }
@@ -408,18 +361,13 @@ impl Plugin for PathfindingPlugin {
             .register_type::<NoPath>()
             .register_type::<Path>()
             .register_type::<CostMap>()
-            .add_system_to_stage(CoreStage::PreUpdate, calculate_path)
+            .add_system(calculate_path)
             .add_system_to_stage(CoreStage::PostUpdate, remove_destination)
-            .add_system_to_stage(CoreStage::PreUpdate, poll_tasks)
+            .add_system(poll_tasks)
             .add_system_to_stage(
                 CoreStage::PreUpdate,
                 negotiate_path.after(InputManagerSystem::Tick),
             )
-            .add_system_to_stage(
-                CoreStage::PreUpdate,
-                actions
-                    .after(InputManagerSystem::Update)
-                    .before(calculate_path),
-            );
+            .add_system(actions);
     }
 }