src/map.rs

@@ -185,23 +185,22 @@ 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>();
+            commands
+                .entity(map_entity)
+                .remove::<SpawnColliders>()
+                .insert(RigidBody::Fixed);
             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();

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, TransformExt},
+    core::{Angle, Area, CardinalDirection, GlobalTransformExt, Player},
     error::error_handler,
     exploration::{ExplorationFocused, Exploring},
     log::Log,
@@ -136,7 +136,6 @@ 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<(
@@ -149,7 +148,7 @@ fn controls(
         Option<&ForwardMovementFactor>,
         Option<&StrafeMovementFactor>,
         &mut Transform,
-        &Collider,
+        Option<&mut KinematicCharacterController>,
     )>,
     exploration_focused: Query<Entity, With<ExplorationFocused>>,
 ) {
@@ -163,7 +162,7 @@ fn controls(
         forward_movement_factor,
         strafe_movement_factor,
         mut transform,
-        collider,
+        character_controller,
     ) in &mut query
     {
         let mut cleanup = false;
@@ -177,21 +176,22 @@ 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.x > 0. {
+                let forward_backward_movement_factor = if direction.y > 0. {
                     forward_movement_factor
-                } else if direction.x < 0. {
+                } else if direction.y < 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.y != 0. {
+                } else if direction.x != 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,12 +199,20 @@ 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));
                 }
+            }
         } else if actions.just_released(&NavigationAction::Move) {
             trace!("{entity:?}: Stopped moving");
             actions.release(&NavigationAction::SetLinearVelocity);
@@ -226,34 +234,21 @@ fn controls(
             velocity.linvel = Vec2::ZERO;
             actions
                 .action_data_mut_or_default(&NavigationAction::SetLinearVelocity)
-                .axis_pair = None;
+                .axis_pair = Some(DualAxisData::from_xy(Vec2::ZERO));
         }
         if actions.pressed(&NavigationAction::Translate) {
             if let Some(pair) = actions.axis_pair(&NavigationAction::Translate) {
-                let vel = pair.xy();
-                if rapier_context
-                    .cast_shape(
-                        transform.translation.truncate(),
-                        transform.yaw().radians(),
-                        vel,
-                        collider,
-                        ShapeCastOptions {
-                            max_time_of_impact: 1.,
-                            ..default()
-                        },
-                        QueryFilter::new()
-                            .exclude_sensors()
-                            .exclude_collider(entity),
-                    )
-                    .is_none()
-                {
-                    transform.translation += vel.extend(0.);
+                if let Some(mut character_controller) = character_controller {
+                    character_controller.translation = Some(pair.xy());
                 }
             }
         } 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 = None;
+                .axis_pair = Some(DualAxisData::from_xy(Vec2::ZERO));
         }
         if !snap_timers.contains_key(&entity) {
             if let Some(rotation_speed) = rotation_speed {

src/pathfinding.rs

@@ -15,7 +15,7 @@ use pathfinding::prelude::*;
 use crate::{
     core::{PointLike, TransformExt},
     map::{Map, MapObstruction},
-    navigation::{NavigationAction, RotationSpeed, Speed},
+    navigation::{NavigationAction, RotationSpeed},
 };
 
 #[derive(PartialEq, Eq, Clone, Copy, Hash, Debug, Reflect)]
@@ -105,18 +105,26 @@ 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 = Isometry2::new(Vector2::new(exit.0 .0 as f32, exit.0 .1 as f32), 0.);
+                let dest = Vector2::new(exit.0 .0 as f32, exit.0 .1 as f32);
+                let shape_vel = dest - pos;
                 if query_pipeline
-                    .intersection_with_shape(
+                    .cast_shape(
                         &rigid_body_set,
                         &collider_set,
-                        &dest,
+                        &shape_pos,
+                        &shape_vel,
                         &*shape.raw,
+                        ShapeCastOptions {
+                            max_time_of_impact: 1.,
+                            stop_at_penetration: true,
+                            ..default()
+                        },
                         bevy_rapier2d::rapier::pipeline::QueryFilter::new()
-                            .exclude_sensors()
-                            .exclude_collider(initiator),
+                            .predicate(&|h, _c| h != initiator),
                     )
                     .is_some()
                 {
@@ -171,7 +179,7 @@ fn calculate_path(
             continue;
         }
         trace!(
-            "{entity:?}: Calculating path from {:?} to {destination:?}",
+            "Calculating path from {:?}",
             coordinates.translation.truncate().i32()
         );
         let coordinates_clone = *coordinates;
@@ -180,7 +188,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![handle.0];
+        let mut to_remove = vec![];
         for handle in collider_set.iter() {
             if !obstructions.iter().map(|v| v.0).any(|x| x == handle.0) {
                 to_remove.push(handle.0);
@@ -219,21 +227,14 @@ fn calculate_path(
     }
 }
 
-fn poll_tasks(
-    mut commands: Commands,
-    mut query: Query<(Entity, &mut Calculating, &Transform, &Destination)>,
-) {
-    for (entity, mut calculating, transform, destination) in &mut query {
+fn poll_tasks(mut commands: Commands, mut query: Query<(Entity, &mut Calculating)>) {
+    for (entity, mut calculating) 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 from {:?} to {:?}",
-                    transform.translation.truncate().i32(),
-                    **destination
-                );
+                trace!("{entity:?}: path: no path");
                 commands.entity(entity).insert(NoPath);
             }
             commands.entity(entity).remove::<Calculating>();
@@ -255,83 +256,80 @@ 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>,
 ) {
-    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
-    {
+    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 {:?}: at {:?}",
+            "{entity:?}: start pathfinding from {start_i32:?} to {:?}: {:?}",
             path.last(),
-            transform.translation.truncate().i32()
+            transform.translation.truncate()
         );
-        if path.len() > 0 && path[0] == start_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 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.x -= 0.5;
+                next -= 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 rapier_context
-                .cast_shape(
+            if let Some((_hit, _toi)) = rapier_context.cast_shape(
                 start,
                 transform.yaw().radians(),
                 direction,
                 collider,
                 ShapeCastOptions {
                     max_time_of_impact: 1.,
+                    stop_at_penetration: true,
                     ..default()
                 },
                 QueryFilter::new()
+                    .predicate(&|entity| obstructions.get(entity).is_ok())
                     .exclude_sensors()
                     .exclude_collider(entity),
-                )
-                .is_some()
-            {
-                // trace!("{entity:?} is stuck, hit: {hit:?}, TOI: {toi:?}");
+            ) {
+                // println!("{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::Translate);
+            actions.press(&NavigationAction::Move);
             actions
-                .action_data_mut_or_default(&NavigationAction::Translate)
-                .axis_pair = Some(DualAxisData::from_xy(direction));
+                .action_data_mut_or_default(&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);
@@ -343,6 +341,7 @@ fn negotiate_path(
                 .remove::<Path>()
                 .remove::<NoPath>()
                 .remove::<Destination>();
+            actions.release(&NavigationAction::Move);
             trace!("{entity:?}: pathfinding: cleaned up");
         }
     }
@@ -360,12 +359,11 @@ 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!("{entity:?}: Negotiating path to {pair:?}");
-                let dest = Destination(pair.xy().i32());
+                trace!("Negotiating path to {pair:?}");
+                let dest = Destination((pair.x() as i32, pair.y() as i32));
                 if let Some(mut current_dest) = destination {
-                    trace!("Got a destination");
                     if *current_dest != dest {
-                        trace!("{entity:?}: New destination {dest:?} differs from {current_dest:?}, zeroing velocity");
+                        trace!("{entity:?}: New destination, zeroing velocity");
                         navigation_action.press(&NavigationAction::SetLinearVelocity);
                         navigation_action
                             .action_data_mut_or_default(&NavigationAction::SetLinearVelocity)
@@ -375,17 +373,16 @@ 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);
@@ -405,13 +402,10 @@ impl Plugin for PathfindingPlugin {
             .register_type::<NoPath>()
             .register_type::<Path>()
             .register_type::<CostMap>()
+            .add_systems(PreUpdate, (poll_tasks, negotiate_path).chain())
             .add_systems(
                 PreUpdate,
-                (poll_tasks, apply_deferred, negotiate_path).chain(),
-            )
-            .add_systems(
-                PreUpdate,
-                (actions, apply_deferred, calculate_path)
+                (actions, calculate_path)
                     .chain()
                     .after(InputManagerSystem::Tick),
             )

src/visibility.rs

@@ -17,7 +17,7 @@ use crate::{
     bevy_rapier2d::prelude::*,
     core::{GlobalTransformExt, Player, PointLike},
     log::Log,
-    map::{Map, MapPlugin},
+    map::{Map, MapObstruction, MapPlugin},
 };
 
 #[derive(Component, Clone, Copy, Debug, Default, Reflect)]
@@ -55,6 +55,7 @@ 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>)>,
     ) {
@@ -65,9 +66,7 @@ impl Viewshed {
             start.translation().truncate(),
             0.,
             &shape,
-            QueryFilter::new()
-                .exclude_collider(*viewer_entity)
-                .predicate(&|e| visible_query.contains(e)),
+            QueryFilter::new().predicate(&|e| visible_query.get(e).is_ok()),
             |entity| {
                 if let Ok((_, collider, transform)) = visible_query.get(entity) {
                     let position = Isometry2::new(
@@ -123,16 +122,19 @@ impl Viewshed {
                             shape_pos,
                             0.,
                             &shape,
-                            QueryFilter::new()
-                                .exclude_collider(*viewer_entity)
-                                .predicate(&|v| visible_query.contains(v)),
+                            QueryFilter::new().predicate(&|v| visible_query.get(v).is_ok()),
                             |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);
                                 }
-                                true
+                                !obstruction
                             },
                         );
                         cache.insert((coord.x, coord.y), (opacity, coord_entities.clone()));
@@ -148,7 +150,13 @@ 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
+                    }
                 } else {
                     0
                 }
@@ -293,6 +301,7 @@ where
 fn update_viewshed(
     config: Res<RapierConfiguration>,
     visible: Query<(&Visible, &Collider, &GlobalTransform)>,
+    obstructions: Query<&MapObstruction>,
     mut viewers: Query<(
         Entity,
         &mut Viewshed,
@@ -313,6 +322,7 @@ fn update_viewshed(
             viewer_transform,
             &rapier_context,
             &visible,
+            &obstructions,
             &mut changed,
             &mut cache,
         );
@@ -329,6 +339,7 @@ fn remove_visible(
     )>,
     rapier_context: Res<RapierContext>,
     visible: Query<(&Visible, &Collider, &GlobalTransform)>,
+    obstructions: Query<&MapObstruction>,
     mut changed: EventWriter<VisibilityChanged>,
 ) {
     if !removed.is_empty() {
@@ -345,6 +356,7 @@ fn remove_visible(
                     start,
                     &rapier_context,
                     &visible,
+                    &obstructions,
                     &mut changed,
                     &mut cache,
                 );