From e04865c71507b389f69ecac34c100d15fc32bc7a Mon Sep 17 00:00:00 2001 From: Mr-Wiseguy Date: Mon, 22 Jan 2024 23:08:59 -0500 Subject: [PATCH] Initial implementation of gyro aim with GamepadMotionHelpers for sensor fusion --- CMakeLists.txt | 1 + include/recomp_input.h | 1 + lib/GamepadMotionHelpers/.gitignore | 32 + lib/GamepadMotionHelpers/CMakeLists.txt | 11 + lib/GamepadMotionHelpers/GamepadMotion.hpp | 1312 ++++++++++++++++++++ lib/GamepadMotionHelpers/LICENSE | 21 + lib/GamepadMotionHelpers/README.md | 76 ++ patches/input.c | 49 + patches/input.h | 1 + patches/syms.ld | 1 + src/game/controls.cpp | 7 + src/game/input.cpp | 80 +- src/main/main.cpp | 1 + ultramodern/events.cpp | 2 +- 14 files changed, 1589 insertions(+), 6 deletions(-) create mode 100644 lib/GamepadMotionHelpers/.gitignore create mode 100644 lib/GamepadMotionHelpers/CMakeLists.txt create mode 100644 lib/GamepadMotionHelpers/GamepadMotion.hpp create mode 100644 lib/GamepadMotionHelpers/LICENSE create mode 100644 lib/GamepadMotionHelpers/README.md diff --git a/CMakeLists.txt b/CMakeLists.txt index 0a17f46..2d252fa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -148,6 +148,7 @@ set (SOURCES target_include_directories(MMRecomp PRIVATE ${CMAKE_SOURCE_DIR}/include ${CMAKE_SOURCE_DIR}/lib/concurrentqueue + ${CMAKE_SOURCE_DIR}/lib/GamepadMotionHelpers ${CMAKE_SOURCE_DIR}/lib/RmlUi/Include ${CMAKE_SOURCE_DIR}/lib/RmlUi/Backends ${CMAKE_SOURCE_DIR}/lib/RT64-HLE/src/contrib diff --git a/include/recomp_input.h b/include/recomp_input.h index e64cd05..7892597 100644 --- a/include/recomp_input.h +++ b/include/recomp_input.h @@ -63,6 +63,7 @@ namespace recomp { float get_input_analog(const std::span fields); bool get_input_digital(const InputField& field); bool get_input_digital(const std::span fields); + void get_gyro_deltas(float* x, float* y); enum class InputDevice { Controller, diff --git a/lib/GamepadMotionHelpers/.gitignore b/lib/GamepadMotionHelpers/.gitignore new file mode 100644 index 0000000..259148f --- /dev/null +++ b/lib/GamepadMotionHelpers/.gitignore @@ -0,0 +1,32 @@ +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app diff --git a/lib/GamepadMotionHelpers/CMakeLists.txt b/lib/GamepadMotionHelpers/CMakeLists.txt new file mode 100644 index 0000000..27f7748 --- /dev/null +++ b/lib/GamepadMotionHelpers/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.8) + +project(GamepadMotionHelpers LANGUAGES CXX) + +add_library(${PROJECT_NAME} INTERFACE) +add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) +target_include_directories(${PROJECT_NAME} + INTERFACE + $ + $) + \ No newline at end of file diff --git a/lib/GamepadMotionHelpers/GamepadMotion.hpp b/lib/GamepadMotionHelpers/GamepadMotion.hpp new file mode 100644 index 0000000..02497ac --- /dev/null +++ b/lib/GamepadMotionHelpers/GamepadMotion.hpp @@ -0,0 +1,1312 @@ +// Copyright (c) 2020-2023 Julian "Jibb" Smart +// Released under the MIT license. See https://github.com/JibbSmart/GamepadMotionHelpers/blob/main/LICENSE for more info +// Version 9 + +#pragma once + +#define _USE_MATH_DEFINES +#include +#include // std::min, std::max and std::clamp + +// You don't need to look at these. These will just be used internally by the GamepadMotion class declared below. +// You can ignore anything in namespace GamepadMotionHelpers. +class GamepadMotionSettings; +class GamepadMotion; + +namespace GamepadMotionHelpers +{ + struct GyroCalibration + { + float X; + float Y; + float Z; + float AccelMagnitude; + int NumSamples; + }; + + struct Quat + { + float w; + float x; + float y; + float z; + + Quat(); + Quat(float inW, float inX, float inY, float inZ); + void Set(float inW, float inX, float inY, float inZ); + Quat& operator*=(const Quat& rhs); + friend Quat operator*(Quat lhs, const Quat& rhs); + void Normalize(); + Quat Normalized() const; + void Invert(); + Quat Inverse() const; + }; + + struct Vec + { + float x; + float y; + float z; + + Vec(); + Vec(float inValue); + Vec(float inX, float inY, float inZ); + void Set(float inX, float inY, float inZ); + float Length() const; + float LengthSquared() const; + void Normalize(); + Vec Normalized() const; + float Dot(const Vec& other) const; + Vec Cross(const Vec& other) const; + Vec Min(const Vec& other) const; + Vec Max(const Vec& other) const; + Vec Abs() const; + Vec Lerp(const Vec& other, float factor) const; + Vec Lerp(const Vec& other, const Vec& factor) const; + Vec& operator+=(const Vec& rhs); + friend Vec operator+(Vec lhs, const Vec& rhs); + Vec& operator-=(const Vec& rhs); + friend Vec operator-(Vec lhs, const Vec& rhs); + Vec& operator*=(const float rhs); + friend Vec operator*(Vec lhs, const float rhs); + Vec& operator/=(const float rhs); + friend Vec operator/(Vec lhs, const float rhs); + Vec& operator*=(const Quat& rhs); + friend Vec operator*(Vec lhs, const Quat& rhs); + Vec operator-() const; + }; + + struct SensorMinMaxWindow + { + Vec MinGyro; + Vec MaxGyro; + Vec MeanGyro; + Vec MinAccel; + Vec MaxAccel; + Vec MeanAccel; + Vec StartAccel; + int NumSamples = 0; + float TimeSampled = 0.f; + + SensorMinMaxWindow(); + void Reset(float remainder); + void AddSample(const Vec& inGyro, const Vec& inAccel, float deltaTime); + Vec GetMidGyro(); + }; + + struct AutoCalibration + { + SensorMinMaxWindow MinMaxWindow; + Vec SmoothedAngularVelocityGyro; + Vec SmoothedAngularVelocityAccel; + Vec SmoothedPreviousAccel; + Vec PreviousAccel; + + AutoCalibration(); + void Reset(); + bool AddSampleStillness(const Vec& inGyro, const Vec& inAccel, float deltaTime, bool doSensorFusion); + void NoSampleStillness(); + bool AddSampleSensorFusion(const Vec& inGyro, const Vec& inAccel, float deltaTime); + void NoSampleSensorFusion(); + void SetCalibrationData(GyroCalibration* calibrationData); + void SetSettings(GamepadMotionSettings* settings); + + float Confidence = 0.f; + bool IsSteady() { return bIsSteady; } + + private: + Vec MinDeltaGyro = Vec(1.f); + Vec MinDeltaAccel = Vec(0.25f); + float RecalibrateThreshold = 1.f; + float SensorFusionSkippedTime = 0.f; + float TimeSteadySensorFusion = 0.f; + float TimeSteadyStillness = 0.f; + bool bIsSteady = false; + + GyroCalibration* CalibrationData; + GamepadMotionSettings* Settings; + }; + + struct Motion + { + Quat Quaternion; + Vec Accel; + Vec Grav; + + Vec SmoothAccel = Vec(); + float Shakiness = 0.f; + const float ShortSteadinessHalfTime = 0.25f; + const float LongSteadinessHalfTime = 1.f; + + Motion(); + void Reset(); + void Update(float inGyroX, float inGyroY, float inGyroZ, float inAccelX, float inAccelY, float inAccelZ, float gravityLength, float deltaTime); + void SetSettings(GamepadMotionSettings* settings); + + private: + GamepadMotionSettings* Settings; + }; + + enum CalibrationMode + { + Manual = 0, + Stillness = 1, + SensorFusion = 2, + }; + + // https://stackoverflow.com/a/1448478/1130520 + inline CalibrationMode operator|(CalibrationMode a, CalibrationMode b) + { + return static_cast(static_cast(a) | static_cast(b)); + } + + inline CalibrationMode operator&(CalibrationMode a, CalibrationMode b) + { + return static_cast(static_cast(a) & static_cast(b)); + } + + inline CalibrationMode operator~(CalibrationMode a) + { + return static_cast(~static_cast(a)); + } + + // https://stackoverflow.com/a/23152590/1130520 + inline CalibrationMode& operator|=(CalibrationMode& a, CalibrationMode b) + { + return (CalibrationMode&)((int&)(a) |= static_cast(b)); + } + + inline CalibrationMode& operator&=(CalibrationMode& a, CalibrationMode b) + { + return (CalibrationMode&)((int&)(a) &= static_cast(b)); + } +} + +// Note that I'm using a Y-up coordinate system. This is to follow the convention set by the motion sensors in +// PlayStation controllers, which was what I was using when writing in this. But for the record, Z-up is +// better for most games (XY ground-plane in 3D games simplifies using 2D vectors in navigation, for example). + +// Gyro units should be degrees per second. Accelerometer should be g-force (approx. 9.8 m/s^2 = 1 g). If you're using +// radians per second, meters per second squared, etc, conversion should be simple. + +class GamepadMotionSettings +{ +public: + int MinStillnessSamples = 10; + float MinStillnessCollectionTime = 0.5f; + float MinStillnessCorrectionTime = 2.f; + float MaxStillnessError = 2.f; + float StillnessSampleDeteriorationRate = 0.2f; + float StillnessErrorClimbRate = 0.1f; + float StillnessErrorDropOnRecalibrate = 0.1f; + float StillnessCalibrationEaseInTime = 3.f; + float StillnessCalibrationHalfTime = 0.1f; + float StillnessConfidenceRate = 1.f; + + float StillnessGyroDelta = -1.f; + float StillnessAccelDelta = -1.f; + + float SensorFusionCalibrationSmoothingStrength = 2.f; + float SensorFusionAngularAccelerationThreshold = 20.f; + float SensorFusionCalibrationEaseInTime = 3.f; + float SensorFusionCalibrationHalfTime = 0.1f; + float SensorFusionConfidenceRate = 1.f; + + float GravityCorrectionShakinessMaxThreshold = 0.4f; + float GravityCorrectionShakinessMinThreshold = 0.01f; + + float GravityCorrectionStillSpeed = 1.f; + float GravityCorrectionShakySpeed = 0.1f; + + float GravityCorrectionGyroFactor = 0.1f; + float GravityCorrectionGyroMinThreshold = 0.05f; + float GravityCorrectionGyroMaxThreshold = 0.25f; + + float GravityCorrectionMinimumSpeed = 0.01f; +}; + +class GamepadMotion +{ +public: + GamepadMotion(); + + void Reset(); + + void ProcessMotion(float gyroX, float gyroY, float gyroZ, + float accelX, float accelY, float accelZ, float deltaTime); + + // reading the current state + void GetCalibratedGyro(float& x, float& y, float& z); + void GetGravity(float& x, float& y, float& z); + void GetProcessedAcceleration(float& x, float& y, float& z); + void GetOrientation(float& w, float& x, float& y, float& z); + void GetPlayerSpaceGyro(float& x, float& y, const float yawRelaxFactor = 1.41f); + static void CalculatePlayerSpaceGyro(float& x, float& y, const float gyroX, const float gyroY, const float gyroZ, const float gravX, const float gravY, const float gravZ, const float yawRelaxFactor = 1.41f); + void GetWorldSpaceGyro(float& x, float& y, const float sideReductionThreshold = 0.125f); + static void CalculateWorldSpaceGyro(float& x, float& y, const float gyroX, const float gyroY, const float gyroZ, const float gravX, const float gravY, const float gravZ, const float sideReductionThreshold = 0.125f); + + // gyro calibration functions + void StartContinuousCalibration(); + void PauseContinuousCalibration(); + void ResetContinuousCalibration(); + void GetCalibrationOffset(float& xOffset, float& yOffset, float& zOffset); + void SetCalibrationOffset(float xOffset, float yOffset, float zOffset, int weight); + float GetAutoCalibrationConfidence(); + void SetAutoCalibrationConfidence(float newConfidence); + bool GetAutoCalibrationIsSteady(); + + GamepadMotionHelpers::CalibrationMode GetCalibrationMode(); + void SetCalibrationMode(GamepadMotionHelpers::CalibrationMode calibrationMode); + + void ResetMotion(); + + GamepadMotionSettings Settings; + +private: + GamepadMotionHelpers::Vec Gyro; + GamepadMotionHelpers::Vec RawAccel; + GamepadMotionHelpers::Motion Motion; + GamepadMotionHelpers::GyroCalibration GyroCalibration; + GamepadMotionHelpers::AutoCalibration AutoCalibration; + GamepadMotionHelpers::CalibrationMode CurrentCalibrationMode; + + bool IsCalibrating; + void PushSensorSamples(float gyroX, float gyroY, float gyroZ, float accelMagnitude); + void GetCalibratedSensor(float& gyroOffsetX, float& gyroOffsetY, float& gyroOffsetZ, float& accelMagnitude); +}; + +///////////// Everything below here are just implementation details ///////////// + +namespace GamepadMotionHelpers +{ + inline Quat::Quat() + { + w = 1.0f; + x = 0.0f; + y = 0.0f; + z = 0.0f; + } + + inline Quat::Quat(float inW, float inX, float inY, float inZ) + { + w = inW; + x = inX; + y = inY; + z = inZ; + } + + inline static Quat AngleAxis(float inAngle, float inX, float inY, float inZ) + { + const float sinHalfAngle = sinf(inAngle * 0.5f); + Vec inAxis = Vec(inX, inY, inZ); + inAxis.Normalize(); + inAxis *= sinHalfAngle; + Quat result = Quat(cosf(inAngle * 0.5f), inAxis.x, inAxis.y, inAxis.z); + return result; + } + + inline void Quat::Set(float inW, float inX, float inY, float inZ) + { + w = inW; + x = inX; + y = inY; + z = inZ; + } + + inline Quat& Quat::operator*=(const Quat& rhs) + { + Set(w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z, + w * rhs.x + x * rhs.w + y * rhs.z - z * rhs.y, + w * rhs.y - x * rhs.z + y * rhs.w + z * rhs.x, + w * rhs.z + x * rhs.y - y * rhs.x + z * rhs.w); + return *this; + } + + inline Quat operator*(Quat lhs, const Quat& rhs) + { + lhs *= rhs; + return lhs; + } + + inline void Quat::Normalize() + { + const float length = sqrtf(w * w + x * x + y * y + z * z); + const float fixFactor = 1.0f / length; + + w *= fixFactor; + x *= fixFactor; + y *= fixFactor; + z *= fixFactor; + + return; + } + + inline Quat Quat::Normalized() const + { + Quat result = *this; + result.Normalize(); + return result; + } + + inline void Quat::Invert() + { + x = -x; + y = -y; + z = -z; + return; + } + + inline Quat Quat::Inverse() const + { + Quat result = *this; + result.Invert(); + return result; + } + + inline Vec::Vec() + { + x = 0.0f; + y = 0.0f; + z = 0.0f; + } + + inline Vec::Vec(float inValue) + { + x = inValue; + y = inValue; + z = inValue; + } + + inline Vec::Vec(float inX, float inY, float inZ) + { + x = inX; + y = inY; + z = inZ; + } + + inline void Vec::Set(float inX, float inY, float inZ) + { + x = inX; + y = inY; + z = inZ; + } + + inline float Vec::Length() const + { + return sqrtf(x * x + y * y + z * z); + } + + inline float Vec::LengthSquared() const + { + return x * x + y * y + z * z; + } + + inline void Vec::Normalize() + { + const float length = Length(); + if (length == 0.0) + { + return; + } + const float fixFactor = 1.0f / length; + + x *= fixFactor; + y *= fixFactor; + z *= fixFactor; + return; + } + + inline Vec Vec::Normalized() const + { + Vec result = *this; + result.Normalize(); + return result; + } + + inline Vec& Vec::operator+=(const Vec& rhs) + { + Set(x + rhs.x, y + rhs.y, z + rhs.z); + return *this; + } + + inline Vec operator+(Vec lhs, const Vec& rhs) + { + lhs += rhs; + return lhs; + } + + inline Vec& Vec::operator-=(const Vec& rhs) + { + Set(x - rhs.x, y - rhs.y, z - rhs.z); + return *this; + } + + inline Vec operator-(Vec lhs, const Vec& rhs) + { + lhs -= rhs; + return lhs; + } + + inline Vec& Vec::operator*=(const float rhs) + { + Set(x * rhs, y * rhs, z * rhs); + return *this; + } + + inline Vec operator*(Vec lhs, const float rhs) + { + lhs *= rhs; + return lhs; + } + + inline Vec& Vec::operator/=(const float rhs) + { + Set(x / rhs, y / rhs, z / rhs); + return *this; + } + + inline Vec operator/(Vec lhs, const float rhs) + { + lhs /= rhs; + return lhs; + } + + inline Vec& Vec::operator*=(const Quat& rhs) + { + Quat temp = rhs * Quat(0.0f, x, y, z) * rhs.Inverse(); + Set(temp.x, temp.y, temp.z); + return *this; + } + + inline Vec operator*(Vec lhs, const Quat& rhs) + { + lhs *= rhs; + return lhs; + } + + inline Vec Vec::operator-() const + { + Vec result = Vec(-x, -y, -z); + return result; + } + + inline float Vec::Dot(const Vec& other) const + { + return x * other.x + y * other.y + z * other.z; + } + + inline Vec Vec::Cross(const Vec& other) const + { + return Vec(y * other.z - z * other.y, + z * other.x - x * other.z, + x * other.y - y * other.x); + } + + inline Vec Vec::Min(const Vec& other) const + { + return Vec(x < other.x ? x : other.x, + y < other.y ? y : other.y, + z < other.z ? z : other.z); + } + + inline Vec Vec::Max(const Vec& other) const + { + return Vec(x > other.x ? x : other.x, + y > other.y ? y : other.y, + z > other.z ? z : other.z); + } + + inline Vec Vec::Abs() const + { + return Vec(x > 0 ? x : -x, + y > 0 ? y : -y, + z > 0 ? z : -z); + } + + inline Vec Vec::Lerp(const Vec& other, float factor) const + { + return *this + (other - *this) * factor; + } + + inline Vec Vec::Lerp(const Vec& other, const Vec& factor) const + { + return Vec(this->x + (other.x - this->x) * factor.x, + this->y + (other.y - this->y) * factor.y, + this->z + (other.z - this->z) * factor.z); + } + + inline Motion::Motion() + { + Reset(); + } + + inline void Motion::Reset() + { + Quaternion.Set(1.f, 0.f, 0.f, 0.f); + Accel.Set(0.f, 0.f, 0.f); + Grav.Set(0.f, 0.f, 0.f); + SmoothAccel.Set(0.f, 0.f, 0.f); + Shakiness = 0.f; + } + + /// + /// The gyro inputs should be calibrated degrees per second but have no other processing. Acceleration is in G units (1 = approx. 9.8m/s^2) + /// + inline void Motion::Update(float inGyroX, float inGyroY, float inGyroZ, float inAccelX, float inAccelY, float inAccelZ, float gravityLength, float deltaTime) + { + if (!Settings) + { + return; + } + + // get settings + const float gravityCorrectionShakinessMinThreshold = Settings->GravityCorrectionShakinessMinThreshold; + const float gravityCorrectionShakinessMaxThreshold = Settings->GravityCorrectionShakinessMaxThreshold; + const float gravityCorrectionStillSpeed = Settings->GravityCorrectionStillSpeed; + const float gravityCorrectionShakySpeed = Settings->GravityCorrectionShakySpeed; + const float gravityCorrectionGyroFactor = Settings->GravityCorrectionGyroFactor; + const float gravityCorrectionGyroMinThreshold = Settings->GravityCorrectionGyroMinThreshold; + const float gravityCorrectionGyroMaxThreshold = Settings->GravityCorrectionGyroMaxThreshold; + const float gravityCorrectionMinimumSpeed = Settings->GravityCorrectionMinimumSpeed; + + const Vec axis = Vec(inGyroX, inGyroY, inGyroZ); + const Vec accel = Vec(inAccelX, inAccelY, inAccelZ); + const float angleSpeed = axis.Length() * (float)M_PI / 180.0f; + const float angle = angleSpeed * deltaTime; + + // rotate + Quat rotation = AngleAxis(angle, axis.x, axis.y, axis.z); + Quaternion *= rotation; // do it this way because it's a local rotation, not global + + //printf("Quat: %.4f %.4f %.4f %.4f\n", + // Quaternion.w, Quaternion.x, Quaternion.y, Quaternion.z); + float accelMagnitude = accel.Length(); + if (accelMagnitude > 0.0f) + { + const Vec accelNorm = accel / accelMagnitude; + // account for rotation when tracking smoothed acceleration + SmoothAccel *= rotation.Inverse(); + //printf("Absolute Accel: %.4f %.4f %.4f\n", + // absoluteAccel.x, absoluteAccel.y, absoluteAccel.z); + const float smoothFactor = ShortSteadinessHalfTime <= 0.f ? 0.f : exp2f(-deltaTime / ShortSteadinessHalfTime); + Shakiness *= smoothFactor; + Shakiness = std::max(Shakiness, (accel - SmoothAccel).Length()); + SmoothAccel = accel.Lerp(SmoothAccel, smoothFactor); + + //printf("Shakiness: %.4f\n", Shakiness); + + // update grav by rotation + Grav *= rotation.Inverse(); + // we want to close the gap between grav and raw acceleration. What's the difference + const Vec gravToAccel = (accelNorm * -gravityLength) - Grav; + const Vec gravToAccelDir = gravToAccel.Normalized(); + // adjustment rate + float gravCorrectionSpeed; + if (gravityCorrectionShakinessMinThreshold < gravityCorrectionShakinessMaxThreshold) + { + gravCorrectionSpeed = gravityCorrectionStillSpeed + (gravityCorrectionShakySpeed - gravityCorrectionStillSpeed) * std::clamp((Shakiness - gravityCorrectionShakinessMinThreshold) / (gravityCorrectionShakinessMaxThreshold - gravityCorrectionShakinessMinThreshold), 0.f, 1.f); + } + else + { + gravCorrectionSpeed = Shakiness < gravityCorrectionShakinessMaxThreshold ? gravityCorrectionStillSpeed : gravityCorrectionShakySpeed; + } + // we also limit it to be no faster than a given proportion of the gyro rate, or the minimum gravity correction speed + const float gyroGravCorrectionLimit = std::max(angleSpeed * gravityCorrectionGyroFactor, gravityCorrectionMinimumSpeed); + if (gravCorrectionSpeed > gyroGravCorrectionLimit) + { + float closeEnoughFactor; + if (gravityCorrectionGyroMinThreshold < gravityCorrectionGyroMaxThreshold) + { + closeEnoughFactor = std::clamp((gravToAccel.Length() - gravityCorrectionGyroMinThreshold) / (gravityCorrectionGyroMaxThreshold - gravityCorrectionGyroMinThreshold), 0.f, 1.f); + } + else + { + closeEnoughFactor = gravToAccel.Length() < gravityCorrectionGyroMaxThreshold ? 0.f : 1.f; + } + gravCorrectionSpeed = gyroGravCorrectionLimit + (gravCorrectionSpeed - gyroGravCorrectionLimit) * closeEnoughFactor; + } + const Vec gravToAccelDelta = gravToAccelDir * gravCorrectionSpeed * deltaTime; + if (gravToAccelDelta.LengthSquared() < gravToAccel.LengthSquared()) + { + Grav += gravToAccelDelta; + } + else + { + Grav = accelNorm * -gravityLength; + } + + const Vec gravityDirection = Grav.Normalized() * Quaternion.Inverse(); // absolute gravity direction + const float errorAngle = acosf(std::clamp(Vec(0.0f, -1.0f, 0.0f).Dot(gravityDirection), -1.f, 1.f)); + const Vec flattened = Vec(0.0f, -1.0f, 0.0f).Cross(gravityDirection); + Quat correctionQuat = AngleAxis(errorAngle, flattened.x, flattened.y, flattened.z); + Quaternion = Quaternion * correctionQuat; + + Accel = accel + Grav; + } + else + { + Grav *= rotation.Inverse(); + Accel = Grav; + } + Quaternion.Normalize(); + } + + inline void Motion::SetSettings(GamepadMotionSettings* settings) + { + Settings = settings; + } + + inline SensorMinMaxWindow::SensorMinMaxWindow() + { + Reset(0.f); + } + + inline void SensorMinMaxWindow::Reset(float remainder) + { + NumSamples = 0; + TimeSampled = remainder; + } + + inline void SensorMinMaxWindow::AddSample(const Vec& inGyro, const Vec& inAccel, float deltaTime) + { + if (NumSamples == 0) + { + MaxGyro = inGyro; + MinGyro = inGyro; + MeanGyro = inGyro; + MaxAccel = inAccel; + MinAccel = inAccel; + MeanAccel = inAccel; + StartAccel = inAccel; + NumSamples = 1; + TimeSampled += deltaTime; + return; + } + + MaxGyro = MaxGyro.Max(inGyro); + MinGyro = MinGyro.Min(inGyro); + MaxAccel = MaxAccel.Max(inAccel); + MinAccel = MinAccel.Min(inAccel); + + NumSamples++; + TimeSampled += deltaTime; + + Vec delta = inGyro - MeanGyro; + MeanGyro += delta * (1.f / NumSamples); + delta = inAccel - MeanAccel; + MeanAccel += delta * (1.f / NumSamples); + } + + inline Vec SensorMinMaxWindow::GetMidGyro() + { + return MeanGyro; + } + + inline AutoCalibration::AutoCalibration() + { + CalibrationData = nullptr; + Reset(); + } + + inline void AutoCalibration::Reset() + { + MinMaxWindow.Reset(0.f); + Confidence = 0.f; + bIsSteady = false; + MinDeltaGyro = Vec(1.f); + MinDeltaAccel = Vec(0.25f); + RecalibrateThreshold = 1.f; + SensorFusionSkippedTime = 0.f; + TimeSteadySensorFusion = 0.f; + TimeSteadyStillness = 0.f; + } + + inline bool AutoCalibration::AddSampleStillness(const Vec& inGyro, const Vec& inAccel, float deltaTime, bool doSensorFusion) + { + if (inGyro.x == 0.f && inGyro.y == 0.f && inGyro.z == 0.f && + inAccel.x == 0.f && inAccel.y == 0.f && inAccel.z == 0.f) + { + // zeroes are almost certainly not valid inputs + return false; + } + + if (!Settings) + { + return false; + } + + if (!CalibrationData) + { + return false; + } + + // get settings + const int minStillnessSamples = Settings->MinStillnessSamples; + const float minStillnessCollectionTime = Settings->MinStillnessCollectionTime; + const float minStillnessCorrectionTime = Settings->MinStillnessCorrectionTime; + const float maxStillnessError = Settings->MaxStillnessError; + const float stillnessSampleDeteriorationRate = Settings->StillnessSampleDeteriorationRate; + const float stillnessErrorClimbRate = Settings->StillnessErrorClimbRate; + const float stillnessErrorDropOnRecalibrate = Settings->StillnessErrorDropOnRecalibrate; + const float stillnessCalibrationEaseInTime = Settings->StillnessCalibrationEaseInTime; + const float stillnessCalibrationHalfTime = Settings->StillnessCalibrationHalfTime * Confidence; + const float stillnessConfidenceRate = Settings->StillnessConfidenceRate; + const float stillnessGyroDelta = Settings->StillnessGyroDelta; + const float stillnessAccelDelta = Settings->StillnessAccelDelta; + + MinMaxWindow.AddSample(inGyro, inAccel, deltaTime); + // get deltas + const Vec gyroDelta = MinMaxWindow.MaxGyro - MinMaxWindow.MinGyro; + const Vec accelDelta = MinMaxWindow.MaxAccel - MinMaxWindow.MinAccel; + + bool calibrated = false; + bool isSteady = false; + const Vec climbThisTick = Vec(stillnessSampleDeteriorationRate * deltaTime); + if (stillnessGyroDelta < 0.f) + { + if (Confidence < 1.f) + { + MinDeltaGyro += climbThisTick; + } + } + else + { + MinDeltaGyro = Vec(stillnessGyroDelta); + } + if (stillnessAccelDelta < 0.f) + { + if (Confidence < 1.f) + { + MinDeltaAccel += climbThisTick; + } + } + else + { + MinDeltaAccel = Vec(stillnessAccelDelta); + } + + //printf("Deltas: %.4f %.4f %.4f; %.4f %.4f %.4f\n", + // gyroDelta.x, gyroDelta.y, gyroDelta.z, + // accelDelta.x, accelDelta.y, accelDelta.z); + + if (MinMaxWindow.NumSamples >= minStillnessSamples && MinMaxWindow.TimeSampled >= minStillnessCollectionTime) + { + MinDeltaGyro = MinDeltaGyro.Min(gyroDelta); + MinDeltaAccel = MinDeltaAccel.Min(accelDelta); + } + else + { + RecalibrateThreshold = std::min(RecalibrateThreshold + stillnessErrorClimbRate * deltaTime, maxStillnessError); + return false; + } + + // check that all inputs are below appropriate thresholds to be considered "still" + if (gyroDelta.x <= MinDeltaGyro.x * RecalibrateThreshold && + gyroDelta.y <= MinDeltaGyro.y * RecalibrateThreshold && + gyroDelta.z <= MinDeltaGyro.z * RecalibrateThreshold && + accelDelta.x <= MinDeltaAccel.x * RecalibrateThreshold && + accelDelta.y <= MinDeltaAccel.y * RecalibrateThreshold && + accelDelta.z <= MinDeltaAccel.z * RecalibrateThreshold) + { + if (MinMaxWindow.NumSamples >= minStillnessSamples && MinMaxWindow.TimeSampled >= minStillnessCorrectionTime) + { + /*if (TimeSteadyStillness == 0.f) + { + printf("Still!\n"); + }/**/ + + TimeSteadyStillness = std::min(TimeSteadyStillness + deltaTime, stillnessCalibrationEaseInTime); + const float calibrationEaseIn = stillnessCalibrationEaseInTime <= 0.f ? 1.f : TimeSteadyStillness / stillnessCalibrationEaseInTime; + + const Vec calibratedGyro = MinMaxWindow.GetMidGyro(); + + const Vec oldGyroBias = Vec(CalibrationData->X, CalibrationData->Y, CalibrationData->Z) / std::max((float)CalibrationData->NumSamples, 1.f); + const float stillnessLerpFactor = stillnessCalibrationHalfTime <= 0.f ? 0.f : exp2f(-calibrationEaseIn * deltaTime / stillnessCalibrationHalfTime); + Vec newGyroBias = calibratedGyro.Lerp(oldGyroBias, stillnessLerpFactor); + Confidence = std::min(Confidence + deltaTime * stillnessConfidenceRate, 1.f); + isSteady = true; + + if (doSensorFusion) + { + const Vec previousNormal = MinMaxWindow.StartAccel.Normalized(); + const Vec thisNormal = inAccel.Normalized(); + Vec angularVelocity = thisNormal.Cross(previousNormal); + const float crossLength = angularVelocity.Length(); + if (crossLength > 0.f) + { + const float thisDotPrev = std::clamp(thisNormal.Dot(previousNormal), -1.f, 1.f); + const float angleChange = acosf(thisDotPrev) * 180.0f / (float)M_PI; + const float anglePerSecond = angleChange / MinMaxWindow.TimeSampled; + angularVelocity *= anglePerSecond / crossLength; + } + + Vec axisCalibrationStrength = thisNormal.Abs(); + Vec sensorFusionBias = (calibratedGyro - angularVelocity).Lerp(oldGyroBias, stillnessLerpFactor); + if (axisCalibrationStrength.x <= 0.7f) + { + newGyroBias.x = sensorFusionBias.x; + } + if (axisCalibrationStrength.y <= 0.7f) + { + newGyroBias.y = sensorFusionBias.y; + } + if (axisCalibrationStrength.z <= 0.7f) + { + newGyroBias.z = sensorFusionBias.z; + } + } + + CalibrationData->X = newGyroBias.x; + CalibrationData->Y = newGyroBias.y; + CalibrationData->Z = newGyroBias.z; + + CalibrationData->AccelMagnitude = MinMaxWindow.MeanAccel.Length(); + CalibrationData->NumSamples = 1; + + calibrated = true; + } + else + { + RecalibrateThreshold = std::min(RecalibrateThreshold + stillnessErrorClimbRate * deltaTime, maxStillnessError); + } + } + else if (TimeSteadyStillness > 0.f) + { + //printf("Moved!\n"); + RecalibrateThreshold -= stillnessErrorDropOnRecalibrate; + if (RecalibrateThreshold < 1.f) RecalibrateThreshold = 1.f; + + TimeSteadyStillness = 0.f; + MinMaxWindow.Reset(0.f); + } + else + { + RecalibrateThreshold = std::min(RecalibrateThreshold + stillnessErrorClimbRate * deltaTime, maxStillnessError); + MinMaxWindow.Reset(0.f); + } + + bIsSteady = isSteady; + return calibrated; + } + + inline void AutoCalibration::NoSampleStillness() + { + MinMaxWindow.Reset(0.f); + } + + inline bool AutoCalibration::AddSampleSensorFusion(const Vec& inGyro, const Vec& inAccel, float deltaTime) + { + if (deltaTime <= 0.f) + { + return false; + } + + if (inGyro.x == 0.f && inGyro.y == 0.f && inGyro.z == 0.f && + inAccel.x == 0.f && inAccel.y == 0.f && inAccel.z == 0.f) + { + // all zeroes are almost certainly not valid inputs + TimeSteadySensorFusion = 0.f; + SensorFusionSkippedTime = 0.f; + PreviousAccel = inAccel; + SmoothedPreviousAccel = inAccel; + SmoothedAngularVelocityGyro = GamepadMotionHelpers::Vec(); + SmoothedAngularVelocityAccel = GamepadMotionHelpers::Vec(); + return false; + } + + if (PreviousAccel.x == 0.f && PreviousAccel.y == 0.f && PreviousAccel.z == 0.f) + { + TimeSteadySensorFusion = 0.f; + SensorFusionSkippedTime = 0.f; + PreviousAccel = inAccel; + SmoothedPreviousAccel = inAccel; + SmoothedAngularVelocityGyro = GamepadMotionHelpers::Vec(); + SmoothedAngularVelocityAccel = GamepadMotionHelpers::Vec(); + return false; + } + + // in case the controller state hasn't updated between samples + if (inAccel.x == PreviousAccel.x && inAccel.y == PreviousAccel.y && inAccel.z == PreviousAccel.z) + { + SensorFusionSkippedTime += deltaTime; + return false; + } + + if (!Settings) + { + return false; + } + + // get settings + const float sensorFusionCalibrationSmoothingStrength = Settings->SensorFusionCalibrationSmoothingStrength; + const float sensorFusionAngularAccelerationThreshold = Settings->SensorFusionAngularAccelerationThreshold; + const float sensorFusionCalibrationEaseInTime = Settings->SensorFusionCalibrationEaseInTime; + const float sensorFusionCalibrationHalfTime = Settings->SensorFusionCalibrationHalfTime * Confidence; + const float sensorFusionConfidenceRate = Settings->SensorFusionConfidenceRate; + + deltaTime += SensorFusionSkippedTime; + SensorFusionSkippedTime = 0.f; + bool calibrated = false; + bool isSteady = false; + + // framerate independent lerp smoothing: https://www.gamasutra.com/blogs/ScottLembcke/20180404/316046/Improved_Lerp_Smoothing.php + const float smoothingLerpFactor = exp2f(-sensorFusionCalibrationSmoothingStrength * deltaTime); + // velocity from smoothed accel matches better if we also smooth gyro + const Vec previousGyro = SmoothedAngularVelocityGyro; + SmoothedAngularVelocityGyro = inGyro.Lerp(SmoothedAngularVelocityGyro, smoothingLerpFactor); // smooth what remains + const float gyroAccelerationMag = (SmoothedAngularVelocityGyro - previousGyro).Length() / deltaTime; + // get angle between old and new accel + const Vec previousNormal = SmoothedPreviousAccel.Normalized(); + const Vec thisAccel = inAccel.Lerp(SmoothedPreviousAccel, smoothingLerpFactor); + const Vec thisNormal = thisAccel.Normalized(); + Vec angularVelocity = thisNormal.Cross(previousNormal); + const float crossLength = angularVelocity.Length(); + if (crossLength > 0.f) + { + const float thisDotPrev = std::clamp(thisNormal.Dot(previousNormal), -1.f, 1.f); + const float angleChange = acosf(thisDotPrev) * 180.0f / (float)M_PI; + const float anglePerSecond = angleChange / deltaTime; + angularVelocity *= anglePerSecond / crossLength; + } + SmoothedAngularVelocityAccel = angularVelocity; + + // apply corrections + if (gyroAccelerationMag > sensorFusionAngularAccelerationThreshold || CalibrationData == nullptr) + { + /*if (TimeSteadySensorFusion > 0.f) + { + printf("Shaken!\n"); + }/**/ + TimeSteadySensorFusion = 0.f; + //printf("No calibration due to acceleration of %.4f\n", gyroAccelerationMag); + } + else + { + /*if (TimeSteadySensorFusion == 0.f) + { + printf("Steady!\n"); + }/**/ + + TimeSteadySensorFusion = std::min(TimeSteadySensorFusion + deltaTime, sensorFusionCalibrationEaseInTime); + const float calibrationEaseIn = sensorFusionCalibrationEaseInTime <= 0.f ? 1.f : TimeSteadySensorFusion / sensorFusionCalibrationEaseInTime; + const Vec oldGyroBias = Vec(CalibrationData->X, CalibrationData->Y, CalibrationData->Z) / std::max((float)CalibrationData->NumSamples, 1.f); + // recalibrate over time proportional to the difference between the calculated bias and the current assumed bias + const float sensorFusionLerpFactor = sensorFusionCalibrationHalfTime <= 0.f ? 0.f : exp2f(-calibrationEaseIn * deltaTime / sensorFusionCalibrationHalfTime); + Vec newGyroBias = (SmoothedAngularVelocityGyro - SmoothedAngularVelocityAccel).Lerp(oldGyroBias, sensorFusionLerpFactor); + Confidence = std::min(Confidence + deltaTime * sensorFusionConfidenceRate, 1.f); + isSteady = true; + // don't change bias in axes that can't be affected by the gravity direction + Vec axisCalibrationStrength = thisNormal.Abs(); + if (axisCalibrationStrength.x > 0.7f) + { + axisCalibrationStrength.x = 1.f; + } + if (axisCalibrationStrength.y > 0.7f) + { + axisCalibrationStrength.y = 1.f; + } + if (axisCalibrationStrength.z > 0.7f) + { + axisCalibrationStrength.z = 1.f; + } + newGyroBias = newGyroBias.Lerp(oldGyroBias, axisCalibrationStrength.Min(Vec(1.f))); + + CalibrationData->X = newGyroBias.x; + CalibrationData->Y = newGyroBias.y; + CalibrationData->Z = newGyroBias.z; + + CalibrationData->AccelMagnitude = thisAccel.Length(); + + CalibrationData->NumSamples = 1; + + calibrated = true; + + //printf("Recalibrating at a strength of %.4f\n", calibrationEaseIn); + } + + SmoothedPreviousAccel = thisAccel; + PreviousAccel = inAccel; + + //printf("Gyro: %.4f, %.4f, %.4f | Accel: %.4f, %.4f, %.4f\n", + // SmoothedAngularVelocityGyro.x, SmoothedAngularVelocityGyro.y, SmoothedAngularVelocityGyro.z, + // SmoothedAngularVelocityAccel.x, SmoothedAngularVelocityAccel.y, SmoothedAngularVelocityAccel.z); + + bIsSteady = isSteady; + + return calibrated; + } + + inline void AutoCalibration::NoSampleSensorFusion() + { + TimeSteadySensorFusion = 0.f; + SensorFusionSkippedTime = 0.f; + PreviousAccel = GamepadMotionHelpers::Vec(); + SmoothedPreviousAccel = GamepadMotionHelpers::Vec(); + SmoothedAngularVelocityGyro = GamepadMotionHelpers::Vec(); + SmoothedAngularVelocityAccel = GamepadMotionHelpers::Vec(); + } + + inline void AutoCalibration::SetCalibrationData(GyroCalibration* calibrationData) + { + CalibrationData = calibrationData; + } + + inline void AutoCalibration::SetSettings(GamepadMotionSettings* settings) + { + Settings = settings; + } + +} // namespace GamepadMotionHelpers + +inline GamepadMotion::GamepadMotion() +{ + IsCalibrating = false; + CurrentCalibrationMode = GamepadMotionHelpers::CalibrationMode::Manual; + Reset(); + AutoCalibration.SetCalibrationData(&GyroCalibration); + AutoCalibration.SetSettings(&Settings); + Motion.SetSettings(&Settings); +} + +inline void GamepadMotion::Reset() +{ + GyroCalibration = {}; + Gyro = {}; + RawAccel = {}; + Settings = GamepadMotionSettings(); + Motion.Reset(); +} + +inline void GamepadMotion::ProcessMotion(float gyroX, float gyroY, float gyroZ, + float accelX, float accelY, float accelZ, float deltaTime) +{ + if (gyroX == 0.f && gyroY == 0.f && gyroZ == 0.f && + accelX == 0.f && accelY == 0.f && accelZ == 0.f) + { + // all zeroes are almost certainly not valid inputs + return; + } + + float accelMagnitude = sqrtf(accelX * accelX + accelY * accelY + accelZ * accelZ); + + if (IsCalibrating) + { + // manual calibration + PushSensorSamples(gyroX, gyroY, gyroZ, accelMagnitude); + AutoCalibration.NoSampleSensorFusion(); + AutoCalibration.NoSampleStillness(); + } + else if (CurrentCalibrationMode & GamepadMotionHelpers::CalibrationMode::Stillness) + { + AutoCalibration.AddSampleStillness(GamepadMotionHelpers::Vec(gyroX, gyroY, gyroZ), GamepadMotionHelpers::Vec(accelX, accelY, accelZ), deltaTime, CurrentCalibrationMode & GamepadMotionHelpers::CalibrationMode::SensorFusion); + AutoCalibration.NoSampleSensorFusion(); + } + else + { + AutoCalibration.NoSampleStillness(); + if (CurrentCalibrationMode & GamepadMotionHelpers::CalibrationMode::SensorFusion) + { + AutoCalibration.AddSampleSensorFusion(GamepadMotionHelpers::Vec(gyroX, gyroY, gyroZ), GamepadMotionHelpers::Vec(accelX, accelY, accelZ), deltaTime); + } + else + { + AutoCalibration.NoSampleSensorFusion(); + } + } + + float gyroOffsetX, gyroOffsetY, gyroOffsetZ; + GetCalibratedSensor(gyroOffsetX, gyroOffsetY, gyroOffsetZ, accelMagnitude); + + gyroX -= gyroOffsetX; + gyroY -= gyroOffsetY; + gyroZ -= gyroOffsetZ; + + Motion.Update(gyroX, gyroY, gyroZ, accelX, accelY, accelZ, accelMagnitude, deltaTime); + + Gyro.x = gyroX; + Gyro.y = gyroY; + Gyro.z = gyroZ; + RawAccel.x = accelX; + RawAccel.y = accelY; + RawAccel.z = accelZ; +} + +// reading the current state +inline void GamepadMotion::GetCalibratedGyro(float& x, float& y, float& z) +{ + x = Gyro.x; + y = Gyro.y; + z = Gyro.z; +} + +inline void GamepadMotion::GetGravity(float& x, float& y, float& z) +{ + x = Motion.Grav.x; + y = Motion.Grav.y; + z = Motion.Grav.z; +} + +inline void GamepadMotion::GetProcessedAcceleration(float& x, float& y, float& z) +{ + x = Motion.Accel.x; + y = Motion.Accel.y; + z = Motion.Accel.z; +} + +inline void GamepadMotion::GetOrientation(float& w, float& x, float& y, float& z) +{ + w = Motion.Quaternion.w; + x = Motion.Quaternion.x; + y = Motion.Quaternion.y; + z = Motion.Quaternion.z; +} + +inline void GamepadMotion::GetPlayerSpaceGyro(float& x, float& y, const float yawRelaxFactor) +{ + CalculatePlayerSpaceGyro(x, y, Gyro.x, Gyro.y, Gyro.z, Motion.Grav.x, Motion.Grav.y, Motion.Grav.z, yawRelaxFactor); +} + +inline void GamepadMotion::CalculatePlayerSpaceGyro(float& x, float& y, const float gyroX, const float gyroY, const float gyroZ, const float gravX, const float gravY, const float gravZ, const float yawRelaxFactor) +{ + // take gravity into account without taking on any error from gravity. Explained in depth at http://gyrowiki.jibbsmart.com/blog:player-space-gyro-and-alternatives-explained#toc7 + const float worldYaw = -(gravY * gyroY + gravZ * gyroZ); + const float worldYawSign = worldYaw < 0.f ? -1.f : 1.f; + y = worldYawSign * std::min(std::abs(worldYaw) * yawRelaxFactor, sqrtf(gyroY * gyroY + gyroZ * gyroZ)); + x = gyroX; +} + +inline void GamepadMotion::GetWorldSpaceGyro(float& x, float& y, const float sideReductionThreshold) +{ + CalculateWorldSpaceGyro(x, y, Gyro.x, Gyro.y, Gyro.z, Motion.Grav.x, Motion.Grav.y, Motion.Grav.z, sideReductionThreshold); +} + +inline void GamepadMotion::CalculateWorldSpaceGyro(float& x, float& y, const float gyroX, const float gyroY, const float gyroZ, const float gravX, const float gravY, const float gravZ, const float sideReductionThreshold) +{ + // use the gravity direction as the yaw axis, and derive an appropriate pitch axis. Explained in depth at http://gyrowiki.jibbsmart.com/blog:player-space-gyro-and-alternatives-explained#toc6 + const float worldYaw = -gravX * gyroX - gravY * gyroY - gravZ * gyroZ; + // project local pitch axis (X) onto gravity plane + const float gravDotPitchAxis = gravX; + GamepadMotionHelpers::Vec pitchAxis(1.f - gravX * gravDotPitchAxis, + -gravY * gravDotPitchAxis, + -gravZ * gravDotPitchAxis); + // normalize + const float pitchAxisLengthSquared = pitchAxis.LengthSquared(); + if (pitchAxisLengthSquared > 0.f) + { + const float pitchAxisLength = sqrtf(pitchAxisLengthSquared); + const float lengthReciprocal = 1.f / pitchAxisLength; + pitchAxis *= lengthReciprocal; + + const float flatness = std::abs(gravY); + const float upness = std::abs(gravZ); + const float sideReduction = sideReductionThreshold <= 0.f ? 1.f : std::clamp((std::max(flatness, upness) - sideReductionThreshold) / sideReductionThreshold, 0.f, 1.f); + + x = sideReduction * pitchAxis.Dot(GamepadMotionHelpers::Vec(gyroX, gyroY, gyroZ)); + } + else + { + x = 0.f; + } + + y = worldYaw; +} + +// gyro calibration functions +inline void GamepadMotion::StartContinuousCalibration() +{ + IsCalibrating = true; +} + +inline void GamepadMotion::PauseContinuousCalibration() +{ + IsCalibrating = false; +} + +inline void GamepadMotion::ResetContinuousCalibration() +{ + GyroCalibration = {}; + AutoCalibration.Reset(); +} + +inline void GamepadMotion::GetCalibrationOffset(float& xOffset, float& yOffset, float& zOffset) +{ + float accelMagnitude; + GetCalibratedSensor(xOffset, yOffset, zOffset, accelMagnitude); +} + +inline void GamepadMotion::SetCalibrationOffset(float xOffset, float yOffset, float zOffset, int weight) +{ + if (GyroCalibration.NumSamples > 1) + { + GyroCalibration.AccelMagnitude *= ((float)weight) / GyroCalibration.NumSamples; + } + else + { + GyroCalibration.AccelMagnitude = (float)weight; + } + + GyroCalibration.NumSamples = weight; + GyroCalibration.X = xOffset * weight; + GyroCalibration.Y = yOffset * weight; + GyroCalibration.Z = zOffset * weight; +} + +inline float GamepadMotion::GetAutoCalibrationConfidence() +{ + return AutoCalibration.Confidence; +} + +inline void GamepadMotion::SetAutoCalibrationConfidence(float newConfidence) +{ + AutoCalibration.Confidence = newConfidence; +} + +inline bool GamepadMotion::GetAutoCalibrationIsSteady() +{ + return AutoCalibration.IsSteady(); +} + +inline GamepadMotionHelpers::CalibrationMode GamepadMotion::GetCalibrationMode() +{ + return CurrentCalibrationMode; +} + +inline void GamepadMotion::SetCalibrationMode(GamepadMotionHelpers::CalibrationMode calibrationMode) +{ + CurrentCalibrationMode = calibrationMode; +} + +inline void GamepadMotion::ResetMotion() +{ + Motion.Reset(); +} + +// Private Methods + +inline void GamepadMotion::PushSensorSamples(float gyroX, float gyroY, float gyroZ, float accelMagnitude) +{ + // accumulate + GyroCalibration.NumSamples++; + GyroCalibration.X += gyroX; + GyroCalibration.Y += gyroY; + GyroCalibration.Z += gyroZ; + GyroCalibration.AccelMagnitude += accelMagnitude; +} + +inline void GamepadMotion::GetCalibratedSensor(float& gyroOffsetX, float& gyroOffsetY, float& gyroOffsetZ, float& accelMagnitude) +{ + if (GyroCalibration.NumSamples <= 0) + { + gyroOffsetX = 0.f; + gyroOffsetY = 0.f; + gyroOffsetZ = 0.f; + accelMagnitude = 1.f; + return; + } + + const float inverseSamples = 1.f / GyroCalibration.NumSamples; + gyroOffsetX = GyroCalibration.X * inverseSamples; + gyroOffsetY = GyroCalibration.Y * inverseSamples; + gyroOffsetZ = GyroCalibration.Z * inverseSamples; + accelMagnitude = GyroCalibration.AccelMagnitude * inverseSamples; +} diff --git a/lib/GamepadMotionHelpers/LICENSE b/lib/GamepadMotionHelpers/LICENSE new file mode 100644 index 0000000..a46396a --- /dev/null +++ b/lib/GamepadMotionHelpers/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2020-2023 Julian "Jibb" Smart + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/lib/GamepadMotionHelpers/README.md b/lib/GamepadMotionHelpers/README.md new file mode 100644 index 0000000..c5c1bc9 --- /dev/null +++ b/lib/GamepadMotionHelpers/README.md @@ -0,0 +1,76 @@ +# GamepadMotionHelpers +GamepadMotionHelpers is a lightweight header-only library for sensor fusion, gyro calibration, etc. BYO input library (eg [SDL2](https://github.com/libsdl-org/SDL)). + +## Units +Convert your gyro units into **degrees per second** and accelerometer units to **g-force** (1 g = 9.8 m/s^2). You don't have to use these units in your application, but convert to these units when writing to GamepadMotionHelpers and convert back when reading from it. Your input reader might prefer radians per second and metres per second squared, but the datasheets for every IMU I've seen talk about degrees per second and g-force. + +## Coordinate Space +This library uses a Y-up coordinate system. While Z-up is (only slightly) preferable for many games, PlayStation controllers use Y-up, and have set the standard for input libraries like [SDL2](https://github.com/libsdl-org/SDL) and [JSL](https://github.com/JibbSmart/JoyShockLibrary). These libraries convert inputs from other controller types to the same space used by PlayStation's DualShock 4 and DualSense, so that's what's used here. + +## Basic Use +Include the GamepadMotion.hpp file in your C++ project. That's it! Everything you need is in that file, and its only dependency is ``````. + +For each controller with gyro (and optionally accelerometer), create a ```GamepadMotion``` object. At regular intervals, whether when a new report comes in from the controller or when polling the controller's state, you should call ```ProcessMotion(...)```. This is when you tell your GamepadMotion object the latest gyro (in degrees per second) and accelerometer (in g-force) inputs. You'll also give it the time since the last update for this controller (in seconds). + +ProcessMotion takes these inputs, updates some internal values, and then you can use any of the following to read its current state: +- ```GetCalibratedGyro(float& x, float& y, float& z)``` - Get the controller's angular velocity in degrees per second. This is just the raw gyro you gave it minus the gyro's bias as determined by your calibration settings (more on that below). +- ```GetGravity(float& x, float& y, float& z)``` - Get the gravity direction in the controller's local space. When the controller is still on a flat surface it'll be approximately (0, -1, 0). The controller can't detect the gravity direction when it's in freefall or being shaken around, but it can make a pretty good guess if its gyro is correctly calibrated and then make further corrections when the controller is still again. +- ```GetProcessedAcceleration(float& x, float& y, float& z)``` - Get the controller's current acceleration in g-force with gravity removed. Raw accelerometer input includes gravity -- it is only (0, 0, 0) when the controller is in freefall. However, using the gravity direction as calculated for GetGravity, it can remove that component and detect how you're shaking the controller about. This function gives you that acceleration vector with the gravity removed. +- ```GetOrientation(float& w, float& x, float& y, float& z)``` - Get the controller's orientation. Gyro and accelerometer input are combined to give a good estimate of the controller's orientation. + +Additional helper functions are available for taking gravity into account and returning a "world space" or "player space" rotation in two axes. Bear in mind that the **X** and **Y** set by these functions is still around the controller's axes. This means **Y** is the *horizontal* part of the rotation, and **X** is the vertical part. To convert to a mouse-like input, you'll treat the **Y** as the horizontal or yaw input and **X** as the vertical or pitch input. This might be unintuitive, but since it's also true of the "local space" angular velocities obtained from GetCalibratedGyro, this makes it simple to let the user choose between *local space*, *world space*, and *player space* in your game or application by just swapping GetCalibratedGyro for these functions depending on that selection: +- ```GetWorldSpaceGyro(float& x, float& y, const float sideReductionThreshold = 0.125f)``` - Get the controller's angular velocity in *world space* as described on GyroWiki in the [player space article here](http://gyrowiki.jibbsmart.com/blog:player-space-gyro-and-alternatives-explained#toc6). Yaw input will be derived from motion around the gravity axis, and pitch input from an appropriate pitch axis calculated from the controller's orientation with respect to the gravity axis. Any errors in the calculated gravity axis (though likely very small) will be taken on by the calculated world space gyro rotation, making it slightly less robust than using calibrated gyro directly ("local space" gyro) or using *player space* gyro below. More info in the linked article. +- ```GetPlayerSpaceGyro(float& x, float& y, const float yawRelaxFactor = 1.41f)``` - Get the controller's angular velocity in *player space* as described on GyroWiki in the [player space article here](http://gyrowiki.jibbsmart.com/blog:player-space-gyro-and-alternatives-explained#toc7). Yaw input will be derived from motion approximately around the gravity axis, without any impact from errors in the gravity calculation. Pitch is just local pitch. It is robust, accommodates players who are used to both local space and world space gyro, while taking on most of the advantages of each. It is proven in popular games and is an ideal default for players using a standalone controller. For handheld (where the screen is part of the controller, such as mobile, Switch, or Steam Deck) local space (using the calibrated gyro input directly) may be preferable. More info in the linked article. + +If you want to plug in the gyro and gravity values yourself (perhaps you're using an externally calculated gravity), you can use ```CalculateWorldSpaceGyro``` and ```CalculatePlayerSpaceGyro``` instead. Make sure you use this GamepadMotionHelpers' coordinate space, units, and gravity is normalized, since those are all assumed for these functions. + +## Sensor Fusion +Combining multiple types of sensor like this to get a better picture of the controller's state is called "sensor fusion". Moment-to-moment changes in orientation are detected using the gyro, but that only gives local angular velocity and needs to be correctly calibrated. Errors can accumulate over time. The gravity vector as detected by the accelerometer is used to make corrections to the relevant components of the controller's orientation. + +But this cannot be used to correct the controller's orientation around the gravity vector (the **yaw** axis). If you're using the controller's absolute orientation for some reason, this "yaw drift" may need to be accounted for somehow. Some devices also have a magnetometer (compass) to counter yaw drift, but since popular game controllers don't have a magnetometer, I haven't tried it myself. In future, if I get such a device, I'd like to add the option for GamepadMotionHelpers to accept magnetometer input and account for it when calculating values for the above functions. + +## Gyro Calibration +Modern gyroscopes often need calibration. This is like how a [weighing scale](https://en.wikipedia.org/wiki/Weighing_scale) can need calibration to tell it what 'zero' is. Like a weighing scale, a correctly calibrated gyroscope will give an accurate reading. If you're using the gyro input as a mouse, which is the simplest application of a controller's gyro, you can find essential reading on [GyroWiki here](http://gyrowiki.jibbsmart.com/blog:good-gyro-controls-part-1:the-gyro-is-a-mouse). + +Calibration just means having the controller sit still and remembering the average reported angular velocity in each axis. This is the gyro's "bias". In GamepadMotionHelpers, I call our best guess at the controller's bias the "calibration offset". GamepadMotionHelpers has some options to help with calibrating: + +At any time, you can begin manually calibrating a controller by calling ```StartContinuousCalibration()```. This will start recording the average angular velocity and apply it immediately to any subsequent **GetGalibratedGyro(...)** call. At any time you can ```PauseContinuousCalibration()``` to no longer add current values to the average angular velocity being recorded. You can ```ResetContinousCalibration()``` to remove the recorded average before starting over with **StartContinuousCalibration** again. + +You can read the stored calibration values using ```GetCalibrationOffset(float& xOffset, float& yOffset, float& zOffset)```. You can manually set the calibration offset yourself with ```SetCalibrationOffset(float xOffset, float yOffset, float zOffset, int weight)```. This will override all stored values. The **weight** argument at the end determines how strongly these values should be considered over time if Continuous Calibration is still active (new values are still being added to the average). Each new sample has a weight of 1, so if you **SetCalibrationOffset** with a weight of 10, it'll have the weight of 10 samples when calculating the average. If you're not continuing to add samples (Continuous Calibration is not active), the weight will be meaningless. Setting this manually is unusual, so don't worry about it too much if that sounds complicated. + +Most games don't ask the user to calibrate the gyro themselves. They have built-in automatic calibration, which I like to call "auto-calibration". There's no such thing as a "good enough" auto-calibration solution -- at least not with only gyro and accelerometer. Every game that has an auto-calibration solution would be made better for more serious players with the option to manually calibrate their gyro, so I urge you to provide players the option to do the same in your game. Having said that, auto-calibration is a useful option for casual players, and you may choose to have it enabled in your game by default. + +So GamepadMotionHelpers provides some auto-calibration options. You can call ```SetCalibrationMode(CalibrationMode)``` on each GamepadMotion instance with the following options: +- ```CalibrationMode::Manual``` - No auto-calibration. This is the default. +- ```CalibrationMode::Stillness``` - Automatically try to detect when the controller is being held still and update the calibration offset accordingly. +- ```CalibrationMode::SensorFusion``` - Calculate an angular velocity from changes in the gravity direction as detected by the accelerometer. If these are steady enough, use them to make corrections to the calibration offset. This will only apply to relevant axes. + +Many players are already aware of the shortcomings of trying to automatically detect stillness to automatically calibrate the gyro. Whether on Switch, PlayStation, or using PlayStation controllers on PC, players have tried to track a slow or distant target only to have the aimer suddenly stop moving! The game or the platform has **misinterpreted their slow and steady input as the controller being held still**, and they've incorrectly recalibrated accordingly. Players *hate it* when this happens. + +**This is why it's important to let players manually calibrate their gyro** if they want to. + +Auto-calibration is used so widely in console games that it's speculated that game developers may not have the option to disable it on these platforms. If this is the case, GamepadMotionHelpers offers a big advantage over those platforms: you can disable it and enable it at any time. + +You, the game developer, can have your game tell if the player is tracking a distant or slow-moving target. You can tell if the player's aimer is moving towards a visible target or roughly following the movement of one. When it is, maybe disabling the auto-calibration (```SetCalibrationMode(CalibrationMode::Manual)```) could be the difference between good and bad auto-calibration. I don't know if the GamepadMotionHelpers auto-calibration functions are better or worse than their Switch and PlayStation counterparts generally, but by letting you take the game's context into account, you may be able to offer players a way better experience without them having to manually calibrate. + +But still give them the option to calibrate manually, please :) + +The **SensorFusion** calibration mode has shortcomings of its own. It's much harder to accidentally trick the game into incorrectly calibrating, but the angular velocity calculated from the accelerometer moment-to-moment is generally much less precise. Leaving the controller still, you'll notice the calibrated gyro moving slightly up and down over time. So while the **Stillness** mode is characterised by good behaviour occasionally punctuated by frustrating errors, the **SensorFusion** mode will tend to be more consistently not-quite-right without being terrible. + +Secondly, this library currently only combines accelerometer and gyro, so the **SensorFusion** auto-calibration cannot correct the gyro in all axes at the same time. The **SensorFusion**-only mode will be more useful in future when magnetometer input is supported, which can account for the axes that the accelerometer can't. + +Both auto-calibration modes can be combined by passing ```CalibrationMode::Stillness | CalibrationMode::SensorFusion``` to **SetCalibrationMode**. In this case, it'll use **Stillness** auto-calibration, but it'll adjust the calibration offset based on any angular velocity implied by changes in the accelerometer input. This tends to give better results than just using **Stillness** or **SensorFusion** on their own. + +If you aren't sure what to choose, I'd suggest using the combined ```CalibrationMode::Stillness | CalibrationMode::SensorFusion``` when auto calibration is enabled, but also allowing the player to manually calibrate. + +**TODO** This is a clunky way to let the user set up what is obviously the best solution. Maybe I should just call it "hybrid" or something and be done with it? + +Auto-calibration can also be used to communicate manual calibration to the player. ```GetAutoCalibrationIsSteady()``` will tell you whether GamepadMotionHelpers thinks the controller is currently being held steady (if auto-calibration is enabled). ```GetAutoCalibrationConfidence()``` will tell you how confident GamepadMotionHelpers is that it has a good calibration value from auto-calibration, from 0-1. Higher confidence means that new calibration changes will be applied more gradually. You can use these functions to detect when a controller needs to be calibrated, prompt the player to put their controller down, detect when they have put their controller down, and show progress for calibration (default 1 second once it starts). You can also override the confidence yourself (```SetAutoCalibrationConfidence()```), and resetting calibration will reset confidence to 0. How quickly confidence grows as well as other calibration settings can be customised in **GamepadMotionSettings**. + +## In the Wild +GamepadMotionHelpers is currently used in: +- [JoyShockMapper](https://github.com/Electronicks/JoyShockMapper) +- [JoyShockLibrary](https://github.com/JibbSmart/JoyShockLibrary) +- JoyShockOverlay + +If you know of any other games or applications using GamepadMotionHelpers, please let me know! \ No newline at end of file diff --git a/patches/input.c b/patches/input.c index eccf601..b1e0b0e 100644 --- a/patches/input.c +++ b/patches/input.c @@ -1,5 +1,54 @@ #include "patches.h" #include "input.h" + + +s32 func_80847190(PlayState* play, Player* this, s32 arg2); +s16 func_80832754(Player* this, s32 arg1); +s32 func_8082EF20(Player* this); + +// Patched to add gyro aiming +s32 func_80847190(PlayState* play, Player* this, s32 arg2) { + s32 pad; + s16 var_s0; + + if (!func_800B7128(this) && !func_8082EF20(this) && !arg2) { + var_s0 = play->state.input[0].rel.stick_y * 0xF0; + Math_SmoothStepToS(&this->actor.focus.rot.x, var_s0, 0xE, 0xFA0, 0x1E); + + var_s0 = play->state.input[0].rel.stick_x * -0x10; + var_s0 = CLAMP(var_s0, -0xBB8, 0xBB8); + this->actor.focus.rot.y += var_s0; + } + else { + float gyro_x, gyro_y; + recomp_get_gyro_deltas(&gyro_x, &gyro_y); + + s16 temp3; + + temp3 = ((play->state.input[0].rel.stick_y >= 0) ? 1 : -1) * + (s32)((1.0f - Math_CosS(play->state.input[0].rel.stick_y * 0xC8)) * 1500.0f); + this->actor.focus.rot.x += temp3 + (s32)(gyro_x * -3.0f); + + if (this->stateFlags1 & PLAYER_STATE1_800000) { + this->actor.focus.rot.x = CLAMP(this->actor.focus.rot.x, -0x1F40, 0xFA0); + } + else { + this->actor.focus.rot.x = CLAMP(this->actor.focus.rot.x, -0x36B0, 0x36B0); + } + + var_s0 = this->actor.focus.rot.y - this->actor.shape.rot.y; + temp3 = ((play->state.input[0].rel.stick_x >= 0) ? 1 : -1) * + (s32)((1.0f - Math_CosS(play->state.input[0].rel.stick_x * 0xC8)) * -1500.0f); + var_s0 += temp3 + (s32)(gyro_y * 3.0f); + + this->actor.focus.rot.y = CLAMP(var_s0, -0x4AAA, 0x4AAA) + this->actor.shape.rot.y; + } + + this->unk_AA6 |= 2; + + return func_80832754(this, (play->unk_1887C != 0) || func_800B7128(this) || func_8082EF20(this)); +} + #if 0 u32 sPlayerItemButtons[] = { BTN_B, diff --git a/patches/input.h b/patches/input.h index b9b88ed..7b5c37a 100644 --- a/patches/input.h +++ b/patches/input.h @@ -26,6 +26,7 @@ extern "C" { void name(uint8_t* rdram, recomp_context* ctx); #endif +DECLARE_FUNC(void, recomp_get_gyro_deltas, float* x, float* y); // TODO move these DECLARE_FUNC(void, recomp_puts, const char* data, u32 size); DECLARE_FUNC(void, recomp_exit); diff --git a/patches/syms.ld b/patches/syms.ld index eff8c57..b964e73 100644 --- a/patches/syms.ld +++ b/patches/syms.ld @@ -7,3 +7,4 @@ recomp_handle_quicksave_actions = 0x81000008; recomp_handle_quicksave_actions_main = 0x8100000C; osRecvMesg_recomp = 0x81000010; osSendMesg_recomp = 0x81000014; +recomp_get_gyro_deltas = 0x81000018; diff --git a/src/game/controls.cpp b/src/game/controls.cpp index 265310a..4bc0b99 100644 --- a/src/game/controls.cpp +++ b/src/game/controls.cpp @@ -120,3 +120,10 @@ extern "C" void recomp_puts(uint8_t* rdram, recomp_context* ctx) { extern "C" void recomp_exit(uint8_t* rdram, recomp_context* ctx) { ultramodern::quit(); } + +extern "C" void recomp_get_gyro_deltas(uint8_t* rdram, recomp_context* ctx) { + float* x_out = _arg<0, float*>(rdram, ctx); + float* y_out = _arg<1, float*>(rdram, ctx); + + recomp::get_gyro_deltas(x_out, y_out); +} diff --git a/src/game/input.cpp b/src/game/input.cpp index d99548e..a0c990b 100644 --- a/src/game/input.cpp +++ b/src/game/input.cpp @@ -6,15 +6,30 @@ #include "recomp_ui.h" #include "SDL.h" #include "rt64_layer.h" +#include "GamepadMotion.hpp" constexpr float axis_threshold = 0.5f; +struct ControllerState { + SDL_GameController* controller; + std::array latest_accelerometer; + GamepadMotion motion; + uint32_t prev_gyro_timestamp; + ControllerState() : controller{}, latest_accelerometer{}, motion{}, prev_gyro_timestamp{} { + motion.Reset(); + motion.SetCalibrationMode(GamepadMotionHelpers::CalibrationMode::Stillness | GamepadMotionHelpers::CalibrationMode::SensorFusion); + }; +}; + static struct { const Uint8* keys = nullptr; int numkeys = 0; std::atomic_int32_t mouse_wheel_pos = 0; - std::vector controller_ids{}; std::vector cur_controllers{}; + std::unordered_map controller_states; + std::array rotation_delta{}; + std::mutex pending_rotation_mutex; + std::array pending_rotation_delta{}; } InputState; std::atomic scanning_device = recomp::InputDevice::COUNT; @@ -72,7 +87,13 @@ bool sdl_event_filter(void* userdata, SDL_Event* event) { printf("Controller added: %d\n", controller_event->which); if (controller != nullptr) { printf(" Instance ID: %d\n", SDL_JoystickInstanceID(SDL_GameControllerGetJoystick(controller))); - InputState.controller_ids.push_back(SDL_JoystickInstanceID(SDL_GameControllerGetJoystick(controller))); + ControllerState& state = InputState.controller_states[SDL_JoystickInstanceID(SDL_GameControllerGetJoystick(controller))]; + state.controller = controller; + + if (SDL_GameControllerHasSensor(controller, SDL_SensorType::SDL_SENSOR_GYRO) && SDL_GameControllerHasSensor(controller, SDL_SensorType::SDL_SENSOR_ACCEL)) { + SDL_GameControllerSetSensorEnabled(controller, SDL_SensorType::SDL_SENSOR_GYRO, SDL_TRUE); + SDL_GameControllerSetSensorEnabled(controller, SDL_SensorType::SDL_SENSOR_ACCEL, SDL_TRUE); + } } } break; @@ -80,7 +101,7 @@ bool sdl_event_filter(void* userdata, SDL_Event* event) { { SDL_ControllerDeviceEvent* controller_event = &event->cdevice; printf("Controller removed: %d\n", controller_event->which); - std::erase(InputState.controller_ids, controller_event->which); + InputState.controller_states.erase(controller_event->which); } break; case SDL_EventType::SDL_QUIT: @@ -113,6 +134,41 @@ bool sdl_event_filter(void* userdata, SDL_Event* event) { } queue_if_enabled(event); break; + case SDL_EventType::SDL_CONTROLLERSENSORUPDATE: + if (event->csensor.sensor == SDL_SensorType::SDL_SENSOR_ACCEL) { + // Convert acceleration to g's. + float x = event->csensor.data[0] / SDL_STANDARD_GRAVITY; + float y = event->csensor.data[1] / SDL_STANDARD_GRAVITY; + float z = event->csensor.data[2] / SDL_STANDARD_GRAVITY; + ControllerState& state = InputState.controller_states[event->csensor.which]; + state.latest_accelerometer[0] = x; + state.latest_accelerometer[1] = y; + state.latest_accelerometer[2] = z; + } + else if (event->csensor.sensor == SDL_SensorType::SDL_SENSOR_GYRO) { + // constexpr float gyro_threshold = 0.05f; + // Convert rotational velocity to degrees per second. + constexpr float rad_to_deg = 180.0f / M_PI; + float x = event->csensor.data[0] * rad_to_deg; + float y = event->csensor.data[1] * rad_to_deg; + float z = event->csensor.data[2] * rad_to_deg; + ControllerState& state = InputState.controller_states[event->csensor.which]; + uint64_t cur_timestamp = event->csensor.timestamp; + uint32_t delta_ms = cur_timestamp - state.prev_gyro_timestamp; + state.motion.ProcessMotion(x, y, z, state.latest_accelerometer[0], state.latest_accelerometer[1], state.latest_accelerometer[2], delta_ms * 0.001f); + state.prev_gyro_timestamp = cur_timestamp; + + float rot_x = 0.0f; + float rot_y = 0.0f; + state.motion.GetPlayerSpaceGyro(rot_x, rot_y); + + { + std::lock_guard lock{ InputState.pending_rotation_mutex }; + InputState.pending_rotation_delta[0] += rot_x; + InputState.pending_rotation_delta[1] += rot_y; + } + } + break; default: queue_if_enabled(event); break; @@ -254,13 +310,21 @@ void recomp::poll_inputs() { InputState.cur_controllers.clear(); - for (SDL_JoystickID id : InputState.controller_ids) { - SDL_GameController* controller = SDL_GameControllerFromInstanceID(id); + for (const auto& [id, state] : InputState.controller_states) { + (void)id; // Avoid unused variable warning. + SDL_GameController* controller = state.controller; if (controller != nullptr) { InputState.cur_controllers.push_back(controller); } } + // Read the deltas while resetting them to zero. + { + std::lock_guard lock{ InputState.pending_rotation_mutex }; + InputState.rotation_delta = InputState.pending_rotation_delta; + InputState.pending_rotation_delta = { 0.0f, 0.0f }; + } + // Quicksaving is disabled for now and will likely have more limited functionality // when restored, rather than allowing saving and loading at any point in time. #if 0 @@ -368,6 +432,12 @@ bool recomp::get_input_digital(const std::span fields) return ret; } +void recomp::get_gyro_deltas(float* x, float* y) { + std::array cur_rotation_delta = InputState.rotation_delta; + *x = cur_rotation_delta[0]; + *y = cur_rotation_delta[1]; +} + bool recomp::game_input_disabled() { // Disable input if any menu is open. return recomp::get_current_menu() != recomp::Menu::None; diff --git a/src/main/main.cpp b/src/main/main.cpp index 1f25c59..f35e0de 100644 --- a/src/main/main.cpp +++ b/src/main/main.cpp @@ -40,6 +40,7 @@ void exit_error(const char* str, Ts ...args) { ultramodern::gfx_callbacks_t::gfx_data_t create_gfx() { SDL_SetHint(SDL_HINT_WINDOWS_DPI_AWARENESS, "system"); + SDL_SetHint(SDL_HINT_GAMECONTROLLER_USE_BUTTON_LABELS, "0"); if (SDL_Init(SDL_INIT_VIDEO | SDL_INIT_GAMECONTROLLER) > 0) { exit_error("Failed to initialize SDL2: %s\n", SDL_GetError()); } diff --git a/ultramodern/events.cpp b/ultramodern/events.cpp index 03ea2ac..4b381f6 100644 --- a/ultramodern/events.cpp +++ b/ultramodern/events.cpp @@ -263,7 +263,7 @@ void ultramodern::set_graphics_config(const ultramodern::GraphicsConfig& config) events_context.action_queue.enqueue(UpdateConfigAction{}); } -const ultramodern::GraphicsConfig& ultramodern::get_graphics_config() { +ultramodern::GraphicsConfig ultramodern::get_graphics_config() { return cur_config; }