Add random motion input to SDL

5 files changed, 230 insertions, 5 deletions

diff --git a/src/input_common/motion_input.cpp b/src/input_common/motion_input.cpp
index 22a849866..b99d3497f 100644
--- a/src/input_common/motion_input.cpp
+++ b/src/input_common/motion_input.cpp
@@ -2,6 +2,7 @@
 // Licensed under GPLv2 or any later version
 // Refer to the license.txt file included
 
+#include <random>
 #include "common/math_util.h"
 #include "input_common/motion_input.h"
 
@@ -159,6 +160,37 @@ Common::Vec3f MotionInput::GetRotations() const {
     return rotations;
 }
 
+Input::MotionStatus MotionInput::GetMotion() const {
+    const Common::Vec3f gyroscope = GetGyroscope();
+    const Common::Vec3f accelerometer = GetAcceleration();
+    const Common::Vec3f rotation = GetRotations();
+    const std::array<Common::Vec3f, 3> orientation = GetOrientation();
+    return {accelerometer, gyroscope, rotation, orientation};
+}
+
+Input::MotionStatus MotionInput::GetRandomMotion(int accel_magnitude, int gyro_magnitude) const {
+    std::random_device device;
+    std::mt19937 gen(device());
+    std::uniform_int_distribution<s16> distribution(-1000, 1000);
+    const Common::Vec3f gyroscope = {
+        distribution(gen) * 0.001f,
+        distribution(gen) * 0.001f,
+        distribution(gen) * 0.001f,
+    };
+    const Common::Vec3f accelerometer = {
+        distribution(gen) * 0.001f,
+        distribution(gen) * 0.001f,
+        distribution(gen) * 0.001f,
+    };
+    const Common::Vec3f rotation = {};
+    const std::array<Common::Vec3f, 3> orientation = {
+        Common::Vec3f{1.0f, 0, 0},
+        Common::Vec3f{0, 1.0f, 0},
+        Common::Vec3f{0, 0, 1.0f},
+    };
+    return {accelerometer * accel_magnitude, gyroscope * gyro_magnitude, rotation, orientation};
+}
+
 void MotionInput::ResetOrientation() {
     if (!reset_enabled) {
         return;
diff --git a/src/input_common/motion_input.h b/src/input_common/motion_input.h
index 54b4439d9..12b7d0d3f 100644
--- a/src/input_common/motion_input.h
+++ b/src/input_common/motion_input.h
@@ -7,6 +7,7 @@
 #include "common/common_types.h"
 #include "common/quaternion.h"
 #include "common/vector_math.h"
+#include "core/frontend/input.h"
 
 namespace InputCommon {
 
@@ -37,6 +38,8 @@ public:
     Common::Vec3f GetGyroscope() const;
     Common::Vec3f GetRotations() const;
     Common::Quaternion<f32> GetQuaternion() const;
+    Input::MotionStatus GetMotion() const;
+    Input::MotionStatus GetRandomMotion(int accel_magnitude, int gyro_magnitude) const;
 
     bool IsMoving(f32 sensitivity) const;
     bool IsCalibrated(f32 sensitivity) const;
diff --git a/src/input_common/sdl/sdl_impl.cpp b/src/input_common/sdl/sdl_impl.cpp
index a9e676f4b..0b0095978 100644
--- a/src/input_common/sdl/sdl_impl.cpp
+++ b/src/input_common/sdl/sdl_impl.cpp
@@ -21,6 +21,7 @@
 #include "common/param_package.h"
 #include "common/threadsafe_queue.h"
 #include "core/frontend/input.h"
+#include "input_common/motion_input.h"
 #include "input_common/sdl/sdl_impl.h"
 #include "input_common/settings.h"
 
@@ -95,6 +96,10 @@ public:
         return std::make_tuple(x, y);
     }
 
+    const InputCommon::MotionInput& GetMotion() const {
+        return motion;
+    }
+
     void SetHat(int hat, Uint8 direction) {
         std::lock_guard lock{mutex};
         state.hats.insert_or_assign(hat, direction);
@@ -142,6 +147,9 @@ private:
     std::unique_ptr<SDL_Joystick, decltype(&SDL_JoystickClose)> sdl_joystick;
     std::unique_ptr<SDL_GameController, decltype(&SDL_GameControllerClose)> sdl_controller;
     mutable std::mutex mutex;
+
+    // motion is initalized without PID values as motion input is not aviable for SDL2
+    InputCommon::MotionInput motion{0.0f, 0.0f, 0.0f};
 };
 
 std::shared_ptr<SDLJoystick> SDLState::GetSDLJoystickByGUID(const std::string& guid, int port) {
@@ -386,6 +394,68 @@ private:
     const float range;
 };
 
+class SDLDirectionMotion final : public Input::MotionDevice {
+public:
+    explicit SDLDirectionMotion(std::shared_ptr<SDLJoystick> joystick_, int hat_, Uint8 direction_)
+        : joystick(std::move(joystick_)), hat(hat_), direction(direction_) {}
+
+    Input::MotionStatus GetStatus() const override {
+        if (joystick->GetHatDirection(hat, direction)) {
+            return joystick->GetMotion().GetRandomMotion(2, 6);
+        }
+        return joystick->GetMotion().GetRandomMotion(0, 0);
+    }
+
+private:
+    std::shared_ptr<SDLJoystick> joystick;
+    int hat;
+    Uint8 direction;
+};
+
+class SDLAxisMotion final : public Input::MotionDevice {
+public:
+    explicit SDLAxisMotion(std::shared_ptr<SDLJoystick> joystick_, int axis_, float threshold_,
+                           bool trigger_if_greater_)
+        : joystick(std::move(joystick_)), axis(axis_), threshold(threshold_),
+          trigger_if_greater(trigger_if_greater_) {}
+
+    Input::MotionStatus GetStatus() const override {
+        const float axis_value = joystick->GetAxis(axis, 1.0f);
+        bool trigger = axis_value < threshold;
+        if (trigger_if_greater) {
+            trigger = axis_value > threshold;
+        }
+
+        if (trigger) {
+            return joystick->GetMotion().GetRandomMotion(2, 6);
+        }
+        return joystick->GetMotion().GetRandomMotion(0, 0);
+    }
+
+private:
+    std::shared_ptr<SDLJoystick> joystick;
+    int axis;
+    float threshold;
+    bool trigger_if_greater;
+};
+
+class SDLButtonMotion final : public Input::MotionDevice {
+public:
+    explicit SDLButtonMotion(std::shared_ptr<SDLJoystick> joystick_, int button_)
+        : joystick(std::move(joystick_)), button(button_) {}
+
+    Input::MotionStatus GetStatus() const override {
+        if (joystick->GetButton(button)) {
+            return joystick->GetMotion().GetRandomMotion(2, 6);
+        }
+        return joystick->GetMotion().GetRandomMotion(0, 0);
+    }
+
+private:
+    std::shared_ptr<SDLJoystick> joystick;
+    int button;
+};
+
 /// A button device factory that creates button devices from SDL joystick
 class SDLButtonFactory final : public Input::Factory<Input::ButtonDevice> {
 public:
@@ -492,12 +562,78 @@ private:
     SDLState& state;
 };
 
+/// A motion device factory that creates motion devices from SDL joystick
+class SDLMotionFactory final : public Input::Factory<Input::MotionDevice> {
+public:
+    explicit SDLMotionFactory(SDLState& state_) : state(state_) {}
+    /**
+     * Creates motion device from joystick axes
+     * @param params contains parameters for creating the device:
+     *     - "guid": the guid of the joystick to bind
+     *     - "port": the nth joystick of the same type
+     */
+    std::unique_ptr<Input::MotionDevice> Create(const Common::ParamPackage& params) override {
+        const std::string guid = params.Get("guid", "0");
+        const int port = params.Get("port", 0);
+
+        auto joystick = state.GetSDLJoystickByGUID(guid, port);
+
+        if (params.Has("hat")) {
+            const int hat = params.Get("hat", 0);
+            const std::string direction_name = params.Get("direction", "");
+            Uint8 direction;
+            if (direction_name == "up") {
+                direction = SDL_HAT_UP;
+            } else if (direction_name == "down") {
+                direction = SDL_HAT_DOWN;
+            } else if (direction_name == "left") {
+                direction = SDL_HAT_LEFT;
+            } else if (direction_name == "right") {
+                direction = SDL_HAT_RIGHT;
+            } else {
+                direction = 0;
+            }
+            // This is necessary so accessing GetHat with hat won't crash
+            joystick->SetHat(hat, SDL_HAT_CENTERED);
+            return std::make_unique<SDLDirectionMotion>(joystick, hat, direction);
+        }
+
+        if (params.Has("axis")) {
+            const int axis = params.Get("axis", 0);
+            const float threshold = params.Get("threshold", 0.5f);
+            const std::string direction_name = params.Get("direction", "");
+            bool trigger_if_greater;
+            if (direction_name == "+") {
+                trigger_if_greater = true;
+            } else if (direction_name == "-") {
+                trigger_if_greater = false;
+            } else {
+                trigger_if_greater = true;
+                LOG_ERROR(Input, "Unknown direction {}", direction_name);
+            }
+            // This is necessary so accessing GetAxis with axis won't crash
+            joystick->SetAxis(axis, 0);
+            return std::make_unique<SDLAxisMotion>(joystick, axis, threshold, trigger_if_greater);
+        }
+
+        const int button = params.Get("button", 0);
+        // This is necessary so accessing GetButton with button won't crash
+        joystick->SetButton(button, false);
+        return std::make_unique<SDLButtonMotion>(joystick, button);
+    }
+
+private:
+    SDLState& state;
+};
+
 SDLState::SDLState() {
     using namespace Input;
     analog_factory = std::make_shared<SDLAnalogFactory>(*this);
     button_factory = std::make_shared<SDLButtonFactory>(*this);
+    motion_factory = std::make_shared<SDLMotionFactory>(*this);
     RegisterFactory<AnalogDevice>("sdl", analog_factory);
     RegisterFactory<ButtonDevice>("sdl", button_factory);
+    RegisterFactory<MotionDevice>("sdl", motion_factory);
 
     // If the frontend is going to manage the event loop, then we dont start one here
     start_thread = !SDL_WasInit(SDL_INIT_JOYSTICK);
@@ -533,6 +669,7 @@ SDLState::~SDLState() {
     using namespace Input;
     UnregisterFactory<ButtonDevice>("sdl");
     UnregisterFactory<AnalogDevice>("sdl");
+    UnregisterFactory<MotionDevice>("sdl");
 
     CloseJoysticks();
     SDL_DelEventWatch(&SDLEventWatcher, this);
@@ -644,6 +781,27 @@ Common::ParamPackage SDLEventToButtonParamPackage(SDLState& state, const SDL_Eve
     return {};
 }
 
+Common::ParamPackage SDLEventToMotionParamPackage(SDLState& state, const SDL_Event& event) {
+    switch (event.type) {
+    case SDL_JOYAXISMOTION: {
+        const auto joystick = state.GetSDLJoystickBySDLID(event.jaxis.which);
+        return BuildAnalogParamPackageForButton(joystick->GetPort(), joystick->GetGUID(),
+                                                event.jaxis.axis, event.jaxis.value);
+    }
+    case SDL_JOYBUTTONUP: {
+        const auto joystick = state.GetSDLJoystickBySDLID(event.jbutton.which);
+        return BuildButtonParamPackageForButton(joystick->GetPort(), joystick->GetGUID(),
+                                                event.jbutton.button);
+    }
+    case SDL_JOYHATMOTION: {
+        const auto joystick = state.GetSDLJoystickBySDLID(event.jhat.which);
+        return BuildHatParamPackageForButton(joystick->GetPort(), joystick->GetGUID(),
+                                             event.jhat.hat, event.jhat.value);
+    }
+    }
+    return {};
+}
+
 Common::ParamPackage BuildParamPackageForBinding(int port, const std::string& guid,
                                                  const SDL_GameControllerButtonBind& binding) {
     switch (binding.bindType) {
@@ -809,6 +967,35 @@ public:
     }
 };
 
+class SDLMotionPoller final : public SDLPoller {
+public:
+    explicit SDLMotionPoller(SDLState& state_) : SDLPoller(state_) {}
+
+    Common::ParamPackage GetNextInput() override {
+        SDL_Event event;
+        while (state.event_queue.Pop(event)) {
+            const auto package = FromEvent(event);
+            if (package) {
+                return *package;
+            }
+        }
+        return {};
+    }
+    [[nodiscard]] std::optional<Common::ParamPackage> FromEvent(const SDL_Event& event) const {
+        switch (event.type) {
+        case SDL_JOYAXISMOTION:
+            if (std::abs(event.jaxis.value / 32767.0) < 0.5) {
+                break;
+            }
+            [[fallthrough]];
+        case SDL_JOYBUTTONUP:
+        case SDL_JOYHATMOTION:
+            return {SDLEventToMotionParamPackage(state, event)};
+        }
+        return std::nullopt;
+    }
+};
+
 /**
  * Attempts to match the press to a controller joy axis (left/right stick) and if a match
  * isn't found, checks if the event matches anything from SDLButtonPoller and uses that
@@ -900,6 +1087,9 @@ SDLState::Pollers SDLState::GetPollers(InputCommon::Polling::DeviceType type) {
     case InputCommon::Polling::DeviceType::Button:
         pollers.emplace_back(std::make_unique<Polling::SDLButtonPoller>(*this));
         break;
+    case InputCommon::Polling::DeviceType::Motion:
+        pollers.emplace_back(std::make_unique<Polling::SDLMotionPoller>(*this));
+        break;
     }
 
     return pollers;
diff --git a/src/input_common/sdl/sdl_impl.h b/src/input_common/sdl/sdl_impl.h
index bd19ba61d..b9bb4dc56 100644
--- a/src/input_common/sdl/sdl_impl.h
+++ b/src/input_common/sdl/sdl_impl.h
@@ -21,6 +21,7 @@ namespace InputCommon::SDL {
 
 class SDLAnalogFactory;
 class SDLButtonFactory;
+class SDLMotionFactory;
 class SDLJoystick;
 
 class SDLState : public State {
@@ -71,6 +72,7 @@ private:
 
     std::shared_ptr<SDLButtonFactory> button_factory;
     std::shared_ptr<SDLAnalogFactory> analog_factory;
+    std::shared_ptr<SDLMotionFactory> motion_factory;
 
     bool start_thread = false;
     std::atomic<bool> initialized = false;
diff --git a/src/input_common/udp/client.cpp b/src/input_common/udp/client.cpp
index 2b6a68d4b..b6323d56f 100644
--- a/src/input_common/udp/client.cpp
+++ b/src/input_common/udp/client.cpp
@@ -219,14 +219,10 @@ void Client::OnPadData(Response::PadData data) {
     clients[client].motion.SetGyroscope(raw_gyroscope / 312.0f);
     clients[client].motion.UpdateRotation(time_difference);
     clients[client].motion.UpdateOrientation(time_difference);
-    Common::Vec3f gyroscope = clients[client].motion.GetGyroscope();
-    Common::Vec3f accelerometer = clients[client].motion.GetAcceleration();
-    Common::Vec3f rotation = clients[client].motion.GetRotations();
-    std::array<Common::Vec3f, 3> orientation = clients[client].motion.GetOrientation();
 
     {
         std::lock_guard guard(clients[client].status.update_mutex);
-        clients[client].status.motion_status = {accelerometer, gyroscope, rotation, orientation};
+        clients[client].status.motion_status = clients[client].motion.GetMotion();
 
         // TODO: add a setting for "click" touch. Click touch refers to a device that differentiates
         // between a simple "tap" and a hard press that causes the touch screen to click.
@@ -250,6 +246,8 @@ void Client::OnPadData(Response::PadData data) {
         clients[client].status.touch_status = {x, y, is_active};
 
         if (configuring) {
+            const Common::Vec3f gyroscope = clients[client].motion.GetGyroscope();
+            const Common::Vec3f accelerometer = clients[client].motion.GetAcceleration();
             UpdateYuzuSettings(client, accelerometer, gyroscope, is_active);
         }
     }