From 7efa0a8ab2080a07a45bd9deab6d57ebcdbde74c Mon Sep 17 00:00:00 2001
From: Joe Ardent <code@ardent.nebcorp.com>
Date: Mon, 30 Jan 2023 16:05:42 -0800
Subject: [PATCH] Add system to de-intersect wheels and planet surface.

---
 src/action/components.rs | 27 ++++++++++---
 src/action/mod.rs        |  8 ++++
 src/action/systems.rs    | 85 +++++++++++++++++++++++++++++++++-------
 src/bike.rs              | 19 ++++-----
 src/camera.rs            |  4 +-
 5 files changed, 112 insertions(+), 31 deletions(-)

diff --git a/src/action/components.rs b/src/action/components.rs
index f16fcf9..d71bdd4 100644
--- a/src/action/components.rs
+++ b/src/action/components.rs
@@ -1,7 +1,7 @@
 use std::time::Instant;
 
 use bevy::{
-    prelude::{Component, ReflectResource, Resource},
+    prelude::{Component, ReflectResource, Resource, Vec3},
     reflect::Reflect,
 };
 
@@ -14,6 +14,21 @@ impl Default for ActionDebugInstant {
     }
 }
 
+#[derive(Debug, Component)]
+pub(super) struct Tunneling {
+    pub frames: usize,
+    pub dir: Vec3,
+}
+
+impl Default for Tunneling {
+    fn default() -> Self {
+        Tunneling {
+            frames: 15,
+            dir: Vec3::ZERO,
+        }
+    }
+}
+
 #[derive(Debug, Resource, Reflect)]
 #[reflect(Resource)]
 pub struct MovementSettings {
@@ -27,7 +42,7 @@ impl Default for MovementSettings {
         Self {
             sensitivity: 6.0,
             accel: 40.0,
-            gravity: 7.0,
+            gravity: 9.8,
         }
     }
 }
@@ -68,7 +83,7 @@ impl Default for CatControllerState {
             roll_prev: Default::default(),
             pitch_integral: Default::default(),
             pitch_prev: Default::default(),
-            decay_factor: 0.9,
+            decay_factor: 0.99,
             roll_limit: 1.5,
             pitch_limit: 1.2,
         }
@@ -77,10 +92,10 @@ impl Default for CatControllerState {
 
 impl CatControllerState {
     pub fn decay(&mut self) {
-        if self.roll_integral.abs() > 0.1 {
+        if self.roll_integral.abs() > 0.001 {
             self.roll_integral *= self.decay_factor;
         }
-        if self.pitch_integral.abs() > 0.1 {
+        if self.pitch_integral.abs() > 0.001 {
             self.pitch_integral *= self.decay_factor;
         }
     }
@@ -101,7 +116,7 @@ impl CatControllerState {
         (derivative, self.pitch_integral)
     }
 
-    pub fn set_limits(&mut self, roll: f32, pitch: f32) {
+    pub fn set_integral_limits(&mut self, roll: f32, pitch: f32) {
         self.roll_limit = roll;
         self.pitch_limit = pitch;
     }
diff --git a/src/action/mod.rs b/src/action/mod.rs
index 8646ba7..642b1ed 100644
--- a/src/action/mod.rs
+++ b/src/action/mod.rs
@@ -21,9 +21,17 @@ impl Plugin for CyberActionPlugin {
             .add_plugin(RapierPhysicsPlugin::<NoUserData>::default())
             .add_plugin(FrameTimeDiagnosticsPlugin::default())
             .add_startup_system(zero_gravity)
+            .add_system(surface_fix)
             .add_system(gravity.before("cat"))
             .add_system(falling_cat.label("cat"))
             .add_system(input_forces.label("iforces").after("cat"))
+            .add_system(
+                tunnel_out
+                    .label("tunnel")
+                    .before("surface_fix")
+                    .after("drag"),
+            )
+            .add_system(surface_fix.label("surface_fix").after("cat"))
             .add_system(drag.label("drag").after("iforces"));
     }
 }
diff --git a/src/action/systems.rs b/src/action/systems.rs
index 29b5d8b..bc81bc8 100644
--- a/src/action/systems.rs
+++ b/src/action/systems.rs
@@ -1,35 +1,38 @@
 #[cfg(feature = "inspector")]
 use std::time::Instant;
 
-use bevy::prelude::{Query, Res, ResMut, Time, Transform, Vec3, With};
-use bevy_rapier3d::prelude::{ExternalForce, Friction, RapierConfiguration, Velocity};
+use bevy::prelude::{Commands, Entity, Query, Res, ResMut, Time, Transform, Vec3, With, Without};
+use bevy_rapier3d::prelude::{
+    CollisionGroups, ExternalForce, Friction, Group, QueryFilter, RapierConfiguration,
+    RapierContext, ReadMassProperties, Velocity,
+};
 
 #[cfg(feature = "inspector")]
 use super::ActionDebugInstant;
-use super::{CatControllerSettings, CatControllerState, MovementSettings};
+use super::{CatControllerSettings, CatControllerState, MovementSettings, Tunneling};
 use crate::{
-    bike::{CyberBikeBody, CyberWheel},
+    bike::{CyberBikeBody, CyberWheel, BIKE_WHEEL_COLLISION_GROUP},
     input::InputState,
 };
 
 /// Disable gravity in Rapier.
-pub(crate) fn zero_gravity(mut config: ResMut<RapierConfiguration>) {
+pub(super) fn zero_gravity(mut config: ResMut<RapierConfiguration>) {
     config.gravity = Vec3::ZERO;
 }
 
 /// The gravity vector points from the cyberbike to the center of the planet.
-pub(crate) fn gravity(
-    mut query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
+pub(super) fn gravity(
+    mut query: Query<(&Transform, &mut ExternalForce, &ReadMassProperties), With<CyberBikeBody>>,
     settings: Res<MovementSettings>,
 ) {
-    let (xform, mut forces) = query.single_mut();
-    let grav = xform.translation.normalize() * -settings.gravity;
+    let (xform, mut forces, mprops) = query.single_mut();
+    let grav = xform.translation.normalize() * -settings.gravity * mprops.0.mass;
     forces.force = grav;
     forces.torque = Vec3::ZERO;
 }
 
 /// PID-based controller for stabilizing attitude; keeps the cyberbike upright.
-pub(crate) fn falling_cat(
+pub(super) fn falling_cat(
     mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CatControllerState)>,
     time: Res<Time>,
     settings: Res<CatControllerSettings>,
@@ -58,14 +61,14 @@ pub(crate) fn falling_cat(
     let (derivative, integral) = control_vars.update_pitch(pitch_error, time.delta_seconds());
     let mag = (settings.kp * pitch_error) + (settings.ki * integral) + (settings.kd * derivative);
     if mag.is_finite() {
-        forces.torque += wright * mag * 0.2;
+        forces.torque += wright * mag;
     }
 
     #[cfg(feature = "inspector")]
     {
         let now = Instant::now();
         if (now - debug_instant.0).as_millis() > 500 {
-            dbg!(roll_error, pitch_error, &control_vars);
+            dbg!(roll_error, pitch_error, &control_vars, mag);
             debug_instant.0 = now;
         }
     }
@@ -73,7 +76,7 @@ pub(crate) fn falling_cat(
 }
 
 /// Apply forces to the cyberbike as a result of input.
-pub(crate) fn input_forces(
+pub(super) fn input_forces(
     settings: Res<MovementSettings>,
     input: Res<InputState>,
     mut cquery: Query<&mut Friction, With<CyberWheel>>,
@@ -97,9 +100,63 @@ pub(crate) fn input_forces(
     *forces += force;
 }
 
+/// Don't let the wheels get stuck underneat the planet
+pub(super) fn surface_fix(
+    mut commands: Commands,
+    mut wheel_query: Query<
+        (Entity, &Transform, &mut CollisionGroups),
+        (With<CyberWheel>, Without<Tunneling>),
+    >,
+    context: Res<RapierContext>,
+) {
+    // assume the body is not below the planet surface
+    for (entity, xform, mut cgroups) in wheel_query.iter_mut() {
+        let ray_dir = xform.translation.normalize();
+        if let Some(hit) = context.cast_ray_and_get_normal(
+            xform.translation,
+            ray_dir,
+            10.0,
+            false,
+            QueryFilter::only_fixed(),
+        ) {
+            cgroups.memberships = Group::NONE;
+            cgroups.filters = Group::NONE;
+            commands.entity(entity).insert(Tunneling {
+                frames: 15,
+                dir: -hit.1.normal,
+            });
+        }
+    }
+}
+
+pub(super) fn tunnel_out(
+    mut commands: Commands,
+    mut wheel_query: Query<
+        (
+            Entity,
+            &mut Tunneling,
+            &mut CollisionGroups,
+            &mut ExternalForce,
+        ),
+        With<CyberWheel>,
+    >,
+) {
+    for (entity, mut tunneling, mut cgroups, mut force) in wheel_query.iter_mut() {
+        if tunneling.frames == 0 {
+            commands.entity(entity).remove::<Tunneling>();
+            cgroups.memberships = BIKE_WHEEL_COLLISION_GROUP.0;
+            cgroups.filters = BIKE_WHEEL_COLLISION_GROUP.1;
+            continue;
+        }
+        tunneling.frames -= 1;
+        force.force += tunneling.dir * tunneling.frames as f32;
+        dbg!(&tunneling);
+    }
+}
+
 /// General velocity-based drag-force calculation; does not take orientation
 /// into account.
-pub(crate) fn drag(mut query: Query<(&Velocity, &mut ExternalForce), With<CyberBikeBody>>) {
+pub(super) fn drag(mut query: Query<(&Velocity, &mut ExternalForce), With<CyberBikeBody>>) {
     let (vels, mut forces) = query.single_mut();
 
     if let Some(vel) = vels.linvel.try_normalize() {
diff --git a/src/bike.rs b/src/bike.rs
index aa03dc6..a9ac527 100644
--- a/src/bike.rs
+++ b/src/bike.rs
@@ -5,8 +5,8 @@ use bevy_rapier3d::{
     geometry::Group,
     prelude::{
         Ccd, Collider, ColliderMassProperties, CollisionGroups, Damping, ExternalForce, Friction,
-        MultibodyJoint, PrismaticJointBuilder, Restitution, RigidBody, Sleeping,
-        TransformInterpolation, Velocity,
+        MultibodyJoint, PrismaticJointBuilder, ReadMassProperties, Restitution, RigidBody,
+        Sleeping, TransformInterpolation, Velocity,
     },
 };
 
@@ -42,17 +42,17 @@ impl Default for WheelConfig {
             front_forward: 0.9,
             front_stance: 0.65,
             rear_back: 1.1,
-            y: -0.4,
-            limits: [-0.5, 0.1],
-            stiffness: 75.0,
-            damping: 2.0,
+            y: -0.45,
+            limits: [-0.7, 0.1],
+            stiffness: 90.0,
+            damping: 8.0,
             radius: 0.3,
         }
     }
 }
 
-const BIKE_BODY_COLLISION_GROUP: (Group, Group) = (Group::GROUP_1, Group::GROUP_1);
-const BIKE_WHEEL_COLLISION_GROUP: (Group, Group) = (Group::GROUP_10, Group::GROUP_10);
+pub const BIKE_BODY_COLLISION_GROUP: (Group, Group) = (Group::GROUP_1, Group::GROUP_1);
+pub const BIKE_WHEEL_COLLISION_GROUP: (Group, Group) = (Group::GROUP_10, Group::GROUP_10);
 
 fn spawn_cyberbike(
     mut commands: Commands,
@@ -89,7 +89,7 @@ fn spawn_cyberbike(
         ..Default::default()
     };
 
-    let mass_properties = ColliderMassProperties::Density(0.2);
+    let mass_properties = ColliderMassProperties::Density(0.9);
 
     let (membership, filter) = BIKE_BODY_COLLISION_GROUP;
     let bike_collision_group = CollisionGroups::new(membership, filter);
@@ -113,6 +113,7 @@ fn spawn_cyberbike(
             friction,
             not_sleeping,
             ccd,
+            ReadMassProperties::default(),
         ))
         .insert(TransformInterpolation {
             start: None,
diff --git a/src/camera.rs b/src/camera.rs
index ef47adc..6c86cfd 100644
--- a/src/camera.rs
+++ b/src/camera.rs
@@ -70,8 +70,8 @@ fn follow_cyberbike(
     for (mut cam_xform, cam_type) in query.p1().iter_mut() {
         match *cam_type {
             CyberCameras::Hero => {
-                let look_at = bike_xform.translation + (bike_xform.forward() * 200.0);
-                let cam_pos = bike_xform.translation + (bike_xform.back() * 1.05) + (up * 1.08);
+                let look_at = bike_xform.translation + (bike_xform.forward() * 500.0);
+                let cam_pos = bike_xform.translation + (bike_xform.back() * 0.1) + (up * 0.8);
 
                 cam_xform.translation = cam_pos;
                 cam_xform.look_at(look_at, up);