From b4a210a546c6730d140ad97edf4988c1d3a67297 Mon Sep 17 00:00:00 2001
From: SDraw <sdraw@protonmail.com>
Date: Thu, 1 May 2025 19:18:17 +0300
Subject: [PATCH] Improved rotations

---
 ml_ppu/PickUpManager.cs | 17 ++++++++++++-----
 ml_ppu/Utils.cs         | 21 ++++++++++++++++++++-
 2 files changed, 32 insertions(+), 6 deletions(-)

diff --git a/ml_ppu/PickUpManager.cs b/ml_ppu/PickUpManager.cs
index bac293a..b9e2685 100644
--- a/ml_ppu/PickUpManager.cs
+++ b/ml_ppu/PickUpManager.cs
@@ -13,8 +13,10 @@ namespace ml_ppu
 
         Collider m_holderPointA = null;
         CVRPointer m_holderPointerA = null;
+        Quaternion m_holderPointAOffset;
         Collider m_holderPointB = null;
         CVRPointer m_holderPointerB = null;
+        Quaternion m_holderPointBOffset;
 
         CapsuleCollider m_collider = null;
         Matrix4x4 m_colliderOffSet;
@@ -26,8 +28,8 @@ namespace ml_ppu
         bool m_ready = false;
         bool m_held = false;
 
-        Vector3 m_lastPosition;
-        Vector3 m_velocity;
+        Vector3 m_lastPosition = Vector3.zero;
+        Vector3 m_velocity = Vector3.zero;
 
         void Awake()
         {
@@ -95,7 +97,7 @@ namespace ml_ppu
                     {
                         Matrix4x4 l_midPoint = Matrix4x4.TRS(
                             Vector3.Lerp(m_holderPointA.transform.position, m_holderPointB.transform.position, 0.5f),
-                            Quaternion.Slerp(m_holderPointA.transform.rotation, m_holderPointB.transform.rotation, 0.5f),
+                            Quaternion.Slerp(m_holderPointA.transform.rotation * m_holderPointAOffset, m_holderPointB.transform.rotation * m_holderPointBOffset, 0.5f),
                             Vector3.one
                         );
                         Matrix4x4 l_colliderMat = l_midPoint * m_colliderOffSet;
@@ -103,7 +105,7 @@ namespace ml_ppu
                         m_collider.transform.rotation = l_colliderMat.rotation;
 
                         Matrix4x4 l_avatarMat = l_colliderMat * m_avatarOffSet;
-                        BetterBetterCharacterController.Instance.TeleportPlayerTo(l_avatarMat.GetPosition(), l_avatarMat.rotation.eulerAngles, true, false);
+                        BetterBetterCharacterController.Instance.TeleportPlayerTo(l_avatarMat.GetPosition(), l_avatarMat.rotation, true, false); // Extension method with Quaternion as rotation
 
                         Vector3 l_position = l_avatarMat.GetPosition();
                         m_velocity = (l_position - m_lastPosition) / Time.deltaTime;
@@ -229,14 +231,19 @@ namespace ml_ppu
                         m_holderPointerB = p_pointer;
 
                         // Remember offsets
+                        Quaternion l_avatarRot = PlayerSetup.Instance._avatar.transform.rotation;
+                        m_holderPointAOffset = Quaternion.Inverse(m_holderPointA.transform.rotation) * l_avatarRot;
+                        m_holderPointBOffset = Quaternion.Inverse(m_holderPointB.transform.rotation) * l_avatarRot;
+
                         Matrix4x4 l_midPoint = Matrix4x4.TRS(
                             Vector3.Lerp(m_holderPointA.transform.position, m_holderPointB.transform.position, 0.5f),
-                            Quaternion.Slerp(m_holderPointA.transform.rotation, m_holderPointB.transform.rotation, 0.5f),
+                            l_avatarRot,
                             Vector3.one
                         );
                         m_colliderOffSet = l_midPoint.inverse * m_collider.transform.GetMatrix();
                         m_avatarOffSet = m_collider.transform.GetMatrix().inverse * PlayerSetup.Instance._avatar.transform.GetMatrix();
                         m_lastPosition = PlayerSetup.Instance._avatar.transform.position;
+                        m_velocity = Vector3.zero;
                         m_held = true;
                     }
                 }
diff --git a/ml_ppu/Utils.cs b/ml_ppu/Utils.cs
index 54686b1..de56ef0 100644
--- a/ml_ppu/Utils.cs
+++ b/ml_ppu/Utils.cs
@@ -1,4 +1,8 @@
-using UnityEngine;
+using ABI_RC.Core;
+using ABI_RC.Core.Player;
+using ABI_RC.Systems.Movement;
+using ABI_RC.Systems.Safety.AdvancedSafety;
+using UnityEngine;
 
 namespace ml_ppu
 {
@@ -8,5 +12,20 @@ namespace ml_ppu
         {
             return Matrix4x4.TRS(p_pos ? p_transform.position : Vector3.zero, p_rot ? p_transform.rotation : Quaternion.identity, p_scl ? p_transform.lossyScale : Vector3.one);
         }
+
+        // Remade method to remove Q-E-Q conversions to prevent bad angles
+        public static void TeleportPlayerTo(this BetterBetterCharacterController p_instance, Vector3 p_targetPos, Quaternion p_targetRot, bool p_interpolate, bool p_updateGround, bool p_preserveVelocity = false)
+        {
+            Quaternion l_quaternion = p_targetRot * Quaternion.Inverse(PlayerSetup.Instance.GetPlayerRotation());
+            Quaternion l_newRotation = l_quaternion * p_instance.characterMovement.rotation;
+            if(l_newRotation.eulerAngles.IsAbsurd())
+            {
+                CommonTools.LogAuto(CommonTools.LogLevelType_t.Warning, "Attempted to teleport using an absurd rotation. Ignoring it...", "", "Assets\\ABI RC\\Systems\\Movement\\BetterBetterCharacterController.cs", "TeleportPlayerTo", 1845);
+                return;
+            }
+            p_interpolate = false;
+            p_instance.TeleportRotation(l_newRotation, p_interpolate);
+            p_instance.TeleportPlayerTo(p_targetPos, p_interpolate, p_updateGround, p_preserveVelocity, l_quaternion);
+        }
     }
 }