From 4357a5838af9fc0246b9d3a3f415537936a44eb1 Mon Sep 17 00:00:00 2001
From: Nolan Darilek <nolan@thewordnerd.info>
Date: Mon, 21 Mar 2022 21:58:34 -0500
Subject: [PATCH] Add snapping.

---
 src/navigation.rs | 304 +++++++++++++++++++++++++++++++---------------
 1 file changed, 209 insertions(+), 95 deletions(-)

diff --git a/src/navigation.rs b/src/navigation.rs
index 4f5f6cd..4417ca6 100644
--- a/src/navigation.rs
+++ b/src/navigation.rs
@@ -1,4 +1,7 @@
-use std::{collections::HashMap, error::Error, fmt::Debug, hash::Hash, marker::PhantomData};
+use std::{
+    collections::HashMap, error::Error, f32::consts::PI, fmt::Debug, hash::Hash,
+    marker::PhantomData,
+};
 
 use bevy::prelude::*;
 use bevy_input_actionmap::InputMap;
@@ -41,11 +44,15 @@ pub struct Speed(pub f32);
 #[reflect(Component)]
 pub struct Sprinting;
 
+#[derive(Default, Deref, DerefMut)]
+struct Snapping(bool);
+
 fn movement_controls<S, A: 'static>(
     mut commands: Commands,
     config: Res<NavigationConfig<S, A>>,
     input: Res<InputMap<A>>,
     time: Res<Time>,
+    mut snapping: ResMut<Snapping>,
     mut query: Query<
         (
             Entity,
@@ -66,114 +73,209 @@ fn movement_controls<S, A: 'static>(
     for (entity, mut velocity, mut speed, max_speed, rotation_speed, mut position, destination) in
         query.iter_mut()
     {
-        let sprinting = if let Some(action) = config.action_sprint.clone() {
-            input.active(action)
+        if **snapping {
+            if let Some(action) = config.action_forward.clone() {
+                if input.active(action) {
+                    continue;
+                }
+            }
+            if let Some(action) = config.action_backward.clone() {
+                if input.active(action) {
+                    continue;
+                }
+            }
+            if let Some(action) = config.action_left.clone() {
+                if input.active(action) {
+                    continue;
+                }
+            }
+            if let Some(action) = config.action_right.clone() {
+                if input.active(action) {
+                    continue;
+                }
+            }
+            if let (Some(_), Some(rotate_left), Some(rotate_right)) = (
+                rotation_speed,
+                config.action_rotate_left.clone(),
+                config.action_rotate_right.clone(),
+            ) {
+                if input.active(rotate_left) {
+                    continue;
+                }
+                if input.active(rotate_right) {
+                    continue;
+                }
+            }
+            **snapping = false;
         } else {
-            false
-        };
-        if sprinting {
-            commands.entity(entity).insert(Sprinting::default());
-        } else {
-            commands.entity(entity).remove::<Sprinting>();
-        }
-        let mut direction = Vec2::default();
-        if let Some(action) = config.action_forward.clone() {
-            if input.active(action) {
-                direction.x += 1.;
+            let sprinting = if let Some(action) = config.action_sprint.clone() {
+                input.active(action)
+            } else {
+                false
+            };
+            if sprinting {
+                commands.entity(entity).insert(Sprinting::default());
+            } else {
+                commands.entity(entity).remove::<Sprinting>();
             }
-        }
-        if let Some(action) = config.action_backward.clone() {
-            if input.active(action) {
-                direction.x -= 1.;
+            let mut direction = Vec2::default();
+            if let Some(action) = config.action_forward.clone() {
+                if input.active(action) {
+                    direction.x += 1.;
+                }
             }
-        }
-        if let Some(action) = config.action_left.clone() {
-            if input.active(action) {
-                direction.y += 1.;
+            if let Some(action) = config.action_backward.clone() {
+                if input.active(action) {
+                    direction.x -= 1.;
+                }
             }
-        }
-        if let Some(action) = config.action_right.clone() {
-            if input.active(action) {
-                direction.y -= 1.;
+            if let Some(action) = config.action_left.clone() {
+                if input.active(action) {
+                    direction.y += 1.;
+                }
             }
-        }
-        if let (Some(rotation_speed), Some(rotate_left), Some(rotate_right)) = (
-            rotation_speed,
-            config.action_rotate_left.clone(),
-            config.action_rotate_right.clone(),
-        ) {
-            let delta = rotation_speed.radians() * time.delta_seconds();
-            if input.active(rotate_left) {
-                position.position.rotation *= UnitComplex::new(delta);
+            if let Some(action) = config.action_right.clone() {
+                if input.active(action) {
+                    direction.y -= 1.;
+                }
             }
-            if input.active(rotate_right) {
-                position.position.rotation *= UnitComplex::new(-delta);
+            if let (Some(rotation_speed), Some(rotate_left), Some(rotate_right)) = (
+                rotation_speed,
+                config.action_rotate_left.clone(),
+                config.action_rotate_right.clone(),
+            ) {
+                let delta = rotation_speed.radians() * time.delta_seconds();
+                if input.active(rotate_left) {
+                    position.position.rotation *= UnitComplex::new(delta);
+                }
+                if input.active(rotate_right) {
+                    position.position.rotation *= UnitComplex::new(-delta);
+                } else {
+                    velocity.angvel = 0.;
+                }
             } else {
                 velocity.angvel = 0.;
             }
-        } else {
-            velocity.angvel = 0.;
-        }
-        if direction.length_squared() != 0. {
-            commands.entity(entity).remove::<Destination>();
-            commands.entity(entity).remove::<Exploring>();
-            for entity in exploration_focused.iter() {
-                commands.entity(entity).remove::<ExplorationFocused>();
-            }
-            direction = direction.normalize();
-            if direction.x > 0. {
-                direction.x *= config.forward_movement_factor;
-            } else if direction.x < 0. {
-                direction.x *= config.backward_movement_factor;
-            }
-            if direction.y != 0. {
-                direction.y *= config.strafe_movement_factor;
-            }
-            let strength = if let (Some(forward), Some(backward), Some(left), Some(right)) = (
-                config.action_forward.clone(),
-                config.action_backward.clone(),
-                config.action_left.clone(),
-                config.action_right.clone(),
-            ) {
-                let forward_x = input.strength(forward).abs();
-                let backward_x = input.strength(backward).abs();
-                let x = if forward_x > backward_x {
-                    forward_x
+            if direction.length_squared() != 0. {
+                commands.entity(entity).remove::<Destination>();
+                commands.entity(entity).remove::<Exploring>();
+                for entity in exploration_focused.iter() {
+                    commands.entity(entity).remove::<ExplorationFocused>();
+                }
+                direction = direction.normalize();
+                if direction.x > 0. {
+                    direction.x *= config.forward_movement_factor;
+                } else if direction.x < 0. {
+                    direction.x *= config.backward_movement_factor;
+                }
+                if direction.y != 0. {
+                    direction.y *= config.strafe_movement_factor;
+                }
+                let strength = if let (Some(forward), Some(backward), Some(left), Some(right)) = (
+                    config.action_forward.clone(),
+                    config.action_backward.clone(),
+                    config.action_left.clone(),
+                    config.action_right.clone(),
+                ) {
+                    let forward_x = input.strength(forward).abs();
+                    let backward_x = input.strength(backward).abs();
+                    let x = if forward_x > backward_x {
+                        forward_x
+                    } else {
+                        backward_x
+                    };
+                    let right_y = input.strength(right).abs();
+                    let left_y = input.strength(left).abs();
+                    let y = if right_y > left_y { right_y } else { left_y };
+                    Some(Vec2::new(x, y))
                 } else {
-                    backward_x
+                    None
                 };
-                let right_y = input.strength(right).abs();
-                let left_y = input.strength(left).abs();
-                let y = if right_y > left_y { right_y } else { left_y };
-                Some(Vec2::new(x, y))
+                let s = if sprinting {
+                    **max_speed
+                } else {
+                    **max_speed / config.sprint_movement_factor
+                };
+                **speed = s;
+                if let Some(strength) = strength {
+                    direction *= strength;
+                }
+                let mut v: Vector<Real> = (direction * **speed).into();
+                v = position.position.rotation.transform_vector(&v);
+                velocity.linvel = v;
+                //velocity.linvel = position.position.rotation.transform_vector(&velocity.linvel);
+            } else if destination.is_none() {
+                velocity.linvel = Vec2::ZERO.into();
+                **speed = 0.;
+            } else if sprinting {
+                **speed = **max_speed;
             } else {
-                None
-            };
-            let s = if sprinting {
-                **max_speed
-            } else {
-                **max_speed / config.sprint_movement_factor
-            };
-            **speed = s;
-            if let Some(strength) = strength {
-                direction *= strength;
+                velocity.linvel = Vec2::ZERO.into();
+                **speed = **max_speed / 3.;
             }
-            let mut v: Vector<Real> = (direction * **speed).into();
-            v = position.position.rotation.transform_vector(&v);
-            velocity.linvel = v;
-            //velocity.linvel = position.position.rotation.transform_vector(&velocity.linvel);
-        } else if destination.is_none() {
-            velocity.linvel = Vec2::ZERO.into();
-            **speed = 0.;
-        } else if sprinting {
-            **speed = **max_speed;
-        } else {
-            velocity.linvel = Vec2::ZERO.into();
-            **speed = **max_speed / 3.;
         }
     }
 }
 
+fn snap<S, A: 'static>(
+    mut tts: ResMut<Tts>,
+    input: Res<InputMap<A>>,
+    config: Res<NavigationConfig<S, A>>,
+    mut snapping: ResMut<Snapping>,
+    mut position: Query<(&mut RigidBodyPositionComponent, &CardinalDirection), With<Player>>,
+) -> Result<(), Box<dyn Error>>
+where
+    S: bevy::ecs::component::Component + Clone + Debug + Eq + Hash,
+    A: Hash + Eq + Clone + Send + Sync,
+{
+    if let Some(action) = config.action_snap_left.clone() {
+        if input.just_active(action) {
+            for (mut position, direction) in position.iter_mut() {
+                **snapping = true;
+                position.position.rotation = UnitComplex::new(match direction {
+                    CardinalDirection::North => PI,
+                    CardinalDirection::East => PI / 2.,
+                    CardinalDirection::South => 0.,
+                    CardinalDirection::West => PI * 1.5,
+                });
+            }
+        }
+    }
+    if let Some(action) = config.action_snap_right.clone() {
+        if input.just_active(action) {
+            for (mut position, direction) in position.iter_mut() {
+                **snapping = true;
+                position.position.rotation = UnitComplex::new(match direction {
+                    CardinalDirection::North => 0.,
+                    CardinalDirection::East => PI * 1.5,
+                    CardinalDirection::South => PI,
+                    CardinalDirection::West => PI / 2.,
+                });
+            }
+        }
+    }
+    if let Some(action) = config.action_snap_cardinal.clone() {
+        if input.just_active(action) {
+            for (mut position, direction) in position.iter_mut() {
+                **snapping = true;
+                let yaw: Angle = direction.into();
+                let yaw = yaw.radians();
+                position.position.rotation = UnitComplex::new(yaw);
+                tts.speak(direction.to_string(), true)?;
+            }
+        }
+    }
+    if let Some(action) = config.action_snap_reverse.clone() {
+        if input.just_active(action) {
+            for (mut position, _) in position.iter_mut() {
+                **snapping = true;
+                position.position.rotation *= UnitComplex::new(PI);
+            }
+        }
+    }
+    Ok(())
+}
+
 fn update_direction(
     mut commands: Commands,
     mut query: Query<
@@ -230,6 +332,10 @@ pub struct NavigationConfig<S, A> {
     pub action_right: Option<A>,
     pub action_rotate_left: Option<A>,
     pub action_rotate_right: Option<A>,
+    pub action_snap_left: Option<A>,
+    pub action_snap_right: Option<A>,
+    pub action_snap_reverse: Option<A>,
+    pub action_snap_cardinal: Option<A>,
     pub action_sprint: Option<A>,
     pub forward_movement_factor: f32,
     pub backward_movement_factor: f32,
@@ -247,6 +353,10 @@ impl<S, A> Default for NavigationConfig<S, A> {
             action_right: None,
             action_rotate_left: None,
             action_rotate_right: None,
+            action_snap_left: None,
+            action_snap_right: None,
+            action_snap_reverse: None,
+            action_snap_cardinal: None,
             action_sprint: None,
             forward_movement_factor: 1.,
             backward_movement_factor: 1.,
@@ -280,19 +390,23 @@ where
             .get_resource::<NavigationConfig<S, A>>()
             .unwrap()
             .clone();
-        app.register_type::<MaxSpeed>()
+        app.init_resource::<Snapping>()
+            .register_type::<MaxSpeed>()
             .register_type::<RotationSpeed>()
             .register_type::<Sprinting>()
             .add_system(update_direction)
             .add_system(speak_direction.chain(error_handler))
             .add_system_to_stage(CoreStage::PostUpdate, remove_speed);
         if config.movement_control_states.is_empty() {
-            app.add_system(movement_controls::<S, A>);
+            app.add_system(movement_controls::<S, A>)
+                .add_system(snap::<S, A>.chain(error_handler));
         } else {
             let states = config.movement_control_states;
             for state in states {
                 app.add_system_set(
-                    SystemSet::on_update(state).with_system(movement_controls::<S, A>),
+                    SystemSet::on_update(state)
+                        .with_system(movement_controls::<S, A>)
+                        .with_system(snap::<S, A>.chain(error_handler)),
                 );
             }
         }