Frontend: emulate motion sensor

src/citra/emu_window/emu_window_sdl2.cpp

@@ -25,17 +25,23 @@
 
 void EmuWindow_SDL2::OnMouseMotion(s32 x, s32 y) {
     TouchMoved((unsigned)std::max(x, 0), (unsigned)std::max(y, 0));
+    motion_emu->Tilt(x, y);
 }
 
 void EmuWindow_SDL2::OnMouseButton(u32 button, u8 state, s32 x, s32 y) {
-    if (button != SDL_BUTTON_LEFT)
-        return;
-
+    if (button == SDL_BUTTON_LEFT) {
         if (state == SDL_PRESSED) {
             TouchPressed((unsigned)std::max(x, 0), (unsigned)std::max(y, 0));
         } else {
             TouchReleased();
         }
+    } else if (button == SDL_BUTTON_RIGHT) {
+        if (state == SDL_PRESSED) {
+            motion_emu->BeginTilt(x, y);
+        } else {
+            motion_emu->EndTilt();
+        }
+    }
 }
 
 void EmuWindow_SDL2::OnKeyEvent(int key, u8 state) {
@@ -62,6 +68,7 @@ EmuWindow_SDL2::EmuWindow_SDL2() {
     keyboard_id = KeyMap::NewDeviceId();
 
     ReloadSetKeymaps();
+    motion_emu = std::make_unique<Motion::MotionEmu>(*this);
 
     SDL_SetMainReady();
 
@@ -115,6 +122,7 @@ EmuWindow_SDL2::EmuWindow_SDL2() {
 EmuWindow_SDL2::~EmuWindow_SDL2() {
     SDL_GL_DeleteContext(gl_context);
     SDL_Quit();
+    motion_emu = nullptr;
 }
 
 void EmuWindow_SDL2::SwapBuffers() {

src/citra/emu_window/emu_window_sdl2.h

@@ -4,9 +4,11 @@
 
 #pragma once
 
+#include <memory>
 #include <utility>
 
 #include "common/emu_window.h"
+#include "common/motion_emu.h"
 
 struct SDL_Window;
 
@@ -61,4 +63,7 @@ private:
 
     /// Device id of keyboard for use with KeyMap
     int keyboard_id;
+
+    /// Motion sensors emulation
+    std::unique_ptr<Motion::MotionEmu> motion_emu;
 };

src/citra_qt/bootmanager.cpp

@@ -229,6 +229,7 @@ qreal GRenderWindow::windowPixelRatio()
 }
 
 void GRenderWindow::closeEvent(QCloseEvent* event) {
+    motion_emu = nullptr;
     emit Closed();
     QWidget::closeEvent(event);
 }
@@ -245,12 +246,13 @@ void GRenderWindow::keyReleaseEvent(QKeyEvent* event)
 
 void GRenderWindow::mousePressEvent(QMouseEvent *event)
 {
-    if (event->button() == Qt::LeftButton)
-    {
     auto pos = event->pos();
+    if (event->button() == Qt::LeftButton) {
         qreal pixelRatio = windowPixelRatio();
         this->TouchPressed(static_cast<unsigned>(pos.x() * pixelRatio),
                            static_cast<unsigned>(pos.y() * pixelRatio));
+    } else if (event->button() == Qt::RightButton) {
+        motion_emu->BeginTilt(pos.x(), pos.y());
     }
 }
 
@@ -260,12 +262,15 @@ void GRenderWindow::mouseMoveEvent(QMouseEvent *event)
     qreal pixelRatio = windowPixelRatio();
     this->TouchMoved(std::max(static_cast<unsigned>(pos.x() * pixelRatio), 0u),
                      std::max(static_cast<unsigned>(pos.y() * pixelRatio), 0u));
+    motion_emu->Tilt(pos.x(), pos.y());
 }
 
 void GRenderWindow::mouseReleaseEvent(QMouseEvent *event)
 {
     if (event->button() == Qt::LeftButton)
         this->TouchReleased();
+    else if (event->button() == Qt::RightButton)
+        motion_emu->EndTilt();
 }
 
 void GRenderWindow::ReloadSetKeymaps()
@@ -286,11 +291,13 @@ void GRenderWindow::OnMinimalClientAreaChangeRequest(const std::pair<unsigned,un
 }
 
 void GRenderWindow::OnEmulationStarting(EmuThread* emu_thread) {
+    motion_emu = std::make_unique<Motion::MotionEmu>(*this);
     this->emu_thread = emu_thread;
     child->DisablePainting();
 }
 
 void GRenderWindow::OnEmulationStopping() {
+    motion_emu = nullptr;
     emu_thread = nullptr;
     child->EnablePainting();
 }

src/citra_qt/bootmanager.h

@@ -10,6 +10,7 @@
 #include <QThread>
 
 #include "common/emu_window.h"
+#include "common/motion_emu.h"
 #include "common/thread.h"
 
 class QKeyEvent;
@@ -149,6 +150,9 @@ private:
 
     EmuThread* emu_thread;
 
+    /// Motion sensors emulation
+    std::unique_ptr<Motion::MotionEmu> motion_emu;
+
 protected:
     void showEvent(QShowEvent* event) override;
 };

src/common/CMakeLists.txt

@@ -13,6 +13,7 @@ set(SRCS
             memory_util.cpp
             microprofile.cpp
             misc.cpp
+            motion_emu.cpp
             profiler.cpp
             scm_rev.cpp
             string_util.cpp
@@ -46,8 +47,10 @@ set(HEADERS
             memory_util.h
             microprofile.h
             microprofileui.h
+            motion_emu.h
             platform.h
             profiler_reporting.h
+            quaternion.h
             scm_rev.h
             scope_exit.h
             string_util.h

src/common/emu_window.cpp

@@ -7,6 +7,7 @@
 
 #include "common/assert.h"
 #include "common/key_map.h"
+#include "common/profiler_reporting.h"
 
 #include "emu_window.h"
 #include "video_core/video_core.h"
@@ -90,6 +91,26 @@ void EmuWindow::TouchMoved(unsigned framebuffer_x, unsigned framebuffer_y) {
     TouchPressed(framebuffer_x, framebuffer_y);
 }
 
+void EmuWindow::AccelerometerChanged(float x, float y, float z) {
+    constexpr float coef = 512;
+
+    // TODO(wwylele): do a time stretch as it in GyroscopeChanged
+    // The time stretch formula should be like
+    // stretched_vector = (raw_vector - gravity) * stretch_ratio + gravity
+    accel_x = x * coef;
+    accel_y = y * coef;
+    accel_z = z * coef;
+}
+
+void EmuWindow::GyroscopeChanged(float x, float y, float z) {
+    constexpr float FULL_FPS = 60;
+    float coef = GetGyroscopeRawToDpsCoefficient();
+    float stretch = FULL_FPS / Common::Profiling::GetTimingResultsAggregator()->GetAggregatedResults().fps;
+    gyro_x = x * coef * stretch;
+    gyro_y = y * coef * stretch;
+    gyro_z = z * coef * stretch;
+}
+
 EmuWindow::FramebufferLayout EmuWindow::FramebufferLayout::DefaultScreenLayout(unsigned width, unsigned height) {
     // When hiding the widget, the function receives a size of 0
     if (width == 0) width = 1;

src/common/emu_window.h

@@ -111,6 +111,27 @@ public:
      */
     void TouchMoved(unsigned framebuffer_x, unsigned framebuffer_y);
 
+    /**
+     * Signal accelerometer state has changed.
+     * @param x X-axis accelerometer value
+     * @param y Y-axis accelerometer value
+     * @param z Z-axis accelerometer value
+     * @note all values are in unit of g (gravitational acceleration).
+     *    e.g. x = 1.0 means 9.8m/s^2 in x direction.
+     * @see GetAccelerometerState for axis explanation.
+     */
+    void AccelerometerChanged(float x, float y, float z);
+
+    /**
+     * Signal gyroscope state has changed.
+     * @param x X-axis accelerometer value
+     * @param y Y-axis accelerometer value
+     * @param z Z-axis accelerometer value
+     * @note all values are in deg/sec.
+     * @see GetGyroscopeState for axis explanation.
+     */
+    void GyroscopeChanged(float x, float y, float z);
+
     /**
      * Gets the current pad state (which buttons are pressed).
      * @note This should be called by the core emu thread to get a state set by the window thread.
@@ -153,12 +174,11 @@ public:
      *   1 unit of return value = 1/512 g (measured by hw test),
      *   where g is the gravitational acceleration (9.8 m/sec2).
      * @note This should be called by the core emu thread to get a state set by the window thread.
-     * @todo Implement accelerometer input in front-end.
+     * @todo Fix this function to be thread-safe.
      * @return std::tuple of (x, y, z)
      */
-    std::tuple<s16, s16, s16> GetAccelerometerState() const {
-        // stubbed
-        return std::make_tuple(0, -512, 0);
+    std::tuple<s16, s16, s16> GetAccelerometerState() {
+        return std::make_tuple(accel_x, accel_y, accel_z);
     }
 
     /**
@@ -172,12 +192,11 @@ public:
      *   1 unit of return value = (1/coef) deg/sec,
      *   where coef is the return value of GetGyroscopeRawToDpsCoefficient().
      * @note This should be called by the core emu thread to get a state set by the window thread.
-     * @todo Implement gyroscope input in front-end.
+     * @todo Fix this function to be thread-safe.
      * @return std::tuple of (x, y, z)
      */
-    std::tuple<s16, s16, s16> GetGyroscopeState() const {
-        // stubbed
-        return std::make_tuple(0, 0, 0);
+    std::tuple<s16, s16, s16> GetGyroscopeState() {
+        return std::make_tuple(gyro_x, gyro_y, gyro_z);
     }
 
     /**
@@ -226,6 +245,12 @@ protected:
         circle_pad_x = 0;
         circle_pad_y = 0;
         touch_pressed = false;
+        accel_x = 0;
+        accel_y = -512;
+        accel_z = 0;
+        gyro_x = 0;
+        gyro_y = 0;
+        gyro_z = 0;
     }
     virtual ~EmuWindow() {}
 
@@ -288,6 +313,14 @@ private:
     s16 circle_pad_x; ///< Circle pad X-position in native 3DS pixel coordinates (-156 - 156)
     s16 circle_pad_y; ///< Circle pad Y-position in native 3DS pixel coordinates (-156 - 156)
 
+    s16 accel_x; ///< Accelerometer X-axis value in native 3DS units
+    s16 accel_y; ///< Accelerometer Y-axis value in native 3DS units
+    s16 accel_z; ///< Accelerometer Z-axis value in native 3DS units
+
+    s16 gyro_x; ///< Gyroscope X-axis value in native 3DS units
+    s16 gyro_y; ///< Gyroscope Y-axis value in native 3DS units
+    s16 gyro_z; ///< Gyroscope Z-axis value in native 3DS units
+
    /**
     * Clip the provided coordinates to be inside the touchscreen area.
     */

src/common/motion_emu.cpp

@@ -0,0 +1,88 @@
+// Copyright 2016 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include "common/emu_window.h"
+#include "common/math_util.h"
+#include "common/motion_emu.h"
+#include "common/quaternion.h"
+
+namespace Motion {
+
+static constexpr int update_millisecond = 100;
+static constexpr auto update_duration =
+    std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::milliseconds(update_millisecond));
+static constexpr float PI = 3.14159265f;
+
+MotionEmu::MotionEmu(EmuWindow& emu_window) :
+    motion_emu_thread(&MotionEmu::MotionEmuThread, this, std::ref(emu_window)) {
+}
+
+MotionEmu::~MotionEmu() {
+    if (motion_emu_thread.joinable()) {
+        shutdown_event.Set();
+        motion_emu_thread.join();
+    }
+}
+
+void MotionEmu::MotionEmuThread(EmuWindow& emu_window) {
+    auto update_time = std::chrono::steady_clock::now();
+    Math::Quaternion<float> q = MakeQuaternion(Math::Vec3<float>(), 0);
+    Math::Quaternion<float> old_q;
+
+    while (!shutdown_event.WaitUntil(update_time)) {
+        update_time += update_duration;
+        old_q = q;
+
+        {
+            std::lock_guard<std::mutex> guard(tilt_mutex);
+
+            // Find the quaternion describing current 3DS tilting
+            q = MakeQuaternion(Math::MakeVec(-tilt_direction.y, 0.0f, tilt_direction.x), tilt_angle);
+        }
+
+        auto inv_q = q.Inverse();
+
+        // Set the gravity vector in world space
+        auto gravity = Math::MakeVec(0.0f, -1.0f, 0.0f);
+
+        // Find the angular rate vector in world space
+        auto angular_rate = ((q - old_q) * inv_q).xyz * 2;
+        angular_rate *= 1000 / update_millisecond / PI * 180;
+
+        // Transform the two vectors from world space to 3DS space
+        gravity = QuaternionRotate(inv_q, gravity);
+        angular_rate = QuaternionRotate(inv_q, angular_rate);
+
+        // Update the sensor state
+        emu_window.AccelerometerChanged(gravity.x, gravity.y, gravity.z);
+        emu_window.GyroscopeChanged(angular_rate.x, angular_rate.y, angular_rate.z);
+    }
+}
+
+void MotionEmu::BeginTilt(int x, int y) {
+    mouse_origin = Math::MakeVec(x, y);
+    is_tilting = true;
+}
+
+void MotionEmu::Tilt(int x, int y) {
+    constexpr float SENSITIVITY = 0.01f;
+    auto mouse_move = Math::MakeVec(x, y) - mouse_origin;
+    if (is_tilting) {
+        std::lock_guard<std::mutex> guard(tilt_mutex);
+        if (mouse_move.x == 0 && mouse_move.y == 0) {
+            tilt_angle = 0;
+        } else {
+            tilt_direction = mouse_move.Cast<float>();
+            tilt_angle = MathUtil::Clamp(tilt_direction.Normalize() * SENSITIVITY, 0.0f, PI * 0.5f);
+        }
+    }
+}
+
+void MotionEmu::EndTilt() {
+    std::lock_guard<std::mutex> guard(tilt_mutex);
+    tilt_angle = 0;
+    is_tilting = false;
+}
+
+} // namespace Motion

src/common/motion_emu.h

@@ -0,0 +1,52 @@
+// Copyright 2016 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+#include "common/thread.h"
+#include "common/vector_math.h"
+
+class EmuWindow;
+
+namespace Motion {
+
+class MotionEmu final {
+public:
+    MotionEmu(EmuWindow& emu_window);
+    ~MotionEmu();
+
+    /**
+     * Signals that a motion sensor tilt has begun.
+     * @param x the x-coordinate of the cursor
+     * @param y the y-coordinate of the cursor
+     */
+    void BeginTilt(int x, int y);
+
+    /**
+     * Signals that a motion sensor tilt is occurring.
+     * @param x the x-coordinate of the cursor
+     * @param y the y-coordinate of the cursor
+     */
+    void Tilt(int x, int y);
+
+    /**
+     * Signals that a motion sensor tilt has ended.
+     */
+    void EndTilt();
+
+private:
+    Math::Vec2<int> mouse_origin;
+
+    std::mutex tilt_mutex;
+    Math::Vec2<float> tilt_direction;
+    float tilt_angle = 0;
+
+    bool is_tilting = false;
+
+    Common::Event shutdown_event;
+    std::thread motion_emu_thread;
+
+    void MotionEmuThread(EmuWindow& emu_window);
+};
+
+} // namespace Motion

src/common/quaternion.h

@@ -0,0 +1,49 @@
+// Copyright 2016 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include "common/vector_math.h"
+
+namespace Math {
+
+template <typename T>
+class Quaternion {
+public:
+    Math::Vec3<T> xyz;
+    T w;
+
+    Quaternion<decltype(-T{})> Inverse() const {
+        return { -xyz, w };
+    }
+
+    Quaternion<decltype(T{} + T{})> operator+ (const Quaternion& other) const {
+        return { xyz + other.xyz, w + other.w };
+    }
+
+    Quaternion<decltype(T{} - T{})> operator- (const Quaternion& other) const {
+        return { xyz - other.xyz, w - other.w };
+    }
+
+    Quaternion<decltype(T{} * T{} - T{} * T{})> operator* (const Quaternion& other) const {
+        return {
+            xyz * other.w + other.xyz * w + Cross(xyz, other.xyz),
+            w * other.w - Dot(xyz, other.xyz)
+        };
+    }
+};
+
+template <typename T>
+auto QuaternionRotate(const Quaternion<T>& q, const Math::Vec3<T>& v) {
+    return v + 2 * Cross(q.xyz, Cross(q.xyz, v) + v * q.w);
+}
+
+inline Quaternion<float> MakeQuaternion(const Math::Vec3<float>& axis, float angle) {
+    return {
+        axis * std::sin(angle / 2),
+        std::cos(angle / 2)
+    };
+}
+
+} // namspace Math

src/common/thread.h

@@ -6,6 +6,7 @@
 
 #include <cstddef>
 #include <thread>
+#include <chrono>
 #include <condition_variable>
 #include <mutex>
 
@@ -55,6 +56,15 @@ public:
         is_set = false;
     }
 
+    template <class Clock, class Duration>
+    bool WaitUntil(const std::chrono::time_point<Clock, Duration>& time) {
+        std::unique_lock<std::mutex> lk(mutex);
+        if (!condvar.wait_until(lk, time, [this]{ return is_set; }))
+            return false;
+        is_set = false;
+        return true;
+    }
+
     void Reset() {
         std::unique_lock<std::mutex> lk(mutex);
         // no other action required, since wait loops on the predicate and any lingering signal will get cleared on the first iteration

src/common/vector_math.h

@@ -173,6 +173,18 @@ Vec2<T> operator * (const V& f, const Vec2<T>& vec)
 
 typedef Vec2<float> Vec2f;
 
+template<>
+inline float Vec2<float>::Length() const {
+    return std::sqrt(x * x + y * y);
+}
+
+template<>
+inline float Vec2<float>::Normalize() {
+    float length = Length();
+    *this /= length;
+    return length;
+}
+
 template<typename T>
 class Vec3
 {
@@ -341,6 +353,12 @@ inline Vec3<float> Vec3<float>::Normalized() const {
     return *this / Length();
 }
 
+template<>
+inline float Vec3<float>::Normalize() {
+    float length = Length();
+    *this /= length;
+    return length;
+}
 
 typedef Vec3<float> Vec3f;