From 46e835f2d6531baea061a2723d171a2f5a1abf6a Mon Sep 17 00:00:00 2001
From: Narr the Reg <juangerman-13@hotmail.com>
Date: Fri, 5 May 2023 12:29:26 -0600
Subject: [PATCH 1/2] yuzu: Add motion preview to controller input

---
 src/common/vector_math.h                      | 14 +++
 src/core/hid/emulated_controller.cpp          |  6 +-
 src/core/hid/emulated_controller.h            |  1 +
 src/core/hid/motion_input.cpp                 | 36 ++++++++
 src/core/hid/motion_input.h                   |  2 +
 .../configure_input_player_widget.cpp         | 91 +++++++++++++++++++
 .../configure_input_player_widget.h           |  5 +
 7 files changed, 151 insertions(+), 4 deletions(-)

diff --git a/src/common/vector_math.h b/src/common/vector_math.h
index 0e2095c455..9a7d50abd7 100644
--- a/src/common/vector_math.h
+++ b/src/common/vector_math.h
@@ -259,6 +259,20 @@ public:
         return *this;
     }
 
+    void RotateFromOrigin(float roll, float pitch, float yaw) {
+        float temp = y;
+        y = std::cos(roll) * y - std::sin(roll) * z;
+        z = std::sin(roll) * temp + std::cos(roll) * z;
+
+        temp = x;
+        x = std::cosf(pitch) * x + std::sin(pitch) * z;
+        z = -std::sin(pitch) * temp + std::cos(pitch) * z;
+
+        temp = x;
+        x = std::cos(yaw) * x - std::sin(yaw) * y;
+        y = std::sin(yaw) * temp + std::cos(yaw) * y;
+    }
+
     [[nodiscard]] constexpr T Length2() const {
         return x * x + y * y + z * z;
     }
diff --git a/src/core/hid/emulated_controller.cpp b/src/core/hid/emulated_controller.cpp
index a70f8807cb..a5c2e3db86 100644
--- a/src/core/hid/emulated_controller.cpp
+++ b/src/core/hid/emulated_controller.cpp
@@ -376,6 +376,7 @@ void EmulatedController::ReloadInput() {
         motion.accel = emulated_motion.GetAcceleration();
         motion.gyro = emulated_motion.GetGyroscope();
         motion.rotation = emulated_motion.GetRotations();
+        motion.euler = emulated_motion.GetEulerAngles();
         motion.orientation = emulated_motion.GetOrientation();
         motion.is_at_rest = !emulated_motion.IsMoving(motion_sensitivity);
     }
@@ -976,14 +977,11 @@ void EmulatedController::SetMotion(const Common::Input::CallbackStatus& callback
     emulated.UpdateOrientation(raw_status.delta_timestamp);
     force_update_motion = raw_status.force_update;
 
-    if (is_configuring) {
-        return;
-    }
-
     auto& motion = controller.motion_state[index];
     motion.accel = emulated.GetAcceleration();
     motion.gyro = emulated.GetGyroscope();
     motion.rotation = emulated.GetRotations();
+    motion.euler = emulated.GetEulerAngles();
     motion.orientation = emulated.GetOrientation();
     motion.is_at_rest = !emulated.IsMoving(motion_sensitivity);
 }
diff --git a/src/core/hid/emulated_controller.h b/src/core/hid/emulated_controller.h
index 4296553551..6e01f4e121 100644
--- a/src/core/hid/emulated_controller.h
+++ b/src/core/hid/emulated_controller.h
@@ -106,6 +106,7 @@ struct ControllerMotion {
     Common::Vec3f accel{};
     Common::Vec3f gyro{};
     Common::Vec3f rotation{};
+    Common::Vec3f euler{};
     std::array<Common::Vec3f, 3> orientation{};
     bool is_at_rest{};
 };
diff --git a/src/core/hid/motion_input.cpp b/src/core/hid/motion_input.cpp
index 0dd66c1ccf..b60478dbbc 100644
--- a/src/core/hid/motion_input.cpp
+++ b/src/core/hid/motion_input.cpp
@@ -1,6 +1,8 @@
 // SPDX-FileCopyrightText: Copyright 2020 yuzu Emulator Project
 // SPDX-License-Identifier: GPL-2.0-or-later
 
+#include <cmath>
+
 #include "common/math_util.h"
 #include "core/hid/motion_input.h"
 
@@ -51,6 +53,20 @@ void MotionInput::SetQuaternion(const Common::Quaternion<f32>& quaternion) {
     quat = quaternion;
 }
 
+void MotionInput::SetEulerAngles(const Common::Vec3f& euler_angles) {
+    const float cr = std::cos(euler_angles.x * 0.5f);
+    const float sr = std::sin(euler_angles.x * 0.5f);
+    const float cp = std::cos(euler_angles.y * 0.5f);
+    const float sp = std::sin(euler_angles.y * 0.5f);
+    const float cy = std::cos(euler_angles.z * 0.5f);
+    const float sy = std::sin(euler_angles.z * 0.5f);
+
+    quat.w = cr * cp * cy + sr * sp * sy;
+    quat.xyz.x = sr * cp * cy - cr * sp * sy;
+    quat.xyz.y = cr * sp * cy + sr * cp * sy;
+    quat.xyz.z = cr * cp * sy - sr * sp * cy;
+}
+
 void MotionInput::SetGyroBias(const Common::Vec3f& bias) {
     gyro_bias = bias;
 }
@@ -222,6 +238,26 @@ Common::Vec3f MotionInput::GetRotations() const {
     return rotations;
 }
 
+Common::Vec3f MotionInput::GetEulerAngles() const {
+    // roll (x-axis rotation)
+    const float sinr_cosp = 2 * (quat.w * quat.xyz.x + quat.xyz.y * quat.xyz.z);
+    const float cosr_cosp = 1 - 2 * (quat.xyz.x * quat.xyz.x + quat.xyz.y * quat.xyz.y);
+
+    // pitch (y-axis rotation)
+    const float sinp = std::sqrt(1 + 2 * (quat.w * quat.xyz.y - quat.xyz.x * quat.xyz.z));
+    const float cosp = std::sqrt(1 - 2 * (quat.w * quat.xyz.y - quat.xyz.x * quat.xyz.z));
+
+    // yaw (z-axis rotation)
+    const float siny_cosp = 2 * (quat.w * quat.xyz.z + quat.xyz.x * quat.xyz.y);
+    const float cosy_cosp = 1 - 2 * (quat.xyz.y * quat.xyz.y + quat.xyz.z * quat.xyz.z);
+
+    return {
+        std::atan2(sinr_cosp, cosr_cosp),
+        2 * std::atan2(sinp, cosp) - Common::PI / 2,
+        std::atan2(siny_cosp, cosy_cosp),
+    };
+}
+
 void MotionInput::ResetOrientation() {
     if (!reset_enabled || only_accelerometer) {
         return;
diff --git a/src/core/hid/motion_input.h b/src/core/hid/motion_input.h
index 9f3fc1cf78..482719359d 100644
--- a/src/core/hid/motion_input.h
+++ b/src/core/hid/motion_input.h
@@ -35,6 +35,7 @@ public:
     void SetAcceleration(const Common::Vec3f& acceleration);
     void SetGyroscope(const Common::Vec3f& gyroscope);
     void SetQuaternion(const Common::Quaternion<f32>& quaternion);
+    void SetEulerAngles(const Common::Vec3f& euler_angles);
     void SetGyroBias(const Common::Vec3f& bias);
     void SetGyroThreshold(f32 threshold);
 
@@ -54,6 +55,7 @@ public:
     [[nodiscard]] Common::Vec3f GetGyroBias() const;
     [[nodiscard]] Common::Vec3f GetRotations() const;
     [[nodiscard]] Common::Quaternion<f32> GetQuaternion() const;
+    [[nodiscard]] Common::Vec3f GetEulerAngles() const;
 
     [[nodiscard]] bool IsMoving(f32 sensitivity) const;
     [[nodiscard]] bool IsCalibrated(f32 sensitivity) const;
diff --git a/src/yuzu/configuration/configure_input_player_widget.cpp b/src/yuzu/configuration/configure_input_player_widget.cpp
index c287220fcd..fe1ee2289e 100644
--- a/src/yuzu/configuration/configure_input_player_widget.cpp
+++ b/src/yuzu/configuration/configure_input_player_widget.cpp
@@ -180,6 +180,10 @@ void PlayerControlPreview::ControllerUpdate(Core::HID::ControllerTriggerType typ
         battery_values = controller->GetBatteryValues();
         needs_redraw = true;
         break;
+    case Core::HID::ControllerTriggerType::Motion:
+        motion_values = controller->GetMotions();
+        needs_redraw = true;
+        break;
     default:
         break;
     }
@@ -313,6 +317,15 @@ void PlayerControlPreview::DrawLeftController(QPainter& p, const QPointF center)
         DrawRawJoystick(p, center + QPointF(-140, 90), QPointF(0, 0));
     }
 
+    {
+        // Draw motion cubes
+        using namespace Settings::NativeMotion;
+        p.setPen(colors.outline);
+        p.setBrush(colors.transparent);
+        Draw3dCube(p, center + QPointF(-140, 90),
+                   motion_values[Settings::NativeMotion::MotionLeft].euler, 20.0f);
+    }
+
     using namespace Settings::NativeButton;
 
     // D-pad constants
@@ -435,6 +448,15 @@ void PlayerControlPreview::DrawRightController(QPainter& p, const QPointF center
         DrawRawJoystick(p, QPointF(0, 0), center + QPointF(140, 90));
     }
 
+    {
+        // Draw motion cubes
+        using namespace Settings::NativeMotion;
+        p.setPen(colors.outline);
+        p.setBrush(colors.transparent);
+        Draw3dCube(p, center + QPointF(140, 90),
+                   motion_values[Settings::NativeMotion::MotionRight].euler, 20.0f);
+    }
+
     using namespace Settings::NativeButton;
 
     // Face buttons constants
@@ -555,6 +577,17 @@ void PlayerControlPreview::DrawDualController(QPainter& p, const QPointF center)
         DrawRawJoystick(p, center + QPointF(-180, 90), center + QPointF(180, 90));
     }
 
+    {
+        // Draw motion cubes
+        using namespace Settings::NativeMotion;
+        p.setPen(colors.outline);
+        p.setBrush(colors.transparent);
+        Draw3dCube(p, center + QPointF(-180, -5),
+                   motion_values[Settings::NativeMotion::MotionLeft].euler, 20.0f);
+        Draw3dCube(p, center + QPointF(180, -5),
+                   motion_values[Settings::NativeMotion::MotionRight].euler, 20.0f);
+    }
+
     using namespace Settings::NativeButton;
 
     // Face buttons constants
@@ -647,6 +680,15 @@ void PlayerControlPreview::DrawHandheldController(QPainter& p, const QPointF cen
         DrawRawJoystick(p, center + QPointF(-50, 0), center + QPointF(50, 0));
     }
 
+    {
+        // Draw motion cubes
+        using namespace Settings::NativeMotion;
+        p.setPen(colors.outline);
+        p.setBrush(colors.transparent);
+        Draw3dCube(p, center + QPointF(0, -115),
+                   motion_values[Settings::NativeMotion::MotionLeft].euler, 15.0f);
+    }
+
     using namespace Settings::NativeButton;
 
     // Face buttons constants
@@ -750,6 +792,15 @@ void PlayerControlPreview::DrawProController(QPainter& p, const QPointF center)
         DrawRawJoystick(p, center + QPointF(-50, 105), center + QPointF(50, 105));
     }
 
+    {
+        // Draw motion cubes
+        using namespace Settings::NativeMotion;
+        p.setPen(colors.button);
+        p.setBrush(colors.transparent);
+        Draw3dCube(p, center + QPointF(0, -100),
+                   motion_values[Settings::NativeMotion::MotionLeft].euler, 15.0f);
+    }
+
     using namespace Settings::NativeButton;
 
     // Face buttons constants
@@ -2871,6 +2922,46 @@ void PlayerControlPreview::DrawArrow(QPainter& p, const QPointF center, const Di
     DrawPolygon(p, arrow_symbol);
 }
 
+// Draw motion functions
+void PlayerControlPreview::Draw3dCube(QPainter& p, QPointF center, const Common::Vec3f& euler,
+                                      float size) {
+    std::array<Common::Vec3f, 8> cube{
+        Common::Vec3f{-1, -1, -1},
+        {-1, 1, -1},
+        {1, 1, -1},
+        {1, -1, -1},
+        {-1, -1, 1},
+        {-1, 1, 1},
+        {1, 1, 1},
+        {1, -1, 1},
+    };
+
+    for (Common::Vec3f& point : cube) {
+        point.RotateFromOrigin(euler.x, euler.y, euler.z);
+        point *= size;
+    }
+
+    const std::array<QPointF, 4> front_face{
+        center + QPointF{cube[0].x, cube[0].y},
+        center + QPointF{cube[1].x, cube[1].y},
+        center + QPointF{cube[2].x, cube[2].y},
+        center + QPointF{cube[3].x, cube[3].y},
+    };
+    const std::array<QPointF, 4> back_face{
+        center + QPointF{cube[4].x, cube[4].y},
+        center + QPointF{cube[5].x, cube[5].y},
+        center + QPointF{cube[6].x, cube[6].y},
+        center + QPointF{cube[7].x, cube[7].y},
+    };
+
+    DrawPolygon(p, front_face);
+    DrawPolygon(p, back_face);
+    p.drawLine(center + QPointF{cube[0].x, cube[0].y}, center + QPointF{cube[4].x, cube[4].y});
+    p.drawLine(center + QPointF{cube[1].x, cube[1].y}, center + QPointF{cube[5].x, cube[5].y});
+    p.drawLine(center + QPointF{cube[2].x, cube[2].y}, center + QPointF{cube[6].x, cube[6].y});
+    p.drawLine(center + QPointF{cube[3].x, cube[3].y}, center + QPointF{cube[7].x, cube[7].y});
+}
+
 template <size_t N>
 void PlayerControlPreview::DrawPolygon(QPainter& p, const std::array<QPointF, N>& polygon) {
     p.drawPolygon(polygon.data(), static_cast<int>(polygon.size()));
diff --git a/src/yuzu/configuration/configure_input_player_widget.h b/src/yuzu/configuration/configure_input_player_widget.h
index 267d134de6..a16943c3cb 100644
--- a/src/yuzu/configuration/configure_input_player_widget.h
+++ b/src/yuzu/configuration/configure_input_player_widget.h
@@ -9,6 +9,7 @@
 
 #include "common/input.h"
 #include "common/settings_input.h"
+#include "common/vector_math.h"
 #include "core/hid/emulated_controller.h"
 #include "core/hid/hid_types.h"
 
@@ -193,6 +194,9 @@ private:
     void DrawSymbol(QPainter& p, QPointF center, Symbol symbol, float icon_size);
     void DrawArrow(QPainter& p, QPointF center, Direction direction, float size);
 
+    // Draw motion functions
+    void Draw3dCube(QPainter& p, QPointF center, const Common::Vec3f& euler, float size);
+
     // Draw primitive types
     template <size_t N>
     void DrawPolygon(QPainter& p, const std::array<QPointF, N>& polygon);
@@ -222,4 +226,5 @@ private:
     Core::HID::SticksValues stick_values{};
     Core::HID::TriggerValues trigger_values{};
     Core::HID::BatteryValues battery_values{};
+    Core::HID::MotionState motion_values{};
 };

From f017335fef95d2cecc0fcda185f0e59cc1945101 Mon Sep 17 00:00:00 2001
From: Narr the Reg <juangerman-13@hotmail.com>
Date: Fri, 5 May 2023 17:11:53 -0600
Subject: [PATCH 2/2] input_common: Add property to invert an axis button

---
 src/common/input.h                                |  2 ++
 src/common/vector_math.h                          |  2 +-
 src/core/hid/input_converter.cpp                  |  1 +
 src/input_common/input_engine.cpp                 |  2 ++
 src/input_common/input_poller.cpp                 |  1 +
 src/yuzu/configuration/configure_input_player.cpp | 10 ++++++++--
 6 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/src/common/input.h b/src/common/input.h
index 51b277c1f9..66fb15f0a7 100644
--- a/src/common/input.h
+++ b/src/common/input.h
@@ -111,6 +111,8 @@ struct AnalogProperties {
     float offset{};
     // Invert direction of the sensor data
     bool inverted{};
+    // Invert the state if it's converted to a button
+    bool inverted_button{};
     // Press once to activate, press again to release
     bool toggle{};
 };
diff --git a/src/common/vector_math.h b/src/common/vector_math.h
index 9a7d50abd7..b4885835df 100644
--- a/src/common/vector_math.h
+++ b/src/common/vector_math.h
@@ -265,7 +265,7 @@ public:
         z = std::sin(roll) * temp + std::cos(roll) * z;
 
         temp = x;
-        x = std::cosf(pitch) * x + std::sin(pitch) * z;
+        x = std::cos(pitch) * x + std::sin(pitch) * z;
         z = -std::sin(pitch) * temp + std::cos(pitch) * z;
 
         temp = x;
diff --git a/src/core/hid/input_converter.cpp b/src/core/hid/input_converter.cpp
index 7cee39a538..a38e3bb3f2 100644
--- a/src/core/hid/input_converter.cpp
+++ b/src/core/hid/input_converter.cpp
@@ -54,6 +54,7 @@ Common::Input::ButtonStatus TransformToButton(const Common::Input::CallbackStatu
     case Common::Input::InputType::Analog:
         status.value = TransformToTrigger(callback).pressed.value;
         status.toggle = callback.analog_status.properties.toggle;
+        status.inverted = callback.analog_status.properties.inverted_button;
         break;
     case Common::Input::InputType::Trigger:
         status.value = TransformToTrigger(callback).pressed.value;
diff --git a/src/input_common/input_engine.cpp b/src/input_common/input_engine.cpp
index 91aa96aa73..49f5e7f54b 100644
--- a/src/input_common/input_engine.cpp
+++ b/src/input_common/input_engine.cpp
@@ -58,6 +58,8 @@ void InputEngine::SetHatButton(const PadIdentifier& identifier, int button, u8 v
 }
 
 void InputEngine::SetAxis(const PadIdentifier& identifier, int axis, f32 value) {
+    value /= 2.0f;
+    value -= 0.5f;
     {
         std::scoped_lock lock{mutex};
         ControllerData& controller = controller_list.at(identifier);
diff --git a/src/input_common/input_poller.cpp b/src/input_common/input_poller.cpp
index 8c6a6521ad..5c2c4a463d 100644
--- a/src/input_common/input_poller.cpp
+++ b/src/input_common/input_poller.cpp
@@ -939,6 +939,7 @@ std::unique_ptr<Common::Input::InputDevice> InputFactory::CreateAnalogDevice(
         .threshold = std::clamp(params.Get("threshold", 0.5f), 0.0f, 1.0f),
         .offset = std::clamp(params.Get("offset", 0.0f), -1.0f, 1.0f),
         .inverted = params.Get("invert", "+") == "-",
+        .inverted_button = params.Get("inverted", false) != 0,
         .toggle = params.Get("toggle", false) != 0,
     };
     input_engine->PreSetController(identifier);
diff --git a/src/yuzu/configuration/configure_input_player.cpp b/src/yuzu/configuration/configure_input_player.cpp
index 50b62293eb..54f42e0c9a 100644
--- a/src/yuzu/configuration/configure_input_player.cpp
+++ b/src/yuzu/configuration/configure_input_player.cpp
@@ -206,7 +206,7 @@ QString ConfigureInputPlayer::ButtonToText(const Common::ParamPackage& param) {
         }
         if (param.Has("axis")) {
             const QString axis = QString::fromStdString(param.Get("axis", ""));
-            return QObject::tr("%1%2Axis %3").arg(toggle, invert, axis);
+            return QObject::tr("%1%2%3Axis %4").arg(toggle, inverted, invert, axis);
         }
         if (param.Has("axis_x") && param.Has("axis_y") && param.Has("axis_z")) {
             const QString axis_x = QString::fromStdString(param.Get("axis_x", ""));
@@ -229,7 +229,7 @@ QString ConfigureInputPlayer::ButtonToText(const Common::ParamPackage& param) {
         return QObject::tr("%1%2%3Hat %4").arg(turbo, toggle, inverted, button_name);
     }
     if (param.Has("axis")) {
-        return QObject::tr("%1%2Axis %3").arg(toggle, inverted, button_name);
+        return QObject::tr("%1%2%3Axis %4").arg(toggle, inverted, invert, button_name);
     }
     if (param.Has("motion")) {
         return QObject::tr("%1%2Axis %3").arg(toggle, inverted, button_name);
@@ -410,6 +410,12 @@ ConfigureInputPlayer::ConfigureInputPlayer(QWidget* parent, std::size_t player_i
                             button_map[button_id]->setText(ButtonToText(param));
                             emulated_controller->SetButtonParam(button_id, param);
                         });
+                        context_menu.addAction(tr("Invert button"), [&] {
+                            const bool invert_value = !param.Get("inverted", false);
+                            param.Set("inverted", invert_value);
+                            button_map[button_id]->setText(ButtonToText(param));
+                            emulated_controller->SetButtonParam(button_id, param);
+                        });
                         context_menu.addAction(tr("Set threshold"), [&] {
                             const int button_threshold =
                                 static_cast<int>(param.Get("threshold", 0.5f) * 100.0f);