File vita3k-PR2628.patch of Package vita3k

Overview Repositories Revisions Requests Users Attributes Meta

File vita3k-PR2628.patch of Package vita3k (Revision a56d075736a20fe42d2f13d144f547bb)

Currently displaying revision a56d075736a20fe42d2f13d144f547bb , Show latest

From c407bafdc7188e4a8fe82c758104831f6dbe7c3a Mon Sep 17 00:00:00 2001
From: Narr the Reg <juangerman-13@hotmail.com>
Date: Fri, 17 Dec 2021 22:55:59 -0600
Subject: [PATCH 1/2] motion: create basic structure

---
 vita3k/modules/SceDriverUser/SceMotion.cpp  |  13 +-
 vita3k/motion/CMakeLists.txt                |   2 +
 vita3k/motion/include/motion/functions.h    |  12 +-
 vita3k/motion/include/motion/motion_input.h |  90 ++++++
 vita3k/motion/include/motion/state.h        |   3 +-
 vita3k/motion/src/motion.cpp                |  45 ++-
 vita3k/motion/src/motion_input.cpp          | 293 +++++++++++++++++++
 vita3k/util/include/util/quaternion.h       |  80 +++++
 vita3k/util/include/util/vector_math.h      | 307 ++++++++++++++++++++
 9 files changed, 837 insertions(+), 8 deletions(-)
 create mode 100644 vita3k/motion/include/motion/motion_input.h
 create mode 100644 vita3k/motion/src/motion_input.cpp
 create mode 100644 vita3k/util/include/util/quaternion.h
 create mode 100644 vita3k/util/include/util/vector_math.h

diff --git a/vita3k/modules/SceDriverUser/SceMotion.cpp b/vita3k/modules/SceDriverUser/SceMotion.cpp
index 7e0c2d4039..81ad076b50 100644
--- a/vita3k/modules/SceDriverUser/SceMotion.cpp
+++ b/vita3k/modules/SceDriverUser/SceMotion.cpp
@@ -34,9 +34,10 @@ EXPORT(int, sceMotionGetBasicOrientation, SceFVector3 *basicOrientation) {
         return RET_ERROR(SCE_MOTION_ERROR_NULL_PARAMETER);
 
     STUBBED("set default value");
-    basicOrientation->x = 0;
-    basicOrientation->y = 0;
-    basicOrientation->z = 1;
+    const SceFVector3 current_acceleration = get_acceleration(host.motion);
+    basicOrientation->x = current_acceleration.x;
+    basicOrientation->y = current_acceleration.y;
+    basicOrientation->z = current_acceleration.z;
 
     return 0;
 }
@@ -58,7 +59,7 @@ EXPORT(int, sceMotionGetDeviceLocation, SceMotionDeviceLocation *devLocation) {
 
 EXPORT(SceBool, sceMotionGetGyroBiasCorrection) {
     TRACY_FUNC(sceMotionGetGyroBiasCorrection);
-    return UNIMPLEMENTED();
+    return get_gyro_bias_correction(host.motion);
 }
 
 EXPORT(SceBool, sceMotionGetMagnetometerState) {
@@ -154,7 +155,9 @@ EXPORT(int, sceMotionSetDeadbandExt) {
 
 EXPORT(int, sceMotionSetGyroBiasCorrection, SceBool setValue) {
     TRACY_FUNC(sceMotionSetGyroBiasCorrection, setValue);
-    return UNIMPLEMENTED();
+    set_gyro_bias_correction(host.motion, setValue);
+
+    return 0;
 }
 
 EXPORT(int, sceMotionSetTiltCorrection, SceBool setValue) {
diff --git a/vita3k/motion/CMakeLists.txt b/vita3k/motion/CMakeLists.txt
index f89e6d8ad6..51f037bc1e 100644
--- a/vita3k/motion/CMakeLists.txt
+++ b/vita3k/motion/CMakeLists.txt
@@ -3,8 +3,10 @@ add_library(
 	STATIC
 	include/motion/functions.h
 	include/motion/motion.h
+	include/motion/motion_input.h
 	include/motion/state.h
 	src/motion.cpp
+	src/motion_input.cpp
 )
 
 target_include_directories(motion PUBLIC include)
diff --git a/vita3k/motion/include/motion/functions.h b/vita3k/motion/include/motion/functions.h
index b7575b0c9b..e6c1555480 100644
--- a/vita3k/motion/include/motion/functions.h
+++ b/vita3k/motion/include/motion/functions.h
@@ -17,7 +17,17 @@
 
 #pragma once
 
+#include <util/types.h>
+#include <util/quaternion.h>
 #include <emuenv/state.h>
 #include <motion/state.h>
 
-// Put here definition of function used for get motion
+SceFVector3 get_acceleration(const MotionState &state);
+SceFVector3 get_gyroscope(const MotionState &state);
+SceFVector3 get_gyro_bias(const MotionState &state);
+Util::Quaternion<SceFloat> get_quaternion(const MotionState &state);
+SceBool get_gyro_bias_correction(const MotionState &state);
+
+SceFVector3 set_gyro_bias_correction(MotionState &state, SceBool setValue);
+
+void refresh_motion(MotionState &state);
\ No newline at end of file
diff --git a/vita3k/motion/include/motion/motion_input.h b/vita3k/motion/include/motion/motion_input.h
new file mode 100644
index 0000000000..1d6caa0e0f
--- /dev/null
+++ b/vita3k/motion/include/motion/motion_input.h
@@ -0,0 +1,90 @@
+// Copyright 2020 yuzu Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included
+
+#pragma once
+
+#include <array>
+#include "util/types.h"
+#include "util/quaternion.h"
+#include "util/vector_math.h"
+
+class MotionInput {
+public:
+    explicit MotionInput();
+
+    MotionInput(const MotionInput&) = default;
+    MotionInput& operator=(const MotionInput&) = default;
+
+    MotionInput(MotionInput&&) = default;
+    MotionInput& operator=(MotionInput&&) = default;
+
+    void SetPID(SceFloat new_kp, SceFloat new_ki, SceFloat new_kd);
+    void SetAcceleration(const Util::Vec3f &acceleration);
+    void SetGyroscope(const Util::Vec3f &gyroscope);
+    void SetQuaternion(const Util::Quaternion<SceFloat> &quaternion);
+    void SetGyroBias(const Util::Vec3f &bias);
+    void SetGyroThreshold(SceFloat threshold);
+
+    void EnableGyroBias(bool enable);
+    void EnableReset(bool reset);
+    void ResetRotations();
+
+    void UpdateRotation(SceULong64 elapsed_time);
+    void UpdateOrientation(SceULong64 elapsed_time);
+
+    [[nodiscard]] std::array<Util::Vec3f, 3> GetOrientation() const;
+    [[nodiscard]] Util::Vec3f GetAcceleration() const;
+    [[nodiscard]] Util::Vec3f GetGyroscope() const;
+    [[nodiscard]] Util::Vec3f GetGyroscopeBias() const;
+    [[nodiscard]] Util::Vec3f GetRotations() const;
+    [[nodiscard]] Util::Quaternion<SceFloat> GetQuaternion() const;
+
+    [[nodiscard]] bool IsMoving(SceFloat sensitivity) const;
+    [[nodiscard]] bool IsCalibrated(SceFloat sensitivity) const;
+    SceBool IsGyroBiasEnabled() const;
+
+private:
+    void ResetOrientation();
+    void SetOrientationFromAccelerometer();
+
+    // PID constants
+    SceFloat kp;
+    SceFloat ki;
+    SceFloat kd;
+
+    // PID errors
+    Util::Vec3f real_error;
+    Util::Vec3f integral_error;
+    Util::Vec3f derivative_error;
+
+    // Quaternion containing the device orientation
+    Util::Quaternion<SceFloat> quat{ { 0.0f, 0.0f, -1.0f }, 0.0f };
+
+    // Number of full rotations in each axis
+    Util::Vec3f rotations;
+
+    // Acceleration vector measurement in G force
+    Util::Vec3f accel;
+
+    // Gyroscope vector measurement in radians/s.
+    Util::Vec3f gyro;
+
+    // Vector to be substracted from gyro measurements
+    Util::Vec3f gyro_bias;
+
+    // Minimum gyro amplitude to detect if the device is moving
+    SceFloat gyro_threshold = 0.0f;
+
+    // Number of invalid sequential data
+    SceFloat reset_counter = 0;
+
+    // If the provided data is invalid the device will be autocalibrated
+    bool reset_enabled = true;
+
+    // If the provided gyro uses bias correction
+    bool bias_enabled = true;
+
+    // Use accelerometer values to calculate position
+    bool only_accelerometer = true;
+};
diff --git a/vita3k/motion/include/motion/state.h b/vita3k/motion/include/motion/state.h
index 1bad5d71e5..6321c97331 100644
--- a/vita3k/motion/include/motion/state.h
+++ b/vita3k/motion/include/motion/state.h
@@ -18,9 +18,10 @@
 #pragma once
 
 #include <motion/motion.h>
+#include <motion/motion_input.h>
 
 #include <SDL_gamecontroller.h>
 
 struct MotionState {
-    // Put variable used by app here
+    MotionInput motion_data;
 };
diff --git a/vita3k/motion/src/motion.cpp b/vita3k/motion/src/motion.cpp
index 704911f1b0..972b45ab12 100644
--- a/vita3k/motion/src/motion.cpp
+++ b/vita3k/motion/src/motion.cpp
@@ -19,4 +19,47 @@
 #include <motion/motion.h>
 #include <motion/state.h>
 
-// Put here function used for get motion states
+SceFVector3 get_acceleration(const MotionState &state) {
+    Util::Vec3f accelerometer =  state.motion_data.GetAcceleration();
+    return {
+        accelerometer.x,
+        accelerometer.y,
+        accelerometer.z,
+    };
+}
+
+SceFVector3 get_gyroscope(const MotionState &state) {
+    Util::Vec3f gyroscope = state.motion_data.GetGyroscope();
+    return {
+        gyroscope.x,
+        gyroscope.y,
+        gyroscope.z,
+    };
+}
+
+SceFVector3 get_gyro_bias(const MotionState &state) {
+    Util::Vec3f bias = state.motion_data.GetGyroscopeBias();
+    return {
+        bias.x,
+        bias.y,
+        bias.z,
+    };
+}
+
+Util::Quaternion<SceFloat> get_quaternion(const MotionState &state) {
+    return state.motion_data.GetQuaternion();
+}
+
+SceBool get_gyro_bias_correction(const MotionState &state) {
+    return state.motion_data.IsGyroBiasEnabled();
+}
+
+SceFVector3 set_gyro_bias_correction(MotionState& state, SceBool setValue) {
+    state.motion_data.EnableGyroBias(setValue);
+}
+
+void refresh_motion(MotionState &state) {
+    SceULong64 elapsed_time = 5000;
+    state.motion_data.UpdateRotation(elapsed_time);
+    state.motion_data.UpdateOrientation(elapsed_time);
+}
\ No newline at end of file
diff --git a/vita3k/motion/src/motion_input.cpp b/vita3k/motion/src/motion_input.cpp
new file mode 100644
index 0000000000..969cbbb42e
--- /dev/null
+++ b/vita3k/motion/src/motion_input.cpp
@@ -0,0 +1,293 @@
+// Copyright 2020 yuzu Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included
+
+//#include "common/math_util.h"
+#include "motion/motion_input.h"
+
+MotionInput::MotionInput() {
+    // Initialize PID constants with default values
+    SetPID(0.3f, 0.005f, 0.0f);
+}
+
+void MotionInput::SetPID(SceFloat new_kp, SceFloat new_ki, SceFloat new_kd) {
+    kp = new_kp;
+    ki = new_ki;
+    kd = new_kd;
+}
+
+void MotionInput::SetAcceleration(const Util::Vec3f &acceleration) {
+    accel = acceleration;
+}
+
+void MotionInput::SetGyroscope(const Util::Vec3f &gyroscope) {
+    if (bias_enabled) {
+        gyro = gyroscope - gyro_bias;
+    } else {
+        gyro = gyroscope;
+    }
+
+    // Auto adjust drift to minimize drift
+    if (!IsMoving(0.1f)) {
+        gyro_bias = (gyro_bias * 0.9999f) + (gyroscope * 0.0001f);
+    }
+
+    if (gyro.Length2() < gyro_threshold) {
+        gyro = {};
+    } else {
+        only_accelerometer = false;
+    }
+}
+
+void MotionInput::SetQuaternion(const Util::Quaternion<SceFloat> &quaternion) {
+    quat = quaternion;
+}
+
+void MotionInput::SetGyroBias(const Util::Vec3f &bias) {
+    gyro_bias = bias;
+}
+
+void MotionInput::SetGyroThreshold(SceFloat threshold) {
+    gyro_threshold = threshold;
+}
+
+void MotionInput::EnableGyroBias(bool enable) {
+    bias_enabled = enable;
+}
+
+void MotionInput::EnableReset(bool reset) {
+    reset_enabled = reset;
+}
+
+void MotionInput::ResetRotations() {
+    rotations = {};
+}
+
+bool MotionInput::IsMoving(SceFloat sensitivity) const {
+    return gyro.Length() >= sensitivity || accel.Length() <= 0.9f || accel.Length() >= 1.1f;
+}
+
+bool MotionInput::IsCalibrated(SceFloat sensitivity) const {
+    return real_error.Length() < sensitivity;
+}
+
+SceBool MotionInput::IsGyroBiasEnabled() const {
+    return bias_enabled;
+}
+
+void MotionInput::UpdateRotation(SceULong64 elapsed_time) {
+    const auto sample_period = static_cast<SceFloat>(elapsed_time) / 1000000.0f;
+    if (sample_period > 0.1f) {
+        return;
+    }
+    rotations += gyro * sample_period;
+}
+
+// Based on Madgwick's implementation of Mayhony's AHRS algorithm.
+// 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
+void MotionInput::UpdateOrientation(SceULong64 elapsed_time) {
+    if (!IsCalibrated(0.1f)) {
+        ResetOrientation();
+    }
+    // Short name local variable for readability
+    SceFloat q1 = quat.w;
+    SceFloat q2 = quat.xyz[0];
+    SceFloat q3 = quat.xyz[1];
+    SceFloat q4 = quat.xyz[2];
+    const auto sample_period = static_cast<SceFloat>(elapsed_time) / 1000000.0f;
+
+    // Ignore invalid elapsed time
+    if (sample_period > 0.1f) {
+        return;
+    }
+
+    const auto normal_accel = accel.Normalized();
+    auto rad_gyro = gyro * 3.1415926f * 2;
+    const SceFloat swap = rad_gyro.x;
+    rad_gyro.x = rad_gyro.y;
+    rad_gyro.y = -swap;
+    rad_gyro.z = -rad_gyro.z;
+
+    // Clear gyro values if there is no gyro present
+    if (only_accelerometer) {
+        rad_gyro.x = 0;
+        rad_gyro.y = 0;
+        rad_gyro.z = 0;
+    }
+
+    // Ignore drift correction if acceleration is not reliable
+    if (accel.Length() >= 0.75f && accel.Length() <= 1.25f) {
+        const SceFloat ax = -normal_accel.x;
+        const SceFloat ay = normal_accel.y;
+        const SceFloat az = -normal_accel.z;
+
+        // Estimated direction of gravity
+        const SceFloat vx = 2.0f * (q2 * q4 - q1 * q3);
+        const SceFloat vy = 2.0f * (q1 * q2 + q3 * q4);
+        const SceFloat vz = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4;
+
+        // Error is cross product between estimated direction and measured direction of gravity
+        const Util::Vec3f new_real_error = {
+            az * vx - ax * vz,
+            ay * vz - az * vy,
+            ax * vy - ay * vx,
+        };
+
+        derivative_error = new_real_error - real_error;
+        real_error = new_real_error;
+
+        // Prevent integral windup
+        if (ki != 0.0f && !IsCalibrated(0.05f)) {
+            integral_error += real_error;
+        } else {
+            integral_error = {};
+        }
+
+        // Apply feedback terms
+        if (!only_accelerometer) {
+            rad_gyro += kp * real_error;
+            rad_gyro += ki * integral_error;
+            rad_gyro += kd * derivative_error;
+        } else {
+            // Give more weight to accelerometer values to compensate for the lack of gyro
+            rad_gyro += 35.0f * kp * real_error;
+            rad_gyro += 10.0f * ki * integral_error;
+            rad_gyro += 10.0f * kd * derivative_error;
+
+            // Emulate gyro values for games that need them
+            gyro.x = -rad_gyro.y;
+            gyro.y = rad_gyro.x;
+            gyro.z = -rad_gyro.z;
+            UpdateRotation(elapsed_time);
+        }
+    }
+
+    const SceFloat gx = rad_gyro.y;
+    const SceFloat gy = rad_gyro.x;
+    const SceFloat gz = rad_gyro.z;
+
+    // Integrate rate of change of quaternion
+    const SceFloat pa = q2;
+    const SceFloat pb = q3;
+    const SceFloat pc = q4;
+    q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * sample_period);
+    q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * sample_period);
+    q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * sample_period);
+    q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * sample_period);
+
+    quat.w = q1;
+    quat.xyz[0] = q2;
+    quat.xyz[1] = q3;
+    quat.xyz[2] = q4;
+    quat = quat.Normalized();
+}
+
+std::array<Util::Vec3f, 3> MotionInput::GetOrientation() const {
+    const Util::Quaternion<float> quad{
+        { -quat.xyz[1], -quat.xyz[0], -quat.w },
+        -quat.xyz[2],
+    };
+    const std::array<float, 16> matrix4x4 = quad.ToMatrix();
+
+    return {Util::Vec3f(matrix4x4[0], matrix4x4[1], -matrix4x4[2]),
+            Util::Vec3f(matrix4x4[4], matrix4x4[5], -matrix4x4[6]),
+            Util::Vec3f(-matrix4x4[8], -matrix4x4[9], matrix4x4[10])};
+}
+
+Util::Vec3f MotionInput::GetAcceleration() const {
+    return accel;
+}
+
+Util::Vec3f MotionInput::GetGyroscope() const {
+    return gyro;
+}
+
+Util::Vec3f MotionInput::GetGyroscopeBias() const {
+    return gyro_bias;
+}
+
+Util::Quaternion<SceFloat> MotionInput::GetQuaternion() const {
+    return quat;
+}
+
+Util::Vec3f MotionInput::GetRotations() const {
+    return rotations;
+}
+
+void MotionInput::ResetOrientation() {
+    if (!reset_enabled || only_accelerometer) {
+        return;
+    }
+    if (!IsMoving(0.5f) && accel.z <= -0.9f) {
+        ++reset_counter;
+        if (reset_counter > 900) {
+            quat.w = 0;
+            quat.xyz[0] = 0;
+            quat.xyz[1] = 0;
+            quat.xyz[2] = -1;
+            SetOrientationFromAccelerometer();
+            integral_error = {};
+            reset_counter = 0;
+        }
+    } else {
+        reset_counter = 0;
+    }
+}
+
+void MotionInput::SetOrientationFromAccelerometer() {
+    int iterations = 0;
+    const SceFloat sample_period = 0.015f;
+
+    const auto normal_accel = accel.Normalized();
+
+    while (!IsCalibrated(0.01f) && ++iterations < 100) {
+        // Short name local variable for readability
+        SceFloat q1 = quat.w;
+        SceFloat q2 = quat.xyz[0];
+        SceFloat q3 = quat.xyz[1];
+        SceFloat q4 = quat.xyz[2];
+
+        Util::Vec3f rad_gyro;
+        const SceFloat ax = -normal_accel.x;
+        const SceFloat ay = normal_accel.y;
+        const SceFloat az = -normal_accel.z;
+
+        // Estimated direction of gravity
+        const SceFloat vx = 2.0f * (q2 * q4 - q1 * q3);
+        const SceFloat vy = 2.0f * (q1 * q2 + q3 * q4);
+        const SceFloat vz = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4;
+
+        // Error is cross product between estimated direction and measured direction of gravity
+        const Util::Vec3f new_real_error = {
+            az * vx - ax * vz,
+            ay * vz - az * vy,
+            ax * vy - ay * vx,
+        };
+
+        derivative_error = new_real_error - real_error;
+        real_error = new_real_error;
+
+        rad_gyro += 10.0f * kp * real_error;
+        rad_gyro += 5.0f * ki * integral_error;
+        rad_gyro += 10.0f * kd * derivative_error;
+
+        const SceFloat gx = rad_gyro.y;
+        const SceFloat gy = rad_gyro.x;
+        const SceFloat gz = rad_gyro.z;
+
+        // Integrate rate of change of quaternion
+        const SceFloat pa = q2;
+        const SceFloat pb = q3;
+        const SceFloat pc = q4;
+        q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * sample_period);
+        q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * sample_period);
+        q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * sample_period);
+        q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * sample_period);
+
+        quat.w = q1;
+        quat.xyz[0] = q2;
+        quat.xyz[1] = q3;
+        quat.xyz[2] = q4;
+        quat = quat.Normalized();
+    }
+}
diff --git a/vita3k/util/include/util/quaternion.h b/vita3k/util/include/util/quaternion.h
new file mode 100644
index 0000000000..89f04b0e4a
--- /dev/null
+++ b/vita3k/util/include/util/quaternion.h
@@ -0,0 +1,80 @@
+// Copyright 2016 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include "util/vector_math.h"
+
+namespace Util {
+
+template <typename T>
+class Quaternion {
+public:
+    Vec3<T> xyz;
+    T w{};
+
+    [[nodiscard]] Quaternion<decltype(-T{})> Inverse() const {
+        return {-xyz, w};
+    }
+
+    [[nodiscard]] Quaternion<decltype(T{} + T{})> operator+(const Quaternion& other) const {
+        return {xyz + other.xyz, w + other.w};
+    }
+
+    [[nodiscard]] Quaternion<decltype(T{} - T{})> operator-(const Quaternion& other) const {
+        return {xyz - other.xyz, w - other.w};
+    }
+
+    [[nodiscard]] 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)};
+    }
+
+    [[nodiscard]] Quaternion<T> Normalized() const {
+        T length = std::sqrt(xyz.Length2() + w * w);
+        return {xyz / length, w / length};
+    }
+
+    [[nodiscard]] std::array<decltype(-T{}), 16> ToMatrix() const {
+        const T x2 = xyz[0] * xyz[0];
+        const T y2 = xyz[1] * xyz[1];
+        const T z2 = xyz[2] * xyz[2];
+
+        const T xy = xyz[0] * xyz[1];
+        const T wz = w * xyz[2];
+        const T xz = xyz[0] * xyz[2];
+        const T wy = w * xyz[1];
+        const T yz = xyz[1] * xyz[2];
+        const T wx = w * xyz[0];
+
+        return {1.0f - 2.0f * (y2 + z2),
+                2.0f * (xy + wz),
+                2.0f * (xz - wy),
+                0.0f,
+                2.0f * (xy - wz),
+                1.0f - 2.0f * (x2 + z2),
+                2.0f * (yz + wx),
+                0.0f,
+                2.0f * (xz + wy),
+                2.0f * (yz - wx),
+                1.0f - 2.0f * (x2 + y2),
+                0.0f,
+                0.0f,
+                0.0f,
+                0.0f,
+                1.0f};
+    }
+};
+
+template <typename T>
+[[nodiscard]] auto QuaternionRotate(const Quaternion<T>& q, const Vec3<T>& v) {
+    return v + 2 * Cross(q.xyz, Cross(q.xyz, v) + v * q.w);
+}
+
+[[nodiscard]] inline Quaternion<float> MakeQuaternion(const Vec3<float>& axis, float angle) {
+    return {axis * std::sin(angle / 2), std::cos(angle / 2)};
+}
+
+} // namespace Common
diff --git a/vita3k/util/include/util/vector_math.h b/vita3k/util/include/util/vector_math.h
new file mode 100644
index 0000000000..0ad7a40f40
--- /dev/null
+++ b/vita3k/util/include/util/vector_math.h
@@ -0,0 +1,307 @@
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+// Copyright 2014 Tony Wasserka
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the owner nor the names of its contributors may
+//       be used to endorse or promote products derived from this software
+//       without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <cmath>
+#include <type_traits>
+#include "util/types.h"
+
+namespace Util {
+
+template <typename T>
+class Vec3;
+
+template <typename T>
+class Vec3 {
+public:
+    T x{};
+    T y{};
+    T z{};
+
+    constexpr Vec3() = default;
+    constexpr Vec3(const T& x_, const T& y_, const T& z_) : x(x_), y(y_), z(z_) {}
+
+    template <typename T2>
+    [[nodiscard]] constexpr Vec3<T2> Cast() const {
+        return Vec3<T2>(static_cast<T2>(x), static_cast<T2>(y), static_cast<T2>(z));
+    }
+
+    [[nodiscard]] static constexpr Vec3 AssignToAll(const T& f) {
+        return Vec3(f, f, f);
+    }
+
+    [[nodiscard]] constexpr Vec3<decltype(T{} + T{})> operator+(const Vec3& other) const {
+        return {x + other.x, y + other.y, z + other.z};
+    }
+
+    constexpr Vec3& operator+=(const Vec3& other) {
+        x += other.x;
+        y += other.y;
+        z += other.z;
+        return *this;
+    }
+
+    [[nodiscard]] constexpr Vec3<decltype(T{} - T{})> operator-(const Vec3& other) const {
+        return {x - other.x, y - other.y, z - other.z};
+    }
+
+    constexpr Vec3& operator-=(const Vec3& other) {
+        x -= other.x;
+        y -= other.y;
+        z -= other.z;
+        return *this;
+    }
+
+    template <typename U = T>
+    [[nodiscard]] constexpr Vec3<std::enable_if_t<std::is_signed_v<U>, U>> operator-() const {
+        return {-x, -y, -z};
+    }
+
+    [[nodiscard]] constexpr Vec3<decltype(T{} * T{})> operator*(const Vec3& other) const {
+        return {x * other.x, y * other.y, z * other.z};
+    }
+
+    template <typename V>
+    [[nodiscard]] constexpr Vec3<decltype(T{} * V{})> operator*(const V& f) const {
+        using TV = decltype(T{} * V{});
+        using C = std::common_type_t<T, V>;
+
+        return {
+            static_cast<TV>(static_cast<C>(x) * static_cast<C>(f)),
+            static_cast<TV>(static_cast<C>(y) * static_cast<C>(f)),
+            static_cast<TV>(static_cast<C>(z) * static_cast<C>(f)),
+        };
+    }
+
+    template <typename V>
+    constexpr Vec3& operator*=(const V& f) {
+        *this = *this * f;
+        return *this;
+    }
+    template <typename V>
+    [[nodiscard]] constexpr Vec3<decltype(T{} / V{})> operator/(const V& f) const {
+        using TV = decltype(T{} / V{});
+        using C = std::common_type_t<T, V>;
+
+        return {
+            static_cast<TV>(static_cast<C>(x) / static_cast<C>(f)),
+            static_cast<TV>(static_cast<C>(y) / static_cast<C>(f)),
+            static_cast<TV>(static_cast<C>(z) / static_cast<C>(f)),
+        };
+    }
+
+    template <typename V>
+    constexpr Vec3& operator/=(const V& f) {
+        *this = *this / f;
+        return *this;
+    }
+
+    [[nodiscard]] constexpr T Length2() const {
+        return x * x + y * y + z * z;
+    }
+
+    // Only implemented for T=float
+    [[nodiscard]] SceFloat Length() const;
+    [[nodiscard]] Vec3 Normalized() const;
+    [[nodiscard]] SceFloat Normalize(); // returns the previous length, which is often useful
+
+    [[nodiscard]] constexpr T& operator[](std::size_t i) {
+        return *((&x) + i);
+    }
+
+    [[nodiscard]] constexpr const T& operator[](std::size_t i) const {
+        return *((&x) + i);
+    }
+
+    constexpr void SetZero() {
+        x = 0;
+        y = 0;
+        z = 0;
+    }
+
+    // Common aliases: UVW (texel coordinates), RGB (colors), STQ (texture coordinates)
+    [[nodiscard]] constexpr T& u() {
+        return x;
+    }
+    [[nodiscard]] constexpr T& v() {
+        return y;
+    }
+    [[nodiscard]] constexpr T& w() {
+        return z;
+    }
+
+    [[nodiscard]] constexpr T& r() {
+        return x;
+    }
+    [[nodiscard]] constexpr T& g() {
+        return y;
+    }
+    [[nodiscard]] constexpr T& b() {
+        return z;
+    }
+
+    [[nodiscard]] constexpr T& s() {
+        return x;
+    }
+    [[nodiscard]] constexpr T& t() {
+        return y;
+    }
+    [[nodiscard]] constexpr T& q() {
+        return z;
+    }
+
+    [[nodiscard]] constexpr const T& u() const {
+        return x;
+    }
+    [[nodiscard]] constexpr const T& v() const {
+        return y;
+    }
+    [[nodiscard]] constexpr const T& w() const {
+        return z;
+    }
+
+    [[nodiscard]] constexpr const T& r() const {
+        return x;
+    }
+    [[nodiscard]] constexpr const T& g() const {
+        return y;
+    }
+    [[nodiscard]] constexpr const T& b() const {
+        return z;
+    }
+
+    [[nodiscard]] constexpr const T& s() const {
+        return x;
+    }
+    [[nodiscard]] constexpr const T& t() const {
+        return y;
+    }
+    [[nodiscard]] constexpr const T& q() const {
+        return z;
+    }
+
+// swizzlers - create a subvector of specific components
+// e.g. Vec2 uv() { return Vec2(x,y); }
+// _DEFINE_SWIZZLER2 defines a single such function, DEFINE_SWIZZLER2 defines all of them for all
+// component names (x<->r) and permutations (xy<->yx)
+#define _DEFINE_SWIZZLER2(a, b, name)                                                              \
+    [[nodiscard]] constexpr Vec2<T> name() const {                                                 \
+        return Vec2<T>(a, b);                                                                      \
+    }
+#define DEFINE_SWIZZLER2(a, b, a2, b2, a3, b3, a4, b4)                                             \
+    _DEFINE_SWIZZLER2(a, b, a##b);                                                                 \
+    _DEFINE_SWIZZLER2(a, b, a2##b2);                                                               \
+    _DEFINE_SWIZZLER2(a, b, a3##b3);                                                               \
+    _DEFINE_SWIZZLER2(a, b, a4##b4);                                                               \
+    _DEFINE_SWIZZLER2(b, a, b##a);                                                                 \
+    _DEFINE_SWIZZLER2(b, a, b2##a2);                                                               \
+    _DEFINE_SWIZZLER2(b, a, b3##a3);                                                               \
+    _DEFINE_SWIZZLER2(b, a, b4##a4)
+
+    DEFINE_SWIZZLER2(x, y, r, g, u, v, s, t);
+    DEFINE_SWIZZLER2(x, z, r, b, u, w, s, q);
+    DEFINE_SWIZZLER2(y, z, g, b, v, w, t, q);
+#undef DEFINE_SWIZZLER2
+#undef _DEFINE_SWIZZLER2
+};
+
+template <typename T, typename V>
+[[nodiscard]] constexpr Vec3<T> operator*(const V& f, const Vec3<T>& vec) {
+    using C = std::common_type_t<T, V>;
+
+    return Vec3<T>(static_cast<T>(static_cast<C>(f) * static_cast<C>(vec.x)),
+                   static_cast<T>(static_cast<C>(f) * static_cast<C>(vec.y)),
+                   static_cast<T>(static_cast<C>(f) * static_cast<C>(vec.z)));
+}
+
+template <>
+inline SceFloat Vec3<SceFloat>::Length() const {
+    return std::sqrt(x * x + y * y + z * z);
+}
+
+template <>
+inline Vec3<SceFloat> Vec3<SceFloat>::Normalized() const {
+    return *this / Length();
+}
+
+template <>
+inline SceFloat Vec3<SceFloat>::Normalize() {
+    SceFloat length = Length();
+    *this /= length;
+    return length;
+}
+
+using Vec3f = Vec3<SceFloat>;
+
+template <typename T>
+[[nodiscard]] constexpr decltype(T{} * T{} + T{} * T{}) Dot(const Vec3<T>& a, const Vec3<T>& b) {
+    return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+
+template <typename T>
+[[nodiscard]] constexpr Vec3<decltype(T{} * T{} - T{} * T{})> Cross(const Vec3<T>& a,
+                                                                    const Vec3<T>& b) {
+    return {a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x};
+}
+
+// linear interpolation via SceFloat: 0.0=begin, 1.0=end
+template <typename X>
+[[nodiscard]] constexpr decltype(X{} * SceFloat{} + X{} * SceFloat{})
+    Lerp(const X &begin, const X &end, const SceFloat t) {
+    return begin * (1.f - t) + end * t;
+}
+
+// linear interpolation via int: 0=begin, base=end
+template <typename X, int base>
+[[nodiscard]] constexpr decltype((X{} * int{} + X{} * int{}) / base) LerpInt(const X& begin,
+                                                                             const X& end,
+                                                                             const int t) {
+    return (begin * (base - t) + end * t) / base;
+}
+
+// bilinear interpolation. s is for interpolating x00-x01 and x10-x11, and t is for the second
+// interpolation.
+template <typename X>
+[[nodiscard]] constexpr auto BilinearInterp(const X& x00, const X& x01, const X& x10, const X& x11,
+    const SceFloat s, const SceFloat t) {
+    auto y0 = Lerp(x00, x01, s);
+    auto y1 = Lerp(x10, x11, s);
+    return Lerp(y0, y1, t);
+}
+
+// Utility vector factories
+
+template <typename T>
+[[nodiscard]] constexpr Vec3<T> MakeVec(const T& x, const T& y, const T& z) {
+    return Vec3<T>{x, y, z};
+}
+
+} // namespace Common

From 9e3cada2396a01c9aeef68efe368e65f088c82fd Mon Sep 17 00:00:00 2001
From: Macdu <Macdu@users.noreply.github.com>
Date: Sun, 4 Jun 2023 23:59:17 +0200
Subject: [PATCH 2/2] motion: Add motion support

---
 vita3k/ctrl/include/ctrl/state.h            |   5 +
 vita3k/ctrl/src/ctrl.cpp                    |  20 ++++
 vita3k/display/CMakeLists.txt               |   2 +-
 vita3k/display/src/display.cpp              |   2 +
 vita3k/gui/src/controllers_dialog.cpp       |   5 +
 vita3k/interface.cpp                        |   2 +
 vita3k/modules/SceDriverUser/SceMotion.cpp  | 104 +++++++++++++++++---
 vita3k/motion/CMakeLists.txt                |   5 +-
 vita3k/motion/include/motion/functions.h    |  10 +-
 vita3k/motion/include/motion/motion_input.h |  19 ++--
 vita3k/motion/include/motion/state.h        |   8 ++
 vita3k/motion/src/motion.cpp                |  93 +++++++++++++----
 vita3k/motion/src/motion_input.cpp          |  28 +-----
 vita3k/util/include/util/quaternion.h       |   2 +
 vita3k/util/include/util/vector_math.h      |  24 -----
 15 files changed, 226 insertions(+), 103 deletions(-)

diff --git a/vita3k/ctrl/include/ctrl/state.h b/vita3k/ctrl/include/ctrl/state.h
index ec3124d6d8..e7eaea24b6 100644
--- a/vita3k/ctrl/include/ctrl/state.h
+++ b/vita3k/ctrl/include/ctrl/state.h
@@ -25,6 +25,7 @@
 
 #include <map>
 #include <memory>
+#include <mutex>
 
 struct _SDL_GameController;
 
@@ -34,6 +35,8 @@ typedef std::shared_ptr<_SDL_Haptic> HapticPtr;
 struct Controller {
     GameControllerPtr controller;
     int port;
+    bool has_accel;
+    bool has_gyro;
 };
 
 struct ControllerBinding {
@@ -44,8 +47,10 @@ struct ControllerBinding {
 typedef std::map<SDL_JoystickGUID, Controller> ControllerList;
 
 struct CtrlState {
+    std::mutex mutex;
     ControllerList controllers;
     int controllers_num = 0;
+    bool has_motion_support = false;
     const char *controllers_name[SCE_CTRL_MAX_WIRELESS_NUM];
     bool free_ports[SCE_CTRL_MAX_WIRELESS_NUM] = { true, true, true, true };
     SceCtrlPadInputMode input_mode = SCE_CTRL_MODE_DIGITAL;
diff --git a/vita3k/ctrl/src/ctrl.cpp b/vita3k/ctrl/src/ctrl.cpp
index 16e876ee95..a72a954981 100644
--- a/vita3k/ctrl/src/ctrl.cpp
+++ b/vita3k/ctrl/src/ctrl.cpp
@@ -86,8 +86,13 @@ static bool operator<(const SDL_JoystickGUID &a, const SDL_JoystickGUID &b) {
 
 void refresh_controllers(CtrlState &state) {
     // Remove disconnected controllers
+    bool found_gyro = false;
+    bool found_accel = false;
     for (ControllerList::iterator controller = state.controllers.begin(); controller != state.controllers.end();) {
         if (SDL_GameControllerGetAttached(controller->second.controller.get())) {
+            found_accel |= controller->second.has_accel;
+            found_gyro |= controller->second.has_gyro;
+
             ++controller;
         } else {
             state.free_ports[controller->second.port - 1] = true;
@@ -109,12 +114,25 @@ void refresh_controllers(CtrlState &state) {
                 const GameControllerPtr controller(SDL_GameControllerOpen(joystick_index), SDL_GameControllerClose);
                 new_controller.controller = controller;
                 new_controller.port = reserve_port(state);
+
+                new_controller.has_gyro = SDL_GameControllerHasSensor(controller.get(), SDL_SENSOR_GYRO);
+                if (new_controller.has_gyro)
+                    SDL_GameControllerSetSensorEnabled(controller.get(), SDL_SENSOR_GYRO, SDL_TRUE);
+                new_controller.has_accel = SDL_GameControllerHasSensor(controller.get(), SDL_SENSOR_ACCEL);
+                if (new_controller.has_accel)
+                    SDL_GameControllerSetSensorEnabled(controller.get(), SDL_SENSOR_ACCEL, SDL_TRUE);
+
+                found_gyro |= new_controller.has_gyro;
+                found_accel |= new_controller.has_accel;
+
                 state.controllers.emplace(guid, new_controller);
                 state.controllers_name[joystick_index] = SDL_GameControllerNameForIndex(joystick_index);
                 state.controllers_num++;
             }
         }
     }
+
+    state.has_motion_support = found_gyro && found_accel;
 }
 
 static float keys_to_axis(const uint8_t *keys, SDL_Scancode code1, SDL_Scancode code2) {
@@ -231,6 +249,8 @@ static void apply_controller(uint32_t *buttons, float axes[4], SDL_GameControlle
 }
 
 static void retrieve_ctrl_data(EmuEnvState &emuenv, int port, bool is_v2, bool negative, bool from_ext_function, SceUInt32 &buttons, SceUInt8 &lx, SceUInt8 &ly, SceUInt8 &rx, SceUInt8 &ry) {
+    std::lock_guard<std::mutex> guard(emuenv.ctrl.mutex);
+
     if (port == 0) {
         port++;
     }
diff --git a/vita3k/display/CMakeLists.txt b/vita3k/display/CMakeLists.txt
index 4b58ded30e..e468fa3832 100644
--- a/vita3k/display/CMakeLists.txt
+++ b/vita3k/display/CMakeLists.txt
@@ -8,4 +8,4 @@ add_library(
 
 target_include_directories(display PUBLIC include)
 target_link_libraries(display PUBLIC emuenv kernel)
-target_link_libraries(display PRIVATE kernel touch renderer dialog)
+target_link_libraries(display PRIVATE kernel touch renderer dialog motion)
diff --git a/vita3k/display/src/display.cpp b/vita3k/display/src/display.cpp
index b0d44318ee..b5b71ff0f9 100644
--- a/vita3k/display/src/display.cpp
+++ b/vita3k/display/src/display.cpp
@@ -24,6 +24,7 @@
 #include <renderer/state.h>
 
 #include <chrono>
+#include <motion/functions.h>
 #include <touch/functions.h>
 #include <util/find.h>
 
@@ -54,6 +55,7 @@ static void vblank_sync_thread(EmuEnvState &emuenv) {
 
             // maybe we should also use a mutex for this part, but it shouldn't be an issue
             touch_vsync_update(emuenv);
+            refresh_motion(emuenv.motion, emuenv.ctrl);
 
             // Notify Vblank callback in each VBLANK start
             for (auto &cb : display.vblank_callbacks)
diff --git a/vita3k/gui/src/controllers_dialog.cpp b/vita3k/gui/src/controllers_dialog.cpp
index 16c78af7c1..682132a10f 100644
--- a/vita3k/gui/src/controllers_dialog.cpp
+++ b/vita3k/gui/src/controllers_dialog.cpp
@@ -53,6 +53,11 @@ void draw_controllers_dialog(GuiState &gui, EmuEnvState &emuenv) {
         }
     } else
         ImGui::TextColored(GUI_COLOR_TEXT_MENUBAR, "%s", lang["not_connected"].c_str());
+
+    if (emuenv.ctrl.has_motion_support) {
+        ImGui::Spacing();
+        ImGui::TextColored(GUI_COLOR_TEXT_TITLE, "Gamepad has motion support");
+    }
     ImGui::End();
 }
 
diff --git a/vita3k/interface.cpp b/vita3k/interface.cpp
index 886f6e4c3b..af4f3bb4bd 100644
--- a/vita3k/interface.cpp
+++ b/vita3k/interface.cpp
@@ -471,6 +471,8 @@ static ExitCode load_app_impl(Ptr<const void> &entry_point, EmuEnvState &emuenv,
         LOG_INFO("{} Controllers Connected", emuenv.ctrl.controllers_num);
         for (auto i = 0; i < emuenv.ctrl.controllers_num; i++)
             LOG_INFO("Controller {}: {}", i, emuenv.ctrl.controllers_name[i]);
+        if (emuenv.ctrl.has_motion_support)
+            LOG_INFO("Controller has motion support");
     }
     LOG_INFO("modules mode: {}", config_modules_mode[emuenv.cfg.current_config.modules_mode][ModulesModeType::MODE]);
     if ((emuenv.cfg.current_config.modules_mode != ModulesMode::AUTOMATIC) && !emuenv.cfg.current_config.lle_modules.empty()) {
diff --git a/vita3k/modules/SceDriverUser/SceMotion.cpp b/vita3k/modules/SceDriverUser/SceMotion.cpp
index 81ad076b50..5ca484a87b 100644
--- a/vita3k/modules/SceDriverUser/SceMotion.cpp
+++ b/vita3k/modules/SceDriverUser/SceMotion.cpp
@@ -17,6 +17,7 @@
 
 #include "SceMotion.h"
 
+#include <ctrl/state.h>
 #include <motion/functions.h>
 #include <motion/motion.h>
 
@@ -33,11 +34,20 @@ EXPORT(int, sceMotionGetBasicOrientation, SceFVector3 *basicOrientation) {
     if (!basicOrientation)
         return RET_ERROR(SCE_MOTION_ERROR_NULL_PARAMETER);
 
-    STUBBED("set default value");
-    const SceFVector3 current_acceleration = get_acceleration(host.motion);
-    basicOrientation->x = current_acceleration.x;
-    basicOrientation->y = current_acceleration.y;
-    basicOrientation->z = current_acceleration.z;
+    std::lock_guard<std::mutex> guard(emuenv.motion.mutex);
+    Util::Quaternion quat = get_orientation(emuenv.motion);
+
+    *basicOrientation = { 0.f, 0.f, 0.f };
+    // get the basic orientation, only one component is not zero and will be 1 or -1
+    // TODO: this is probably wrong
+    float max_val = std::max({ std::abs(quat.xyz.x), std::abs(quat.xyz.y), std::abs(quat.xyz.z) });
+    if (max_val == std::abs(quat.xyz.x)) {
+        basicOrientation->x = quat.xyz.x > 0.0f ? 1.0f : -1.0f;
+    } else if (max_val == std::abs(quat.xyz.y)) {
+        basicOrientation->y = quat.xyz.y > 0.0f ? 1.0f : -1.0f;
+    } else {
+        basicOrientation->z = quat.xyz.z > 0.0f ? 1.0f : -1.0f;
+    }
 
     return 0;
 }
@@ -59,7 +69,7 @@ EXPORT(int, sceMotionGetDeviceLocation, SceMotionDeviceLocation *devLocation) {
 
 EXPORT(SceBool, sceMotionGetGyroBiasCorrection) {
     TRACY_FUNC(sceMotionGetGyroBiasCorrection);
-    return get_gyro_bias_correction(host.motion);
+    return get_gyro_bias_correction(emuenv.motion);
 }
 
 EXPORT(SceBool, sceMotionGetMagnetometerState) {
@@ -69,7 +79,35 @@ EXPORT(SceBool, sceMotionGetMagnetometerState) {
 
 EXPORT(int, sceMotionGetSensorState, SceMotionSensorState *sensorState, int numRecords) {
     TRACY_FUNC(sceMotionGetSensorState, sensorState, numRecords);
-    return UNIMPLEMENTED();
+    if (!sensorState)
+        return RET_ERROR(SCE_MOTION_ERROR_NULL_PARAMETER);
+
+    if (emuenv.ctrl.has_motion_support) {
+        std::lock_guard<std::mutex> guard(emuenv.motion.mutex);
+        sensorState->accelerometer = get_acceleration(emuenv.motion);
+        sensorState->gyro = get_gyroscope(emuenv.motion);
+
+        sensorState->timestamp = emuenv.motion.last_accel_timestamp;
+        sensorState->counter = emuenv.motion.last_counter;
+        sensorState->hostTimestamp = sensorState->timestamp;
+        sensorState->dataInfo = 0;
+    } else {
+        // some default values
+        memset(sensorState, 0, sizeof(sensorState));
+        sensorState->accelerometer.z = -1.0;
+
+        std::chrono::time_point<std::chrono::steady_clock> ts = std::chrono::steady_clock::now();
+        uint64_t timestamp = std::chrono::duration_cast<std::chrono::microseconds>(ts.time_since_epoch()).count();
+        sensorState->timestamp = timestamp;
+        sensorState->hostTimestamp = timestamp;
+
+        sensorState->counter = emuenv.motion.last_counter++;
+    }
+
+    for (int i = 1; i < numRecords; i++)
+        sensorState[i] = sensorState[0];
+
+    return 0;
 }
 
 EXPORT(int, sceMotionGetState, SceMotionState *motionState) {
@@ -77,15 +115,45 @@ EXPORT(int, sceMotionGetState, SceMotionState *motionState) {
     if (!motionState)
         return RET_ERROR(SCE_MOTION_ERROR_NULL_PARAMETER);
 
-    STUBBED("Set value to zero");
+    if (emuenv.ctrl.has_motion_support) {
+        std::lock_guard<std::mutex> guard(emuenv.motion.mutex);
+        motionState->timestamp = emuenv.motion.last_accel_timestamp;
 
-    // Set 0 to all values of motionState struct
-    memset(motionState, 0, sizeof(SceMotionState));
+        motionState->acceleration = get_acceleration(emuenv.motion);
+        motionState->angularVelocity = get_gyroscope(emuenv.motion);
 
-    // Set default position to devicePosition
-    motionState->deviceQuat.z = 1;
+        Util::Quaternion dev_quat = get_orientation(emuenv.motion);
 
-    return UNIMPLEMENTED();
+        static_assert(sizeof(motionState->deviceQuat) == sizeof(dev_quat));
+        memcpy(&motionState->deviceQuat, &dev_quat, sizeof(motionState->deviceQuat));
+
+        *reinterpret_cast<decltype(dev_quat.ToMatrix()) *>(&motionState->rotationMatrix) = dev_quat.ToMatrix();
+        // not right, but we can't do better without a magnetometer
+        memcpy(&motionState->nedMatrix, &motionState->rotationMatrix, sizeof(motionState->nedMatrix));
+
+        motionState->hostTimestamp = motionState->timestamp;
+        // set it as unstable because we don't have one
+        motionState->magnFieldStability = SCE_MOTION_MAGNETIC_FIELD_UNSTABLE;
+        motionState->dataInfo = 0;
+    } else {
+        // put some default values
+        memset(motionState, 0, sizeof(SceMotionState));
+
+        std::chrono::time_point<std::chrono::steady_clock> ts = std::chrono::steady_clock::now();
+        uint64_t timestamp = std::chrono::duration_cast<std::chrono::microseconds>(ts.time_since_epoch()).count();
+        motionState->timestamp = timestamp;
+        motionState->hostTimestamp = timestamp;
+
+        motionState->acceleration.z = -1.0;
+        motionState->deviceQuat.z = 1;
+        for (int i = 0; i < 4; i++) {
+            // identity matrices
+            reinterpret_cast<float *>(&motionState->rotationMatrix.x.x)[i * 4 + i] = 1;
+            reinterpret_cast<float *>(&motionState->nedMatrix.x.x)[i * 4 + i] = 1;
+        }
+    }
+
+    return 0;
 }
 
 EXPORT(int, sceMotionGetStateExt) {
@@ -155,7 +223,7 @@ EXPORT(int, sceMotionSetDeadbandExt) {
 
 EXPORT(int, sceMotionSetGyroBiasCorrection, SceBool setValue) {
     TRACY_FUNC(sceMotionSetGyroBiasCorrection, setValue);
-    set_gyro_bias_correction(host.motion, setValue);
+    set_gyro_bias_correction(emuenv.motion, setValue);
 
     return 0;
 }
@@ -172,7 +240,9 @@ EXPORT(int, sceMotionSetTiltCorrectionExt) {
 
 EXPORT(int, sceMotionStartSampling) {
     TRACY_FUNC(sceMotionStartSampling);
-    return UNIMPLEMENTED();
+    emuenv.motion.is_sampling = true;
+
+    return 0;
 }
 
 EXPORT(int, sceMotionStartSamplingExt) {
@@ -182,7 +252,9 @@ EXPORT(int, sceMotionStartSamplingExt) {
 
 EXPORT(int, sceMotionStopSampling) {
     TRACY_FUNC(sceMotionStopSampling);
-    return UNIMPLEMENTED();
+    emuenv.motion.is_sampling = false;
+
+    return 0;
 }
 
 EXPORT(int, sceMotionStopSamplingExt) {
diff --git a/vita3k/motion/CMakeLists.txt b/vita3k/motion/CMakeLists.txt
index 51f037bc1e..576d5111b3 100644
--- a/vita3k/motion/CMakeLists.txt
+++ b/vita3k/motion/CMakeLists.txt
@@ -1,13 +1,10 @@
 add_library(
 	motion
 	STATIC
-	include/motion/functions.h
-	include/motion/motion.h
-	include/motion/motion_input.h
-	include/motion/state.h
 	src/motion.cpp
 	src/motion_input.cpp
 )
 
 target_include_directories(motion PUBLIC include)
 target_link_libraries(motion PUBLIC emuenv sdl2 util)
+target_link_libraries(motion PRIVATE ctrl)
diff --git a/vita3k/motion/include/motion/functions.h b/vita3k/motion/include/motion/functions.h
index e6c1555480..682ca5d3df 100644
--- a/vita3k/motion/include/motion/functions.h
+++ b/vita3k/motion/include/motion/functions.h
@@ -17,17 +17,13 @@
 
 #pragma once
 
-#include <util/types.h>
-#include <util/quaternion.h>
 #include <emuenv/state.h>
 #include <motion/state.h>
 
 SceFVector3 get_acceleration(const MotionState &state);
 SceFVector3 get_gyroscope(const MotionState &state);
-SceFVector3 get_gyro_bias(const MotionState &state);
-Util::Quaternion<SceFloat> get_quaternion(const MotionState &state);
+Util::Quaternion<SceFloat> get_orientation(const MotionState &state);
 SceBool get_gyro_bias_correction(const MotionState &state);
+void set_gyro_bias_correction(MotionState &state, SceBool setValue);
 
-SceFVector3 set_gyro_bias_correction(MotionState &state, SceBool setValue);
-
-void refresh_motion(MotionState &state);
\ No newline at end of file
+void refresh_motion(MotionState &state, CtrlState &ctrl_state);
diff --git a/vita3k/motion/include/motion/motion_input.h b/vita3k/motion/include/motion/motion_input.h
index 1d6caa0e0f..789a74f4c5 100644
--- a/vita3k/motion/include/motion/motion_input.h
+++ b/vita3k/motion/include/motion/motion_input.h
@@ -4,26 +4,25 @@
 
 #pragma once
 
-#include <array>
-#include "util/types.h"
 #include "util/quaternion.h"
+#include "util/types.h"
 #include "util/vector_math.h"
+#include <array>
 
 class MotionInput {
 public:
     explicit MotionInput();
 
-    MotionInput(const MotionInput&) = default;
-    MotionInput& operator=(const MotionInput&) = default;
+    MotionInput(const MotionInput &) = default;
+    MotionInput &operator=(const MotionInput &) = default;
 
-    MotionInput(MotionInput&&) = default;
-    MotionInput& operator=(MotionInput&&) = default;
+    MotionInput(MotionInput &&) = default;
+    MotionInput &operator=(MotionInput &&) = default;
 
     void SetPID(SceFloat new_kp, SceFloat new_ki, SceFloat new_kd);
     void SetAcceleration(const Util::Vec3f &acceleration);
     void SetGyroscope(const Util::Vec3f &gyroscope);
     void SetQuaternion(const Util::Quaternion<SceFloat> &quaternion);
-    void SetGyroBias(const Util::Vec3f &bias);
     void SetGyroThreshold(SceFloat threshold);
 
     void EnableGyroBias(bool enable);
@@ -33,12 +32,10 @@ class MotionInput {
     void UpdateRotation(SceULong64 elapsed_time);
     void UpdateOrientation(SceULong64 elapsed_time);
 
-    [[nodiscard]] std::array<Util::Vec3f, 3> GetOrientation() const;
+    [[nodiscard]] Util::Quaternion<SceFloat> GetOrientation() const;
     [[nodiscard]] Util::Vec3f GetAcceleration() const;
     [[nodiscard]] Util::Vec3f GetGyroscope() const;
-    [[nodiscard]] Util::Vec3f GetGyroscopeBias() const;
     [[nodiscard]] Util::Vec3f GetRotations() const;
-    [[nodiscard]] Util::Quaternion<SceFloat> GetQuaternion() const;
 
     [[nodiscard]] bool IsMoving(SceFloat sensitivity) const;
     [[nodiscard]] bool IsCalibrated(SceFloat sensitivity) const;
@@ -59,7 +56,7 @@ class MotionInput {
     Util::Vec3f derivative_error;
 
     // Quaternion containing the device orientation
-    Util::Quaternion<SceFloat> quat{ { 0.0f, 0.0f, -1.0f }, 0.0f };
+    Util::Quaternion<SceFloat> quat{ { 0.0f, 0.0f, 1.0f }, 0.0f };
 
     // Number of full rotations in each axis
     Util::Vec3f rotations;
diff --git a/vita3k/motion/include/motion/state.h b/vita3k/motion/include/motion/state.h
index 6321c97331..d37e6d80dc 100644
--- a/vita3k/motion/include/motion/state.h
+++ b/vita3k/motion/include/motion/state.h
@@ -22,6 +22,14 @@
 
 #include <SDL_gamecontroller.h>
 
+#include <mutex>
+
 struct MotionState {
+    std::mutex mutex;
     MotionInput motion_data;
+    uint32_t last_counter = 0;
+    uint64_t last_gyro_timestamp = 0;
+    uint64_t last_accel_timestamp = 0;
+
+    bool is_sampling = false;
 };
diff --git a/vita3k/motion/src/motion.cpp b/vita3k/motion/src/motion.cpp
index 972b45ab12..c678641714 100644
--- a/vita3k/motion/src/motion.cpp
+++ b/vita3k/motion/src/motion.cpp
@@ -19,8 +19,12 @@
 #include <motion/motion.h>
 #include <motion/state.h>
 
+#include <ctrl/state.h>
+
+#include <SDL_gamecontroller.h>
+
 SceFVector3 get_acceleration(const MotionState &state) {
-    Util::Vec3f accelerometer =  state.motion_data.GetAcceleration();
+    Util::Vec3f accelerometer = state.motion_data.GetAcceleration();
     return {
         accelerometer.x,
         accelerometer.y,
@@ -29,7 +33,7 @@ SceFVector3 get_acceleration(const MotionState &state) {
 }
 
 SceFVector3 get_gyroscope(const MotionState &state) {
-    Util::Vec3f gyroscope = state.motion_data.GetGyroscope();
+    Util::Vec3f gyroscope = state.motion_data.GetGyroscope() * static_cast<float>(2.f * M_PI);
     return {
         gyroscope.x,
         gyroscope.y,
@@ -37,29 +41,84 @@ SceFVector3 get_gyroscope(const MotionState &state) {
     };
 }
 
-SceFVector3 get_gyro_bias(const MotionState &state) {
-    Util::Vec3f bias = state.motion_data.GetGyroscopeBias();
+Util::Quaternion<SceFloat> get_orientation(const MotionState &state) {
+    auto quat = state.motion_data.GetOrientation();
     return {
-        bias.x,
-        bias.y,
-        bias.z,
+        { quat.xyz[1], quat.xyz[0], -quat.w },
+        -quat.xyz[2],
     };
 }
 
-Util::Quaternion<SceFloat> get_quaternion(const MotionState &state) {
-    return state.motion_data.GetQuaternion();
-}
-
 SceBool get_gyro_bias_correction(const MotionState &state) {
     return state.motion_data.IsGyroBiasEnabled();
 }
 
-SceFVector3 set_gyro_bias_correction(MotionState& state, SceBool setValue) {
+void set_gyro_bias_correction(MotionState &state, SceBool setValue) {
     state.motion_data.EnableGyroBias(setValue);
 }
 
-void refresh_motion(MotionState &state) {
-    SceULong64 elapsed_time = 5000;
-    state.motion_data.UpdateRotation(elapsed_time);
-    state.motion_data.UpdateOrientation(elapsed_time);
-}
\ No newline at end of file
+void refresh_motion(MotionState &state, CtrlState &ctrl_state) {
+    if (!state.is_sampling)
+        return;
+
+    if (!ctrl_state.has_motion_support)
+        return;
+
+    // make sure to use the data from only one accelerometer and gyroscope
+    bool found_gyro = false;
+    bool found_accel = false;
+    Util::Vec3f gyro;
+    uint64_t gyro_timestamp = 0;
+    Util::Vec3f accel;
+    uint64_t accel_timestamp = 0;
+
+    {
+        std::lock_guard<std::mutex> guard(ctrl_state.mutex);
+        for (auto controller : ctrl_state.controllers) {
+            if (!found_gyro && controller.second.has_gyro) {
+                if (SDL_GameControllerGetSensorDataWithTimestamp(controller.second.controller.get(), SDL_SENSOR_GYRO, &gyro_timestamp, reinterpret_cast<float *>(&gyro), 3) == 0) {
+                    found_gyro = true;
+                }
+            }
+
+            if (!found_accel && controller.second.has_accel) {
+                if (SDL_GameControllerGetSensorDataWithTimestamp(controller.second.controller.get(), SDL_SENSOR_ACCEL, &accel_timestamp, reinterpret_cast<float *>(&accel), 3) == 0) {
+                    found_accel = true;
+                }
+            }
+        }
+    }
+
+    if (!found_accel && !found_gyro)
+        return;
+
+    // if timestamp is not available, use the current time instead
+    if (gyro_timestamp == 0 || accel_timestamp == 0) {
+        std::chrono::time_point<std::chrono::steady_clock> ts = std::chrono::steady_clock::now();
+        uint64_t timestamp = std::chrono::duration_cast<std::chrono::microseconds>(ts.time_since_epoch()).count();
+
+        if (gyro_timestamp == 0)
+            gyro_timestamp = timestamp;
+        if (accel_timestamp == 0)
+            accel_timestamp = timestamp;
+    }
+
+    std::lock_guard<std::mutex> guard(state.mutex);
+
+    gyro /= static_cast<float>(2.0 * M_PI);
+    std::swap(gyro.y, gyro.z);
+    gyro.y *= -1;
+    state.motion_data.SetGyroscope(gyro);
+
+    accel /= -SDL_STANDARD_GRAVITY;
+    std::swap(accel.y, accel.z);
+    accel.y *= -1;
+    state.motion_data.SetAcceleration(accel);
+
+    state.motion_data.UpdateRotation(gyro_timestamp - state.last_gyro_timestamp);
+    state.motion_data.UpdateOrientation(accel_timestamp - state.last_accel_timestamp);
+
+    state.last_gyro_timestamp = gyro_timestamp;
+    state.last_accel_timestamp = accel_timestamp;
+    state.last_counter++;
+}
diff --git a/vita3k/motion/src/motion_input.cpp b/vita3k/motion/src/motion_input.cpp
index 969cbbb42e..332a8db086 100644
--- a/vita3k/motion/src/motion_input.cpp
+++ b/vita3k/motion/src/motion_input.cpp
@@ -2,9 +2,11 @@
 // Licensed under GPLv2 or any later version
 // Refer to the license.txt file included
 
-//#include "common/math_util.h"
 #include "motion/motion_input.h"
 
+// for M_PI constant
+#include <SDL_stdinc.h>
+
 MotionInput::MotionInput() {
     // Initialize PID constants with default values
     SetPID(0.3f, 0.005f, 0.0f);
@@ -43,10 +45,6 @@ void MotionInput::SetQuaternion(const Util::Quaternion<SceFloat> &quaternion) {
     quat = quaternion;
 }
 
-void MotionInput::SetGyroBias(const Util::Vec3f &bias) {
-    gyro_bias = bias;
-}
-
 void MotionInput::SetGyroThreshold(SceFloat threshold) {
     gyro_threshold = threshold;
 }
@@ -182,16 +180,8 @@ void MotionInput::UpdateOrientation(SceULong64 elapsed_time) {
     quat = quat.Normalized();
 }
 
-std::array<Util::Vec3f, 3> MotionInput::GetOrientation() const {
-    const Util::Quaternion<float> quad{
-        { -quat.xyz[1], -quat.xyz[0], -quat.w },
-        -quat.xyz[2],
-    };
-    const std::array<float, 16> matrix4x4 = quad.ToMatrix();
-
-    return {Util::Vec3f(matrix4x4[0], matrix4x4[1], -matrix4x4[2]),
-            Util::Vec3f(matrix4x4[4], matrix4x4[5], -matrix4x4[6]),
-            Util::Vec3f(-matrix4x4[8], -matrix4x4[9], matrix4x4[10])};
+Util::Quaternion<float> MotionInput::GetOrientation() const {
+    return quat;
 }
 
 Util::Vec3f MotionInput::GetAcceleration() const {
@@ -202,14 +192,6 @@ Util::Vec3f MotionInput::GetGyroscope() const {
     return gyro;
 }
 
-Util::Vec3f MotionInput::GetGyroscopeBias() const {
-    return gyro_bias;
-}
-
-Util::Quaternion<SceFloat> MotionInput::GetQuaternion() const {
-    return quat;
-}
-
 Util::Vec3f MotionInput::GetRotations() const {
     return rotations;
 }
diff --git a/vita3k/util/include/util/quaternion.h b/vita3k/util/include/util/quaternion.h
index 89f04b0e4a..e965da1ea9 100644
--- a/vita3k/util/include/util/quaternion.h
+++ b/vita3k/util/include/util/quaternion.h
@@ -6,6 +6,8 @@
 
 #include "util/vector_math.h"
 
+#include <array>
+
 namespace Util {
 
 template <typename T>
diff --git a/vita3k/util/include/util/vector_math.h b/vita3k/util/include/util/vector_math.h
index 0ad7a40f40..2831ea842d 100644
--- a/vita3k/util/include/util/vector_math.h
+++ b/vita3k/util/include/util/vector_math.h
@@ -207,30 +207,6 @@ class Vec3 {
     [[nodiscard]] constexpr const T& q() const {
         return z;
     }
-
-// swizzlers - create a subvector of specific components
-// e.g. Vec2 uv() { return Vec2(x,y); }
-// _DEFINE_SWIZZLER2 defines a single such function, DEFINE_SWIZZLER2 defines all of them for all
-// component names (x<->r) and permutations (xy<->yx)
-#define _DEFINE_SWIZZLER2(a, b, name)                                                              \
-    [[nodiscard]] constexpr Vec2<T> name() const {                                                 \
-        return Vec2<T>(a, b);                                                                      \
-    }
-#define DEFINE_SWIZZLER2(a, b, a2, b2, a3, b3, a4, b4)                                             \
-    _DEFINE_SWIZZLER2(a, b, a##b);                                                                 \
-    _DEFINE_SWIZZLER2(a, b, a2##b2);                                                               \
-    _DEFINE_SWIZZLER2(a, b, a3##b3);                                                               \
-    _DEFINE_SWIZZLER2(a, b, a4##b4);                                                               \
-    _DEFINE_SWIZZLER2(b, a, b##a);                                                                 \
-    _DEFINE_SWIZZLER2(b, a, b2##a2);                                                               \
-    _DEFINE_SWIZZLER2(b, a, b3##a3);                                                               \
-    _DEFINE_SWIZZLER2(b, a, b4##a4)
-
-    DEFINE_SWIZZLER2(x, y, r, g, u, v, s, t);
-    DEFINE_SWIZZLER2(x, z, r, b, u, w, s, q);
-    DEFINE_SWIZZLER2(y, z, g, b, v, w, t, q);
-#undef DEFINE_SWIZZLER2
-#undef _DEFINE_SWIZZLER2
 };
 
 template <typename T, typename V>

Places

File vita3k-PR2628.patch of Package vita3k (Revision a56d075736a20fe42d2f13d144f547bb)

Places