From e198ac3cd07f0ba9c9573760846c0d8ca5ba7e16 Mon Sep 17 00:00:00 2001
From: Amatsugu <khamraj@kaisei.app>
Date: Thu, 26 Jun 2025 14:48:27 -0400
Subject: [PATCH] Fix gravity alignment

---
 src/plugins/player.rs | 25 ++++++++++++++++---------
 src/utils/rotation.rs | 13 +++++++++++++
 2 files changed, 29 insertions(+), 9 deletions(-)

diff --git a/src/plugins/player.rs b/src/plugins/player.rs
index bfdacff..724bfc6 100644
--- a/src/plugins/player.rs
+++ b/src/plugins/player.rs
@@ -8,14 +8,17 @@ use crate::{
 		player::{GravityDirection, JumpSpeed, MoveSpeed, PlayerDrag, PlayerForce, PlayerMotion, PlayerVelocity},
 		tags::Player,
 	},
-	utils::{input::get_mouse_delta, rotation::get_alignment_rotation},
+	utils::{
+		input::get_mouse_delta,
+		rotation::{get_alignment_rotation, get_alignment_rotation_preserve_twist},
+	},
 };
 
 pub struct PlayerPlugin;
 
 impl Plugin for PlayerPlugin {
 	fn build(&self, app: &mut App) {
-		app.add_systems(PreUpdate, (keyboard_input).in_set(PlayerInputSystems));
+		app.add_systems(PreUpdate, (keyboard_input, player_look).in_set(PlayerInputSystems));
 		app.add_systems(Update, (apply_gravity, apply_forces, apply_motion, apply_drag).chain());
 		app.add_systems(Update, align_with_gravity);
 	}
@@ -51,8 +54,8 @@ fn apply_gravity(
 		vel.0 += grav * scale.0 * 9.47 * time.delta_secs();
 	} else {
 		let dot = vel.0.dot(grav);
-		if dot > 0.0 {
-			vel.0 = vel.0 - dot * grav;
+		if dot > EPSILON {
+			vel.0 -= dot * grav;
 		}
 	}
 }
@@ -124,9 +127,14 @@ fn keyboard_input(
 	// controller.translation = Some(move_vec * time.delta_secs());
 }
 
-fn player_look(mut player: Single<&mut Transform, With<Player>>, mouse_motion: EventReader<MouseMotion>) {
-	let delta = get_mouse_delta(mouse_motion);
-	player.rotate_y(delta.x);
+fn player_look(
+	mut player: Single<&mut Transform, With<Player>>,
+	mouse_motion: EventReader<MouseMotion>,
+	time: Res<Time>,
+) {
+	let delta = get_mouse_delta(mouse_motion) * -time.delta_secs();
+	let up = player.up();
+	player.rotate_axis(up, delta.x);
 }
 
 fn align_with_gravity(
@@ -151,9 +159,8 @@ fn align_with_gravity(
 		return;
 	};
 
-	let cur_up = transform.up();
 	let grav_up = grav_dir * -1.0;
 
-	let desired_rotation = get_alignment_rotation(cur_up, grav_up);
+	let desired_rotation = get_alignment_rotation_preserve_twist(transform.rotation, grav_up);
 	transform.rotation = transform.rotation.lerp(desired_rotation, time.delta_secs());
 }
diff --git a/src/utils/rotation.rs b/src/utils/rotation.rs
index 130ad90..8106d3f 100644
--- a/src/utils/rotation.rs
+++ b/src/utils/rotation.rs
@@ -23,3 +23,16 @@ pub fn get_alignment_rotation(cur_dir: Dir3, target_dir: Vec3) -> Quat {
 		return Quat::from_axis_angle(axis.normalize(), angle);
 	}
 }
+
+pub fn get_alignment_rotation_preserve_twist(cur_rot: Quat, target_up: Vec3) -> Quat {
+	let target_up = target_up.normalize();
+
+	let cur_fwd = cur_rot * Vec3::Z;
+
+	let forward_projection = (cur_fwd - target_up * cur_fwd.dot(target_up)).normalize();
+	let right = target_up.cross(forward_projection).normalize();
+	let fwd = right.cross(target_up);
+	let basis = Mat3::from_cols(right, target_up, fwd);
+
+	return Quat::from_mat3(&basis);
+}