From afca116394f3560d21684f63d05317d66e1367f8 Mon Sep 17 00:00:00 2001
From: Nolan Darilek <nolan@thewordnerd.info>
Date: Sun, 11 Aug 2024 09:08:20 -0500
Subject: [PATCH] Various mapping/collision/visibility tweaks.

---
 src/map.rs         |   9 ++--
 src/navigation.rs  |  57 +++++++++++----------
 src/pathfinding.rs | 123 ++++++++++++++++++++++++---------------------
 src/visibility.rs  |  30 ++++-------
 4 files changed, 110 insertions(+), 109 deletions(-)

diff --git a/src/map.rs b/src/map.rs
index 89b8f35..c96a61b 100644
--- a/src/map.rs
+++ b/src/map.rs
@@ -185,22 +185,23 @@ fn spawn_colliders<D: 'static + Clone + Default + Send + Sync>(
 ) {
     for (map_entity, map, spawn_colliders) in &maps {
         if **spawn_colliders {
-            commands
-                .entity(map_entity)
-                .remove::<SpawnColliders>()
-                .insert(RigidBody::Fixed);
+            commands.entity(map_entity).remove::<SpawnColliders>();
             for y in 0..map.height {
                 for x in 0..map.width {
                     if let Some(tile) = map.at(x, y) {
                         if tile.blocks_motion() {
                             let id = commands
                                 .spawn((
+                                    RigidBody::Fixed,
                                     TransformBundle::from_transform(Transform::from_xyz(
                                         x as f32 + 0.5,
                                         y as f32 + 0.5,
                                         0.,
                                     )),
                                     Collider::cuboid(0.5, 0.5),
+                                    ActiveCollisionTypes::default()
+                                        | ActiveCollisionTypes::KINEMATIC_STATIC
+                                        | ActiveCollisionTypes::DYNAMIC_STATIC,
                                     MapObstruction,
                                 ))
                                 .id();
diff --git a/src/navigation.rs b/src/navigation.rs
index d424733..02be662 100644
--- a/src/navigation.rs
+++ b/src/navigation.rs
@@ -6,7 +6,7 @@ use bevy_tts::Tts;
 use leafwing_input_manager::{axislike::DualAxisData, prelude::*};
 
 use crate::{
-    core::{Angle, Area, CardinalDirection, GlobalTransformExt, Player},
+    core::{Angle, Area, CardinalDirection, GlobalTransformExt, Player, TransformExt},
     error::error_handler,
     exploration::{ExplorationFocused, Exploring},
     log::Log,
@@ -136,6 +136,7 @@ fn tick_snap_timers(time: Res<Time>, mut snap_timers: ResMut<SnapTimers>) {
 
 fn controls(
     mut commands: Commands,
+    rapier_context: Res<RapierContext>,
     time: Res<Time>,
     snap_timers: Res<SnapTimers>,
     mut query: Query<(
@@ -148,7 +149,7 @@ fn controls(
         Option<&ForwardMovementFactor>,
         Option<&StrafeMovementFactor>,
         &mut Transform,
-        Option<&mut KinematicCharacterController>,
+        &Collider,
     )>,
     exploration_focused: Query<Entity, With<ExplorationFocused>>,
 ) {
@@ -162,7 +163,7 @@ fn controls(
         forward_movement_factor,
         strafe_movement_factor,
         mut transform,
-        character_controller,
+        collider,
     ) in &mut query
     {
         let mut cleanup = false;
@@ -176,22 +177,21 @@ fn controls(
                     backward_movement_factor.map(|v| v.0).unwrap_or_else(|| 1.);
                 let strafe_movement_factor =
                     strafe_movement_factor.map(|v| v.0).unwrap_or_else(|| 1.);
-                let forward_backward_movement_factor = if direction.y > 0. {
+                let forward_backward_movement_factor = if direction.x > 0. {
                     forward_movement_factor
-                } else if direction.y < 0. {
+                } else if direction.x < 0. {
                     backward_movement_factor
                 } else {
                     1.
                 };
                 let movement_factor = if direction.x != 0. && direction.y != 0. {
                     strafe_movement_factor.min(forward_backward_movement_factor)
-                } else if direction.x != 0. {
+                } else if direction.y != 0. {
                     strafe_movement_factor
                 } else {
                     forward_backward_movement_factor
                 };
                 trace!("{entity:?}: move: {direction:?}");
-                direction = Vec2::new(direction.y, -direction.x);
                 direction = transform
                     .compute_matrix()
                     .transform_vector3(direction.extend(0.))
@@ -199,19 +199,11 @@ fn controls(
                 let mut speed = **speed;
                 speed *= movement_factor;
                 let velocity = direction * speed;
-                if character_controller.is_some() {
-                    let translation = velocity * time.delta_seconds();
-                    actions.press(&NavigationAction::Translate);
-                    actions
-                        .action_data_mut_or_default(&NavigationAction::Translate)
-                        .axis_pair = Some(DualAxisData::from_xy(translation));
-                } else {
-                    // println!("{entity:?}: SetLinearVelocity: {velocity:?}");
-                    actions.press(&NavigationAction::SetLinearVelocity);
-                    actions
-                        .action_data_mut_or_default(&NavigationAction::SetLinearVelocity)
-                        .axis_pair = Some(DualAxisData::from_xy(velocity));
-                }
+                // println!("{entity:?}: SetLinearVelocity: {velocity:?}");
+                actions.press(&NavigationAction::SetLinearVelocity);
+                actions
+                    .action_data_mut_or_default(&NavigationAction::SetLinearVelocity)
+                    .axis_pair = Some(DualAxisData::from_xy(velocity));
             }
         } else if actions.just_released(&NavigationAction::Move) {
             trace!("{entity:?}: Stopped moving");
@@ -234,21 +226,32 @@ fn controls(
             velocity.linvel = Vec2::ZERO;
             actions
                 .action_data_mut_or_default(&NavigationAction::SetLinearVelocity)
-                .axis_pair = Some(DualAxisData::from_xy(Vec2::ZERO));
+                .axis_pair = None;
         }
         if actions.pressed(&NavigationAction::Translate) {
             if let Some(pair) = actions.axis_pair(&NavigationAction::Translate) {
-                if let Some(mut character_controller) = character_controller {
-                    character_controller.translation = Some(pair.xy());
+                let vel = pair.xy();
+                if rapier_context
+                    .cast_shape(
+                        transform.translation.truncate(),
+                        transform.yaw().radians(),
+                        vel,
+                        collider,
+                        1.,
+                        true,
+                        QueryFilter::new()
+                            .exclude_sensors()
+                            .exclude_collider(entity),
+                    )
+                    .is_none()
+                {
+                    transform.translation += vel.extend(0.);
                 }
             }
         } else if actions.just_released(&NavigationAction::Translate) {
-            if let Some(mut character_controller) = character_controller {
-                character_controller.translation = None;
-            }
             actions
                 .action_data_mut_or_default(&NavigationAction::Translate)
-                .axis_pair = Some(DualAxisData::from_xy(Vec2::ZERO));
+                .axis_pair = None;
         }
         if !snap_timers.contains_key(&entity) {
             if let Some(rotation_speed) = rotation_speed {
diff --git a/src/pathfinding.rs b/src/pathfinding.rs
index babea0f..d81228f 100644
--- a/src/pathfinding.rs
+++ b/src/pathfinding.rs
@@ -15,7 +15,7 @@ use pathfinding::prelude::*;
 use crate::{
     core::{PointLike, TransformExt},
     map::{Map, MapObstruction},
-    navigation::{NavigationAction, RotationSpeed},
+    navigation::{NavigationAction, RotationSpeed, Speed},
 };
 
 #[derive(PartialEq, Eq, Clone, Copy, Hash, Debug, Reflect)]
@@ -105,24 +105,18 @@ 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.;
+                let dest = Isometry2::new(Vector2::new(exit.0 .0 as f32, exit.0 .1 as f32), 0.);
                 if query_pipeline
-                    .cast_shape(
+                    .intersection_with_shape(
                         &rigid_body_set,
                         &collider_set,
-                        &shape_pos,
-                        &shape_vel,
+                        &dest,
                         &*shape.raw,
-                        max_toi,
-                        true,
                         bevy_rapier2d::rapier::pipeline::QueryFilter::new()
-                            .predicate(&|h, _c| h != initiator),
+                            .exclude_sensors()
+                            .exclude_collider(initiator),
                     )
                     .is_some()
                 {
@@ -177,7 +171,7 @@ fn calculate_path(
             continue;
         }
         trace!(
-            "Calculating path from {:?}",
+            "{entity:?}: Calculating path from {:?} to {destination:?}",
             coordinates.translation.truncate().i32()
         );
         let coordinates_clone = *coordinates;
@@ -186,7 +180,7 @@ fn calculate_path(
         let cost_map_clone = cost_map.cloned();
         let handle_clone = *handle;
         let mut collider_set = rapier_context.colliders.clone();
-        let mut to_remove = vec![];
+        let mut to_remove = vec![handle.0];
         for handle in collider_set.iter() {
             if !obstructions.iter().map(|v| v.0).any(|x| x == handle.0) {
                 to_remove.push(handle.0);
@@ -225,14 +219,21 @@ fn calculate_path(
     }
 }
 
-fn poll_tasks(mut commands: Commands, mut query: Query<(Entity, &mut Calculating)>) {
-    for (entity, mut calculating) in &mut query {
+fn poll_tasks(
+    mut commands: Commands,
+    mut query: Query<(Entity, &mut Calculating, &Transform, &Destination)>,
+) {
+    for (entity, mut calculating, transform, destination) in &mut query {
         if let Some(result) = future::block_on(future::poll_once(&mut **calculating)) {
             if let Some(path) = result {
                 trace!("{entity:?}: Path: {path:?}");
                 commands.entity(entity).insert(path);
             } else {
-                trace!("{entity:?}: path: no path");
+                trace!(
+                    "{entity:?}: path: no path from {:?} to {:?}",
+                    transform.translation.truncate().i32(),
+                    **destination
+                );
                 commands.entity(entity).insert(NoPath);
             }
             commands.entity(entity).remove::<Calculating>();
@@ -254,77 +255,81 @@ fn remove_destination(
 
 fn negotiate_path(
     mut commands: Commands,
+    time: Res<Time>,
+    physics_config: Res<RapierConfiguration>,
     mut query: Query<(
         Entity,
         &mut ActionState<NavigationAction>,
         &mut Path,
         &mut Transform,
         &Collider,
+        &Speed,
         Option<&RotationSpeed>,
     )>,
     rapier_context: Res<RapierContext>,
-    obstructions: Query<&MapObstruction>,
 ) {
-    for (entity, mut actions, mut path, mut transform, collider, rotation_speed) in &mut query {
+    if !physics_config.physics_pipeline_active || !physics_config.query_pipeline_active {
+        return;
+    }
+    for (entity, mut actions, mut path, mut transform, collider, speed, rotation_speed) in
+        &mut query
+    {
         let start_i32 = transform.translation.truncate().i32();
         trace!(
-            "{entity:?}: start pathfinding from {start_i32:?} to {:?}: {:?}",
+            "{entity:?}: start pathfinding from {start_i32:?} to {:?}: at {:?}",
             path.last(),
-            transform.translation.truncate()
+            transform.translation.truncate().i32()
         );
-        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 path.len() > 0 && path[0] == start_i32 {
+            trace!("At start, removing");
+            path.remove(0);
         }
         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. {
+            if next.x >= 0. {
                 next.x += 0.5;
             } else {
-                next -= 0.5;
+                next.x -= 0.5;
             }
-            if next.y > 0. {
+            if next.y >= 0. {
                 next.y += 0.5;
             } else {
                 next.y -= 0.5;
             }
             let mut direction = next - start;
             direction = direction.normalize();
+            direction *= **speed;
+            direction *= time.delta_seconds();
             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,
-                true,
-                QueryFilter::new()
-                    .predicate(&|entity| obstructions.get(entity).is_ok())
-                    .exclude_sensors()
-                    .exclude_collider(entity),
-            ) {
-                // println!("{entity:?} is stuck, hit: {hit:?}, TOI: {toi:?}");
+            if rapier_context
+                .cast_shape(
+                    start,
+                    transform.yaw().radians(),
+                    direction,
+                    collider,
+                    1.,
+                    true,
+                    QueryFilter::new()
+                        .exclude_sensors()
+                        .exclude_collider(entity),
+                )
+                .is_some()
+            {
+                // 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.press(&NavigationAction::Translate);
             actions
-                .action_data_mut_or_default(&NavigationAction::Move)
-                .axis_pair = Some(DualAxisData::from_xy(Vec2::new(-direction.y, direction.x)));
+                .action_data_mut_or_default(&NavigationAction::Translate)
+                .axis_pair = Some(DualAxisData::from_xy(direction));
             if rotation_speed.is_some() {
                 let angle = direction.y.atan2(direction.x);
                 transform.rotation = Quat::from_rotation_z(angle);
@@ -336,7 +341,6 @@ fn negotiate_path(
                 .remove::<Path>()
                 .remove::<NoPath>()
                 .remove::<Destination>();
-            actions.release(&NavigationAction::Move);
             trace!("{entity:?}: pathfinding: cleaned up");
         }
     }
@@ -354,11 +358,12 @@ fn actions(
     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));
+                trace!("{entity:?}: Negotiating path to {pair:?}");
+                let dest = Destination(pair.xy().i32());
                 if let Some(mut current_dest) = destination {
+                    trace!("Got a destination");
                     if *current_dest != dest {
-                        trace!("{entity:?}: New destination, zeroing velocity");
+                        trace!("{entity:?}: New destination {dest:?} differs from {current_dest:?}, zeroing velocity");
                         navigation_action.press(&NavigationAction::SetLinearVelocity);
                         navigation_action
                             .action_data_mut_or_default(&NavigationAction::SetLinearVelocity)
@@ -368,16 +373,17 @@ fn actions(
                 } else {
                     trace!("{entity:?}: Adding destination, zeroing velocity");
                     navigation_action.press(&NavigationAction::SetLinearVelocity);
-
                     navigation_action
                         .action_data_mut_or_default(&NavigationAction::SetLinearVelocity)
                         .axis_pair = Some(DualAxisData::from_xy(Vec2::ZERO));
                     commands.entity(entity).insert(dest);
                 }
             } else if destination.is_some() {
+                trace!("No value, resetting");
                 commands
                     .entity(entity)
                     .remove::<Destination>()
+                    .remove::<Path>()
                     .remove::<NoPath>();
             }
             actions.release(&NegotiatePathAction);
@@ -397,10 +403,13 @@ impl Plugin for PathfindingPlugin {
             .register_type::<NoPath>()
             .register_type::<Path>()
             .register_type::<CostMap>()
-            .add_systems(PreUpdate, (poll_tasks, negotiate_path).chain())
             .add_systems(
                 PreUpdate,
-                (actions, calculate_path)
+                (poll_tasks, apply_deferred, negotiate_path).chain(),
+            )
+            .add_systems(
+                PreUpdate,
+                (actions, apply_deferred, calculate_path)
                     .chain()
                     .after(InputManagerSystem::Tick),
             )
diff --git a/src/visibility.rs b/src/visibility.rs
index 6efb726..446181b 100644
--- a/src/visibility.rs
+++ b/src/visibility.rs
@@ -17,7 +17,7 @@ use crate::{
     bevy_rapier2d::prelude::*,
     core::{GlobalTransformExt, Player, PointLike},
     log::Log,
-    map::{Map, MapObstruction, MapPlugin},
+    map::{Map, MapPlugin},
 };
 
 #[derive(Component, Clone, Copy, Debug, Default, Reflect)]
@@ -55,7 +55,6 @@ impl Viewshed {
         start: &GlobalTransform,
         rapier_context: &RapierContext,
         visible_query: &Query<(&Visible, &Collider, &GlobalTransform)>,
-        obstructions_query: &Query<&MapObstruction>,
         events: &mut EventWriter<VisibilityChanged>,
         cache: &mut HashMap<(i32, i32), (u8, HashSet<Entity>)>,
     ) {
@@ -66,7 +65,9 @@ impl Viewshed {
             start.translation().truncate(),
             0.,
             &shape,
-            QueryFilter::new().predicate(&|e| visible_query.get(e).is_ok()),
+            QueryFilter::new()
+                .exclude_collider(*viewer_entity)
+                .predicate(&|e| visible_query.contains(e)),
             |entity| {
                 if let Ok((_, collider, transform)) = visible_query.get(entity) {
                     let position = Isometry2::new(
@@ -122,19 +123,16 @@ impl Viewshed {
                             shape_pos,
                             0.,
                             &shape,
-                            QueryFilter::new().predicate(&|v| visible_query.get(v).is_ok()),
+                            QueryFilter::new()
+                                .exclude_collider(*viewer_entity)
+                                .predicate(&|v| visible_query.contains(v)),
                             |entity| {
                                 // println!("{:?}", entity);
-                                let obstruction = obstructions_query.get(entity).is_ok();
-                                if obstruction {
-                                    // println!("Obstruction");
-                                    coord_entities.clear();
-                                }
                                 coord_entities.insert(entity);
                                 if let Ok((visible, _, _)) = visible_query.get(entity) {
                                     opacity = opacity.max(**visible);
                                 }
-                                !obstruction
+                                true
                             },
                         );
                         cache.insert((coord.x, coord.y), (opacity, coord_entities.clone()));
@@ -150,13 +148,7 @@ impl Viewshed {
                     for e in &coord_entities {
                         new_visible_entities.insert(*e);
                     }
-                    if coord_entities.contains(viewer_entity) {
-                        // println!("Self hit, 0");
-                        0
-                    } else {
-                        // println!("{}", opacity);
-                        opacity
-                    }
+                    opacity
                 } else {
                     0
                 }
@@ -301,7 +293,6 @@ where
 fn update_viewshed(
     config: Res<RapierConfiguration>,
     visible: Query<(&Visible, &Collider, &GlobalTransform)>,
-    obstructions: Query<&MapObstruction>,
     mut viewers: Query<(
         Entity,
         &mut Viewshed,
@@ -322,7 +313,6 @@ fn update_viewshed(
             viewer_transform,
             &rapier_context,
             &visible,
-            &obstructions,
             &mut changed,
             &mut cache,
         );
@@ -339,7 +329,6 @@ fn remove_visible(
     )>,
     rapier_context: Res<RapierContext>,
     visible: Query<(&Visible, &Collider, &GlobalTransform)>,
-    obstructions: Query<&MapObstruction>,
     mut changed: EventWriter<VisibilityChanged>,
 ) {
     if !removed.is_empty() {
@@ -356,7 +345,6 @@ fn remove_visible(
                     start,
                     &rapier_context,
                     &visible,
-                    &obstructions,
                     &mut changed,
                     &mut cache,
                 );