Fix touchpad handling and change gyro calculation (#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

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;
+    Libraries::Pad::OrbisFQuaternion q = lastOrientation;
+    Libraries::Pad::OrbisFQuaternion ω = {angularVelocity.x, angularVelocity.y, angularVelocity.z,
+                                          0.0f};
 
-    float q1 = o.w, q2 = o.x, q3 = o.y, q4 = o.z;
+    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};
 
-    // 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;
+    Libraries::Pad::OrbisFQuaternion qDot = {0.5f * qω.x, 0.5f * qω.y, 0.5f * qω.z, 0.5f * qω.w};
 
-    // 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;
+    q.x += qDot.x * deltaTime;
+    q.y += qDot.y * deltaTime;
+    q.z += qDot.z * deltaTime;
+    q.w += qDot.w * deltaTime;
 
-    // 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
-    }
+    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;
 
-    // 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;
+    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 @@ public:
     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 @@ private:
     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;
 };