Fix touchpad handling and change gyro calculation (shadps4-emu#3006)

* Change touchpad handling and orientation calculation

* remove unnecessary includes in pad.cpp

* remove the cmake command arguments

* remove the weird file

* try to fix formatting

* limit new gyro and touchpad logic to controller 1

* remove cout

* fix formatting and add the handle check to scePadRead

* swap y and z back

Author: WujekFoliarz

src/core/libraries/pad/pad.cpp

@@ -316,22 +316,79 @@ int PS4_SYSV_ABI scePadRead(s32 handle, OrbisPadData* pData, s32 num) {
         pData[i].angularVelocity.y = states[i].angularVelocity.y;
         pData[i].angularVelocity.z = states[i].angularVelocity.z;
         pData[i].orientation = {0.0f, 0.0f, 0.0f, 1.0f};
-        if (engine) {
+        pData[i].acceleration.x = states[i].acceleration.x * 0.098;
+        pData[i].acceleration.y = states[i].acceleration.y * 0.098;
+        pData[i].acceleration.z = states[i].acceleration.z * 0.098;
+        pData[i].angularVelocity.x = states[i].angularVelocity.x;
+        pData[i].angularVelocity.y = states[i].angularVelocity.y;
+        pData[i].angularVelocity.z = states[i].angularVelocity.z;
+
+        if (engine && handle == 1) {
             const auto gyro_poll_rate = engine->GetAccelPollRate();
             if (gyro_poll_rate != 0.0f) {
-                GameController::CalculateOrientation(pData[i].acceleration,
-                                                     pData[i].angularVelocity,
-                                                     1.0f / gyro_poll_rate, pData[i].orientation);
+                auto now = std::chrono::steady_clock::now();
+                float deltaTime = std::chrono::duration_cast<std::chrono::microseconds>(
+                                      now - controller->GetLastUpdate())
+                                      .count() /
+                                  1000000.0f;
+                controller->SetLastUpdate(now);
+                Libraries::Pad::OrbisFQuaternion lastOrientation = controller->GetLastOrientation();
+                Libraries::Pad::OrbisFQuaternion outputOrientation = {0.0f, 0.0f, 0.0f, 1.0f};
+                GameController::CalculateOrientation(pData->acceleration, pData->angularVelocity,
+                                                     deltaTime, lastOrientation, outputOrientation);
+                pData[i].orientation = outputOrientation;
+                controller->SetLastOrientation(outputOrientation);
             }
         }
+
         pData[i].touchData.touchNum =
             (states[i].touchpad[0].state ? 1 : 0) + (states[i].touchpad[1].state ? 1 : 0);
+
+        if (handle == 1) {
+            if (controller->GetTouchCount() >= 127) {
+                controller->SetTouchCount(0);
+            }
+
+            if (controller->GetSecondaryTouchCount() >= 127) {
+                controller->SetSecondaryTouchCount(0);
+            }
+
+            if (pData->touchData.touchNum == 1 && controller->GetPreviousTouchNum() == 0) {
+                controller->SetTouchCount(controller->GetTouchCount() + 1);
+                controller->SetSecondaryTouchCount(controller->GetTouchCount());
+            } else if (pData->touchData.touchNum == 2 && controller->GetPreviousTouchNum() == 1) {
+                controller->SetSecondaryTouchCount(controller->GetSecondaryTouchCount() + 1);
+            } else if (pData->touchData.touchNum == 0 && controller->GetPreviousTouchNum() > 0) {
+                if (controller->GetTouchCount() < controller->GetSecondaryTouchCount()) {
+                    controller->SetTouchCount(controller->GetSecondaryTouchCount());
+                } else {
+                    if (controller->WasSecondaryTouchReset()) {
+                        controller->SetTouchCount(controller->GetSecondaryTouchCount());
+                        controller->UnsetSecondaryTouchResetBool();
+                    }
+                }
+            }
+
+            controller->SetPreviousTouchNum(pData->touchData.touchNum);
+
+            if (pData->touchData.touchNum == 1) {
+                states[i].touchpad[0].ID = controller->GetTouchCount();
+                states[i].touchpad[1].ID = 0;
+            } else if (pData->touchData.touchNum == 2) {
+                states[i].touchpad[0].ID = controller->GetTouchCount();
+                states[i].touchpad[1].ID = controller->GetSecondaryTouchCount();
+            }
+        } else {
+            states[i].touchpad[0].ID = 1;
+            states[i].touchpad[1].ID = 2;
+        }
+
         pData[i].touchData.touch[0].x = states[i].touchpad[0].x;
         pData[i].touchData.touch[0].y = states[i].touchpad[0].y;
-        pData[i].touchData.touch[0].id = 1;
+        pData[i].touchData.touch[0].id = states[i].touchpad[0].ID;
         pData[i].touchData.touch[1].x = states[i].touchpad[1].x;
         pData[i].touchData.touch[1].y = states[i].touchpad[1].y;
-        pData[i].touchData.touch[1].id = 2;
+        pData[i].touchData.touch[1].id = states[i].touchpad[1].ID;
         pData[i].connected = connected;
         pData[i].timestamp = states[i].time;
         pData[i].connectedCount = connected_count;
@@ -376,31 +433,85 @@ int PS4_SYSV_ABI scePadReadState(s32 handle, OrbisPadData* pData) {
     pData->leftStick.x = state.axes[static_cast<int>(Input::Axis::LeftX)];
     pData->leftStick.y = state.axes[static_cast<int>(Input::Axis::LeftY)];
     pData->rightStick.x = state.axes[static_cast<int>(Input::Axis::RightX)];
+    pData->rightStick.x = state.axes[static_cast<int>(Input::Axis::RightX)];
     pData->rightStick.y = state.axes[static_cast<int>(Input::Axis::RightY)];
     pData->analogButtons.l2 = state.axes[static_cast<int>(Input::Axis::TriggerLeft)];
     pData->analogButtons.r2 = state.axes[static_cast<int>(Input::Axis::TriggerRight)];
-    pData->acceleration.x = state.acceleration.x;
-    pData->acceleration.y = state.acceleration.y;
-    pData->acceleration.z = state.acceleration.z;
+    pData->acceleration.x = state.acceleration.x * 0.098;
+    pData->acceleration.y = state.acceleration.y * 0.098;
+    pData->acceleration.z = state.acceleration.z * 0.098;
     pData->angularVelocity.x = state.angularVelocity.x;
     pData->angularVelocity.y = state.angularVelocity.y;
     pData->angularVelocity.z = state.angularVelocity.z;
     pData->orientation = {0.0f, 0.0f, 0.0f, 1.0f};
-    if (engine) {
+
+    // Only do this on handle 1 for now
+    if (engine && handle == 1) {
         const auto gyro_poll_rate = engine->GetAccelPollRate();
         if (gyro_poll_rate != 0.0f) {
+            auto now = std::chrono::steady_clock::now();
+            float deltaTime = std::chrono::duration_cast<std::chrono::microseconds>(
+                                  now - controller->GetLastUpdate())
+                                  .count() /
+                              1000000.0f;
+            controller->SetLastUpdate(now);
+            Libraries::Pad::OrbisFQuaternion lastOrientation = controller->GetLastOrientation();
+            Libraries::Pad::OrbisFQuaternion outputOrientation = {0.0f, 0.0f, 0.0f, 1.0f};
             GameController::CalculateOrientation(pData->acceleration, pData->angularVelocity,
-                                                 1.0f / gyro_poll_rate, pData->orientation);
+                                                 deltaTime, lastOrientation, outputOrientation);
+            pData->orientation = outputOrientation;
+            controller->SetLastOrientation(outputOrientation);
         }
     }
     pData->touchData.touchNum =
         (state.touchpad[0].state ? 1 : 0) + (state.touchpad[1].state ? 1 : 0);
+
+    // Only do this on handle 1 for now
+    if (handle == 1) {
+        if (controller->GetTouchCount() >= 127) {
+            controller->SetTouchCount(0);
+        }
+
+        if (controller->GetSecondaryTouchCount() >= 127) {
+            controller->SetSecondaryTouchCount(0);
+        }
+
+        if (pData->touchData.touchNum == 1 && controller->GetPreviousTouchNum() == 0) {
+            controller->SetTouchCount(controller->GetTouchCount() + 1);
+            controller->SetSecondaryTouchCount(controller->GetTouchCount());
+        } else if (pData->touchData.touchNum == 2 && controller->GetPreviousTouchNum() == 1) {
+            controller->SetSecondaryTouchCount(controller->GetSecondaryTouchCount() + 1);
+        } else if (pData->touchData.touchNum == 0 && controller->GetPreviousTouchNum() > 0) {
+            if (controller->GetTouchCount() < controller->GetSecondaryTouchCount()) {
+                controller->SetTouchCount(controller->GetSecondaryTouchCount());
+            } else {
+                if (controller->WasSecondaryTouchReset()) {
+                    controller->SetTouchCount(controller->GetSecondaryTouchCount());
+                    controller->UnsetSecondaryTouchResetBool();
+                }
+            }
+        }
+
+        controller->SetPreviousTouchNum(pData->touchData.touchNum);
+
+        if (pData->touchData.touchNum == 1) {
+            state.touchpad[0].ID = controller->GetTouchCount();
+            state.touchpad[1].ID = 0;
+        } else if (pData->touchData.touchNum == 2) {
+            state.touchpad[0].ID = controller->GetTouchCount();
+            state.touchpad[1].ID = controller->GetSecondaryTouchCount();
+        }
+    } else {
+        state.touchpad[0].ID = 1;
+        state.touchpad[1].ID = 2;
+    }
+
     pData->touchData.touch[0].x = state.touchpad[0].x;
     pData->touchData.touch[0].y = state.touchpad[0].y;
-    pData->touchData.touch[0].id = 1;
+    pData->touchData.touch[0].id = state.touchpad[0].ID;
     pData->touchData.touch[1].x = state.touchpad[1].x;
     pData->touchData.touch[1].y = state.touchpad[1].y;
-    pData->touchData.touch[1].id = 2;
+    pData->touchData.touch[1].id = state.touchpad[1].ID;
     pData->timestamp = state.time;
     pData->connected = true;   // isConnected; //TODO fix me proper
     pData->connectedCount = 1; // connectedCount;

src/input/controller.cpp

@@ -1,4 +1,4 @@
-// SPDX-FileCopyrightText: Copyright 2024 shadPS4 Emulator Project
+// SPDX-FileCopyrightText: Copyright 2024 shadPS4 Emulator Project
 // SPDX-License-Identifier: GPL-2.0-or-later
 
 #include <SDL3/SDL.h>
@@ -165,69 +165,37 @@ void GameController::Acceleration(int id, const float acceleration[3]) {
     AddState(state);
 }
 
-// Stolen from
-// https://github.com/xioTechnologies/Open-Source-AHRS-With-x-IMU/blob/master/x-IMU%20IMU%20and%20AHRS%20Algorithms/x-IMU%20IMU%20and%20AHRS%20Algorithms/AHRS/MahonyAHRS.cs
-float eInt[3] = {0.0f, 0.0f, 0.0f}; // Integral error terms
-const float Kp = 50.0f;             // Proportional gain
-const float Ki = 1.0f;              // Integral gain
-Libraries::Pad::OrbisFQuaternion o = {1, 0, 0, 0};
 void GameController::CalculateOrientation(Libraries::Pad::OrbisFVector3& acceleration,
                                           Libraries::Pad::OrbisFVector3& angularVelocity,
                                           float deltaTime,
+                                          Libraries::Pad::OrbisFQuaternion& lastOrientation,
                                           Libraries::Pad::OrbisFQuaternion& orientation) {
-    float ax = acceleration.x, ay = acceleration.y, az = acceleration.z;
-    float gx = angularVelocity.x, gy = angularVelocity.y, gz = angularVelocity.z;
-
-    float q1 = o.w, q2 = o.x, q3 = o.y, q4 = o.z;
-
-    // Normalize accelerometer measurement
-    float norm = std::sqrt(ax * ax + ay * ay + az * az);
-    if (norm == 0.0f || deltaTime == 0.0f)
-        return; // Handle NaN
-    norm = 1.0f / norm;
-    ax *= norm;
-    ay *= norm;
-    az *= norm;
-
-    // Estimated direction of gravity
-    float vx = 2.0f * (q2 * q4 - q1 * q3);
-    float vy = 2.0f * (q1 * q2 + q3 * q4);
-    float vz = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4;
-
-    // Error is cross product between estimated direction and measured direction of gravity
-    float ex = (ay * vz - az * vy);
-    float ey = (az * vx - ax * vz);
-    float ez = (ax * vy - ay * vx);
-    if (Ki > 0.0f) {
-        eInt[0] += ex * deltaTime; // Accumulate integral error
-        eInt[1] += ey * deltaTime;
-        eInt[2] += ez * deltaTime;
-    } else {
-        eInt[0] = eInt[1] = eInt[2] = 0.0f; // Prevent integral wind-up
-    }
-
-    // Apply feedback terms
-    gx += Kp * ex + Ki * eInt[0];
-    gy += Kp * ey + Ki * eInt[1];
-    gz += Kp * ez + Ki * eInt[2];
-
-    //// Integrate rate of change of quaternion
-    q1 += (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltaTime);
-    q2 += (q1 * gx + q3 * gz - q4 * gy) * (0.5f * deltaTime);
-    q3 += (q1 * gy - q2 * gz + q4 * gx) * (0.5f * deltaTime);
-    q4 += (q1 * gz + q2 * gy - q3 * gx) * (0.5f * deltaTime);
-
-    // Normalize quaternion
-    norm = std::sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
-    norm = 1.0f / norm;
-    orientation.w = q1 * norm;
-    orientation.x = q2 * norm;
-    orientation.y = q3 * norm;
-    orientation.z = q4 * norm;
-    o.w = q1 * norm;
-    o.x = q2 * norm;
-    o.y = q3 * norm;
-    o.z = q4 * norm;
+    Libraries::Pad::OrbisFQuaternion q = lastOrientation;
+    Libraries::Pad::OrbisFQuaternion ω = {angularVelocity.x, angularVelocity.y, angularVelocity.z,
+                                          0.0f};
+
+    Libraries::Pad::OrbisFQuaternion qω = {q.w * ω.x + q.x * ω.w + q.y * ω.z - q.z * ω.y,
+                                           q.w * ω.y + q.y * ω.w + q.z * ω.x - q.x * ω.z,
+                                           q.w * ω.z + q.z * ω.w + q.x * ω.y - q.y * ω.x,
+                                           q.w * ω.w - q.x * ω.x - q.y * ω.y - q.z * ω.z};
+
+    Libraries::Pad::OrbisFQuaternion qDot = {0.5f * qω.x, 0.5f * qω.y, 0.5f * qω.z, 0.5f * qω.w};
+
+    q.x += qDot.x * deltaTime;
+    q.y += qDot.y * deltaTime;
+    q.z += qDot.z * deltaTime;
+    q.w += qDot.w * deltaTime;
+
+    float norm = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
+    q.x /= norm;
+    q.y /= norm;
+    q.z /= norm;
+    q.w /= norm;
+
+    orientation.x = q.x;
+    orientation.y = q.y;
+    orientation.z = q.z;
+    orientation.w = q.w;
     LOG_DEBUG(Lib_Pad, "Calculated orientation: {:.2f} {:.2f} {:.2f} {:.2f}", orientation.x,
               orientation.y, orientation.z, orientation.w);
 }
@@ -260,6 +228,69 @@ void GameController::SetTouchpadState(int touchIndex, bool touchDown, float x, f
     }
 }
 
+u8 GameController::GetTouchCount() {
+    std::scoped_lock lock{m_mutex};
+    return m_touch_count;
+}
+
+void GameController::SetTouchCount(u8 touchCount) {
+    std::scoped_lock lock{m_mutex};
+    m_touch_count = touchCount;
+}
+
+u8 GameController::GetSecondaryTouchCount() {
+    std::scoped_lock lock{m_mutex};
+    return m_secondary_touch_count;
+}
+
+void GameController::SetSecondaryTouchCount(u8 touchCount) {
+    std::scoped_lock lock{m_mutex};
+    m_secondary_touch_count = touchCount;
+    if (touchCount == 0) {
+        m_was_secondary_reset = true;
+    }
+}
+
+u8 GameController::GetPreviousTouchNum() {
+    std::scoped_lock lock{m_mutex};
+    return m_previous_touchnum;
+}
+
+void GameController::SetPreviousTouchNum(u8 touchNum) {
+    std::scoped_lock lock{m_mutex};
+    m_previous_touchnum = touchNum;
+}
+
+bool GameController::WasSecondaryTouchReset() {
+    std::scoped_lock lock{m_mutex};
+    return m_was_secondary_reset;
+}
+
+void GameController::UnsetSecondaryTouchResetBool() {
+    std::scoped_lock lock{m_mutex};
+    m_was_secondary_reset = false;
+}
+
+void GameController::SetLastOrientation(Libraries::Pad::OrbisFQuaternion& orientation) {
+    std::scoped_lock lock{m_mutex};
+    m_orientation = orientation;
+}
+
+Libraries::Pad::OrbisFQuaternion GameController::GetLastOrientation() {
+    std::scoped_lock lock{m_mutex};
+    return m_orientation;
+}
+
+std::chrono::steady_clock::time_point GameController::GetLastUpdate() {
+    std::scoped_lock lock{m_mutex};
+    return m_last_update;
+}
+
+void GameController::SetLastUpdate(std::chrono::steady_clock::time_point lastUpdate) {
+    std::scoped_lock lock{m_mutex};
+    m_last_update = lastUpdate;
+}
+
 void GameController::SetEngine(std::unique_ptr<Engine> engine) {
     std::scoped_lock _{m_mutex};
     m_engine = std::move(engine);

src/input/controller.h

@@ -23,6 +23,7 @@ enum class Axis {
 };
 
 struct TouchpadEntry {
+    u8 ID = 0;
     bool state{};
     u16 x{};
     u16 y{};
@@ -82,9 +83,23 @@ class GameController {
     Engine* GetEngine();
     u32 Poll();
 
+    u8 GetTouchCount();
+    void SetTouchCount(u8 touchCount);
+    u8 GetSecondaryTouchCount();
+    void SetSecondaryTouchCount(u8 touchCount);
+    u8 GetPreviousTouchNum();
+    void SetPreviousTouchNum(u8 touchNum);
+    bool WasSecondaryTouchReset();
+    void UnsetSecondaryTouchResetBool();
+
+    void SetLastOrientation(Libraries::Pad::OrbisFQuaternion& orientation);
+    Libraries::Pad::OrbisFQuaternion GetLastOrientation();
+    std::chrono::steady_clock::time_point GetLastUpdate();
+    void SetLastUpdate(std::chrono::steady_clock::time_point lastUpdate);
     static void CalculateOrientation(Libraries::Pad::OrbisFVector3& acceleration,
                                      Libraries::Pad::OrbisFVector3& angularVelocity,
                                      float deltaTime,
+                                     Libraries::Pad::OrbisFQuaternion& lastOrientation,
                                      Libraries::Pad::OrbisFQuaternion& orientation);
 
 private:
@@ -98,8 +113,15 @@ class GameController {
     int m_connected_count = 0;
     u32 m_states_num = 0;
     u32 m_first_state = 0;
+    u8 m_touch_count = 0;
+    u8 m_secondary_touch_count = 0;
+    u8 m_previous_touch_count = 0;
+    u8 m_previous_touchnum = 0;
+    bool m_was_secondary_reset = false;
     std::array<State, MAX_STATES> m_states;
     std::array<StateInternal, MAX_STATES> m_private;
+    std::chrono::steady_clock::time_point m_last_update = {};
+    Libraries::Pad::OrbisFQuaternion m_orientation = {0.0f, 0.0f, 0.0f, 1.0f};
 
     std::unique_ptr<Engine> m_engine = nullptr;
 };