diff --git a/README.md b/README.md index 53022f2bd0..bbce03bc1d 100644 --- a/README.md +++ b/README.md @@ -24,6 +24,8 @@ There are minor graphics- and gameplay-related issues, and possibly occasional c **The following extra features are implemented:** * mouselook; * dual analog controller support; +* gyro aiming support + * compatible controllers only * widescreen resolution support; * configurable field of view; * 60 FPS support, including fixes for some framerate-related issues; @@ -88,8 +90,25 @@ N64 pad buttons X and Y (or `X_BUTTON`, `Y_BUTTON` in the code) refer to the res Support for one controller, two-stick configurations are enabled for 1.2. +Motion Sensor Aiming (also known as Gyro Aiming) is enabled by default while Aiming (`Crosshair only` via `Gyro Settings`). + Note that the mouse only controls player 1. +> [!NOTE] +> Gyro Aiming is only supported for controllers that has Motion Sensor functionality. (**DualShock 4**, **DualSense**, **Nintendo Switch family of controllers**) +> +> Gyro Aiming will not work if using a Input Remapper lacks controller-specific emulation (example: Steam Input), or using a controller that doesn't support Motion Sensors such as Xbox Wireless Controller. + +> [!TIP] +> Gyro Drift may become a occurence during regular gameplay, but far more frequent when using a first-party Nintendo Switch controllers. To remedy this issue: `Gyro Calibration` (under `Player [Number] Controller Options`'s `Gyro Settings`) offers various modes for you to choose from, from `While In Menus` (default), `While Stationary` to `Always`. Depending on what Calibration mode you choose from: just place the controller on the flat surface, and it will automatically calibrate gyro after a few seconds. +> +> If the Autocalibration fails or you don't want the gyro to auto-calibrate during regular gameplay: you have the ability to manually calibrate the gyro. You can head over to "`Gyro Settings...`" menu, either select `Initiate Gyro Calibration...`, pressing the `SHARE` button on your Xbox pad (Xbox Series X|S model-onwards) or `F10` key (Player 1 only) at anytime during both gameplay and menus, (it also complements Gyro Autocalibration process!) +> +> > To clarify: "SHARE" button on Xbox gamepad is a closer equivalent to `Microphone Mute button` (DualSense) or `Capture Button` (Nintendo Switch) button. (under the hood: it's actually the `MISC1` button) +> +> "`Gyro Calibration (Manual)`" can be rebinded within `pd.ini` or Extended Options' `Key Binding` menu. +> + Controls can be rebound in `pd.ini`. Default control scheme is as follows: | Action | Keyboard and mouse | Xbox pad | N64 pad | @@ -98,7 +117,7 @@ Controls can be rebound in `pd.ini`. Default control scheme is as follows: | Aim mode | RMB/Z | LT | R Trigger | | Use / Cancel | E | N/A | B | | Use / Accept | N/A | A | A | -| Crouch cycle | N/A | L3 | `0x80000000` (Extra) | +| Crouch cycle | N/A | LS CLICK | `0x80000000` (Extra) | | Half-Crouch | Shift | N/A | `0x40000000` (Extra) | | Full-Crouch | Control | N/A | `0x20000000` (Extra) | | Reload | R | X | X `(0x40)` | @@ -108,6 +127,8 @@ Controls can be rebound in `pd.ini`. Default control scheme is as follows: | Alt fire mode | F | RB | L Trigger | | Alt-fire oneshot | `F + LMB` or `E + LMB` | `A + RT` or `RB + RT` | `A + Z` or `L + Z` | | Quick-detonate | `E + Q` or `E + R` | `A + B` or `A + X` | `A + D-Left`or `A + X` | +| Reset Camera / Crosshair | C | RS CLICK | N/A | +| Gyro Calibration (Manual) | F10 | SHARE | N/A | ## Building @@ -208,6 +229,9 @@ It might be possible to build and run the game on platforms that are not specifi * [Ship of Harkinian team](https://github.com/Kenix3/libultraship/tree/main/src/graphic/Fast3D), Emill and MaikelChan for the libultraship version of fast3d that this port uses; * lieff for [minimp3](https://github.com/lieff/minimp3); * Mouse Injector and 1964GEPD authors for some of the 60FPS- and mouselook-related fixes; +* [n64emu-SDL2GyroInjector](https://github.com/TauAkiou/n64emu-SDL2GyroInjector) author for inspirations on Gyro Aiming feature sets. +* [GamepadMotionHelpers](https://github.com/JibbSmart/GamepadMotionHelpers) author for motion sensor-related tools like sensor fusion, gyro space and gyro calibration. + * additionally: Jibb Smart for some of the Gyro resources via [GyroWiki](http://gyrowiki.jibbsmart.com/). * Raf for the 64-bit port; * NicNamSam for the icon; * everyone who has submitted pull requests and issues to this repository and tested the port; diff --git a/port/gamepadmotionhelper/GamepadMotion.cpp b/port/gamepadmotionhelper/GamepadMotion.cpp new file mode 100644 index 0000000000..765e7dd837 --- /dev/null +++ b/port/gamepadmotionhelper/GamepadMotion.cpp @@ -0,0 +1,1782 @@ +// 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 +#if defined(_WIN32) + #define GamepadMotion_WRAPPER __declspec(dllexport) +#elif defined(__GNUC__) && __GNUC__ >= 4 + #define GamepadMotion_WRAPPER __attribute__((visibility("default"))) +#else + #define GamepadMotion_WRAPPER +#endif +#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); +}; + +// C wrapper for the GamepadMotion API class. +extern "C" { + // Creates a new GamepadMotion object + GamepadMotion_WRAPPER GamepadMotion* CreateGamepadMotion() { + return new GamepadMotion(); + } + + // Delete a new GamepadMotion object + GamepadMotion_WRAPPER void DeleteGamepadMotion(GamepadMotion* motion) + { + if (motion != nullptr) { + delete motion; + motion = nullptr; // Optional: Set the pointer to nullptr to avoid dangling references + } + } + + // Resets the GamepadMotion object + GamepadMotion_WRAPPER void ResetGamepadMotion(GamepadMotion* motion) { + if (motion) { + motion->Reset(); + } + } + + // Processes motion input for the GamepadMotion object + GamepadMotion_WRAPPER void ProcessMotion(GamepadMotion* motion, float gyroX, float gyroY, float gyroZ, + float accelX, float accelY, float accelZ, float deltaTime) { + if (motion) { + motion->ProcessMotion(gyroX, gyroY, gyroZ, accelX, accelY, accelZ, deltaTime); + } + } + + // Wrapper methods to call GamepadMotion functions + GamepadMotion_WRAPPER void GetCalibratedGyro(GamepadMotion* motion, float& x, float& y, float& z) { + if (motion) { + motion->GetCalibratedGyro(x, y, z); + } + } + + GamepadMotion_WRAPPER void GetGravity(GamepadMotion* motion, float& x, float& y, float& z) { + if (motion) { + motion->GetGravity(x, y, z); + } + } + + GamepadMotion_WRAPPER void GetProcessedAcceleration(GamepadMotion* motion, float& x, float& y, float& z) { + if (motion) { + motion->GetProcessedAcceleration(x, y, z); + } + } + + GamepadMotion_WRAPPER void GetOrientation(GamepadMotion* motion, float& w, float& x, float& y, float& z) { + if (motion) { + motion->GetOrientation(w, x, y, z); + } + } + + GamepadMotion_WRAPPER void GetPlayerSpaceGyro(GamepadMotion* motion, float& x, float& y, const float yawRelaxFactor = 1.41f) { + if (motion) { + motion->GetPlayerSpaceGyro(x, y, yawRelaxFactor); + } + } + + GamepadMotion_WRAPPER void GetWorldSpaceGyro(GamepadMotion* motion, float& x, float& y, const float sideReductionThreshold = 0.125f) { + if (motion) { + motion->GetWorldSpaceGyro(x, y, sideReductionThreshold); + } + } + + // Gyro calibration functions + GamepadMotion_WRAPPER void StartContinuousCalibration(GamepadMotion* motion) { + if (motion) { + motion->StartContinuousCalibration(); + } + } + + GamepadMotion_WRAPPER void PauseContinuousCalibration(GamepadMotion* motion) { + if (motion) { + motion->PauseContinuousCalibration(); + } + } + + GamepadMotion_WRAPPER void ResetContinuousCalibration(GamepadMotion* motion) { + if (motion) { + motion->ResetContinuousCalibration(); + } + } + + GamepadMotion_WRAPPER void GetCalibrationOffset(GamepadMotion* motion, float& xOffset, float& yOffset, float& zOffset) { + if (motion) { + motion->GetCalibrationOffset(xOffset, yOffset, zOffset); + } + } + + GamepadMotion_WRAPPER void SetCalibrationOffset(GamepadMotion* motion, float xOffset, float yOffset, float zOffset, int weight) { + if (motion) { + motion->SetCalibrationOffset(xOffset, yOffset, zOffset, weight); + } + } + + GamepadMotion_WRAPPER float GetAutoCalibrationConfidence(GamepadMotion* motion) { + if (motion) { + return motion->GetAutoCalibrationConfidence(); + } + return 0.0f; // Default confidence value + } + + GamepadMotion_WRAPPER void SetAutoCalibrationConfidence(GamepadMotion* motion, float newConfidence) { + if (motion) { + motion->SetAutoCalibrationConfidence(newConfidence); + } + } + + GamepadMotion_WRAPPER bool GetAutoCalibrationIsSteady(GamepadMotion* motion) { + if (motion) { + return motion->GetAutoCalibrationIsSteady(); + } + return false; // Default steady state + } + + GamepadMotion_WRAPPER GamepadMotionHelpers::CalibrationMode GetCalibrationMode(GamepadMotion* motion) { + if (motion) { + return motion->GetCalibrationMode(); + } + return GamepadMotionHelpers::CalibrationMode::Manual; // Default calibration mode + } + + GamepadMotion_WRAPPER void SetCalibrationMode(GamepadMotion* motion, GamepadMotionHelpers::CalibrationMode calibrationMode) { + if (motion) { + motion->SetCalibrationMode(calibrationMode); + } + } + + GamepadMotion_WRAPPER void ResetMotion(GamepadMotion* motion) { + if (motion) { + motion->ResetMotion(); + } + } + + // GamepadMotionSettings C wrapper functions + GamepadMotion_WRAPPER void SetMinStillnessSamples(GamepadMotion* motion, int value) { + if (motion) { + motion->Settings.MinStillnessSamples = value; + } + } + + GamepadMotion_WRAPPER int GetMinStillnessSamples(GamepadMotion* motion) { + if (motion) { + return motion->Settings.MinStillnessSamples; + } + return 10; + } + + GamepadMotion_WRAPPER void SetMinStillnessCollectionTime(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.MinStillnessCollectionTime = value; + } + } + + GamepadMotion_WRAPPER float GetMinStillnessCollectionTime(GamepadMotion* motion) { + if (motion) { + return motion->Settings.MinStillnessCollectionTime; + } + return 0.5f; + } + + GamepadMotion_WRAPPER void SetMinStillnessCorrectionTime(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.MinStillnessCorrectionTime = value; + } + } + + GamepadMotion_WRAPPER float GetMinStillnessCorrectionTime(GamepadMotion* motion) { + if (motion) { + return motion->Settings.MinStillnessCorrectionTime; + } + return 2.0f; + } + + GamepadMotion_WRAPPER void SetMaxStillnessError(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.MaxStillnessError = value; + } + } + + GamepadMotion_WRAPPER float GetMaxStillnessError(GamepadMotion* motion) { + if (motion) { + return motion->Settings.MaxStillnessError; + } + return 2.0f; + } + + GamepadMotion_WRAPPER void SetStillnessSampleDeteriorationRate(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.StillnessSampleDeteriorationRate = value; + } + } + + GamepadMotion_WRAPPER float GetStillnessSampleDeteriorationRate(GamepadMotion* motion) { + if (motion) { + return motion->Settings.StillnessSampleDeteriorationRate; + } + return 0.2f; + } + + GamepadMotion_WRAPPER void SetStillnessErrorClimbRate(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.StillnessErrorClimbRate = value; + } + } + + GamepadMotion_WRAPPER float GetStillnessErrorClimbRate(GamepadMotion* motion) { + if (motion) { + return motion->Settings.StillnessErrorClimbRate; + } + return 0.1f; + } + + GamepadMotion_WRAPPER void SetStillnessErrorDropOnRecalibrate(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.StillnessErrorDropOnRecalibrate = value; + } + } + + GamepadMotion_WRAPPER float GetStillnessErrorDropOnRecalibrate(GamepadMotion* motion) { + if (motion) { + return motion->Settings.StillnessErrorDropOnRecalibrate; + } + return 0.1f; + } + + GamepadMotion_WRAPPER void SetStillnessCalibrationEaseInTime(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.StillnessCalibrationEaseInTime = value; + } + } + + GamepadMotion_WRAPPER float GetStillnessCalibrationEaseInTime(GamepadMotion* motion) { + if (motion) { + return motion->Settings.StillnessCalibrationEaseInTime; + } + return 3.0f; + } + + GamepadMotion_WRAPPER void SetStillnessCalibrationHalfTime(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.StillnessCalibrationHalfTime = value; + } + } + + GamepadMotion_WRAPPER float GetStillnessCalibrationHalfTime(GamepadMotion* motion) { + if (motion) { + return motion->Settings.StillnessCalibrationHalfTime; + } + return 0.1f; + } + + GamepadMotion_WRAPPER void SetStillnessConfidenceRate(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.StillnessConfidenceRate = value; + } + } + + GamepadMotion_WRAPPER float GetStillnessConfidenceRate(GamepadMotion* motion) { + if (motion) { + return motion->Settings.StillnessConfidenceRate; + } + return 1.0f; + } + + GamepadMotion_WRAPPER void SetStillnessGyroDelta(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.StillnessGyroDelta = value; + } + } + + GamepadMotion_WRAPPER float GetStillnessGyroDelta(GamepadMotion* motion) { + if (motion) { + return motion->Settings.StillnessGyroDelta; + } + return -1.0f; + } + + GamepadMotion_WRAPPER void SetStillnessAccelDelta(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.StillnessAccelDelta = value; + } + } + + GamepadMotion_WRAPPER float GetStillnessAccelDelta(GamepadMotion* motion) { + if (motion) { + return motion->Settings.StillnessAccelDelta; + } + return -1.0f; + } + + GamepadMotion_WRAPPER void SetSensorFusionCalibrationSmoothingStrength(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.SensorFusionCalibrationSmoothingStrength = value; + } + } + + GamepadMotion_WRAPPER float GetSensorFusionCalibrationSmoothingStrength(GamepadMotion* motion) { + if (motion) { + return motion->Settings.SensorFusionCalibrationSmoothingStrength; + } + return 2.0f; + } + + GamepadMotion_WRAPPER void SetSensorFusionAngularAccelerationThreshold(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.SensorFusionAngularAccelerationThreshold = value; + } + } + + GamepadMotion_WRAPPER float GetSensorFusionAngularAccelerationThreshold(GamepadMotion* motion) { + if (motion) { + return motion->Settings.SensorFusionAngularAccelerationThreshold; + } + return 20.0f; + } + + GamepadMotion_WRAPPER void SetSensorFusionCalibrationEaseInTime(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.SensorFusionCalibrationEaseInTime = value; + } + } + + GamepadMotion_WRAPPER float GetSensorFusionCalibrationEaseInTime(GamepadMotion* motion) { + if (motion) { + return motion->Settings.SensorFusionCalibrationEaseInTime; + } + return 3.0f; + } + + GamepadMotion_WRAPPER void SetSensorFusionCalibrationHalfTime(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.SensorFusionCalibrationHalfTime = value; + } + } + + GamepadMotion_WRAPPER float GetSensorFusionCalibrationHalfTime(GamepadMotion* motion) { + if (motion) { + return motion->Settings.SensorFusionCalibrationHalfTime; + } + return 0.1f; + } + + GamepadMotion_WRAPPER void SetSensorFusionConfidenceRate(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.SensorFusionConfidenceRate = value; + } + } + + GamepadMotion_WRAPPER float GetSensorFusionConfidenceRate(GamepadMotion* motion) { + if (motion) { + return motion->Settings.SensorFusionConfidenceRate; + } + return 1.0f; + } + + GamepadMotion_WRAPPER void SetGravityCorrectionShakinessMaxThreshold(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.GravityCorrectionShakinessMaxThreshold = value; + } + } + + GamepadMotion_WRAPPER float GetGravityCorrectionShakinessMaxThreshold(GamepadMotion* motion) { + if (motion) { + return motion->Settings.GravityCorrectionShakinessMaxThreshold; + } + return 0.4f; + } + + GamepadMotion_WRAPPER void SetGravityCorrectionShakinessMinThreshold(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.GravityCorrectionShakinessMinThreshold = value; + } + } + + GamepadMotion_WRAPPER float GetGravityCorrectionShakinessMinThreshold(GamepadMotion* motion) { + if (motion) { + return motion->Settings.GravityCorrectionShakinessMinThreshold; + } + return 0.01f; + } + + GamepadMotion_WRAPPER void SetGravityCorrectionStillSpeed(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.GravityCorrectionStillSpeed = value; + } + } + + GamepadMotion_WRAPPER float GetGravityCorrectionStillSpeed(GamepadMotion* motion) { + if (motion) { + return motion->Settings.GravityCorrectionStillSpeed; + } + return 1.0f; + } + + GamepadMotion_WRAPPER void SetGravityCorrectionShakySpeed(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.GravityCorrectionShakySpeed = value; + } + } + + GamepadMotion_WRAPPER float GetGravityCorrectionShakySpeed(GamepadMotion* motion) { + if (motion) { + return motion->Settings.GravityCorrectionShakySpeed; + } + return 0.1f; + } + + GamepadMotion_WRAPPER void SetGravityCorrectionGyroFactor(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.GravityCorrectionGyroFactor = value; + } + } + + GamepadMotion_WRAPPER float GetGravityCorrectionGyroFactor(GamepadMotion* motion) { + if (motion) { + return motion->Settings.GravityCorrectionGyroFactor; + } + return 0.1f; + } + + GamepadMotion_WRAPPER void SetGravityCorrectionGyroMinThreshold(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.GravityCorrectionGyroMinThreshold = value; + } + } + + GamepadMotion_WRAPPER float GetGravityCorrectionGyroMinThreshold(GamepadMotion* motion) { + if (motion) { + return motion->Settings.GravityCorrectionGyroMinThreshold; + } + return 0.05f; + } + + GamepadMotion_WRAPPER void SetGravityCorrectionGyroMaxThreshold(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.GravityCorrectionGyroMaxThreshold = value; + } + } + + GamepadMotion_WRAPPER float GetGravityCorrectionGyroMaxThreshold(GamepadMotion* motion) { + if (motion) { + return motion->Settings.GravityCorrectionGyroMaxThreshold; + } + return 0.25f; + } + + GamepadMotion_WRAPPER void SetGravityCorrectionMinimumSpeed(GamepadMotion* motion, float value) { + if (motion) { + motion->Settings.GravityCorrectionMinimumSpeed = value; + } + } + + GamepadMotion_WRAPPER float GetGravityCorrectionMinimumSpeed(GamepadMotion* motion) { + if (motion) { + return motion->Settings.GravityCorrectionMinimumSpeed; + } + return 0.01f; + } +} +///////////// 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/port/gamepadmotionhelper/GamepadMotion.h b/port/gamepadmotionhelper/GamepadMotion.h new file mode 100644 index 0000000000..e097c2430d --- /dev/null +++ b/port/gamepadmotionhelper/GamepadMotion.h @@ -0,0 +1,180 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#if defined(_WIN32) + #define GamepadMotion_WRAPPER __declspec(dllexport) +#elif defined(__GNUC__) && __GNUC__ >= 4 + #define GamepadMotion_WRAPPER __attribute__((visibility("default"))) +#else + #define GamepadMotion_WRAPPER +#endif + +// Use the actual C++ class pointer for the handle +typedef struct GamepadMotion GamepadMotion; +typedef GamepadMotion* GamepadMotionHandle; + +// prefix alias for GamepadMotion functions (easier to know which functions are from GamepadMotion) +#define gmhCreateGamepadMotion CreateGamepadMotion +#define gmhDeleteGamepadMotion DeleteGamepadMotion +#define gmhProcessMotion ProcessMotion +#define gmhResetGamepadMotion ResetGamepadMotion +#define gmhResetMotion ResetMotion +#define gmhGetCalibratedGyro GetCalibratedGyro +#define gmhGetGravity GetGravity +#define gmhGetProcessedAcceleration GetProcessedAcceleration +#define gmhGetOrientation GetOrientation +#define gmhGetPlayerSpaceGyro GetPlayerSpaceGyro +#define gmhGetWorldSpaceGyro GetWorldSpaceGyro +#define gmhStartContinuousCalibration StartContinuousCalibration +#define gmhPauseContinuousCalibration PauseContinuousCalibration +#define gmhResetContinuousCalibration ResetContinuousCalibration +#define gmhGetCalibrationOffset GetCalibrationOffset +#define gmhSetCalibrationOffset SetCalibrationOffset +#define gmhGetAutoCalibrationConfidence GetAutoCalibrationConfidence +#define gmhSetAutoCalibrationConfidence SetAutoCalibrationConfidence +#define gmhGetAutoCalibrationIsSteady GetAutoCalibrationIsSteady +#define gmhGetCalibrationMode GetCalibrationMode +#define gmhSetCalibrationMode SetCalibrationMode +#define gmhSetMinStillnessSamples SetMinStillnessSamples +#define gmhGetMinStillnessSamples GetMinStillnessSamples +#define gmhSetMinStillnessCollectionTime SetMinStillnessCollectionTime +#define gmhGetMinStillnessCollectionTime GetMinStillnessCollectionTime +#define gmhSetMinStillnessCorrectionTime SetMinStillnessCorrectionTime +#define gmhGetMinStillnessCorrectionTime GetMinStillnessCorrectionTime +#define gmhSetMaxStillnessError SetMaxStillnessError +#define gmhGetMaxStillnessError GetMaxStillnessError +#define gmhSetStillnessSampleDeteriorationRate SetStillnessSampleDeteriorationRate +#define gmhGetStillnessSampleDeteriorationRate GetStillnessSampleDeteriorationRate +#define gmhSetStillnessErrorClimbRate SetStillnessErrorClimbRate +#define gmhGetStillnessErrorClimbRate GetStillnessErrorClimbRate +#define gmhSetStillnessErrorDropOnRecalibrate SetStillnessErrorDropOnRecalibrate +#define gmhGetStillnessErrorDropOnRecalibrate GetStillnessErrorDropOnRecalibrate +#define gmhSetStillnessCalibrationEaseInTime SetStillnessCalibrationEaseInTime +#define gmhGetStillnessCalibrationEaseInTime GetStillnessCalibrationEaseInTime +#define gmhSetStillnessCalibrationHalfTime SetStillnessCalibrationHalfTime +#define gmhGetStillnessCalibrationHalfTime GetStillnessCalibrationHalfTime +#define gmhSetStillnessConfidenceRate SetStillnessConfidenceRate +#define gmhGetStillnessConfidenceRate GetStillnessConfidenceRate +#define gmhSetStillnessGyroDelta SetStillnessGyroDelta +#define gmhGetStillnessGyroDelta GetStillnessGyroDelta +#define gmhSetStillnessAccelDelta SetStillnessAccelDelta +#define gmhGetStillnessAccelDelta GetStillnessAccelDelta +#define gmhSetSensorFusionCalibrationSmoothingStrength SetSensorFusionCalibrationSmoothingStrength +#define gmhGetSensorFusionCalibrationSmoothingStrength GetSensorFusionCalibrationSmoothingStrength +#define gmhSetSensorFusionAngularAccelerationThreshold SetSensorFusionAngularAccelerationThreshold +#define gmhGetSensorFusionAngularAccelerationThreshold GetSensorFusionAngularAccelerationThreshold +#define gmhSetSensorFusionCalibrationEaseInTime SetSensorFusionCalibrationEaseInTime +#define gmhGetSensorFusionCalibrationEaseInTime GetSensorFusionCalibrationEaseInTime +#define gmhSetSensorFusionCalibrationHalfTime SetSensorFusionCalibrationHalfTime +#define gmhGetSensorFusionCalibrationHalfTime GetSensorFusionCalibrationHalfTime +#define gmhSetSensorFusionConfidenceRate SetSensorFusionConfidenceRate +#define gmhGetSensorFusionConfidenceRate GetSensorFusionConfidenceRate +#define gmhSetGravityCorrectionShakinessMaxThreshold SetGravityCorrectionShakinessMaxThreshold +#define gmhGetGravityCorrectionShakinessMaxThreshold GetGravityCorrectionShakinessMaxThreshold +#define gmhSetGravityCorrectionShakinessMinThreshold SetGravityCorrectionShakinessMinThreshold +#define gmhGetGravityCorrectionShakinessMinThreshold GetGravityCorrectionShakinessMinThreshold +#define gmhSetGravityCorrectionStillSpeed SetGravityCorrectionStillSpeed +#define gmhGetGravityCorrectionStillSpeed GetGravityCorrectionStillSpeed +#define gmhSetGravityCorrectionShakySpeed SetGravityCorrectionShakySpeed +#define gmhGetGravityCorrectionShakySpeed GetGravityCorrectionShakySpeed +#define gmhSetGravityCorrectionGyroFactor SetGravityCorrectionGyroFactor +#define gmhGetGravityCorrectionGyroFactor GetGravityCorrectionGyroFactor +#define gmhSetGravityCorrectionGyroMinThreshold SetGravityCorrectionGyroMinThreshold +#define gmhGetGravityCorrectionGyroMinThreshold GetGravityCorrectionGyroMinThreshold +#define gmhSetGravityCorrectionGyroMaxThreshold SetGravityCorrectionGyroMaxThreshold +#define gmhGetGravityCorrectionGyroMaxThreshold GetGravityCorrectionGyroMaxThreshold +#define gmhSetGravityCorrectionMinimumSpeed SetGravityCorrectionMinimumSpeed +#define gmhGetGravityCorrectionMinimumSpeed GetGravityCorrectionMinimumSpeed + +// Creation and destruction +GamepadMotion_WRAPPER GamepadMotionHandle CreateGamepadMotion(void); +GamepadMotion_WRAPPER void DeleteGamepadMotion(GamepadMotionHandle handle); + +// Core motion processing +GamepadMotion_WRAPPER void ProcessMotion(GamepadMotionHandle handle, float gyroX, float gyroY, float gyroZ, + float accelX, float accelY, float accelZ, float deltaTime); +GamepadMotion_WRAPPER void ResetGamepadMotion(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void ResetMotion(GamepadMotionHandle handle); + +// State getters +GamepadMotion_WRAPPER void GetCalibratedGyro(GamepadMotionHandle handle, float* x, float* y, float* z); +GamepadMotion_WRAPPER void GetGravity(GamepadMotionHandle handle, float* x, float* y, float* z); +GamepadMotion_WRAPPER void GetProcessedAcceleration(GamepadMotionHandle handle, float* x, float* y, float* z); +GamepadMotion_WRAPPER void GetOrientation(GamepadMotionHandle handle, float* w, float* x, float* y, float* z); +GamepadMotion_WRAPPER void GetPlayerSpaceGyro(GamepadMotionHandle handle, float* x, float* y, const float yawRelaxFactor); +GamepadMotion_WRAPPER void GetWorldSpaceGyro(GamepadMotionHandle handle, float* x, float* y, const float sideReductionThreshold); + +// Calibration +GamepadMotion_WRAPPER void StartContinuousCalibration(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void PauseContinuousCalibration(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void ResetContinuousCalibration(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void GetCalibrationOffset(GamepadMotionHandle handle, float* xOffset, float* yOffset, float* zOffset); +GamepadMotion_WRAPPER void SetCalibrationOffset(GamepadMotionHandle handle, float xOffset, float yOffset, float zOffset, int weight); +GamepadMotion_WRAPPER float GetAutoCalibrationConfidence(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetAutoCalibrationConfidence(GamepadMotionHandle handle, float newConfidence); +GamepadMotion_WRAPPER bool GetAutoCalibrationIsSteady(GamepadMotionHandle handle); + +// Calibration mode +GamepadMotion_WRAPPER int GetCalibrationMode(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetCalibrationMode(GamepadMotionHandle handle, int mode); + +// GamepadMotionSettings: Getters and Setters +GamepadMotion_WRAPPER void SetMinStillnessSamples(GamepadMotionHandle handle, int value); +GamepadMotion_WRAPPER int GetMinStillnessSamples(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetMinStillnessCollectionTime(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetMinStillnessCollectionTime(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetMinStillnessCorrectionTime(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetMinStillnessCorrectionTime(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetMaxStillnessError(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetMaxStillnessError(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetStillnessSampleDeteriorationRate(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetStillnessSampleDeteriorationRate(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetStillnessErrorClimbRate(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetStillnessErrorClimbRate(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetStillnessErrorDropOnRecalibrate(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetStillnessErrorDropOnRecalibrate(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetStillnessCalibrationEaseInTime(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetStillnessCalibrationEaseInTime(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetStillnessCalibrationHalfTime(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetStillnessCalibrationHalfTime(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetStillnessConfidenceRate(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetStillnessConfidenceRate(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetStillnessGyroDelta(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetStillnessGyroDelta(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetStillnessAccelDelta(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetStillnessAccelDelta(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetSensorFusionCalibrationSmoothingStrength(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetSensorFusionCalibrationSmoothingStrength(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetSensorFusionAngularAccelerationThreshold(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetSensorFusionAngularAccelerationThreshold(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetSensorFusionCalibrationEaseInTime(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetSensorFusionCalibrationEaseInTime(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetSensorFusionCalibrationHalfTime(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetSensorFusionCalibrationHalfTime(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetSensorFusionConfidenceRate(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetSensorFusionConfidenceRate(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetGravityCorrectionShakinessMaxThreshold(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetGravityCorrectionShakinessMaxThreshold(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetGravityCorrectionShakinessMinThreshold(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetGravityCorrectionShakinessMinThreshold(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetGravityCorrectionStillSpeed(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetGravityCorrectionStillSpeed(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetGravityCorrectionShakySpeed(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetGravityCorrectionShakySpeed(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetGravityCorrectionGyroFactor(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetGravityCorrectionGyroFactor(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetGravityCorrectionGyroMinThreshold(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetGravityCorrectionGyroMinThreshold(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetGravityCorrectionGyroMaxThreshold(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetGravityCorrectionGyroMaxThreshold(GamepadMotionHandle handle); +GamepadMotion_WRAPPER void SetGravityCorrectionMinimumSpeed(GamepadMotionHandle handle, float value); +GamepadMotion_WRAPPER float GetGravityCorrectionMinimumSpeed(GamepadMotionHandle handle); + +#ifdef __cplusplus +} +#endif diff --git a/port/gamepadmotionhelper/LICENSE.txt b/port/gamepadmotionhelper/LICENSE.txt new file mode 100644 index 0000000000..a46396a3c1 --- /dev/null +++ b/port/gamepadmotionhelper/LICENSE.txt @@ -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/port/include/input.h b/port/include/input.h index 6171c26961..df0109ab24 100644 --- a/port/include/input.h +++ b/port/include/input.h @@ -102,9 +102,9 @@ enum contkey { CK_STICK_YPOS, CK_ACCEPT, CK_CANCEL, - CK_0040, - CK_0080, - CK_0100, + CK_0040, // Reset Camera/Crosshair action + CK_0080, // Gyro Modifier action + CK_0100, // Gyro Calibration (Manual) action CK_0200, CK_0400, CK_0800, @@ -121,6 +121,77 @@ enum mouselockmode { MLOCK_AUTO = 2 }; +enum gyroactivation { + GYRO_ALWAYS_ON = 0, // Gyro is always enabled + GYRO_TOGGLE = 1, // Gyro is enabled/disabled by a button press + GYRO_ENABLE_HELD = 2, // Gyro is enabled while a button is held down + GYRO_DISABLE_HELD = 3 // Gyro is disabled while a button is held down +}; + +enum gyroaxismode { + GYRO_YAW = 0, // Gyro controls yaw axis (turn) + GYRO_ROLL = 1, // Gyro controls roll axis (lean) + GYRO_LOCAL = 2, // Gyro controls local space orientation + GYRO_PLAYER = 3, // Gyro controls player space orientation + GYRO_WORLD = 4 // Gyro controls world space orientation +}; + +enum gyroaimmode { + GYRO_AIM_CAMERA = 0, // Gyro controls camera movement + GYRO_AIM_CROSSHAIR = 1, // Gyro controls crosshair movement + GYRO_AIM_BOTH = 2 // Gyro controls both camera and crosshair movement +}; + +typedef enum { + GYRO_CALIB_START, // Start gyro calibration + GYRO_CALIB_FINISH, // Finish gyro calibration + GYRO_CALIB_RESET, // Reset gyro calibration + GYRO_CALIB_QUERY, // Query gyro calibration status + GYRO_CALIB_AUTO // Automatic gyro calibration +} GyroCalibrationOp; + +typedef enum { + CALIBRATIONMODE_MANUAL = 0, // Manual calibration mode + CALIBRATIONMODE_STILLNESS = 1, // Stillness calibration mode + CALIBRATIONMODE_SENSORFUSION = 2 // Sensor fusion calibration mode +} CalibrationMode; + +enum gyroautocalibration { + GYRO_AUTOCALIBRATION_OFF = 0, // Disables Auto-Calibration + GYRO_AUTOCALIBRATION_MENU_ONLY = 1, // Enables Auto-Calibration only when a menu dialog is opened + GYRO_AUTOCALIBRATION_STATIONARY = 2, // Enables Auto-Calibration when controller is stationary (when placed on a flat surface) + GYRO_AUTOCALIBRATION_ALWAYS = 3 // Always active, calibration continuous. +}; + +// Combined gyro calibration configuration and state structure +typedef struct { + // Auto-calibration configuration parameters + unsigned int calibInterval; // Time between calibration attempts + unsigned int initialDelay; // Initial delay when becoming stable + float confidenceTarget; // Target confidence for completion + float confidenceMin; // Minimum confidence before restart + float startThreshold; // Start calibration threshold + float stopThreshold; // Stop calibration threshold + int stableRequired; // Sustained stability checks required + int saveOffset; // Whether to save calibration offset + + // Manual calibration state + int manualCalibActive; // Is manual calibration currently active + unsigned int manualCalibStartTime; // When manual calibration started + f32 manualOffsetX, manualOffsetY, manualOffsetZ; // Manual calibration offsets + s32 manualWeight; // Weight for manual calibration + + // Auto-calibration runtime state + int wasStable; // Was the controller stable during auto-calibration + unsigned int lastAutoCalibTime; // When controller last moved (for cooldown) + unsigned int lastCalibrationTime; // When calibration finished (for resume logic) + f32 lastConfidence; // Last confidence level + s32 stableCount; // Sustained stability counter for STATIONARY mode + + // General state + int justFinishedCalibrating; // Has the controller just finished calibrating +} GyroCalibConfig; + // returns bitmask of connected controllers or -1 if failed s32 inputInit(void); @@ -181,6 +252,9 @@ s32 inputKeyJustPressed(u32 vk); // idx is controller index, contbtn is one of the CONT_ constants s32 inputButtonPressed(s32 idx, u32 contbtn); +// idx is controller index, ck is one of the CK_ constants +s32 inputBindPressed(s32 idx, u32 ck); + // bind virtkey vk to n64 pad #idx's button/axis ck as represented by its contkey value // if bind is -1, picks a bind slot automatically void inputKeyBind(s32 idx, u32 ck, s32 bind, u32 vk); @@ -232,6 +306,79 @@ void inputMouseSetSpeed(f32 x, f32 y); s32 inputMouseIsEnabled(void); void inputMouseEnable(s32 enabled); +// Updates the gyro data for the specified controller index +void inputUpdateGyro(s32 cidx); +void inputUpdateGyroCalibrationSystem(void); + +// Scaled Gyro Movement Retrieval +void inputGyroGetScaledDelta(s32 cidx, f32* dx, f32* dy, f32* dz); +void inputGyroGetScaledDeltaCrosshair(s32 cidx, f32* dx, f32* dy); + +// Raw Gyro Movement Retrieval +void inputGyroGetRawDelta(s32 cidx, s32* dx, s32* dy, s32* dz); + +// Motion Sensor detection +// returns 1 if the controller has motion sensors, 0 otherwise disables Gyro Aiming functionality for the controller. +s32 inputControllerMotionSensorsSupported(s32 cidx); + +// Gyro Enable/Disable +s32 inputGyroIsEnabled(s32 cidx); +void inputGyroEnable(s32 cidx, s32 enabled); + +// Gyro Aim Mode Management +void inputSetGyroAimMode(s32 cidx, s32 mode); +s32 inputGetGyroAimMode(s32 cidx); + +// Gyro Modifier Management +void inputSetGyroModifier(s32 cidx, s32 mode); +s32 inputGetGyroModifier(s32 cidx); + +// Gyro Axis Mode Management +void inputGyroSetAxisMode(s32 cidx, s32 mode); +s32 inputGyroGetAxisMode(s32 cidx); + +// Gyro Speed Management +void inputGyroGetSpeed(s32 cidx, f32* x, f32* y); +void inputGyroSetSpeed(s32 cidx, f32 x, f32 y); +void inputGyroGetAimSpeed(s32 cidx, f32* x, f32* y); +void inputGyroSetAimSpeed(s32 cidx, f32 x, f32 y); + +// Gyro Invert Management +void inputGyroGetInvert(s32 cidx, s32* invertx, s32* inverty); +void inputGyroSetInvert(s32 cidx, s32 invertx, s32 inverty); +void inputGyroGetAimInvert(s32 cidx, s32* invertx, s32* inverty); +void inputGyroSetAimInvert(s32 cidx, s32 invertx, s32 inverty); + +// Gyro X/Y Ratio Mixer Management +f32 inputGetGyroVHMixer(s32 cidx); +void inputSetGyroVHMixer(s32 cidx, f32 value); + +// Gyro Smoothing Management +f32 inputGetGyroSmoothing(s32 cidx); +void inputSetGyroSmoothing(s32 cidx, f32 value); + +// Gyro Deadzone Management +f32 inputGyroGetDeadzone(s32 cidx); +void inputGyroSetDeadzone(s32 cidx, f32 value); + +// Gyro Tightening Management +f32 inputGyroGetTightening(s32 cidx); +void inputGyroSetTightening(s32 cidx, f32 value); + +// Gyro Calibration Management +void inputGyroCalibration(s32 cidx, GyroCalibrationOp op, float* out_confidence, int* out_steady); + +// Gyro Auto Calibration +s32 inputGyroGetAutoCalibration(s32 cidx); +void inputGyroSetAutoCalibration(s32 cidx, s32 enabled); + +// Gyro Manual Calibration +s32 inputGyroGetManualCalibration(s32 cidx); +void inputGyroSetManualCalibration(s32 cidx); + +// Ensures CK_0100 (Manual Calibration bind) is blocked during menu-driven gyro calibration +s32 inputIsMenuGyroCalibrationActive(s32 cidx); + // call this every frame void inputUpdate(void); diff --git a/port/src/config.c b/port/src/config.c index 596cf3a5a6..c57e8f8ac6 100644 --- a/port/src/config.c +++ b/port/src/config.c @@ -10,7 +10,7 @@ #define CONFIG_MAX_SECNAME 128 #define CONFIG_MAX_KEYNAME 256 -#define CONFIG_MAX_SETTINGS 300 +#define CONFIG_MAX_SETTINGS 500 typedef enum { CFG_NONE, diff --git a/port/src/input.c b/port/src/input.c index ebf0c4667b..97828dfbcc 100644 --- a/port/src/input.c +++ b/port/src/input.c @@ -1,16 +1,21 @@ #include +#include #include #include +#include #include #include #include #include "platform.h" #include "input.h" +#include "../gamepadmotionhelper/GamepadMotion.h" #include "video.h" #include "config.h" #include "utils.h" #include "system.h" #include "fs.h" +#include "game/menu.h" +#include "bss.h" #if !SDL_VERSION_ATLEAST(2, 0, 14) // this was added in 2.0.14 @@ -18,7 +23,6 @@ #endif #define CONTROLLERDB_FNAME "gamecontrollerdb.txt" - #define MAX_BIND_STR 256 #define TRIG_THRESHOLD (30 * 256) @@ -31,6 +35,20 @@ #define CURSOR_HIDE_THRESHOLD 1 #define CURSOR_HIDE_TIME 3000000 // us +// Gravity constants +// note: this may break GamepadMotion's Gyro Calibration systems on platforms without SDL_sensor.h +#ifndef SDL_STANDARD_GRAVITY +#define SDL_STANDARD_GRAVITY 9.80665f // standard gravity in m/s^2 +#endif + +// Gyro autocalibration constants +// These control the internal calibration behavior and timing thresholds +#define GMH_STILLNESS_COLLECTION_TIME 2.5f // Time to collect stillness data before calibration starts +#define GMH_STILLNESS_CORRECTION_TIME 3.0f // Time to correct/build calibration confidence +#define GMH_STILLNESS_EASE_TIME 1.0f // Time to ease calibration transitions +#define GMH_MAX_STILLNESS_ERROR 1.5f // Maximum sensor error allowed during stillness detection +#define GYRO_NOISE_THRESHOLD 0.2f // Threshold for additional gyro noise filtering (safety net) + static SDL_GameController *pads[INPUT_MAX_CONTROLLERS]; #define CONTROLLERCFG_DEFAULT { \ @@ -46,6 +64,23 @@ static SDL_GameController *pads[INPUT_MAX_CONTROLLERS]; .swapSticks = 1, \ .deviceIndex = -1, \ .cancelCButtons = 0, \ + .gyroEnabled = 1, \ + .gyroAxisMode = GYRO_YAW, \ + .gyroAimMode = GYRO_AIM_CROSSHAIR, \ + .gyroModifier = GYRO_ALWAYS_ON, \ + .gyroSensX = 2.5f, \ + .gyroSensY = 2.5f, \ + .gyroAimSensX = 5.0f, \ + .gyroAimSensY = 5.0f, \ + .gyroVHMixer = 0.0f, \ + .gyroInvertX = 0, \ + .gyroInvertY = 0, \ + .gyroAimInvertX = 0, \ + .gyroAimInvertY = 0, \ + .gyroSmoothing = 0.18f, \ + .gyroTightening = 0.07f, \ + .gyroDeadzone = 0.03f, \ + .gyroAutoCalibration = 1, \ } static struct controllercfg { @@ -54,10 +89,28 @@ static struct controllercfg { u32 axisMap[2][2]; f32 sens[4]; s32 deadzone[4]; + s32 gyroSensorActive; s32 stickCButtons; s32 swapSticks; s32 deviceIndex; s32 cancelCButtons; + s32 gyroEnabled; + s32 gyroAxisMode; + s32 gyroAimMode; + s32 gyroModifier; + f32 gyroSensX; + f32 gyroSensY; + f32 gyroAimSensX; + f32 gyroAimSensY; + f32 gyroVHMixer; + s32 gyroInvertX; + s32 gyroInvertY; + s32 gyroAimInvertX; + s32 gyroAimInvertY; + f32 gyroSmoothing; + f32 gyroTightening; + f32 gyroDeadzone; + s32 gyroAutoCalibration; } padsCfg[INPUT_MAX_CONTROLLERS] = { CONTROLLERCFG_DEFAULT, CONTROLLERCFG_DEFAULT, @@ -71,6 +124,7 @@ static char bindStrs[MAXCONTROLLERS][CK_TOTAL_COUNT][MAX_BIND_STR]; static s32 fakeControllers = 0; static s32 firstController = 0; static s32 connectedMask = 0; +static s32 inputWindowHasFocus = 1; static s32 numJoysticks = 0; @@ -91,12 +145,49 @@ static s32 mouseShowCursor = 1; static f32 mouseSensX = 2.5f; static f32 mouseSensY = 2.5f; +// Motion Sensor data declaration +static bool inputGetControllerSensorData(s32 cidx, float gyroData[3], float accelData[3]); +static GamepadMotionHandle gpadMotion[INPUT_MAX_CONTROLLERS] = { NULL }; +static f32 gyroYaw[INPUT_MAX_CONTROLLERS], gyroPitch[INPUT_MAX_CONTROLLERS], gyroRoll[INPUT_MAX_CONTROLLERS]; +static f32 gyroDeltaYaw[INPUT_MAX_CONTROLLERS], gyroDeltaPitch[INPUT_MAX_CONTROLLERS], gyroDeltaRoll[INPUT_MAX_CONTROLLERS]; +static f32 accelDeltaX[INPUT_MAX_CONTROLLERS], accelDeltaY[INPUT_MAX_CONTROLLERS], accelDeltaZ[INPUT_MAX_CONTROLLERS]; +static f32 gravityX[INPUT_MAX_CONTROLLERS], gravityY[INPUT_MAX_CONTROLLERS], gravityZ[INPUT_MAX_CONTROLLERS]; +static f32 processedAccelX[INPUT_MAX_CONTROLLERS], processedAccelY[INPUT_MAX_CONTROLLERS], processedAccelZ[INPUT_MAX_CONTROLLERS]; + static s32 lastKey = 0; static char lastChar = 0; static s32 textInput = 0; static char *clipboardText = NULL; +static inline bool inputHasWindowFocus(void); + +// Gyro processing function declarations +static inline void applyGyroInvert(s32 cidx, f32* dx, f32* dy, bool useAimInvert); +static inline void applyGyroVHMixer(s32 cidx, f32* dx, f32* dy); +static void applyGyroAxisMapping(s32 cidx, float gyroData[3], float accelData[3], f32* deltaX, f32* deltaY, f32* deltaZ); +static void applyGyroModifier(f32* deltaX, f32* deltaY, f32* deltaZ, s32 activationMode, s32 idx); +static void applyGyroDeadzone(f32* dx, f32* dy, f32* dz, f32 deadzone); +static void applyGyroTightening(f32* dx, f32* dy, f32* dz, f32 tightening); +static void applyGyroSmoothing(f32* deltaX, f32* deltaY, f32* deltaZ, f32 smoothing, s32 cidx); + +// Gyro calibration configuration +static void inputConfigureGamepadMotionSettings(GamepadMotionHandle handle); +static void inputUpdateGyroCalibrationHandle(void); +static void inputUpdateGyroAutoCalibration(s32 cidx); +static void inputUpdateGyroAutoCalibrationAlways(s32 cidx); +static void inputUpdateGyroAutoCalibrationStationary(s32 cidx); +static void inputUpdateGyroAutoCalibrationUnified(s32 cidx); +static void inputUpdateGyroManualCalibration(s32 cidx); +static void inputGyroCalibrationActivation(s32 cidx, bool start, bool reset); +static void inputGyroCalibrationOffset(s32 cidx, bool save, bool apply); +static bool inputIsGyroCalibrationBlocked(s32 cidx); +static void inputConfigureGyroCalibrationMode(s32 cidx); +static bool inputIsControllerSensorNoiseThreshold(s32 cidx); +static bool inputGyroAutoCalibrationModes(s32 cidx); + +static GyroCalibConfig gyroCalibConfig[INPUT_MAX_CONTROLLERS] = {0}; + static const char *ckNames[CK_TOTAL_COUNT] = { "R_CBUTTONS", "L_CBUTTONS", @@ -208,6 +299,8 @@ void inputSetDefaultKeyBinds(s32 cidx, s32 n64mode) { CK_STICK_XPOS, SDL_SCANCODE_RIGHT, 0 }, { CK_STICK_YNEG, SDL_SCANCODE_DOWN, 0 }, { CK_STICK_YPOS, SDL_SCANCODE_UP, 0 }, + { CK_0040, SDL_SCANCODE_C, 0 }, + { CK_0100, SDL_SCANCODE_F10, 0 }, { CK_4000, SDL_SCANCODE_LSHIFT, 0 }, { CK_2000, SDL_SCANCODE_LCTRL, 0 } }; @@ -228,6 +321,8 @@ void inputSetDefaultKeyBinds(s32 cidx, s32 n64mode) { CK_C_L, SDL_CONTROLLER_BUTTON_DPAD_LEFT }, { CK_ACCEPT, SDL_CONTROLLER_BUTTON_A }, { CK_CANCEL, SDL_CONTROLLER_BUTTON_B }, + { CK_0040, SDL_CONTROLLER_BUTTON_RIGHTSTICK }, + { CK_0100, SDL_CONTROLLER_BUTTON_MISC1 }, { CK_8000, SDL_CONTROLLER_BUTTON_LEFTSTICK }, }; @@ -250,6 +345,7 @@ void inputSetDefaultKeyBinds(s32 cidx, s32 n64mode) { CK_STICK_YPOS, SDL_SCANCODE_I, 0 }, { CK_STICK_XNEG, SDL_SCANCODE_J, 0 }, { CK_STICK_XPOS, SDL_SCANCODE_L, 0 }, + { CK_0100, SDL_SCANCODE_F10, 0 }, }; static const u32 n64joybinds[][2] = { @@ -263,6 +359,7 @@ void inputSetDefaultKeyBinds(s32 cidx, s32 n64mode) { CK_DPAD_U, SDL_CONTROLLER_BUTTON_DPAD_UP }, { CK_DPAD_L, SDL_CONTROLLER_BUTTON_DPAD_LEFT }, { CK_DPAD_R, SDL_CONTROLLER_BUTTON_DPAD_RIGHT }, + { CK_0100, SDL_CONTROLLER_BUTTON_MISC1 }, }; memset(binds[cidx], 0, sizeof(binds[cidx])); @@ -346,6 +443,58 @@ static inline void inputInitController(const s32 cidx, const s32 jidx) SDL_JoystickGetGUIDString(guid, guidStr, sizeof(guidStr)); sysLogPrintf(LOG_NOTE, "input: GUID for controller %d: %s", jidx, guidStr); } + + // Initialize motion sensors + padsCfg[cidx].gyroSensorActive = 0; +#if SDL_VERSION_ATLEAST(2, 0, 14) + // Try to enable gyro and accelerometer sensors + if (SDL_GameControllerHasSensor(pads[cidx], SDL_SENSOR_GYRO) && + SDL_GameControllerHasSensor(pads[cidx], SDL_SENSOR_ACCEL)) { + + if (SDL_GameControllerSetSensorEnabled(pads[cidx], SDL_SENSOR_GYRO, SDL_TRUE) == 0 && + SDL_GameControllerSetSensorEnabled(pads[cidx], SDL_SENSOR_ACCEL, SDL_TRUE) == 0) { + + padsCfg[cidx].gyroSensorActive = 1; + sysLogPrintf(LOG_NOTE, "input: Motion sensors enabled for controller %d", cidx); + + // Initialize GamepadMotion instance + if (!gpadMotion[cidx]) { + gpadMotion[cidx] = gmhCreateGamepadMotion(); + } else { + gmhResetGamepadMotion(gpadMotion[cidx]); + } + + if (gpadMotion[cidx]) { + inputConfigureGamepadMotionSettings(gpadMotion[cidx]); + gmhResetMotion(gpadMotion[cidx]); + inputConfigureGyroCalibrationMode(cidx); + sysLogPrintf(LOG_NOTE, "input: GamepadMotion initialized for controller %d", cidx); + } + } + } +#endif +} + +// Pause gyro deltas and orientation to prevent gyro input leak +static inline void inputPauseGyro(s32 cidx) +{ + gyroDeltaYaw[cidx] = gyroDeltaPitch[cidx] = gyroDeltaRoll[cidx] = 0.f; + gyroYaw[cidx] = gyroPitch[cidx] = gyroRoll[cidx] = 0.f; + + // Also reset accelerometer data + accelDeltaX[cidx] = accelDeltaY[cidx] = accelDeltaZ[cidx] = 0.f; + gravityX[cidx] = gravityY[cidx] = gravityZ[cidx] = 0.f; + processedAccelX[cidx] = processedAccelY[cidx] = processedAccelZ[cidx] = 0.f; +} + +// Enable/disable input processing when window loses focus +static inline bool inputHasWindowFocus(void) +{ + if (inputWindowHasFocus) { + SDL_Window *window = SDL_GetKeyboardFocus(); + return (window != NULL); + } + return true; } static inline void inputCloseController(const s32 cidx) @@ -360,6 +509,16 @@ static inline void inputCloseController(const s32 cidx) pads[cidx] = NULL; padsCfg[cidx].rumbleOn = 0; + padsCfg[cidx].gyroSensorActive = 0; + + // Clean up GamepadMotion instance + if (gpadMotion[cidx]) { + gmhDeleteGamepadMotion(gpadMotion[cidx]); + gpadMotion[cidx] = NULL; + sysLogPrintf(LOG_NOTE, "input: GamepadMotion instance cleaned up for controller %d", cidx); + } + + inputPauseGyro(cidx); if (cidx) { connectedMask &= ~(1 << cidx); @@ -545,6 +704,14 @@ static int inputEventFilter(void *data, SDL_Event *event) } break; + case SDL_WINDOWEVENT: + if (event->window.event == SDL_WINDOWEVENT_FOCUS_GAINED) { + // Window gained focus - no immediate action needed + } else if (event->window.event == SDL_WINDOWEVENT_FOCUS_LOST) { + // Window lost focus - input functions will handle their own state resets + } + break; + default: break; } @@ -752,7 +919,7 @@ s32 inputInit(void) return connectedMask; } -static inline s32 inputBindPressed(const s32 idx, const u32 ck) +s32 inputBindPressed(const s32 idx, const u32 ck) { for (s32 i = 0; i < INPUT_MAX_BINDS; ++i) { if (binds[idx][ck][i]) { @@ -790,6 +957,16 @@ s32 inputReadController(s32 idx, OSContPad *npad) npad->button = 0; + // Check window focus using centralized function + if (!inputHasWindowFocus()) { + // Reset controller state when window is out of focus + npad->stick_x = 0; + npad->stick_y = 0; + npad->rstick_x = 0; + npad->rstick_y = 0; + return 0; + } + if (textInput) { npad->stick_x = 0; npad->stick_y = 0; @@ -868,6 +1045,16 @@ s32 inputReadController(s32 idx, OSContPad *npad) static inline void inputUpdateMouse(void) { + // Check window focus using centralized function + if (!inputHasWindowFocus()) { + // Reset mouse state when window is out of focus + mouseButtons = 0; + mouseDX = 0; + mouseDY = 0; + mouseWheel = 0; + return; + } + s32 mx, my; mouseButtons = SDL_GetMouseState(&mx, &my); @@ -908,6 +1095,166 @@ static inline void inputUpdateMouse(void) } } +s32 inputControllerMotionSensorsSupported(s32 cidx) +{ + if (cidx < 0 || cidx >= INPUT_MAX_CONTROLLERS) { + return 0; + } + + return padsCfg[cidx].gyroSensorActive; +} + +static void inputConfigureGamepadMotionSettings(GamepadMotionHandle handle) +{ + if (!handle) return; + + gmhSetMinStillnessCollectionTime(handle, GMH_STILLNESS_COLLECTION_TIME); + gmhSetMinStillnessCorrectionTime(handle, GMH_STILLNESS_CORRECTION_TIME); + gmhSetStillnessGyroDelta(handle, -1.f); + gmhSetStillnessAccelDelta(handle, -1.f); + gmhSetStillnessCalibrationEaseInTime(handle, GMH_STILLNESS_EASE_TIME); + gmhSetMaxStillnessError(handle, GMH_MAX_STILLNESS_ERROR); +} + +static bool inputGetControllerSensorData(s32 cidx, float gyroData[3], float accelData[3]) +{ + if (!padsCfg[cidx].gyroEnabled || !inputControllerMotionSensorsSupported(cidx) || + !pads[cidx] || SDL_GameControllerGetAttached(pads[cidx]) == SDL_FALSE) { + return false; + } + + SDL_GameControllerGetSensorData(pads[cidx], SDL_SENSOR_GYRO, gyroData, 3); + SDL_GameControllerGetSensorData(pads[cidx], SDL_SENSOR_ACCEL, accelData, 3); + return true; +} + +static float inputCalculateGyroDeltaTime(s32 cidx) +{ + static uint64_t lastUpdateTime[INPUT_MAX_CONTROLLERS] = {0}; + uint64_t now = sysGetMicroseconds(); + float deltaTime = 1.0f / 60.0f; // Default to 60fps baseline + +#if SDL_VERSION_ATLEAST(2, 0, 16) + // Use actual sensor data rate for more accurate baseline timing if available + if (pads[cidx]) { + float gyroRate = SDL_GameControllerGetSensorDataRate(pads[cidx], SDL_SENSOR_GYRO); + if (gyroRate > 0.0f) { + deltaTime = 1.0f / gyroRate; // Use actual sensor rate as baseline + } + } +#endif + + if (lastUpdateTime[cidx] != 0) { + float actualDeltaTime = (now - lastUpdateTime[cidx]) / 1000000.0f; + if (actualDeltaTime > 0.0f && actualDeltaTime <= 0.1f) { + // Use the actual deltaTime to ensure GamepadMotionHelper gets proper timing + deltaTime = actualDeltaTime; + } + } + lastUpdateTime[cidx] = now; + + return deltaTime; +} + +static void inputProcessMotionSensorData(s32 cidx, float deltaTime, f32* deltaX, f32* deltaY, f32* deltaZ) +{ + // Validate GamepadMotion instance + if (!gpadMotion[cidx]) { + sysLogPrintf(LOG_WARNING, "GamepadMotion instance missing for controller %d, gyro will not function", cidx); + return; + } + + // Get sensor data + float gyroData[3] = {0.f}, accelData[3] = {0.f}; + if (!inputGetControllerSensorData(cidx, gyroData, accelData)) { + return; + } + + // Process motion data through GamepadMotionHelper + gmhProcessMotion(gpadMotion[cidx], + gyroData[0], gyroData[1], gyroData[2], + accelData[0] / SDL_STANDARD_GRAVITY, + accelData[1] / SDL_STANDARD_GRAVITY, + accelData[2] / SDL_STANDARD_GRAVITY, + deltaTime); + + // Get calibrated gyro output and apply axis mapping + float calibratedGyro[3] = {0.f}; + gmhGetCalibratedGyro(gpadMotion[cidx], &calibratedGyro[0], &calibratedGyro[1], &calibratedGyro[2]); + + // Get gravity vector from GamepadMotion + float gravity[3] = {0.f}; + gmhGetGravity(gpadMotion[cidx], &gravity[0], &gravity[1], &gravity[2]); + gravityX[cidx] = gravity[0]; + gravityY[cidx] = gravity[1]; + gravityZ[cidx] = gravity[2]; + + // Get processed acceleration + float processedAccel[3] = {0.f}; + gmhGetProcessedAcceleration(gpadMotion[cidx], &processedAccel[0], &processedAccel[1], &processedAccel[2]); + processedAccelX[cidx] = processedAccel[0]; + processedAccelY[cidx] = processedAccel[1]; + processedAccelZ[cidx] = processedAccel[2]; + + applyGyroAxisMapping(cidx, calibratedGyro, accelData, deltaX, deltaY, deltaZ); +} + +static void inputApplyGyroProcessing(s32 cidx, f32* deltaX, f32* deltaY, f32* deltaZ) +{ + // Handle post-calibration jump prevention + if (gyroCalibConfig[cidx].justFinishedCalibrating) { + *deltaX = 0.f; + *deltaY = 0.f; + *deltaZ = 0.f; + gyroCalibConfig[cidx].justFinishedCalibrating = 0; + return; + } + + // Apply gyro processing pipeline + applyGyroModifier(deltaX, deltaY, deltaZ, inputGetGyroModifier(cidx), cidx); + applyGyroDeadzone(deltaX, deltaY, deltaZ, inputGyroGetDeadzone(cidx)); + applyGyroTightening(deltaX, deltaY, deltaZ, inputGyroGetTightening(cidx)); + applyGyroSmoothing(deltaX, deltaY, deltaZ, inputGetGyroSmoothing(cidx), cidx); +} + +void inputUpdateGyro(s32 cidx) +{ + // Check window focus + if (!inputHasWindowFocus()) { + // Pause gyro state when window is out of focus + inputPauseGyro(cidx); + return; + } + + // Calculate deltaTime + float deltaTime = inputCalculateGyroDeltaTime(cidx); + + // Process sensor data + f32 deltaX = 0.f, deltaY = 0.f, deltaZ = 0.f; + inputProcessMotionSensorData(cidx, deltaTime, &deltaX, &deltaY, &deltaZ); + + // Monitor auto-calibration status (if enabled) + if (gpadMotion[cidx] && padsCfg[cidx].gyroAutoCalibration != GYRO_AUTOCALIBRATION_OFF) { + float confidence = gmhGetAutoCalibrationConfidence(gpadMotion[cidx]); + + // Store for use in auto-calibration monitoring + gyroCalibConfig[cidx].lastConfidence = confidence; + } + + // Apply gyro processing pipeline + inputApplyGyroProcessing(cidx, &deltaX, &deltaY, &deltaZ); + + // Store processed gyro deltas + gyroDeltaYaw[cidx] = deltaX; + gyroDeltaPitch[cidx] = deltaY; + gyroDeltaRoll[cidx] = deltaZ; + + // Update absolute orientation + gyroYaw[cidx] += deltaX; + gyroPitch[cidx] += deltaY; + gyroRoll[cidx] += deltaZ; +} + void inputUpdate(void) { SDL_GameControllerUpdate(); @@ -915,6 +1262,21 @@ void inputUpdate(void) if (mouseEnabled) { inputUpdateMouse(); } + + // Handle gyro calibration systems + inputUpdateGyroCalibrationHandle(); + + // Process gyro input + for (s32 cidx = 0; cidx < INPUT_MAX_CONTROLLERS; ++cidx) { + if (padsCfg[cidx].gyroEnabled && inputControllerMotionSensorsSupported(cidx)) { + inputUpdateGyro(cidx); + } + } +} + +void inputUpdateGyroCalibrationSystem(void) +{ + inputUpdateGyroCalibrationHandle(); } s32 inputControllerConnected(s32 idx) @@ -1166,6 +1528,11 @@ const u32 *inputKeyGetBinds(s32 idx, u32 ck) s32 inputKeyPressed(u32 vk) { + // Check window focus using centralized function + if (!inputHasWindowFocus()) { + return 0; // No input when window is out of focus + } + if (vk >= VK_KEYBOARD_BEGIN && vk < VK_MOUSE_BEGIN) { const u8 *state = SDL_GetKeyboardState(NULL); return state[vk - VK_KEYBOARD_BEGIN]; @@ -1219,6 +1586,7 @@ s32 inputButtonPressed(s32 idx, u32 contbtn) return inputBindPressed(idx, inputContToContKey(contbtn)); } + void inputLockMouse(s32 lock) { mouseLocked = !!lock; @@ -1323,6 +1691,847 @@ void inputSetMouseLockMode(s32 lockmode) } } +s32 inputGyroIsEnabled(s32 cidx) +{ + if (cidx < 0 || cidx >= INPUT_MAX_CONTROLLERS) { + return 0; + } + + return padsCfg[cidx].gyroEnabled && inputControllerMotionSensorsSupported(cidx); +} + +void inputGyroEnable(s32 cidx, s32 enabled) +{ + if (cidx < 0 || cidx >= INPUT_MAX_CONTROLLERS) { + return; + } + + padsCfg[cidx].gyroEnabled = (enabled != 0); +} + +s32 inputGyroGetAxisMode(s32 cidx) +{ + return padsCfg[cidx].gyroAxisMode; +} + +void inputGyroSetAxisMode(s32 cidx, s32 mode) +{ + padsCfg[cidx].gyroAxisMode = mode; +} + +static void applyGyroAxisMapping(s32 cidx, float calibratedGyro[3], float accelData[3], f32* deltaX, f32* deltaY, f32* deltaZ) +{ + if (!gpadMotion[cidx]) { + *deltaX = *deltaY = *deltaZ = 0.f; + return; + } + + // Check if axis mode has changed and reset motion state to prevent bleedthrough + static s32 previousAxisMode[INPUT_MAX_CONTROLLERS] = {-1, -1, -1, -1}; + s32 currentAxisMode = inputGyroGetAxisMode(cidx); + if (previousAxisMode[cidx] != currentAxisMode) { + gmhResetMotion(gpadMotion[cidx]); + previousAxisMode[cidx] = currentAxisMode; + } + + switch (inputGyroGetAxisMode(cidx)) { + case GYRO_YAW: + *deltaX = -calibratedGyro[1]; + *deltaY = -calibratedGyro[0]; + break; + case GYRO_ROLL: + *deltaX = calibratedGyro[2]; + *deltaY = -calibratedGyro[0]; + break; + case GYRO_LOCAL: { + float orientation_w = 0.f; + float orientation[3] = {0.f}; + gmhGetOrientation(gpadMotion[cidx], &orientation_w, &orientation[0], &orientation[1], &orientation[2]); + *deltaX = -calibratedGyro[1] + (calibratedGyro[2] * 0.85f); + *deltaY = -calibratedGyro[0]; + *deltaZ = calibratedGyro[2]; + break; + } + case GYRO_PLAYER: { + float playerX = 0.f, playerY = 0.f; + gmhGetPlayerSpaceGyro(gpadMotion[cidx], &playerX, &playerY, 1.41f); + *deltaX = -playerY; + *deltaY = -playerX; + *deltaZ = calibratedGyro[2]; + break; + } + case GYRO_WORLD: { + float worldX = 0.f, worldY = 0.f; + gmhGetWorldSpaceGyro(gpadMotion[cidx], &worldX, &worldY, 0.125f); + float worldZ = calibratedGyro[2]; + float tiltReduction = 1.0f - fabsf(gravityX[cidx]); + worldZ *= tiltReduction; + + *deltaX = -worldY; + *deltaY = -worldX; + *deltaZ = worldZ; + break; + } + default: + *deltaX = *deltaY = *deltaZ = 0.f; + break; + } +} + +s32 inputGetGyroAimMode(s32 cidx) +{ + return padsCfg[cidx].gyroAimMode; +} + +void inputSetGyroAimMode(s32 cidx, s32 mode) +{ + padsCfg[cidx].gyroAimMode = mode; +} + +void inputGyroGetRawDelta(s32 cidx, s32* dx, s32* dy, s32* dz) +{ + if (dx) *dx = (s32)gyroDeltaYaw[cidx]; + if (dy) *dy = (s32)gyroDeltaPitch[cidx]; + if (dz) *dz = (s32)gyroDeltaRoll[cidx]; +} + +void inputGyroGetScaledDelta(s32 cidx, f32* dx, f32* dy, f32* dz) +{ + if (!dx || !dy || !dz) return; + + f32 gdx = 0.f, gdy = 0.f, gdz = 0.f; + + if (padsCfg[cidx].gyroEnabled) { + if (!isnan(gyroDeltaYaw[cidx]) && !isnan(gyroDeltaPitch[cidx]) && !isnan(gyroDeltaRoll[cidx])) { + gdx = gyroDeltaYaw[cidx] * padsCfg[cidx].gyroSensX; + gdy = gyroDeltaPitch[cidx] * padsCfg[cidx].gyroSensY; + gdz = gyroDeltaRoll[cidx] * padsCfg[cidx].gyroSensY; + } + } + + *dx = gdx; + *dy = gdy; + *dz = gdz; + + applyGyroInvert(cidx, dx, dy, false); + applyGyroVHMixer(cidx, dx, dy); +} + +void inputGyroGetSpeed(s32 cidx, f32* x, f32* y) +{ + if (x) *x = padsCfg[cidx].gyroSensX; + if (y) *y = padsCfg[cidx].gyroSensY; +} + +void inputGyroSetSpeed(s32 cidx, f32 x, f32 y) +{ + padsCfg[cidx].gyroSensX = x; + padsCfg[cidx].gyroSensY = y; +} + +void inputGyroGetScaledDeltaCrosshair(s32 cidx, f32* dx, f32* dy) +{ + f32 gdx = 0.f, gdy = 0.f; + + if (padsCfg[cidx].gyroEnabled) { + gdx = gyroDeltaYaw[cidx] * (0.022f / 2.0f) * padsCfg[cidx].gyroAimSensX; + gdy = gyroDeltaPitch[cidx] * (0.022f / 2.0f) * padsCfg[cidx].gyroAimSensY; + } + + if (dx) *dx = gdx; + if (dy) *dy = gdy; + + if (dx && dy) { + applyGyroInvert(cidx, dx, dy, true); + applyGyroVHMixer(cidx, dx, dy); + } +} + +void inputGyroGetAimSpeed(s32 cidx, f32* x, f32* y) +{ + if (x) *x = padsCfg[cidx].gyroAimSensX; + if (y) *y = padsCfg[cidx].gyroAimSensY; +} + +void inputGyroSetAimSpeed(s32 cidx, f32 x, f32 y) +{ + padsCfg[cidx].gyroAimSensX = x; + padsCfg[cidx].gyroAimSensY = y; +} + +static inline void applyGyroInvert(s32 cidx, f32* dx, f32* dy, bool useAimInvert) { + if (useAimInvert) { + if (padsCfg[cidx].gyroAimInvertX) *dx = -*dx; + if (padsCfg[cidx].gyroAimInvertY) *dy = -*dy; + } else { + if (padsCfg[cidx].gyroInvertX) *dx = -*dx; + if (padsCfg[cidx].gyroInvertY) *dy = -*dy; + } +} + +void inputGyroGetInvert(s32 cidx, s32* out_invertx, s32* out_inverty) +{ + if (out_invertx) *out_invertx = padsCfg[cidx].gyroInvertX; + if (out_inverty) *out_inverty = padsCfg[cidx].gyroInvertY; +} + +void inputGyroSetInvert(s32 cidx, s32 invertx, s32 inverty) +{ + padsCfg[cidx].gyroInvertX = invertx ? 1 : 0; + padsCfg[cidx].gyroInvertY = inverty ? 1 : 0; +} + +void inputGyroGetAimInvert(s32 cidx, s32* out_invertx, s32* out_inverty) +{ + if (out_invertx) *out_invertx = padsCfg[cidx].gyroAimInvertX; + if (out_inverty) *out_inverty = padsCfg[cidx].gyroAimInvertY; +} + +void inputGyroSetAimInvert(s32 cidx, s32 invertx, s32 inverty) +{ + padsCfg[cidx].gyroAimInvertX = invertx ? 1 : 0; + padsCfg[cidx].gyroAimInvertY = inverty ? 1 : 0; +} + +static inline void applyGyroVHMixer(s32 cidx, f32* dx, f32* dy) { + float mix = fminf(fmaxf(padsCfg[cidx].gyroVHMixer, -1.0f), 1.0f); + + float hScale = 1.0f - fmaxf(0.0f, mix); + float vScale = 1.0f + fminf(0.0f, mix); + + *dx *= hScale; + *dy *= vScale; +} + +f32 inputGetGyroVHMixer(s32 cidx) +{ + return padsCfg[cidx].gyroVHMixer; +} + +void inputSetGyroVHMixer(s32 cidx, f32 value) +{ + if (value < -1.0f) value = -1.0f; + if (value > 1.0f) value = 1.0f; + padsCfg[cidx].gyroVHMixer = value; +} + +s32 inputGetGyroModifier(s32 cidx) +{ + return padsCfg[cidx].gyroModifier; +} + +void inputSetGyroModifier(s32 cidx, s32 mode) +{ + padsCfg[cidx].gyroModifier = mode; +} + +static void applyGyroModifier(f32* deltaX, f32* deltaY, f32* deltaZ, s32 activationMode, s32 idx) { + static bool toggleState[INPUT_MAX_CONTROLLERS] = { true, true, true, true }; + static int prevGyroMod[INPUT_MAX_CONTROLLERS] = { 0 }; + + const int modPressed = inputBindPressed(idx, CK_0080); + const int justPressed = modPressed && !prevGyroMod[idx]; + prevGyroMod[idx] = modPressed; + + bool gyroActive = false; + + switch (activationMode) { + case GYRO_ALWAYS_ON: + gyroActive = true; + break; + case GYRO_TOGGLE: + if (justPressed) { + toggleState[idx] = !toggleState[idx]; + } + gyroActive = toggleState[idx]; + break; + case GYRO_ENABLE_HELD: + gyroActive = modPressed; + break; + case GYRO_DISABLE_HELD: + gyroActive = !modPressed; + break; + default: + gyroActive = false; + break; + } + + if (!gyroActive) { + *deltaX = 0.f; + *deltaY = 0.f; + *deltaZ = 0.f; + } +} + +f32 inputGetGyroSmoothing(s32 cidx) +{ + return padsCfg[cidx].gyroSmoothing; +} + +void inputSetGyroSmoothing(s32 cidx, f32 smoothing) +{ + if (smoothing < 0.0f) smoothing = 0.0f; + if (smoothing > 1.0f) smoothing = 1.0f; + padsCfg[cidx].gyroSmoothing = smoothing; +} + +static void applyGyroSmoothing(f32* deltaX, f32* deltaY, f32* deltaZ, f32 smoothing, s32 cidx) +{ + if (!deltaX || !deltaY || !deltaZ || smoothing <= 0.0f) return; + + // Use a circular buffer to store recent gyro deltas for smoothing + static f32 history[INPUT_MAX_CONTROLLERS][3][8] = {0}; + static s32 historyIndex[INPUT_MAX_CONTROLLERS] = {0}; + + s32 windowSize = 1 + (s32)(smoothing * 3.0f); + + s32 idx = historyIndex[cidx]; + history[cidx][0][idx] = *deltaX; + history[cidx][1][idx] = *deltaY; + history[cidx][2][idx] = *deltaZ; + + f32 avgX = 0.f, avgY = 0.f, avgZ = 0.f; + for (s32 i = 0; i < windowSize; ++i) { + s32 sampleIdx = (idx - i + 8) % 8; + avgX += history[cidx][0][sampleIdx]; + avgY += history[cidx][1][sampleIdx]; + avgZ += history[cidx][2][sampleIdx]; + } + + avgX /= windowSize; + avgY /= windowSize; + avgZ /= windowSize; + + // Simple blend: 70% smoothed + 30% raw + f32 blend = smoothing * 0.7f; + *deltaX = avgX * blend + (*deltaX) * (1.0f - blend); + *deltaY = avgY * blend + (*deltaY) * (1.0f - blend); + *deltaZ = avgZ * blend + (*deltaZ) * (1.0f - blend); + + historyIndex[cidx] = (idx + 1) % 8; +} + +f32 inputGyroGetTightening(s32 cidx) +{ + return padsCfg[cidx].gyroTightening; +} + +void inputGyroSetTightening(s32 cidx, f32 tightening) +{ + if (tightening < 0.f) tightening = 0.f; + if (tightening > 1.f) tightening = 1.f; + padsCfg[cidx].gyroTightening = tightening; +} + +static void applyGyroTightening(f32* dx, f32* dy, f32* dz, f32 tightening) +{ + if (!dx || !dy || !dz || tightening <= 0.0f) return; + + f32 mag = sqrtf((*dx) * (*dx) + (*dy) * (*dy) + (*dz) * (*dz)); + if (mag > 0.f) { + f32 scale = 1.0f; + + // Soft tiered scaling with multiple breakpoints for smoother transitions + // based on http://gyrowiki.jibbsmart.com/blog:tight-and-smooth:soft-tiered-smoothing + if (mag < tightening) { + f32 ratio = mag / tightening; + + if (ratio < 0.25f) { + // Very small movements: moderate reduction instead of heavy + scale = ratio * ratio * 4.0f; + } else if (ratio < 0.5f) { + // Small movements: gentler reduction with quadratic curve + f32 adjustedRatio = (ratio - 0.25f) / 0.25f; + scale = 0.25f + adjustedRatio * adjustedRatio * 0.5f; + } else if (ratio < 0.75f) { + // Medium-small movements: minimal reduction with linear interpolation + f32 adjustedRatio = (ratio - 0.5f) / 0.25f; + scale = 0.75f + adjustedRatio * 0.2f; + } else { + // Near-threshold movements: very minimal reduction + f32 adjustedRatio = (ratio - 0.75f) / 0.25f; + scale = 0.95f + adjustedRatio * adjustedRatio * 0.05f; + } + } + + *dx *= scale; + *dy *= scale; + *dz *= scale; + } +} + +f32 inputGyroGetDeadzone(s32 cidx) +{ + return padsCfg[cidx].gyroDeadzone; +} + +void inputGyroSetDeadzone(s32 cidx, f32 deadzone) +{ + if (deadzone < 0.f) deadzone = 0.f; + if (deadzone > 1.f) deadzone = 1.f; + + if (deadzone > 0.f && deadzone < 0.01f) deadzone = 0.01f; + padsCfg[cidx].gyroDeadzone = deadzone; +} + +static void applyGyroDeadzone(f32* dx, f32* dy, f32* dz, f32 deadzone) +{ + if (deadzone <= 0.f || !dx || !dy || !dz) return; + + f32 mag = sqrtf((*dx) * (*dx) + (*dy) * (*dy) + (*dz) * (*dz)); + if (mag > 0.f) { + f32 scale = 1.0f; + + if (mag <= deadzone) { + // Complete deadzone - no output + scale = 0.0f; + } else { + // Gentle exit transition zone + f32 exitZone = deadzone * 0.2f; // 20% of deadzone as exit zone + f32 exitThreshold = deadzone + exitZone; + + if (mag < exitThreshold) { + // Smooth transition in exit zone + f32 exitRatio = (mag - deadzone) / exitZone; + scale = exitRatio * exitRatio; // Quadratic for smooth ramp-up + } + } + + *dx *= scale; + *dy *= scale; + *dz *= scale; + } +} + +static void inputUpdateGyroCalibrationHandle(void) +{ + // Process calibration for all controllers + for (s32 cidx = 0; cidx < INPUT_MAX_CONTROLLERS; ++cidx) { + if (!padsCfg[cidx].gyroEnabled || !inputControllerMotionSensorsSupported(cidx)) { + continue; + } + + // Handle manual calibration input + inputUpdateGyroManualCalibration(cidx); + + // Handle auto-calibration if enabled + if (padsCfg[cidx].gyroAutoCalibration != GYRO_AUTOCALIBRATION_OFF) { + inputUpdateGyroAutoCalibration(cidx); + } + } +} + +static bool inputIsControllerSensorNoiseThreshold(s32 cidx) +{ + if (!gpadMotion[cidx]) return false; + + float gyroData[3] = {0.f}, accelData[3] = {0.f}; + if (!inputGetControllerSensorData(cidx, gyroData, accelData)) return false; + + // Check gyro threshold + float strictGyroThreshold = GYRO_NOISE_THRESHOLD * 0.15f; + for (s32 i = 0; i < 3; ++i) { + if (fabsf(gyroData[i]) > strictGyroThreshold) return false; + } + + // Check gyro magnitude + float gyroMagnitude = sqrtf(gyroData[0] * gyroData[0] + gyroData[1] * gyroData[1] + gyroData[2] * gyroData[2]); + if (gyroMagnitude > strictGyroThreshold) return false; + + // Check accelerometer stability + float accelThreshold = 0.15f; + for (s32 i = 0; i < 3; ++i) { + float expectedGravity = (i == 1) ? SDL_STANDARD_GRAVITY : 0.0f; + if (fabsf(accelData[i] - expectedGravity) > accelThreshold) return false; + } + + // Check acceleration magnitude + float accelMagnitude = sqrtf(accelData[0] * accelData[0] + accelData[1] * accelData[1] + accelData[2] * accelData[2]); + return fabsf(accelMagnitude - SDL_STANDARD_GRAVITY) < 0.05f; +} + +// Check if auto-calibration should be active based on the current mode and game state +static bool inputGyroAutoCalibrationModes(s32 cidx) +{ + s32 autoCalibMode = padsCfg[cidx].gyroAutoCalibration; + + switch (autoCalibMode) { + case GYRO_AUTOCALIBRATION_OFF: + return false; + case GYRO_AUTOCALIBRATION_MENU_ONLY: + return g_MenuData.isdialogopen; + case GYRO_AUTOCALIBRATION_STATIONARY: + return true; + case GYRO_AUTOCALIBRATION_ALWAYS: + return true; + default: + return false; + } +} + +void inputGyroCalibration(s32 cidx, GyroCalibrationOp op, float* out_confidence, int* out_steady) +{ + if (cidx < 0 || cidx >= INPUT_MAX_CONTROLLERS) return; + + switch (op) { + case GYRO_CALIB_START: + inputGyroCalibrationActivation(cidx, true, false); + break; + case GYRO_CALIB_FINISH: + inputGyroCalibrationActivation(cidx, false, false); + break; + case GYRO_CALIB_RESET: + inputGyroCalibrationActivation(cidx, false, true); + break; + case GYRO_CALIB_QUERY: + if (gpadMotion[cidx]) { + if (out_confidence) *out_confidence = gmhGetAutoCalibrationConfidence(gpadMotion[cidx]); + if (out_steady) *out_steady = gmhGetAutoCalibrationIsSteady(gpadMotion[cidx]); + } + break; + case GYRO_CALIB_AUTO: + inputUpdateGyroAutoCalibration(cidx); + break; + } +} + +static void inputGyroCalibrationOffset(s32 cidx, bool save, bool apply) +{ + GyroCalibConfig *config = &gyroCalibConfig[cidx]; + + if (!gpadMotion[cidx]) return; + + // Only handle offsets for modes that use persistent calibration + bool isPersistentMode = (padsCfg[cidx].gyroAutoCalibration == GYRO_AUTOCALIBRATION_OFF || + padsCfg[cidx].gyroAutoCalibration == GYRO_AUTOCALIBRATION_MENU_ONLY); + + if (!isPersistentMode) return; + + if (save) { + // Save current calibration offset + gmhGetCalibrationOffset(gpadMotion[cidx], + &config->manualOffsetX, + &config->manualOffsetY, + &config->manualOffsetZ); + config->manualWeight = 100; + } + + if (apply && config->manualWeight > 0) { + // Apply saved calibration offset + gmhSetCalibrationOffset(gpadMotion[cidx], + config->manualOffsetX, config->manualOffsetY, config->manualOffsetZ, + config->manualWeight); + } +} + +static void inputUpdateGyroAutoCalibration(s32 cidx) +{ + if (!gpadMotion[cidx] || padsCfg[cidx].gyroAutoCalibration == GYRO_AUTOCALIBRATION_OFF) return; + + GyroCalibConfig *config = &gyroCalibConfig[cidx]; + + // Allow Gyro Manual Calibration if MENU_ONLY Auto-Calibration mode is enabled, amongst others + if (config->manualCalibActive && !inputIsGyroCalibrationBlocked(cidx)) { + return; + } + + if (inputIsMenuGyroCalibrationActive(cidx)) { + // Pause ALL auto-calibration modes when gyro calibration UI is active + // This prevents conflicts both Auto and Manual calibration during the countdown + gmhPauseContinuousCalibration(gpadMotion[cidx]); + return; + } + + // Check if auto-calibration should be active based on the current mode and game state + if (!inputGyroAutoCalibrationModes(cidx)) { + // Auto-calibration is not active, pause but preserve progress + gmhPauseContinuousCalibration(gpadMotion[cidx]); + return; + } + + // Dispatch to specialized auto-calibration functions + if (padsCfg[cidx].gyroAutoCalibration == GYRO_AUTOCALIBRATION_ALWAYS) { + inputUpdateGyroAutoCalibrationAlways(cidx); + } else { + // STATIONARY and MENU_ONLY modes + inputUpdateGyroAutoCalibrationStationary(cidx); + } +} + +static void inputUpdateGyroAutoCalibrationUnified(s32 cidx) +{ + GyroCalibConfig *config = &gyroCalibConfig[cidx]; + + Uint32 now = SDL_GetTicks(); + float confidence = gmhGetAutoCalibrationConfidence(gpadMotion[cidx]); + bool isSteady = gmhGetAutoCalibrationIsSteady(gpadMotion[cidx]); + bool isStable = isSteady && inputIsControllerSensorNoiseThreshold(cidx); + + if (isStable) { + // Just became stable - apply initial delay + if (!config->wasStable) { + config->justFinishedCalibrating = 0; + config->lastAutoCalibTime = now - (config->calibInterval - config->initialDelay); + } + + Uint32 elapsed = now - config->lastAutoCalibTime; + + // Start calibration if conditions are met + if (elapsed > config->calibInterval && confidence < config->startThreshold) { + if (config->stableRequired > 1) { + // STATIONARY mode with stability requirements + if (isSteady && inputIsControllerSensorNoiseThreshold(cidx)) { + if (++config->stableCount >= config->stableRequired) { + gmhStartContinuousCalibration(gpadMotion[cidx]); + config->justFinishedCalibrating = 0; + config->lastAutoCalibTime = now; + config->stableCount = 0; + } + } else { + config->stableCount = 0; + } + } else { + // ALWAYS mode - start immediately + gmhStartContinuousCalibration(gpadMotion[cidx]); + } + } + + // Complete calibration when confidence is sufficient + if (!config->justFinishedCalibrating && confidence > config->confidenceTarget) { + if (config->saveOffset) { + inputGyroCalibrationOffset(cidx, true, true); + } + config->justFinishedCalibrating = 1; + gmhPauseContinuousCalibration(gpadMotion[cidx]); + config->lastAutoCalibTime = now; + } + + // Check for restart (ALWAYS mode only) + if (config->justFinishedCalibrating && config->confidenceMin > 0.0f && + elapsed > config->initialDelay && confidence < config->confidenceMin) { + config->justFinishedCalibrating = 0; + config->lastAutoCalibTime = now; + } + } else { + // Controller moving + config->stableCount = 0; + if (config->wasStable) { + gmhPauseContinuousCalibration(gpadMotion[cidx]); + if (confidence < config->stopThreshold) { + config->justFinishedCalibrating = 0; + } + } + } + + config->wasStable = isStable; + config->lastConfidence = confidence; +} + +static void inputUpdateGyroAutoCalibrationAlways(s32 cidx) +{ + GyroCalibConfig *config = &gyroCalibConfig[cidx]; + + config->calibInterval = 1000; // 1s minimum stable before starting + config->initialDelay = 500; // Start calibration in 0.5s + config->confidenceTarget = 0.80f; // Target confidence for completion + config->confidenceMin = 0.60f; // Minimum confidence before restart + config->startThreshold = 1.0f; // Always start (high threshold) + config->stopThreshold = 0.25f; // Stop if confidence drops this low + config->stableRequired = 1; // No stability count required + config->saveOffset = 0; // No offset saving for ALWAYS mode + + inputUpdateGyroAutoCalibrationUnified(cidx); +} + +static void inputUpdateGyroAutoCalibrationStationary(s32 cidx) +{ + GyroCalibConfig *config = &gyroCalibConfig[cidx]; + + config->calibInterval = 10000; // 10s between calibration attempts + config->initialDelay = 2000; // Start calibration in 2s + config->confidenceTarget = 0.90f; // High confidence threshold + config->confidenceMin = 0.0f; // No restart logic + config->startThreshold = 0.8f; // Start calibration threshold + config->stopThreshold = 0.3f; // Stop calibration threshold + config->stableRequired = 3; // Sustained stability checks required + config->saveOffset = (padsCfg[cidx].gyroAutoCalibration == GYRO_AUTOCALIBRATION_MENU_ONLY); + + inputUpdateGyroAutoCalibrationUnified(cidx); +} + +s32 inputGyroGetAutoCalibration(s32 cidx) +{ + return (cidx >= 0 && cidx < INPUT_MAX_CONTROLLERS) ? padsCfg[cidx].gyroAutoCalibration : 0; +} + +void inputGyroSetAutoCalibration(s32 cidx, s32 enabled) +{ + if (cidx < 0 || cidx >= INPUT_MAX_CONTROLLERS) return; + + s32 wasEnabled = padsCfg[cidx].gyroAutoCalibration; + padsCfg[cidx].gyroAutoCalibration = enabled; + + if (wasEnabled != padsCfg[cidx].gyroAutoCalibration) { + const char* modeNames[] = {"Disabled", "While Stationary", "In Menus Only", "Always"}; + const char* modeName = (enabled >= 0 && enabled < 4) ? modeNames[enabled] : "Unknown"; + sysLogPrintf(LOG_NOTE, "Input: Gyro auto-calibration set to '%s' for controller %d.", modeName, cidx); + + if (!gpadMotion[cidx]) { + inputGyroCalibrationActivation(cidx, false, true); + } else { + inputConfigureGyroCalibrationMode(cidx); + } + } +} + +static void inputUpdateGyroManualCalibration(s32 cidx) +{ + if (!pads[cidx] || !gpadMotion[cidx]) { + gyroCalibConfig[cidx].manualCalibActive = 0; + return; + } + + GyroCalibConfig *config = &gyroCalibConfig[cidx]; + + // Stop calibration if blocked + if (inputIsGyroCalibrationBlocked(cidx)) { + if (config->manualCalibActive) { + inputGyroCalibrationActivation(cidx, false, false); + } + return; + } + + static bool prevPressed[INPUT_MAX_CONTROLLERS] = {false}; + bool pressed = inputBindPressed(cidx, CK_0100); + + // Start manual calibration on button press + if (pressed && !prevPressed[cidx] && !config->manualCalibActive) { + inputGyroCalibrationActivation(cidx, true, false); + } + + // Auto-finish after 0.5 seconds + if (config->manualCalibActive && + (SDL_GetTicks() - config->manualCalibStartTime) >= 500) { + inputGyroCalibrationActivation(cidx, false, false); + } + + prevPressed[cidx] = pressed; +} + +static void inputGyroCalibrationActivation(s32 cidx, bool start, bool reset) +{ + GyroCalibConfig *config = &gyroCalibConfig[cidx]; + if (!gpadMotion[cidx]) return; + + if (reset) { + // Reset GamepadMotion and all gyro state + gmhResetGamepadMotion(gpadMotion[cidx]); + inputConfigureGamepadMotionSettings(gpadMotion[cidx]); + + gyroYaw[cidx] = gyroPitch[cidx] = gyroRoll[cidx] = 0.f; + gyroDeltaYaw[cidx] = gyroDeltaPitch[cidx] = gyroDeltaRoll[cidx] = 0.f; + accelDeltaX[cidx] = accelDeltaY[cidx] = accelDeltaZ[cidx] = 0.f; + + memset(&gyroCalibConfig[cidx], 0, sizeof(GyroCalibConfig)); + inputConfigureGyroCalibrationMode(cidx); + return; + } + + Uint32 now = SDL_GetTicks(); + + if (start) { + config->manualCalibActive = 1; + config->manualCalibStartTime = now; + config->justFinishedCalibrating = 0; + + gmhSetCalibrationMode(gpadMotion[cidx], CALIBRATIONMODE_MANUAL); + gmhResetContinuousCalibration(gpadMotion[cidx]); + gmhStartContinuousCalibration(gpadMotion[cidx]); + } else { + config->manualCalibActive = 0; + config->justFinishedCalibrating = 1; + config->lastCalibrationTime = now; + + gmhPauseContinuousCalibration(gpadMotion[cidx]); + inputGyroCalibrationOffset(cidx, true, true); + } +} + +s32 inputGyroGetManualCalibration(s32 cidx) +{ + return (cidx >= 0 && cidx < INPUT_MAX_CONTROLLERS) ? gyroCalibConfig[cidx].manualCalibActive : 0; +} + +void inputGyroSetManualCalibration(s32 cidx) +{ + if (cidx < 0 || cidx >= INPUT_MAX_CONTROLLERS || !pads[cidx] || !gpadMotion[cidx] || + inputIsGyroCalibrationBlocked(cidx)) return; + + inputGyroCalibrationActivation(cidx, true, false); +} + +static void inputConfigureGyroCalibrationMode(s32 cidx) +{ + if (!gpadMotion[cidx]) return; + + GyroCalibConfig *config = &gyroCalibConfig[cidx]; + + // Preserve offset for OFF/MENU_ONLY modes + if ((padsCfg[cidx].gyroAutoCalibration <= GYRO_AUTOCALIBRATION_MENU_ONLY) && + config->manualWeight == 0) { + float currentOffsetX, currentOffsetY, currentOffsetZ; + gmhGetCalibrationOffset(gpadMotion[cidx], ¤tOffsetX, ¤tOffsetY, ¤tOffsetZ); + + if (currentOffsetX != 0.0f || currentOffsetY != 0.0f || currentOffsetZ != 0.0f) { + config->manualOffsetX = currentOffsetX; + config->manualOffsetY = currentOffsetY; + config->manualOffsetZ = currentOffsetZ; + config->manualWeight = 100; + } + } + + // Reset state for all modes + Uint32 now = SDL_GetTicks(); + *config = (GyroCalibConfig){ + .manualOffsetX = config->manualOffsetX, .manualOffsetY = config->manualOffsetY, + .manualOffsetZ = config->manualOffsetZ, .manualWeight = config->manualWeight, + .lastAutoCalibTime = now + }; + + // Common reset operations for all modes + gmhResetGamepadMotion(gpadMotion[cidx]); + inputConfigureGamepadMotionSettings(gpadMotion[cidx]); + gmhResetContinuousCalibration(gpadMotion[cidx]); + + // Mode-specific configuration + switch (padsCfg[cidx].gyroAutoCalibration) { + case GYRO_AUTOCALIBRATION_OFF: + case GYRO_AUTOCALIBRATION_MENU_ONLY: + gmhPauseContinuousCalibration(gpadMotion[cidx]); + inputGyroCalibrationOffset(cidx, false, true); + break; + + case GYRO_AUTOCALIBRATION_STATIONARY: + gmhSetCalibrationMode(gpadMotion[cidx], CALIBRATIONMODE_STILLNESS); + gmhPauseContinuousCalibration(gpadMotion[cidx]); + break; + + case GYRO_AUTOCALIBRATION_ALWAYS: + gmhSetCalibrationMode(gpadMotion[cidx], CALIBRATIONMODE_STILLNESS | CALIBRATIONMODE_SENSORFUSION); + break; + + default: + gmhPauseContinuousCalibration(gpadMotion[cidx]); + break; + } +} + +// Block gyro calibration if Gyro Calibration UI is active +static bool inputIsGyroCalibrationBlocked(s32 cidx) +{ + return inputIsMenuGyroCalibrationActive(cidx); +} + const char *inputGetContKeyName(u32 ck) { if (ck >= CK_TOTAL_COUNT) { @@ -1515,6 +2724,7 @@ PD_CONSTRUCTOR static void inputConfigInit(void) configRegisterFloat("Input.MouseSpeedY", &mouseSensY, -30.f, 30.f); configRegisterInt("Input.FakeGamepads", &fakeControllers, 0, 4); configRegisterInt("Input.FirstGamepadNum", &firstController, 0, 3); + configRegisterInt("Input.InputWindowFocus", &inputWindowHasFocus, 0, 1); configRegisterInt("Input.UseHIDAPI", &useHIDAPI, 0, 1); configRegisterInt("Input.UseRawInput", &useRawInput, 0, 1); @@ -1532,6 +2742,23 @@ PD_CONSTRUCTOR static void inputConfigInit(void) configRegisterFloat(strFmt("%s.LStickScaleY", secname), &padsCfg[c].sens[1], -10.f, 10.f); configRegisterFloat(strFmt("%s.RStickScaleX", secname), &padsCfg[c].sens[2], -10.f, 10.f); configRegisterFloat(strFmt("%s.RStickScaleY", secname), &padsCfg[c].sens[3], -10.f, 10.f); + configRegisterInt(strFmt("%s.GyroEnabled", secname), &padsCfg[c].gyroEnabled, 0, 1); + configRegisterInt(strFmt("%s.GyroAxisMode", secname), &padsCfg[c].gyroAxisMode, GYRO_YAW, GYRO_WORLD); + configRegisterInt(strFmt("%s.GyroAimMode", secname), &padsCfg[c].gyroAimMode, GYRO_AIM_CAMERA, GYRO_AIM_BOTH); + configRegisterInt(strFmt("%s.GyroModifier", secname), &padsCfg[c].gyroModifier, GYRO_ALWAYS_ON, GYRO_DISABLE_HELD); + configRegisterFloat(strFmt("%s.GyroSpeedX", secname), &padsCfg[c].gyroSensX, -30.f, 30.f); + configRegisterFloat(strFmt("%s.GyroSpeedY", secname), &padsCfg[c].gyroSensY, -30.f, 30.f); + configRegisterFloat(strFmt("%s.GyroAimSensX", secname), &padsCfg[c].gyroAimSensX, -10.f, 10.f); + configRegisterFloat(strFmt("%s.GyroAimSensY", secname), &padsCfg[c].gyroAimSensY, -10.f, 10.f); + configRegisterFloat(strFmt("%s.GyroVHMixer", secname), &padsCfg[c].gyroVHMixer, -1.0f, 1.0f); + configRegisterInt(strFmt("%s.GyroInvertX", secname), &padsCfg[c].gyroInvertX, 0, 1); + configRegisterInt(strFmt("%s.GyroInvertY", secname), &padsCfg[c].gyroInvertY, 0, 1); + configRegisterInt(strFmt("%s.GyroAimInvertX", secname), &padsCfg[c].gyroAimInvertX, 0, 1); + configRegisterInt(strFmt("%s.GyroAimInvertY", secname), &padsCfg[c].gyroAimInvertY, 0, 1); + configRegisterFloat(strFmt("%s.GyroDeadzone", secname), &padsCfg[c].gyroDeadzone, 0.f, 1.f); + configRegisterFloat(strFmt("%s.GyroTightening", secname), &padsCfg[c].gyroTightening, 0.f, 10.f); + configRegisterFloat(strFmt("%s.GyroSmoothing", secname), &padsCfg[c].gyroSmoothing, 0.f, 1.f); + configRegisterInt(strFmt("%s.GyroAutoCalibration", secname), &padsCfg[c].gyroAutoCalibration, GYRO_AUTOCALIBRATION_OFF, GYRO_AUTOCALIBRATION_ALWAYS); configRegisterInt(strFmt("%s.StickCButtons", secname), &padsCfg[c].stickCButtons, 0, 1); configRegisterInt(strFmt("%s.CancelCButtons", secname), &padsCfg[c].cancelCButtons, 0, 1); configRegisterInt(strFmt("%s.SwapSticks", secname), &padsCfg[c].swapSticks, 0, 1); diff --git a/port/src/optionsmenu.c b/port/src/optionsmenu.c index 5632153346..168a7dea9b 100644 --- a/port/src/optionsmenu.c +++ b/port/src/optionsmenu.c @@ -2,6 +2,7 @@ #include #include #include +#include #include #include "platform.h" #include "data.h" @@ -17,6 +18,9 @@ static s32 g_ExtMenuPlayer = 0; static struct menudialogdef *g_ExtNextDialog = NULL; +static s32 g_GyroCalibrationState[INPUT_MAX_CONTROLLERS] = {0}; +static u32 g_GyroCalibrationStartTime[INPUT_MAX_CONTROLLERS] = {0}; +static bool g_GyroCalibrationComplete[INPUT_MAX_CONTROLLERS] = {0}; static s32 g_BindIndex = 0; static u32 g_BindContKey = 0; @@ -95,6 +99,16 @@ static MenuItemHandlerResult menuhandlerSelectPlayer(s32 operation, struct menui return 0; } +// Check if menu-driven gyro calibration is active for a player +s32 inputIsMenuGyroCalibrationActive(s32 cidx) +{ + if (cidx < 0 || cidx >= INPUT_MAX_CONTROLLERS) { + return 0; + } + + return (g_GyroCalibrationState[cidx] == 1) ? 1 : 0; +} + static MenuItemHandlerResult menuhandlerMouseEnabled(s32 operation, struct menuitem *item, union handlerdata *data) { switch (operation) { @@ -572,6 +586,629 @@ static MenuItemHandlerResult menuhandlerSwapSticks(s32 operation, struct menuite return 0; } +static MenuItemHandlerResult menuhandlerGyroEnabled(s32 operation, struct menuitem* item, union handlerdata* data) +{ + switch (operation) { + case MENUOP_GET: + return inputGyroIsEnabled(g_ExtMenuPlayer); + case MENUOP_SET: + inputGyroEnable(g_ExtMenuPlayer, data->checkbox.value); + break; + } + return 0; +} + +static MenuItemHandlerResult menuhandlerGyroAimMode(s32 operation, struct menuitem* item, union handlerdata* data) +{ + static const char* opts[] = { + "Camera only", + "Crosshair only", + "Camera & Crosshair" + }; + + switch (operation) { + case MENUOP_GETOPTIONCOUNT: + data->dropdown.value = ARRAYCOUNT(opts); + break; + case MENUOP_GETOPTIONTEXT: + return (intptr_t)opts[data->dropdown.value]; + case MENUOP_SET: + inputSetGyroAimMode(g_ExtMenuPlayer, data->dropdown.value); + break; + case MENUOP_GETSELECTEDINDEX: + data->dropdown.value = inputGetGyroAimMode(g_ExtMenuPlayer); + } + return 0; +} + +static MenuItemHandlerResult menuhandlerGyroModifier(s32 operation, struct menuitem* item, union handlerdata* data) +{ + static const char* opts[] = { + "Always On", + "Toggle", + "Enable while Held", + "Disable while Held" + }; + + switch (operation) { + case MENUOP_GETOPTIONCOUNT: + data->dropdown.value = ARRAYCOUNT(opts); + break; + case MENUOP_GETOPTIONTEXT: + return (intptr_t)opts[data->dropdown.value]; + case MENUOP_SET: + inputSetGyroModifier(g_ExtMenuPlayer, data->dropdown.value); + break; + case MENUOP_GETSELECTEDINDEX: + data->dropdown.value = inputGetGyroModifier(g_ExtMenuPlayer); + } + return 0; +} + +static MenuItemHandlerResult menuhandlerGyroAxisMode(s32 operation, struct menuitem* item, union handlerdata* data) +{ + static const char* opts[] = { + "Yaw", + "Roll", + "Local Space", + "Player Space", + "World Space" + }; + + switch (operation) { + case MENUOP_GETOPTIONCOUNT: + data->dropdown.value = ARRAYCOUNT(opts); + break; + case MENUOP_GETOPTIONTEXT: + return (intptr_t)opts[data->dropdown.value]; + case MENUOP_SET: + inputGyroSetAxisMode(g_ExtMenuPlayer, data->dropdown.value); + break; + case MENUOP_GETSELECTEDINDEX: + data->dropdown.value = inputGyroGetAxisMode(g_ExtMenuPlayer); + } + return 0; +} + +static MenuItemHandlerResult menuhandlerGyroSensitivityX(s32 operation, struct menuitem* item, union handlerdata* data) +{ + f32 x, y; + switch (operation) { + case MENUOP_GETSLIDER: + inputGyroGetSpeed(g_ExtMenuPlayer, &x, NULL); + data->slider.value = x * 100.f + 0.5f; + break; + case MENUOP_SET: + inputGyroGetSpeed(g_ExtMenuPlayer, NULL, &y); + inputGyroSetSpeed(g_ExtMenuPlayer, (f32)data->slider.value / 100.f, y); + break; + case MENUOP_GETSLIDERLABEL: + inputGyroGetSpeed(g_ExtMenuPlayer, &x, NULL); + sprintf(data->slider.label, "%.2f", x); + break; + } + return 0; +} + +static MenuItemHandlerResult menuhandlerGyroSensitivityY(s32 operation, struct menuitem* item, union handlerdata* data) +{ + f32 x, y; + switch (operation) { + case MENUOP_GETSLIDER: + inputGyroGetSpeed(g_ExtMenuPlayer, NULL, &y); + data->slider.value = y * 100.f + 0.5f; + break; + case MENUOP_SET: + inputGyroGetSpeed(g_ExtMenuPlayer, &x, NULL); + inputGyroSetSpeed(g_ExtMenuPlayer, x, (f32)data->slider.value / 100.f); + break; + case MENUOP_GETSLIDERLABEL: + inputGyroGetSpeed(g_ExtMenuPlayer, NULL, &y); + sprintf(data->slider.label, "%.2f", y); + break; + } + return 0; +} + +static MenuItemHandlerResult menuhandlerGyroInvertX(s32 operation, struct menuitem* item, union handlerdata* data) +{ + s32 invertx, inverty; + switch (operation) { + case MENUOP_GET: + inputGyroGetInvert(g_ExtMenuPlayer, &invertx, NULL); + return invertx; + case MENUOP_SET: + inputGyroGetInvert(g_ExtMenuPlayer, NULL, &inverty); + inputGyroSetInvert(g_ExtMenuPlayer, data->checkbox.value, inverty); + break; + } + return 0; +} + +static MenuItemHandlerResult menuhandlerGyroInvertY(s32 operation, struct menuitem* item, union handlerdata* data) +{ + s32 invertx, inverty; + switch (operation) { + case MENUOP_GET: + inputGyroGetInvert(g_ExtMenuPlayer, NULL, &inverty); + return inverty; + case MENUOP_SET: + inputGyroGetInvert(g_ExtMenuPlayer, &invertx, NULL); + inputGyroSetInvert(g_ExtMenuPlayer, invertx, data->checkbox.value); + break; + } + return 0; +} + +static MenuItemHandlerResult menuhandlerGyroCrosshairSpeedX(s32 operation, struct menuitem* item, union handlerdata* data) +{ + f32 x, y; + switch (operation) { + case MENUOP_GETSLIDER: + inputGyroGetAimSpeed(g_ExtMenuPlayer, &x, NULL); + data->slider.value = x * 100.f + 0.5f; + break; + case MENUOP_SET: + inputGyroGetAimSpeed(g_ExtMenuPlayer, NULL, &y); + inputGyroSetAimSpeed(g_ExtMenuPlayer, (f32)data->slider.value / 100.f, y); + break; + case MENUOP_GETSLIDERLABEL: + inputGyroGetAimSpeed(g_ExtMenuPlayer, &x, NULL); + sprintf(data->slider.label, "%.2f", x); + break; + } + return 0; +} + +static MenuItemHandlerResult menuhandlerGyroCrosshairSpeedY(s32 operation, struct menuitem* item, union handlerdata* data) +{ + f32 x, y; + switch (operation) { + case MENUOP_GETSLIDER: + inputGyroGetAimSpeed(g_ExtMenuPlayer, NULL, &y); + data->slider.value = y * 100.f + 0.5f; + break; + case MENUOP_SET: + inputGyroGetAimSpeed(g_ExtMenuPlayer, &x, NULL); + inputGyroSetAimSpeed(g_ExtMenuPlayer, x, (f32)data->slider.value / 100.f); + break; + case MENUOP_GETSLIDERLABEL: + inputGyroGetAimSpeed(g_ExtMenuPlayer, NULL, &y); + sprintf(data->slider.label, "%.2f", y); + break; + } + return 0; +} + +static MenuItemHandlerResult menuhandlerGyroAimInvertX(s32 operation, struct menuitem* item, union handlerdata* data) +{ + s32 invertx, inverty; + switch (operation) { + case MENUOP_GET: + inputGyroGetAimInvert(g_ExtMenuPlayer, &invertx, NULL); + return invertx; + case MENUOP_SET: + inputGyroGetAimInvert(g_ExtMenuPlayer, NULL, &inverty); + inputGyroSetAimInvert(g_ExtMenuPlayer, data->checkbox.value, inverty); + break; + } + return 0; +} + +static MenuItemHandlerResult menuhandlerGyroAimInvertY(s32 operation, struct menuitem* item, union handlerdata* data) +{ + s32 invertx, inverty; + switch (operation) { + case MENUOP_GET: + inputGyroGetAimInvert(g_ExtMenuPlayer, NULL, &inverty); + return inverty; + case MENUOP_SET: + inputGyroGetAimInvert(g_ExtMenuPlayer, &invertx, NULL); + inputGyroSetAimInvert(g_ExtMenuPlayer, invertx, data->checkbox.value); + break; + } + return 0; +} + +static MenuItemHandlerResult menuhandlerGyroVHMixer(s32 operation, struct menuitem* item, union handlerdata* data) +{ + switch (operation) { + case MENUOP_GETSLIDER: + data->slider.value = (inputGetGyroVHMixer(g_ExtMenuPlayer) + 1.0f) * 100.0f + 0.5f; + break; + case MENUOP_SET: + inputSetGyroVHMixer(g_ExtMenuPlayer, (float)data->slider.value / 100.0f - 1.0f); + break; + case MENUOP_GETSLIDERLABEL: + sprintf(data->slider.label, "%d%%", (int)data->slider.value - 100); + break; + } + return 0; +} + +static MenuItemHandlerResult menuhandlerGyroSmoothing(s32 operation, struct menuitem* item, union handlerdata *data) +{ + switch (operation) { + case MENUOP_GETSLIDER: + data->slider.value = inputGetGyroSmoothing(g_ExtMenuPlayer) * 100.0f; + break; + case MENUOP_SET: + inputSetGyroSmoothing(g_ExtMenuPlayer, (f32)data->slider.value / 100.0f); + break; + case MENUOP_GETSLIDERLABEL: + sprintf(data->slider.label, "%.0f%%", inputGetGyroSmoothing(g_ExtMenuPlayer) * 100.0f); + break; + } + + return 0; +} + +static MenuItemHandlerResult menuhandlerGyroTightening(s32 operation, struct menuitem* item, union handlerdata *data) +{ + switch (operation) { + case MENUOP_GETSLIDER: + data->slider.value = inputGyroGetTightening(g_ExtMenuPlayer) * 100.0f; + break; + case MENUOP_SET: + inputGyroSetTightening(g_ExtMenuPlayer, (f32)data->slider.value / 100.0f); + break; + case MENUOP_GETSLIDERLABEL: + sprintf(data->slider.label, "%.0f%%", inputGyroGetTightening(g_ExtMenuPlayer) * 100.0f); + break; + } + return 0; +} + +static MenuItemHandlerResult menuhandlerGyroDeadzone(s32 operation, struct menuitem* item, union handlerdata *data) +{ + switch (operation) { + case MENUOP_GETSLIDER: + data->slider.value = inputGyroGetDeadzone(g_ExtMenuPlayer) * 100.0f; + break; + case MENUOP_SET: + inputGyroSetDeadzone(g_ExtMenuPlayer, (f32)data->slider.value / 100.0f); + break; + case MENUOP_GETSLIDERLABEL: + sprintf(data->slider.label, "%.0f%%", inputGyroGetDeadzone(g_ExtMenuPlayer) * 100.0f); + break; + } + return 0; +} + +static MenuItemHandlerResult menuhandlerGyroAutoCalibration(s32 operation, struct menuitem* item, union handlerdata *data) +{ + static const char* opts[] = { + "Disabled", + "While In Menus", + "While Stationary", + "Always" + }; + + switch (operation) { + case MENUOP_GETOPTIONCOUNT: + data->dropdown.value = ARRAYCOUNT(opts); + break; + case MENUOP_GETOPTIONTEXT: + return (intptr_t)opts[data->dropdown.value]; + case MENUOP_SET: + inputGyroSetAutoCalibration(g_ExtMenuPlayer, data->dropdown.value); + break; + case MENUOP_GETSELECTEDINDEX: + data->dropdown.value = inputGyroGetAutoCalibration(g_ExtMenuPlayer); + break; + } + return 0; +} + +static const char *menutextGyroManualCalibration(struct menuitem *item) +{ + static char timer_text[128]; + switch (g_GyroCalibrationState[g_ExtMenuPlayer]) { + case 1: + { + u32 elapsed_ms = SDL_GetTicks() - g_GyroCalibrationStartTime[g_ExtMenuPlayer]; + s32 seconds_left = 5 - (elapsed_ms / 1000); + if (seconds_left < 0) seconds_left = 0; + sprintf(timer_text, "Gyro Calibrating in %d...\nPlace the controller on a flat surface.\n", seconds_left); + return timer_text; + } + case 2: + { + static char reset_text[128]; + const u32 *accept_binds = inputKeyGetBinds(g_ExtMenuPlayer, CK_ACCEPT); + const u32 *ztrig_binds = inputKeyGetBinds(g_ExtMenuPlayer, CK_ZTRIG); + const char *gamepad_key_name = "ACCEPT"; + const char *keyboard_key_name = "Z TRIG"; + + if (accept_binds && accept_binds[0]) { + gamepad_key_name = inputGetKeyName(accept_binds[0]); + } + + if (ztrig_binds && ztrig_binds[0]) { + // Find the first keyboard/mouse bind + for (int i = 0; i < INPUT_MAX_BINDS && ztrig_binds[i] != 0; ++i) { + if (ztrig_binds[i] < VK_JOY_BEGIN) { + keyboard_key_name = inputGetKeyName(ztrig_binds[i]); + break; + } + } + } + + sprintf(reset_text, "Gyro Calibration Complete!\nPress %s/%s to restart.\n", gamepad_key_name, keyboard_key_name); + return reset_text; + } + default: + return "Initiate Gyro Calibration...\n"; + } +} + +static MenuItemHandlerResult menuhandlerGyroManualCalibration(s32 operation, struct menuitem* item, union handlerdata *data) +{ + switch (operation) { + case MENUOP_OPEN: + // Reset the state when the menu is opened + g_GyroCalibrationState[g_ExtMenuPlayer] = 0; + break; + case MENUOP_SET: + switch (g_GyroCalibrationState[g_ExtMenuPlayer]) { + case 0: + // Initial press, will start timer + g_GyroCalibrationState[g_ExtMenuPlayer] = 1; + g_GyroCalibrationStartTime[g_ExtMenuPlayer] = SDL_GetTicks(); + break; + case 1: + // Timer is active, wait for 5 seconds + break; + case 2: + // already completed, restart the calibration + g_GyroCalibrationState[g_ExtMenuPlayer] = 1; + g_GyroCalibrationStartTime[g_ExtMenuPlayer] = SDL_GetTicks(); + break; + } + break; + } + return 0; +} + +struct menuitem g_ExtendedGyroMenuItems[] = { + { + MENUITEMTYPE_CHECKBOX, + 0, + MENUITEMFLAG_LITERAL_TEXT, + (uintptr_t)"Enable Gyro Aim", + 0, + menuhandlerGyroEnabled, + }, + { + MENUITEMTYPE_SEPARATOR, + 0, + 0, + 0, + 0, + NULL, + }, + { + MENUITEMTYPE_DROPDOWN, + 0, + MENUITEMFLAG_LITERAL_TEXT, + (uintptr_t)"Aiming Mode", + 0, + menuhandlerGyroAimMode, + }, + { + MENUITEMTYPE_DROPDOWN, + 0, + MENUITEMFLAG_LITERAL_TEXT, + (uintptr_t)"Gyro Activation (Modifier)", + 0, + menuhandlerGyroModifier, + }, + { + MENUITEMTYPE_DROPDOWN, + 0, + MENUITEMFLAG_LITERAL_TEXT, + (uintptr_t)"Turning Axis Orientation", + 0, + menuhandlerGyroAxisMode, + }, + { + MENUITEMTYPE_DROPDOWN, + 0, + MENUITEMFLAG_LITERAL_TEXT | MENUITEMFLAG_LIST_WIDE, + (uintptr_t)"Gyro Auto-Calibration", + 0, + menuhandlerGyroAutoCalibration, + }, + { + MENUITEMTYPE_SELECTABLE, + 0, + 0, + (uintptr_t)menutextGyroManualCalibration, + 0, + menuhandlerGyroManualCalibration, + }, + { + MENUITEMTYPE_SEPARATOR, + 0, + 0, + 0, + 0, + NULL, + }, + { + MENUITEMTYPE_SLIDER, + 0, + MENUITEMFLAG_LITERAL_TEXT | MENUITEMFLAG_SLIDER_WIDE, + (uintptr_t)"Gyro Speed X", + 3000, + menuhandlerGyroSensitivityX, + }, + { + MENUITEMTYPE_SLIDER, + 0, + MENUITEMFLAG_LITERAL_TEXT | MENUITEMFLAG_SLIDER_WIDE, + (uintptr_t)"Gyro Speed Y", + 3000, + menuhandlerGyroSensitivityY, + }, + { + MENUITEMTYPE_SLIDER, + 0, + MENUITEMFLAG_LITERAL_TEXT | MENUITEMFLAG_SLIDER_WIDE, + (uintptr_t)"Gyro Crosshair Speed X", + 1000, + menuhandlerGyroCrosshairSpeedX, + }, + { + MENUITEMTYPE_SLIDER, + 0, + MENUITEMFLAG_LITERAL_TEXT | MENUITEMFLAG_SLIDER_WIDE, + (uintptr_t)"Gyro Crosshair Speed Y", + 1000, + menuhandlerGyroCrosshairSpeedY, + }, + { + MENUITEMTYPE_SEPARATOR, + 0, + 0, + 0, + 0, + NULL, + }, + { + MENUITEMTYPE_CHECKBOX, + 0, + MENUITEMFLAG_LITERAL_TEXT, + (uintptr_t)"Invert Gyro Speed X", + 0, + menuhandlerGyroInvertX, + }, + { + MENUITEMTYPE_CHECKBOX, + 0, + MENUITEMFLAG_LITERAL_TEXT, + (uintptr_t)"Invert Gyro Speed Y", + 0, + menuhandlerGyroInvertY, + }, + { + MENUITEMTYPE_CHECKBOX, + 0, + MENUITEMFLAG_LITERAL_TEXT, + (uintptr_t)"Invert Gyro Crosshair X", + 0, + menuhandlerGyroAimInvertX, + }, + { + MENUITEMTYPE_CHECKBOX, + 0, + MENUITEMFLAG_LITERAL_TEXT, + (uintptr_t)"Invert Gyro Crosshair Y", + 0, + menuhandlerGyroAimInvertY, + }, + { + MENUITEMTYPE_SEPARATOR, + 0, + 0, + 0, + 0, + NULL, + }, + { + MENUITEMTYPE_SLIDER, + 0, + MENUITEMFLAG_LITERAL_TEXT | MENUITEMFLAG_SLIDER_WIDE, + (uintptr_t)"Gyro X/Y Output Mixer", + 200, + menuhandlerGyroVHMixer, + }, + { + MENUITEMTYPE_SLIDER, + 0, + MENUITEMFLAG_LITERAL_TEXT | MENUITEMFLAG_SLIDER_WIDE, + (uintptr_t)"Smoothing Threshold", + 100, + menuhandlerGyroSmoothing, + }, + { + MENUITEMTYPE_SLIDER, + 0, + MENUITEMFLAG_LITERAL_TEXT | MENUITEMFLAG_SLIDER_WIDE, + (uintptr_t)"Tightening Threshold", + 100, + menuhandlerGyroTightening, + }, + { + MENUITEMTYPE_SLIDER, + 0, + MENUITEMFLAG_LITERAL_TEXT | MENUITEMFLAG_SLIDER_WIDE, + (uintptr_t)"Deadzone Threshold", + 100, + menuhandlerGyroDeadzone, + }, + { + MENUITEMTYPE_SEPARATOR, + 0, + 0, + 0, + 0, + NULL, + }, + { + MENUITEMTYPE_SELECTABLE, + 0, + MENUITEMFLAG_SELECTABLE_CLOSESDIALOG, + L_OPTIONS_213, // "Back" + 0, + NULL, + }, + { MENUITEMTYPE_END }, +}; + +static s32 menuhandlerExtendedGyroMenu(s32 operation, struct menudialogdef *dialog, union handlerdata *data) +{ + if (operation == MENUOP_CLOSE) { + g_GyroCalibrationState[g_ExtMenuPlayer] = 0; + } + + if (operation == MENUOP_TICK) { + if (g_GyroCalibrationState[g_ExtMenuPlayer] == 1) { + if (SDL_GetTicks() - g_GyroCalibrationStartTime[g_ExtMenuPlayer] >= 5000) { + g_GyroCalibrationState[g_ExtMenuPlayer] = 0; + inputGyroSetManualCalibration(g_ExtMenuPlayer); + g_GyroCalibrationState[g_ExtMenuPlayer] = 2; + } + } + } + return 0; +} + +struct menudialogdef g_ExtendedGyroMenuDialog = { + MENUDIALOGTYPE_DEFAULT, + (uintptr_t)"Gyro Settings", + g_ExtendedGyroMenuItems, + menuhandlerExtendedGyroMenu, + MENUDIALOGFLAG_LITERAL_TEXT, + NULL, +}; + +static MenuItemHandlerResult menuhandlerGyroSettingsMenu(s32 operation, struct menuitem *item, union handlerdata *data) +{ + switch (operation) { + case MENUOP_CHECKDISABLED: + // Disable the menu item if controller doesn't have motion sensors support + return !inputControllerMotionSensorsSupported(g_ExtMenuPlayer); + case MENUOP_SET: + // Enables the menu item if controller has motion sensors support + if (inputControllerMotionSensorsSupported(g_ExtMenuPlayer)) { + menuPushDialog(&g_ExtendedGyroMenuDialog); + } + break; + } + return 0; +} + static MenuItemHandlerResult menuhandlerController(s32 operation, struct menuitem *item, union handlerdata *data) { static char ctrlname[35]; @@ -647,7 +1284,15 @@ struct menuitem g_ExtendedControllerMenuItems[] = { MENUITEMFLAG_SELECTABLE_OPENSDIALOG | MENUITEMFLAG_LITERAL_TEXT, (uintptr_t)"Stick Settings...\n", 0, - (void *)&g_ExtendedStickMenuDialog, + (void*)&g_ExtendedStickMenuDialog, + }, + { + MENUITEMTYPE_SELECTABLE, + 0, + MENUITEMFLAG_LITERAL_TEXT, + (uintptr_t)"Gyro Settings...\n", + 0, + menuhandlerGyroSettingsMenu, }, { MENUITEMTYPE_SLIDER, @@ -1627,27 +2272,30 @@ struct menubind { }; static const struct menubind menuBinds[] = { - { CK_ZTRIG, "Fire [ZT]\n", "N64 Z Trigger\n" }, - { CK_LTRIG, "Fire Mode [LT]\n", "N64 L Trigger\n"}, - { CK_RTRIG, "Aim Mode [RT]\n", "N64 R Trigger\n" }, - { CK_A, "Use / Accept [A]\n", "N64 A Button\n" }, - { CK_B, "Use / Cancel [B]\n", "N64 B Button\n" }, - { CK_START, "Pause Menu [ST]\n", "N64 Start\n" }, - { CK_DPAD_U, "D-Pad Up [DU]\n", "N64 D-Pad Up\n" }, - { CK_DPAD_R, "D-Pad Right [DR]\n", "N64 D-Pad Right\n" }, - { CK_DPAD_L, "Prev Weapon [DL]\n", "N64 D-Pad Left\n" }, - { CK_DPAD_D, "Radial Menu [DD]\n", "N64 D-Pad Down\n" }, - { CK_C_U, "Forward [CU]\n", "N64 C-Up\n" }, - { CK_C_D, "Backward [CD]\n", "N64 C-Down\n" }, - { CK_C_R, "Strafe Right [CR]\n", "N64 C-Right\n" }, - { CK_C_L, "Strafe Left [CL]\n", "N64 C-Left\n" }, - { CK_X, "Reload [X]\n", "N64 Ext X\n" }, - { CK_Y, "Next Weapon [Y]\n", "N64 Ext Y\n" }, - { CK_8000, "Cycle Crouch [+]\n", "N64 Ext 8000\n" }, - { CK_4000, "Half Crouch [+]\n", "N64 Ext 4000\n" }, - { CK_2000, "Full Crouch [+]\n", "N64 Ext 2000\n" }, - { CK_ACCEPT, "UI Accept [+]\n", "EXT UI Accept\n" }, - { CK_CANCEL, "UI Cancel [+]\n", "EXT UI Cancel\n" }, + { CK_ZTRIG, "Fire [ZT]\n", "N64 Z Trigger\n" }, + { CK_LTRIG, "Fire Mode [LT]\n", "N64 L Trigger\n" }, + { CK_RTRIG, "Aim Mode [RT]\n", "N64 R Trigger\n" }, + { CK_A, "Use / Accept [A]\n", "N64 A Button\n" }, + { CK_B, "Use / Cancel [B]\n", "N64 B Button\n" }, + { CK_START, "Pause Menu [ST]\n", "N64 Start\n" }, + { CK_DPAD_U, "D-Pad Up [DU]\n", "N64 D-Pad Up\n" }, + { CK_DPAD_R, "D-Pad Right [DR]\n", "N64 D-Pad Right\n" }, + { CK_DPAD_L, "Prev Weapon [DL]\n", "N64 D-Pad Left\n" }, + { CK_DPAD_D, "Radial Menu [DD]\n", "N64 D-Pad Down\n" }, + { CK_C_U, "Forward [CU]\n", "N64 C-Up\n" }, + { CK_C_D, "Backward [CD]\n", "N64 C-Down\n" }, + { CK_C_R, "Strafe Right [CR]\n", "N64 C-Right\n" }, + { CK_C_L, "Strafe Left [CL]\n", "N64 C-Left\n" }, + { CK_X, "Reload [X]\n", "N64 Ext X\n" }, + { CK_Y, "Next Weapon [Y]\n", "N64 Ext Y\n" }, + { CK_8000, "Cycle Crouch [+]\n", "N64 Ext 8000\n" }, + { CK_4000, "Half Crouch [+]\n", "N64 Ext 4000\n" }, + { CK_2000, "Full Crouch [+]\n", "N64 Ext 2000\n" }, + { CK_0040, "Reset Camera/Crosshair [+]\n", "EXT Reset Camera/Crosshair\n" }, + { CK_0080, "Gyro Activation Modifier [+]\n", "EXT Gyro Activation Modifier\n" }, + { CK_0100, "Gyro Calibration (Manual) [+]\n", "EXT Gyro Calibration (Manual)[+]\n" }, + { CK_ACCEPT, "UI Accept [+]\n", "EXT UI Accept\n" }, + { CK_CANCEL, "UI Cancel [+]\n", "EXT UI Cancel\n" }, }; static const char *menutextBind(struct menuitem *item); @@ -1687,6 +2335,9 @@ struct menuitem g_ExtendedBindsMenuItems[] = { DEFINE_MENU_BIND(), DEFINE_MENU_BIND(), DEFINE_MENU_BIND(), + DEFINE_MENU_BIND(), + DEFINE_MENU_BIND(), + DEFINE_MENU_BIND(), { MENUITEMTYPE_SEPARATOR, 0, diff --git a/src/game/bondmove.c b/src/game/bondmove.c index 64e6dee604..79bb0cd479 100644 --- a/src/game/bondmove.c +++ b/src/game/bondmove.c @@ -43,6 +43,14 @@ #include "input.h" #include "video.h" +// Provide a fallback implementation if needed +#ifndef MAX +#define MAX(a, b) ((a) > (b) ? (a) : (b)) +#endif +#ifndef fmax +#define fmax(a, b) MAX(a, b) +#endif + static void bgunProcessQuickDetonate(struct movedata *data, u32 c1buttons, u32 c1buttonsthisframe, u32 buttons1, u32 buttons2) { if ((((c1buttons & (buttons1)) && (c1buttonsthisframe & (buttons2))) || ((c1buttons & (buttons2)) && (c1buttonsthisframe & (buttons1)))) @@ -436,6 +444,113 @@ void bmoveUpdateSpeedThetaControl(f32 value) } } +/** + * Apply crosshair movement with scaling and clamping + */ +static void bmoveApplyCrosshairMovement(f32 aimspeedx, f32 aimspeedy, f32 dx, f32 dy) +{ + const f32 xcoeff = 320.f / 1080.f; + const f32 ycoeff = 240.f / 1080.f; + const f32 xscale = (aimspeedx * xcoeff) / g_Vars.currentplayer->aspect; + const f32 yscale = aimspeedy * ycoeff; + f32 x = g_Vars.currentplayer->swivelpos[0] + (dx * xscale); + f32 y = g_Vars.currentplayer->swivelpos[1] + (dy * yscale); + x = (x < -1.f) ? -1.f : ((x > 1.f) ? 1.f : x); + y = (y < -1.f) ? -1.f : ((y > 1.f) ? 1.f : y); + g_Vars.currentplayer->swivelpos[0] = x; + g_Vars.currentplayer->swivelpos[1] = y; + bgunSwivelWithDamp(x, y, 0.01f); +} + +#ifndef PLATFORM_N64 +/** + * Handle camera/crosshair recentering + */ +void bmoveUpdateCameraRecentering(s32 cidx, bool aim_mode, bool offbike) +{ + static bool prev_reset_pressed[MAX_PLAYERS] = {false, false, false, false}; + static struct { + bool active; + float time; + float duration; + } s_crosshair[MAX_PLAYERS] = { + {false, 0.0f, 0.30f}, {false, 0.0f, 0.30f}, + {false, 0.0f, 0.30f}, {false, 0.0f, 0.30f} + }; + static struct { + bool active; + float time; + float start; + float target; + float duration; + } s_camera[MAX_PLAYERS] = { + {false, 0.0f, 0.0f, 0.0f, 0.24f}, {false, 0.0f, 0.0f, 0.0f, 0.24f}, + {false, 0.0f, 0.0f, 0.0f, 0.24f}, {false, 0.0f, 0.0f, 0.0f, 0.24f} + }; + + bool reset_pressed = inputBindPressed(cidx, CK_0040); + + // Start recentering on button press + if (reset_pressed && !prev_reset_pressed[cidx] && g_Vars.currentplayer) { + if (aim_mode) { + if (fabsf(g_Vars.currentplayer->swivelpos[0]) > 0.01f || fabsf(g_Vars.currentplayer->swivelpos[1]) > 0.01f) { + s_crosshair[cidx].active = true; + s_crosshair[cidx].time = 0.0f; + } + } else { + s_camera[cidx].active = true; + s_camera[cidx].time = 0.0f; + s_camera[cidx].start = g_Vars.currentplayer->vv_verta; + s_camera[cidx].target = 0.0f; + g_Vars.currentplayer->docentreupdown = false; + g_Vars.currentplayer->automovecentre = false; + } + } + prev_reset_pressed[cidx] = reset_pressed; + + // Animate crosshair recentering + if (s_crosshair[cidx].active && g_Vars.currentplayer && g_Vars.currentplayer->insightaimmode) { + float t = s_crosshair[cidx].time / s_crosshair[cidx].duration; + if (t > 1.0f) t = 1.0f; + float smooth = t * t * (3.0f - 2.0f * t); + g_Vars.currentplayer->swivelpos[0] *= (1.0f - smooth); + g_Vars.currentplayer->swivelpos[1] *= (1.0f - smooth); + + s_crosshair[cidx].time += g_Vars.lvupdate60freal / 60.0f; + if (s_crosshair[cidx].time >= s_crosshair[cidx].duration || + (fabsf(g_Vars.currentplayer->swivelpos[0]) < 0.01f && fabsf(g_Vars.currentplayer->swivelpos[1]) < 0.01f)) { + g_Vars.currentplayer->swivelpos[0] = 0.0f; + g_Vars.currentplayer->swivelpos[1] = 0.0f; + s_crosshair[cidx].active = false; + } + } + + // Animate camera vertical recentering + if (s_camera[cidx].active && g_Vars.currentplayer) { + float t = s_camera[cidx].time / s_camera[cidx].duration; + if (t > 1.0f) t = 1.0f; + float smooth = t * t * (3.0f - 2.0f * t); + g_Vars.currentplayer->vv_verta = s_camera[cidx].start + (s_camera[cidx].target - s_camera[cidx].start) * smooth; + + s_camera[cidx].time += g_Vars.lvupdate60freal / 60.0f; + if (s_camera[cidx].time >= s_camera[cidx].duration || + fabsf(g_Vars.currentplayer->vv_verta - s_camera[cidx].target) < 0.01f) { + g_Vars.currentplayer->vv_verta = s_camera[cidx].target; + g_Vars.currentplayer->speedverta = 0.0f; + s_camera[cidx].active = false; + } + } +} + +/** + * Check if recentering is active (used to prevent movements) + */ +bool bmoveIsRecenteringActive(s32 cidx) +{ + return false; +} +#endif + /** * Calculate the lookahead angle. * @@ -662,6 +777,7 @@ void bmoveResetMoveData(struct movedata *data) */ void bmoveProcessInput(bool allowc1x, bool allowc1y, bool allowc1buttons, bool ignorec2) { + const s32 cidx = g_Vars.currentplayernum; struct movedata movedata; s32 controlmode; s32 weaponnum; @@ -712,9 +828,21 @@ void bmoveProcessInput(bool allowc1x, bool allowc1y, bool allowc1buttons, bool i f32 increment2; f32 newverta; #ifndef PLATFORM_N64 - const f32 mlookscale = g_Vars.lvupdate240 ? (4.f / (f32)g_Vars.lvupdate240) : 4.f; - const bool allowmlook = (g_Vars.currentplayernum == 0) && (allowc1x || allowc1y); - bool allowmcross = false; + // Mouse sensitivity scaling + const f32 mlookscale = g_Vars.lvupdate240 ? (4.f / (f32)g_Vars.lvupdate240) : 4.f; + const bool allowmlook = (g_Vars.currentplayernum == 0) && (allowc1x || allowc1y); + + // Gyro sensitivity scaling - Natural sensitivity scale + // Compensate for the FOV-based scaling to achieve near 1:1 rotation + const f32 gyrobasesens = 1.105f / (viGetFovY() / PLAYER_DEFAULT_FOV); + const f32 gyroscale = g_Vars.lvupdate240 ? (gyrobasesens / (f32)g_Vars.lvupdate240) : gyrobasesens; + const bool allowgyro = (g_Vars.players[cidx] != NULL) && (allowc1x || allowc1y) && inputGyroIsEnabled(cidx); + + bool allowmcross = false; + bool allowgcross = (g_Vars.players[cidx] != NULL) && + (allowc1x || allowc1y) && + (PLAYER_EXTCFG().gyroaimmode == GYRO_AIM_CROSSHAIR || + PLAYER_EXTCFG().gyroaimmode == GYRO_AIM_BOTH); #endif controlmode = optionsGetControlMode(g_Vars.currentplayerstats->mpindex); @@ -746,6 +874,10 @@ void bmoveProcessInput(bool allowc1x, bool allowc1y, bool allowc1buttons, bool i numsamples = joyGetNumSamples(); bmoveResetMoveData(&movedata); + // Reset gyro deltas to zero at the start of each frame + movedata.gyrolookdx = 0.0f; + movedata.gyrolookdy = 0.0f; + if (c1stickx < -5) { movedata.c1stickxsafe = c1stickx + 5; } else if (c1stickx > 5) { @@ -772,6 +904,8 @@ void bmoveProcessInput(bool allowc1x, bool allowc1y, bool allowc1buttons, bool i movedata.analogwalk = movedata.c1stickysafe; #ifndef PLATFORM_N64 + + // Handle Mouse Input if (allowmlook) { inputMouseGetScaledDelta(&movedata.freelookdx, &movedata.freelookdy); allowmcross = (PLAYER_EXTCFG().mouseaimmode == MOUSEAIM_CLASSIC) && @@ -780,13 +914,46 @@ void bmoveProcessInput(bool allowc1x, bool allowc1y, bool allowc1buttons, bool i movedata.freelookdy = -movedata.freelookdy; } } - // always pause with ESC - if (allowc1buttons && g_Vars.currentplayer->isdead == false && g_Vars.currentplayer->pausemode == PAUSEMODE_UNPAUSED) { + + // Handle Gyro Input + if (allowgyro) { + float gyroCamDx = 0.f, gyroCamDy = 0.f, gyroCamDz = 0.f; + float gyroCrossDx = 0.f, gyroCrossDy = 0.f; + int gyroAimMode = inputGetGyroAimMode(cidx); + if (gyroAimMode == GYRO_AIM_CAMERA || gyroAimMode == GYRO_AIM_BOTH) { + inputGyroGetScaledDelta(cidx, &gyroCamDx, &gyroCamDy, &gyroCamDz); + const f32 norm = g_Vars.lvupdate60freal; + movedata.gyrolookdx += gyroCamDx * norm; + movedata.gyrolookdy += gyroCamDy * norm; + } + + if (gyroAimMode == GYRO_AIM_CROSSHAIR || gyroAimMode == GYRO_AIM_BOTH) { + inputGyroGetScaledDeltaCrosshair(cidx, &gyroCrossDx, &gyroCrossDy); + if (g_Vars.players[cidx]) { + const f32 norm = g_Vars.lvupdate60freal; + g_Vars.players[cidx]->swivelpos[0] += gyroCrossDx * norm; + g_Vars.players[cidx]->swivelpos[1] += gyroCrossDy * norm; + } + allowmcross = true; + } + + if (movedata.invertpitch) { + movedata.gyrolookdy = -movedata.gyrolookdy; + } + + // Clamp gyro input to prevent runaway camera movement + if (movedata.gyrolookdx < -10.0f) movedata.gyrolookdx = -10.0f; + if (movedata.gyrolookdx > 10.0f) movedata.gyrolookdx = 10.0f; + if (movedata.gyrolookdy < -10.0f) movedata.gyrolookdy = -10.0f; + if (movedata.gyrolookdy > 10.0f) movedata.gyrolookdy = 10.0f; + } +#endif + + if (allowc1buttons && g_Vars.currentplayer && !g_Vars.currentplayer->isdead && g_Vars.currentplayer->pausemode == PAUSEMODE_UNPAUSED) { if (inputKeyJustPressed(VK_ESCAPE)) { c1buttonsthisframe |= START_BUTTON; } } -#endif // Pausing if (g_Vars.currentplayer->isdead == false) { @@ -886,6 +1053,8 @@ void bmoveProcessInput(bool allowc1x, bool allowc1y, bool allowc1buttons, bool i #ifndef PLATFORM_N64 movedata.freelookdx = 0.0f; movedata.freelookdy = 0.0f; + movedata.gyrolookdx = 0.0f; + movedata.gyrolookdy = 0.0f; #endif } @@ -1333,6 +1502,8 @@ void bmoveProcessInput(bool allowc1x, bool allowc1y, bool allowc1buttons, bool i #ifndef PLATFORM_N64 movedata.freelookdx = 0.0f; movedata.freelookdy = 0.0f; + movedata.gyrolookdx = 0.0f; + movedata.gyrolookdy = 0.0f; movedata.analoglean = 0.f; #endif } @@ -1376,6 +1547,8 @@ void bmoveProcessInput(bool allowc1x, bool allowc1y, bool allowc1buttons, bool i #ifndef PLATFORM_N64 movedata.freelookdx = 0.0f; movedata.freelookdy = 0.0f; + movedata.gyrolookdx = 0.0f; + movedata.gyrolookdy = 0.0f; movedata.analoglean = 0.f; #endif } @@ -1412,8 +1585,9 @@ void bmoveProcessInput(bool allowc1x, bool allowc1y, bool allowc1buttons, bool i } #ifndef PLATFORM_N64 - // Handle turning and looking up/down via mouselook when aiming - if (g_Vars.currentplayer->insightaimmode && allowmcross && bgunGetWeaponNum(HAND_RIGHT) != WEAPON_HORIZONSCANNER) { + // Handle turning and looking up/down via mouselook or gyro when aiming + bool allowcross = allowmcross || allowgcross; + if (g_Vars.currentplayer->insightaimmode && allowcross && bgunGetWeaponNum(HAND_RIGHT) != WEAPON_HORIZONSCANNER) { if (g_Vars.currentplayer->swivelpos[0] > 0.9f) { movedata.aimturnrightspeed = (g_Vars.currentplayer->swivelpos[0] - 0.9f) / 0.1f; movedata.aimturnleftspeed = 0.f; @@ -1436,7 +1610,7 @@ void bmoveProcessInput(bool allowc1x, bool allowc1y, bool allowc1buttons, bool i movedata.speedvertadown = vertadown; } } else { - // Reset mouse aim position when not mouse aiming + // Reset mouse/gyro aim position when not aiming g_Vars.currentplayer->swivelpos[0] = 0.f; g_Vars.currentplayer->swivelpos[1] = 0.f; } @@ -1946,10 +2120,14 @@ void bmoveProcessInput(bool allowc1x, bool allowc1y, bool allowc1buttons, bool i // Look ahead if (g_Vars.currentplayer->pausemode == PAUSEMODE_UNPAUSED) { - lookahead = -4; + lookahead = -4; + offbike = g_Vars.currentplayer->bondmovemode == MOVEMODE_WALK + || g_Vars.currentplayer->bondmovemode == MOVEMODE_GRAB; - offbike = g_Vars.currentplayer->bondmovemode == MOVEMODE_WALK - || g_Vars.currentplayer->bondmovemode == MOVEMODE_GRAB; +#ifndef PLATFORM_N64 + // Handle camera/crosshair recentering + bmoveUpdateCameraRecentering(cidx, g_Vars.currentplayer->insightaimmode, offbike); +#endif if (g_Vars.currentplayer->lookaheadcentreenabled) { if (g_Vars.lvframenum != g_Vars.currentplayer->lookaheadframe @@ -2048,19 +2226,28 @@ void bmoveProcessInput(bool allowc1x, bool allowc1y, bool allowc1buttons, bool i } #ifndef PLATFORM_N64 + // Pause camera input during recentering animation fVar25 += movedata.freelookdy * mlookscale; + fVar25 += movedata.gyrolookdy * gyroscale; #endif g_Vars.currentplayer->speedverta = -fVar25 * tmp; +#ifndef PLATFORM_N64 + // Pause vertical camera input during camera recentering } else if (movedata.speedvertadown > 0) { +#else + } else if (movedata.speedvertadown > 0) { +#endif bmoveUpdateSpeedVerta(movedata.speedvertadown); - if (movedata.canlookahead && (movedata.analogwalk > 60 || movedata.analogwalk < -60)) { g_Vars.currentplayer->movecentrerelease = true; } +#ifndef PLATFORM_N64 } else if (movedata.speedvertaup > 0) { +#else + } else if (movedata.speedvertaup > 0) { +#endif bmoveUpdateSpeedVerta(-movedata.speedvertaup); - if (movedata.canlookahead && (movedata.analogwalk > 60 || movedata.analogwalk < -60)) { g_Vars.currentplayer->movecentrerelease = true; } @@ -2068,7 +2255,12 @@ void bmoveProcessInput(bool allowc1x, bool allowc1y, bool allowc1buttons, bool i bmoveUpdateSpeedVerta(0); } +#ifndef PLATFORM_N64 + // Only apply normal speedverta if new animated system is not active + g_Vars.currentplayer->vv_verta += g_Vars.currentplayer->speedverta * g_Vars.lvupdate60freal * 3.5f; +#else g_Vars.currentplayer->vv_verta += g_Vars.currentplayer->speedverta * g_Vars.lvupdate60freal * 3.5f; +#endif } } @@ -2090,6 +2282,7 @@ void bmoveProcessInput(bool allowc1x, bool allowc1y, bool allowc1buttons, bool i #ifndef PLATFORM_N64 fVar25 += movedata.freelookdx * mlookscale; + fVar25 += movedata.gyrolookdx * gyroscale; #endif g_Vars.currentplayer->speedthetacontrol = fVar25 * tmp; @@ -2187,12 +2380,29 @@ void bmoveProcessInput(bool allowc1x, bool allowc1y, bool allowc1buttons, bool i y = -g_Vars.currentplayer->speedverta * 0.1f + g_Vars.currentplayer->gunextraaimy; #else f32 xscale, yscale; - if (movedata.freelookdx || movedata.freelookdy) { - xscale = PLAYER_EXTCFG().crosshairsway * 0.20f; - yscale = PLAYER_EXTCFG().crosshairsway * 0.30f; + // crosshair sway scaling for mouse, gyro, and joystick input + bool mouse_active = (movedata.freelookdx || movedata.freelookdy); + bool gyro_active = (movedata.gyrolookdx || movedata.gyrolookdy); + bool joystick_active = (movedata.c1stickxraw != 0 || movedata.c1stickyraw != 0); + + if ((mouse_active || gyro_active) && joystick_active) { + // Gyro/Mouse + joystick sway + xscale = PLAYER_EXTCFG().crosshairsway * 0.80f; // 80% for precision+joystick sway + yscale = PLAYER_EXTCFG().crosshairsway * 0.80f; // 80% for precision+joystick sway + } else if (mouse_active) { + // Mouse sway + xscale = PLAYER_EXTCFG().crosshairsway * 0.20f; // 20% for mouse sway + yscale = PLAYER_EXTCFG().crosshairsway * 0.30f; // 30% for mouse sway + } else if (gyro_active) { + // Gyro sway + xscale = PLAYER_EXTCFG().crosshairsway * 0.20f; // 20% for gyro sway, mirroring mouse's + yscale = PLAYER_EXTCFG().crosshairsway * 0.30f; // 30% for gyro sway, mirroring mouse's } else { + // Joystick only or no input - full sway xscale = yscale = PLAYER_EXTCFG().crosshairsway; } + // Joystick x/y scaling + // 0.3f for x, 0.1f for y x = g_Vars.currentplayer->speedtheta * 0.3f * xscale + g_Vars.currentplayer->gunextraaimx; y = -g_Vars.currentplayer->speedverta * 0.1f * yscale + g_Vars.currentplayer->gunextraaimy; #endif @@ -2204,23 +2414,26 @@ void bmoveProcessInput(bool allowc1x, bool allowc1y, bool allowc1buttons, bool i // when holding aim and moving stick bgunSetAimType(0); #ifndef PLATFORM_N64 - if (allowmcross) { - // joystick is inactive, move crosshair using the mouse - const f32 xcoeff = 320.f / 1080.f; - const f32 ycoeff = 240.f / 1080.f; - const f32 xscale = (PLAYER_EXTCFG().mouseaimspeedx * xcoeff) / g_Vars.currentplayer->aspect; - const f32 yscale = PLAYER_EXTCFG().mouseaimspeedy * ycoeff; - f32 x = g_Vars.currentplayer->swivelpos[0] + movedata.freelookdx * xscale; - f32 y = g_Vars.currentplayer->swivelpos[1] + movedata.freelookdy * yscale; - x = (x < -1.f) ? -1.f : ((x > 1.f) ? 1.f : x); - y = (y < -1.f) ? -1.f : ((y > 1.f) ? 1.f : y); - g_Vars.currentplayer->swivelpos[0] = x; - g_Vars.currentplayer->swivelpos[1] = y; - bgunSwivelWithDamp(x, y, 0.01f); - return; - } + if (allowgcross) { + // Gyro is active, apply gyro movement + inputGyroGetScaledDeltaCrosshair(g_Vars.currentplayernum, &movedata.gyrolookdx, &movedata.gyrolookdy); + if (movedata.gyrolookdx != 0.0f || movedata.gyrolookdy != 0.0f) { + bmoveApplyCrosshairMovement(PLAYER_EXTCFG().gyroaimsensx, PLAYER_EXTCFG().gyroaimsensy, + movedata.gyrolookdx, movedata.gyrolookdy); + return; + } + } + // Mouse input is active, apply mouse movement + if (allowmcross) { + bmoveApplyCrosshairMovement(PLAYER_EXTCFG().mouseaimspeedx, PLAYER_EXTCFG().mouseaimspeedy, + movedata.freelookdx, movedata.freelookdy); + return; + } #endif - bgunSwivelWithoutDamp((movedata.c1stickxraw * 0.65f) / 80.0f, (movedata.c1stickyraw * 0.65f) / 80.0f); + + // Default joystick-based movement if neither mouse nor gyro crosshair movement is active + bgunSwivelWithoutDamp((movedata.c1stickxraw * 0.65f) / 80.0f, + (movedata.c1stickyraw * 0.65f) / 80.0f); } } diff --git a/src/game/player.c b/src/game/player.c index cd5fe4137d..6494c5bd57 100644 --- a/src/game/player.c +++ b/src/game/player.c @@ -3678,6 +3678,49 @@ void playerTick(bool arg0) sp174 -= mdx; } } + + // Gyro control + { + f32 gdx_cam = 0.f, gdy_cam = 0.f, gdz_cam = 0.f; + f32 gdx_crosshair = 0.f, gdy_crosshair = 0.f; + s32 cidx = g_Vars.currentplayernum; + + // Apply gyro movement based on mode + if (inputGetGyroAimMode(cidx) == GYRO_AIM_CAMERA || inputGetGyroAimMode(cidx) == GYRO_AIM_BOTH) { + inputGyroGetScaledDelta(cidx, &gdx_cam, &gdy_cam, &gdz_cam); // Camera movement + } + + if (inputGetGyroAimMode(cidx) == GYRO_AIM_CROSSHAIR || inputGetGyroAimMode(cidx) == GYRO_AIM_BOTH) { + inputGyroGetScaledDeltaCrosshair(cidx, &gdx_crosshair, &gdy_crosshair); // Crosshair movement + } + + // Apply gyro movement only when detected + if (gdx_cam || gdy_cam || gdx_crosshair || gdy_crosshair) { + gdx_cam *= 0.022f; + gdy_cam *= 0.022f; + gdx_crosshair *= 0.022f; + gdy_crosshair *= 0.022f; + + // Clamping to match mouse sensitivity limits + gdx_cam = (gdx_cam < -128.f) ? -128.f : (gdx_cam > 127.f) ? 127.f : gdx_cam; + gdy_cam = (gdy_cam < -128.f) ? -128.f : (gdy_cam > 127.f) ? 127.f : gdy_cam; + gdx_crosshair = (gdx_crosshair < -128.f) ? -128.f : (gdx_crosshair > 127.f) ? 127.f : gdx_crosshair; + gdy_crosshair = (gdy_crosshair < -128.f) ? -128.f : (gdy_crosshair > 127.f) ? 127.f : gdy_crosshair; + + // Respect forward pitch setting + if (g_Vars.currentplayerstats && !optionsGetForwardPitch(g_Vars.currentplayerstats->mpindex)) { + gdy_cam = -gdy_cam; + gdy_crosshair = -gdy_crosshair; + } + + // Apply movement separately for each mode + sp178 += gdy_cam; + sp174 -= gdx_cam; + + g_Vars.currentplayer->swivelpos[0] += gdx_crosshair; // Crosshair movement + g_Vars.currentplayer->swivelpos[1] += gdy_crosshair; + } + } #endif f20 = sqrtf(sp2ac.f[0] * sp2ac.f[0] + sp2ac.f[2] * sp2ac.f[2]); diff --git a/src/include/constants.h b/src/include/constants.h index 2457a7f4ab..fef3d9ccb9 100644 --- a/src/include/constants.h +++ b/src/include/constants.h @@ -4751,6 +4751,11 @@ enum weaponnum { #define CROUCHMODE_TOGGLE 2 // press the crouch buttons to toggle stance #define CROUCHMODE_TOGGLE_ANALOG (CROUCHMODE_ANALOG | CROUCHMODE_TOGGLE) +#define RESET_CAMERA CK_0040 // reset camera and/or crosshair to default position + +#define GYRO_MODIFIER CK_0080 // press/hold or toggle gyro activation +#define GYRO_CALIBRATION CK_0100 // calibrate gyro manually + #define CROSSHAIR_HEALTH_OFF 0 #define CROSSHAIR_HEALTH_ON_GREEN 1 #define CROSSHAIR_HEALTH_ON_WHITE 2 diff --git a/src/include/types.h b/src/include/types.h index 5dbe003e5e..e16c75bb3a 100644 --- a/src/include/types.h +++ b/src/include/types.h @@ -5093,6 +5093,8 @@ struct movedata { /*0xac*/ s32 alt1tapcount; /* */ f32 freelookdx; // how much the mouse moved ... /* */ f32 freelookdy; // ... scaled by sensitivity + /* */ f32 gyrolookdx; // how much the gyro moved ... + /* */ f32 gyrolookdy; // ... scaled by sensitivity /* */ f32 analoglean; // how much we're trying to lean #endif @@ -6152,6 +6154,9 @@ struct extplayerconfig { s32 mouseaimmode; f32 mouseaimspeedx; f32 mouseaimspeedy; + s32 gyroaimmode; + f32 gyroaimsensx; + f32 gyroaimsensy; s32 crouchmode; f32 radialmenuspeed; f32 crosshairsway;