diff --git a/COPYRIGHT.txt b/COPYRIGHT.txt index 812e0148310..0f4786283d1 100644 --- a/COPYRIGHT.txt +++ b/COPYRIGHT.txt @@ -310,6 +310,11 @@ Comment: The FreeType Project Copyright: 1996-2025, David Turner, Robert Wilhelm, and Werner Lemberg. License: FTL +Files: thirdparty/gamepadmotionhelpers/* +Comment: GamepadMotionHelpers +Copyright: 2020-2023, Julian "Jibb" Smart +License: Expat + Files: thirdparty/glad/* Comment: glad Copyright: 2013-2022, David Herberth diff --git a/core/input/input.cpp b/core/input/input.cpp index c75079206c9..cc6e2afbb19 100644 --- a/core/input/input.cpp +++ b/core/input/input.cpp @@ -40,6 +40,10 @@ #include "core/os/thread.h" #endif +#include "thirdparty/gamepadmotionhelpers/GamepadMotion.hpp" + +#define STANDARD_GRAVITY 9.80665f + static const char *_joy_buttons[(size_t)JoyButton::SDL_MAX] = { "a", "b", @@ -147,6 +151,20 @@ void Input::_bind_methods() { ClassDB::bind_method(D_METHOD("get_accelerometer"), &Input::get_accelerometer); ClassDB::bind_method(D_METHOD("get_magnetometer"), &Input::get_magnetometer); ClassDB::bind_method(D_METHOD("get_gyroscope"), &Input::get_gyroscope); + ClassDB::bind_method(D_METHOD("get_joy_accelerometer", "device"), &Input::get_joy_accelerometer); + ClassDB::bind_method(D_METHOD("get_joy_gravity", "device"), &Input::get_joy_gravity); + ClassDB::bind_method(D_METHOD("get_joy_gyroscope", "device"), &Input::get_joy_gyroscope); + ClassDB::bind_method(D_METHOD("get_joy_motion_sensors_rate", "device"), &Input::get_joy_motion_sensors_rate); + ClassDB::bind_method(D_METHOD("is_joy_motion_sensors_enabled", "device"), &Input::is_joy_motion_sensors_enabled); + ClassDB::bind_method(D_METHOD("set_joy_motion_sensors_enabled", "device", "enable"), &Input::set_joy_motion_sensors_enabled); + ClassDB::bind_method(D_METHOD("has_joy_motion_sensors", "device"), &Input::has_joy_motion_sensors); + ClassDB::bind_method(D_METHOD("start_joy_motion_sensors_calibration", "device"), &Input::start_joy_motion_sensors_calibration); + ClassDB::bind_method(D_METHOD("stop_joy_motion_sensors_calibration", "device"), &Input::stop_joy_motion_sensors_calibration); + ClassDB::bind_method(D_METHOD("clear_joy_motion_sensors_calibration", "device"), &Input::clear_joy_motion_sensors_calibration); + ClassDB::bind_method(D_METHOD("get_joy_motion_sensors_calibration", "device"), &Input::get_joy_motion_sensors_calibration); + ClassDB::bind_method(D_METHOD("set_joy_motion_sensors_calibration", "device", "calibration_info"), &Input::set_joy_motion_sensors_calibration); + ClassDB::bind_method(D_METHOD("is_joy_motion_sensors_calibrated", "device"), &Input::is_joy_motion_sensors_calibrated); + ClassDB::bind_method(D_METHOD("is_joy_motion_sensors_calibrating", "device"), &Input::is_joy_motion_sensors_calibrating); ClassDB::bind_method(D_METHOD("set_gravity", "value"), &Input::set_gravity); ClassDB::bind_method(D_METHOD("set_accelerometer", "value"), &Input::set_accelerometer); ClassDB::bind_method(D_METHOD("set_magnetometer", "value"), &Input::set_magnetometer); @@ -684,6 +702,11 @@ void Input::joy_connection_changed(int p_idx, bool p_connected, const String &p_ for (int i = 0; i < (int)JoyAxis::MAX; i++) { set_joy_axis(p_idx, (JoyAxis)i, 0.0f); } + MotionInfo *motion = joy_motion.getptr(p_idx); + if (motion != nullptr && motion->gamepad_motion != nullptr) { + delete motion->gamepad_motion; + } + joy_motion.erase(p_idx); } joy_names[p_idx] = js; @@ -1018,6 +1041,196 @@ bool Input::has_joy_light(int p_device) const { return joypad && joypad->has_light; } +Vector3 Input::get_joy_accelerometer(int p_device) const { + _THREAD_SAFE_METHOD_ + const MotionInfo *motion = joy_motion.getptr(p_device); + if (motion == nullptr) { + return Vector3(); + } + + float joy_acceleration_data[3]; + motion->gamepad_motion->GetProcessedAcceleration(joy_acceleration_data[0], joy_acceleration_data[1], joy_acceleration_data[2]); + Vector3 joy_acceleration(joy_acceleration_data[0], joy_acceleration_data[1], joy_acceleration_data[2]); + + float joy_gravity_data[3]; + motion->gamepad_motion->GetGravity(joy_gravity_data[0], joy_gravity_data[1], joy_gravity_data[2]); + Vector3 joy_gravity(joy_gravity_data[0], joy_gravity_data[1], joy_gravity_data[2]); + + return (-joy_acceleration + joy_gravity) * STANDARD_GRAVITY; +} + +Vector3 Input::get_joy_gravity(int p_device) const { + _THREAD_SAFE_METHOD_ + const MotionInfo *motion = joy_motion.getptr(p_device); + if (motion == nullptr) { + return Vector3(); + } + + float joy_gravity_data[3]; + motion->gamepad_motion->GetGravity(joy_gravity_data[0], joy_gravity_data[1], joy_gravity_data[2]); + Vector3 joy_gravity(joy_gravity_data[0], joy_gravity_data[1], joy_gravity_data[2]); + + return joy_gravity.normalized() * STANDARD_GRAVITY; +} + +Vector3 Input::get_joy_gyroscope(int p_device) const { + _THREAD_SAFE_METHOD_ + const MotionInfo *motion = joy_motion.getptr(p_device); + if (motion == nullptr) { + return Vector3(); + } + + float joy_gyro_data[3]; + motion->gamepad_motion->GetCalibratedGyro(joy_gyro_data[0], joy_gyro_data[1], joy_gyro_data[2]); + Vector3 joy_gyro(joy_gyro_data[0], joy_gyro_data[1], joy_gyro_data[2]); + + return joy_gyro * M_PI / 180.0; +} + +void Input::set_joy_motion_sensors_enabled(int p_device, bool p_enable) { + _THREAD_SAFE_METHOD_ + Joypad *joypad = joy_names.getptr(p_device); + if (joypad == nullptr || joypad->features == nullptr) { + return; + } + MotionInfo *motion = joy_motion.getptr(p_device); + if (motion == nullptr) { + return; + } + joypad->features->set_joy_motion_sensors_enabled(p_enable); + motion->sensors_enabled = p_enable; +} + +bool Input::is_joy_motion_sensors_enabled(int p_device) const { + _THREAD_SAFE_METHOD_ + const MotionInfo *motion = joy_motion.getptr(p_device); + return motion != nullptr && motion->sensors_enabled; +} + +bool Input::has_joy_motion_sensors(int p_device) const { + _THREAD_SAFE_METHOD_ + return joy_motion.has(p_device); +} + +float Input::get_joy_motion_sensors_rate(int p_device) const { + _THREAD_SAFE_METHOD_ + const MotionInfo *motion = joy_motion.getptr(p_device); + if (motion == nullptr) { + return 0.0f; + } + return motion->sensor_data_rate; +} + +void Input::start_joy_motion_sensors_calibration(int p_device) { + _THREAD_SAFE_METHOD_ + MotionInfo *motion = joy_motion.getptr(p_device); + if (motion == nullptr) { + return; + } + + ERR_FAIL_COND_MSG(!motion->sensors_enabled, "Motion sensors are not enabled on the joypad."); + ERR_FAIL_COND_MSG(motion->calibrating, "Calibration already in progress."); + + motion->gamepad_motion->ResetContinuousCalibration(); + motion->gamepad_motion->StartContinuousCalibration(); + + motion->calibrating = true; + motion->calibrated = false; +} + +void Input::stop_joy_motion_sensors_calibration(int p_device) { + _THREAD_SAFE_METHOD_ + MotionInfo *motion = joy_motion.getptr(p_device); + if (motion == nullptr) { + return; + } + + ERR_FAIL_COND_MSG(!motion->sensors_enabled, "Motion sensors are not enabled on the joypad."); + ERR_FAIL_COND_MSG(!motion->calibrating, "Calibration hasn't been started."); + + motion->gamepad_motion->PauseContinuousCalibration(); + + motion->calibrating = false; + motion->calibrated = true; +} + +void Input::clear_joy_motion_sensors_calibration(int p_device) { + _THREAD_SAFE_METHOD_ + MotionInfo *motion = joy_motion.getptr(p_device); + if (motion == nullptr) { + return; + } + + // Calibration might be in progress and the developer or the user might want to reset it, + // so no need to stop the calibration. + + motion->gamepad_motion->ResetContinuousCalibration(); +} + +Dictionary Input::get_joy_motion_sensors_calibration(int p_device) const { + _THREAD_SAFE_METHOD_ + const MotionInfo *motion = joy_motion.getptr(p_device); + if (motion == nullptr) { + return Dictionary(); + } + + if (!motion->calibrated) { + return Dictionary(); + } + + float joy_gyro_offset_data[3]; + motion->gamepad_motion->GetCalibrationOffset(joy_gyro_offset_data[0], joy_gyro_offset_data[1], joy_gyro_offset_data[2]); + Vector3 joy_gyro_offset(joy_gyro_offset_data[0], joy_gyro_offset_data[1], joy_gyro_offset_data[2]); + + Dictionary result; + result["gyroscope_offset"] = joy_gyro_offset * M_PI / 180.0; + return result; +} + +void Input::set_joy_motion_sensors_calibration(int p_device, const Dictionary &p_calibration_info) { + _THREAD_SAFE_METHOD_ + MotionInfo *motion = joy_motion.getptr(p_device); + if (motion == nullptr) { + return; + } + + ERR_FAIL_COND_MSG(motion->calibrating, "Calibration is currently in progress."); + + Vector3 gyro_offset = p_calibration_info.get("gyroscope_offset", Vector3()).operator Vector3() * 180.0 / M_PI; + + motion->gamepad_motion->SetCalibrationOffset(gyro_offset.x, gyro_offset.y, gyro_offset.z, 1); + motion->calibrating = false; + motion->calibrated = true; +} + +bool Input::is_joy_motion_sensors_calibrating(int p_device) const { + _THREAD_SAFE_METHOD_ + const MotionInfo *motion = joy_motion.getptr(p_device); + if (motion == nullptr) { + return false; + } + return motion->calibrating; +} + +bool Input::is_joy_motion_sensors_calibrated(int p_device) const { + _THREAD_SAFE_METHOD_ + const MotionInfo *motion = joy_motion.getptr(p_device); + if (motion == nullptr) { + return false; + } + return motion->calibrated; +} + +void Input::set_joy_motion_sensors_rate(int p_device, float p_rate) { + _THREAD_SAFE_METHOD_ + MotionInfo *motion = joy_motion.getptr(p_device); + if (motion == nullptr) { + return; + } + + motion->sensor_data_rate = p_rate; +} + void Input::start_joy_vibration(int p_device, float p_weak_magnitude, float p_strong_magnitude, float p_duration) { _THREAD_SAFE_METHOD_ if (p_weak_magnitude < 0.f || p_weak_magnitude > 1.f || p_strong_magnitude < 0.f || p_strong_magnitude > 1.f) { @@ -1469,6 +1682,22 @@ void Input::joy_hat(int p_device, BitField p_val) { joy_names[p_device].hat_current = (int)p_val; } +void Input::joy_motion_sensors(int p_device, const Vector3 &p_accelerometer, const Vector3 &p_gyroscope) { + _THREAD_SAFE_METHOD_ + // TODO: events + MotionInfo *motion = joy_motion.getptr(p_device); + if (motion == nullptr) { + return; + } + + Vector3 gyro_degrees = p_gyroscope * 180.0 / M_PI; + Vector3 accel_g = -p_accelerometer / STANDARD_GRAVITY; + uint64_t new_timestamp = OS::get_singleton()->get_ticks_msec(); + float delta_time = (new_timestamp - motion->last_timestamp) / 1000.0f; + motion->last_timestamp = new_timestamp; + motion->gamepad_motion->ProcessMotion(gyro_degrees.x, gyro_degrees.y, gyro_degrees.z, accel_g.x, accel_g.y, accel_g.z, delta_time); +} + void Input::_button_event(int p_device, JoyButton p_index, bool p_pressed) { Ref ievent; ievent.instantiate(); @@ -1520,6 +1749,16 @@ void Input::_update_joypad_features(int p_device) { if (joypad->features->has_joy_light()) { joypad->has_light = true; } + if (joypad->features->has_joy_motion_sensors()) { + MotionInfo &motion = joy_motion[p_device]; + + if (!motion.gamepad_motion) { + motion.gamepad_motion = new GamepadMotion(); + } else { + motion.gamepad_motion->Reset(); + } + motion.last_timestamp = OS::get_singleton()->get_ticks_msec(); + } } Input::JoyEvent Input::_get_mapped_button_event(const JoyDeviceMapping &mapping, JoyButton p_button) { diff --git a/core/input/input.h b/core/input/input.h index c912cb34d3c..b09558676b4 100644 --- a/core/input/input.h +++ b/core/input/input.h @@ -38,6 +38,8 @@ #include "core/templates/rb_set.h" #include "core/variant/typed_array.h" +class GamepadMotion; + class Input : public Object { GDCLASS(Input, Object); _THREAD_SAFE_CLASS_ @@ -85,6 +87,9 @@ public: virtual bool has_joy_light() const { return false; } virtual void set_joy_light(const Color &p_color) {} + + virtual bool has_joy_motion_sensors() const { return false; } + virtual void set_joy_motion_sensors_enabled(bool p_enable) {} }; static constexpr int32_t JOYPADS_MAX = 16; @@ -157,6 +162,23 @@ private: HashMap joy_vibration; + struct MotionInfo { + bool sensors_enabled : 1; + bool calibrating : 1; + bool calibrated : 1; + float sensor_data_rate = 0.0f; + uint64_t last_timestamp = 0; + GamepadMotion *gamepad_motion = nullptr; + + MotionInfo() { + sensors_enabled = false; + calibrating = false; + calibrated = false; + } + }; + + HashMap joy_motion; + struct VelocityTrack { uint64_t last_tick = 0; Vector2 velocity; @@ -363,6 +385,28 @@ public: void set_joy_light(int p_device, const Color &p_color); bool has_joy_light(int p_device) const; + Vector3 get_joy_accelerometer(int p_device) const; + Vector3 get_joy_gravity(int p_device) const; + Vector3 get_joy_gyroscope(int p_device) const; + + void set_joy_motion_sensors_enabled(int p_device, bool p_enable); + bool is_joy_motion_sensors_enabled(int p_device) const; + + bool has_joy_motion_sensors(int p_device) const; + float get_joy_motion_sensors_rate(int p_device) const; + + void start_joy_motion_sensors_calibration(int p_device); + void stop_joy_motion_sensors_calibration(int p_device); + void clear_joy_motion_sensors_calibration(int p_device); + + Dictionary get_joy_motion_sensors_calibration(int p_device) const; + void set_joy_motion_sensors_calibration(int p_device, const Dictionary &p_calibration_info); + + bool is_joy_motion_sensors_calibrating(int p_device) const; + bool is_joy_motion_sensors_calibrated(int p_device) const; + + void set_joy_motion_sensors_rate(int p_device, float p_rate); + void start_joy_vibration(int p_device, float p_weak_magnitude, float p_strong_magnitude, float p_duration = 0); void stop_joy_vibration(int p_device); void vibrate_handheld(int p_duration_ms = 500, float p_amplitude = -1.0); @@ -388,6 +432,7 @@ public: void joy_button(int p_device, JoyButton p_button, bool p_pressed); void joy_axis(int p_device, JoyAxis p_axis, float p_value); void joy_hat(int p_device, BitField p_val); + void joy_motion_sensors(int p_device, const Vector3 &p_accelerometer, const Vector3 &p_gyroscope); void add_joy_mapping(const String &p_mapping, bool p_update_existing = false); void remove_joy_mapping(const String &p_guid); diff --git a/doc/classes/Input.xml b/doc/classes/Input.xml index 8195ad61618..164ca813c29 100644 --- a/doc/classes/Input.xml +++ b/doc/classes/Input.xml @@ -38,6 +38,15 @@ Adds a new mapping entry (in SDL2 format) to the mapping database. Optionally update already connected devices. + + + + + Clears the calibration information for the specified joypad's motion sensors, if it has any and if they were calibrated. + See [method start_joy_motion_sensors_calibration] for an example on how to use joypad motion sensors and calibration in your games. + [b]Note:[/b] This feature is only supported on Windows, Linux, and macOS. + + @@ -109,6 +118,19 @@ [b]Note:[/b] For Android, [member ProjectSettings.input_devices/sensors/enable_gyroscope] must be enabled. + + + + + Returns the acceleration, including the force of gravity, in m/s² of the joypad's accelerometer sensor, if the joypad has one and it's currently enabled. Otherwise, the method returns [constant Vector3.ZERO]. See also [method get_joy_gravity] and [method set_joy_motion_sensors_enabled]. + For a joypad held in front of you, the returned axes are defined as follows: + +X ... -X: left ... right; + +Y ... -Y: bottom ... top; + +Z ... -Z: farther ... closer. + The gravity part value is measured as a vector with length of [code]9.8[/code] away from the center of the Earth, which is a negative Y value. + [b]Note:[/b] This feature is only supported on Windows, Linux, and macOS. + + @@ -117,6 +139,19 @@ Returns the current value of the joypad axis at index [param axis]. + + + + + Returns the gravity in m/s² of the joypad's accelerometer sensor, if the joypad has one and it's currently enabled. Otherwise, the method returns [constant Vector3.ZERO]. See also [method get_joy_accelerometer] and [method set_joy_motion_sensors_enabled]. + For a joypad held in front of you, the returned axes are defined as follows: + +X ... -X: left ... right; + +Y ... -Y: bottom ... top; + +Z ... -Z: farther ... closer. + The gravity part value is measured as a vector with length of [code]9.8[/code] away from the center of the Earth, which is a negative Y value. + [b]Note:[/b] This feature is only supported on Windows, Linux, and macOS. + + @@ -125,6 +160,20 @@ On Windows, all XInput joypad GUIDs will be overridden by Godot to [code]__XINPUT_DEVICE__[/code], because their mappings are the same. + + + + + Returns the rotation rate in rad/s around a joypad's X, Y, and Z axes of the gyroscope sensor, if the joypad has one and it's currently enabled. Otherwise, the method returns [constant Vector3.ZERO]. See also [method set_joy_motion_sensors_enabled]. + The rotation is positive in the counter-clockwise direction. + For a joypad held in front of you, the returned axes are defined as follows: + X: Angular speed around the X axis (pitch); + Y: Angular speed around the Y axis (yaw); + Z: Angular speed around the Z axis (roll). + See [method start_joy_motion_sensors_calibration] for an example on how to use joypad gyroscope and gyroscope calibration in your games. + [b]Note:[/b] This feature is only supported on Windows, Linux, and macOS. + + @@ -140,6 +189,25 @@ [b]Note:[/b] The returned dictionary is always empty on Android, iOS, visionOS, and Web. + + + + + Returns the calibration information about the specified joypad's motion sensors in the form of a [Dictionary], if it has any and if they have been calibrated, otherwise returns an empty [Dictionary]. + The dictionary contains the following fields: + [code]gyroscope_offset[/code]: average offset in gyroscope values from [constant Vector2.ZERO] in rad/s. + See [method start_joy_motion_sensors_calibration] for an example on how to use joypad motion sensors and calibration in your games. + [b]Note:[/b] This feature is only supported on Windows, Linux, and macOS. + + + + + + + Returns the joypad's motion sensor rate in Hz, if the joypad has motion sensors and they're currently enabled. See also [method set_joy_motion_sensors_enabled]. + [b]Note:[/b] This feature is only supported on Windows, Linux, and macOS. + + @@ -208,6 +276,14 @@ [b]Note:[/b] This feature is only supported on Windows, Linux, and macOS. + + + + + Returns [code]true[/code] if the joypad has motion sensors (accelerometer and gyroscope). + [b]Note:[/b] This feature is only supported on Windows, Linux, and macOS. + + @@ -288,6 +364,33 @@ Returns [code]true[/code] if the system knows the specified device. This means that it sets all button and axis indices. Unknown joypads are not expected to match these constants, but you can still retrieve events from them. + + + + + Returns [code]true[/code] if the joypad's motion sensors have been calibrated. + See [method start_joy_motion_sensors_calibration] for an example on how to use joypad motion sensors and calibration in your games. + [b]Note:[/b] This feature is only supported on Windows, Linux, and macOS. + + + + + + + Returns [code]true[/code] if the joypad's motion sensors are currently being calibrated. + See [method start_joy_motion_sensors_calibration] for an example on how to use joypad motion sensors and calibration in your games. + [b]Note:[/b] This feature is only supported on Windows, Linux, and macOS. + + + + + + + Returns [code]true[/code] if the requested joypad has motion sensors (accelerometer and gyroscope) and they are currently enabled. See also [method set_joy_motion_sensors_enabled] and [method has_joy_motion_sensors]. + See [method start_joy_motion_sensors_calibration] for an example on how to use joypad motion sensors and calibration in your games. + [b]Note:[/b] This feature is only supported on Windows, Linux, and macOS. + + @@ -407,6 +510,27 @@ [b]Note:[/b] This feature is only supported on Windows, Linux, and macOS. + + + + + + Sets the specified joypad's calibration information. See also [method get_joy_motion_sensors_calibration]. + See [method start_joy_motion_sensors_calibration] for an example on how to use joypad motion sensors and calibration in your games. + [b]Note:[/b] This feature is only supported on Windows, Linux, and macOS. + + + + + + + + Enables or disables the motion sensors (accelerometer and gyroscope), if available, on the specified joypad. + See [method start_joy_motion_sensors_calibration] for an example on how to use joypad motion sensors and calibration in your games. + It's recommended to disable the motion sensors when they're no longer being used, because otherwise it might drain the controller battery faster. + [b]Note:[/b] This feature is only supported on Windows, Linux, and macOS. + + @@ -424,6 +548,114 @@ [b]Note:[/b] Some 3rd party tools can contribute to the list of ignored devices. For example, [i]SteamInput[/i] creates virtual devices from physical devices for remapping purposes. To avoid handling the same input device twice, the original device is added to the ignore list. + + + + + Starts the process of calibrating the specified joypad's gyroscope, if it has one. + Once a joypad's gyroscope has been calibrated correctly (e.g. laying still on a table without being rotated), [method get_joy_gyroscope] will return values close or equal to [constant Vector3.ZERO] when the joypad is not being rotated. + Here's an example of how to use joypad gyroscope and gyroscope calibration in your games: + [codeblocks] + [gdscript] + const GYRO_SENSITIVITY = 10.0 + + func _ready(): + # In this example we only use the first connected joypad (id 0). + if 0 not in Input.get_connected_joypads(): + return + + if not Input.has_joy_motion_sensors(0): + return + + # We must enable the motion sensors before using them. + Input.set_joy_motion_sensors_enabled(0, true) + + # (Tell the users here that they need to put their joypads on a flat surface and wait for confirmation.) + + # Start the calibration process. + calibrate_motion() + + func _process(delta): + # Only move the object if the joypad motion sensors are calibrated. + if Input.is_joy_motion_sensors_calibrated(0): + move_object(delta) + + func calibrate_motion(): + Input.start_joy_motion_sensors_calibration(0) + + # Wait for some time + await get_tree().create_timer(1.0).timeout + + Input.stop_joy_motion_sensors_calibration(0) + # The joypad is now calibrated. + + func move_object(delta): + var object: Node3D = ... # Put your object here. + + var gyro := Input.get_joy_gyroscope(0) + object.rotation.x -= -gyro.y * GYRO_SENSITIVITY * 0.5 * delta # Use rotation around the Y axis (yaw) here + object.rotation.y += -gyro.x * GYRO_SENSITIVITY * delta # Use rotation around the X axis (pitch) here + [/gdscript] + [csharp] + const double GYRO_SENSITIVITY = 10.0; + + public override void _Ready() + { + // In this example we only use the first connected joypad (id 0). + if (!Input.GetConnectedJoypads().Has(0)) + { + return; + } + + if (!Input.HasJoyMotionSensors(0)) + { + return; + } + + // We must enable the accelerometer and the gyroscope before using them. + Input.SetJoyMotionSensorsEnabled(0, true); + + // (Tell the users here that they need to put their joypads on a flat surface and wait for confirmation.) + + // Start the calibration process. + CalibrateMotion(); + } + + public override void _Process(double delta) + { + // Only move the object if the joypad motion sensors are calibrated. + if (Input.IsJoyMotionSensorsCalibrated(0)) + { + MoveObject(delta); + } + } + + private void CalibrateMotion() + { + Input.StartJoyMotionSensorsCalibration(0); + + // Wait for some time. + await ToSignal(GetTree().CreateTimer(1.0), "timeout"); + + Input.StopJoyMotionSensorsCalibration(0); + // The joypad is now calibrated. + } + + private void MoveObject(double delta) + { + Node3D object = ... ; // Put your object here. + Vector3 gyro = Input.GetJoyGyroscope(0); + Vector3 rotation = object.Rotation; + rotation.X -= -gyro.Y * GYRO_SENSITIVITY * 0.5 * delta; // Use rotation around the Y axis (yaw) here + rotation.Y += -gyro.X * GYRO_SENSITIVITY * delta; // Use rotation around the X axis (pitch) here + object.Rotation = rotation; + } + [/csharp] + [/codeblocks] + [b]Note:[/b] Accelerometer sensor doesn't usually require calibration. + [b]Note:[/b] This feature is only supported on Windows, Linux, and macOS. + + @@ -436,6 +668,15 @@ [b]Note:[/b] For macOS, vibration is only supported in macOS 11 and later. + + + + + Stops the calibration process of the specified joypad's motion sensors. + See [method start_joy_motion_sensors_calibration] for an example on how to use joypad motion sensors and calibration in your games. + [b]Note:[/b] This feature is only supported on Windows, Linux, and macOS. + + diff --git a/drivers/sdl/joypad_sdl.cpp b/drivers/sdl/joypad_sdl.cpp index 993847365fb..7de3f95a325 100644 --- a/drivers/sdl/joypad_sdl.cpp +++ b/drivers/sdl/joypad_sdl.cpp @@ -169,6 +169,7 @@ void JoypadSDL::process_events() { joypads[joy_id].sdl_instance_idx = sdl_event.jdevice.which; joypads[joy_id].supports_force_feedback = SDL_GetBooleanProperty(propertiesID, SDL_PROP_JOYSTICK_CAP_RUMBLE_BOOLEAN, false); joypads[joy_id].guid = StringName(String(guid)); + joypads[joy_id].supports_motion_sensors = SDL_GamepadHasSensor(gamepad, SDL_SENSOR_ACCEL) && SDL_GamepadHasSensor(gamepad, SDL_SENSOR_GYRO); sdl_instance_id_to_joypad_id.insert(sdl_event.jdevice.which, joy_id); @@ -198,6 +199,11 @@ void JoypadSDL::process_events() { joypad_info); Input::get_singleton()->set_joy_features(joy_id, &joypads[joy_id]); + + if (joypads[joy_id].supports_motion_sensors) { + // Data rate for all sensors should be the same. + Input::get_singleton()->set_joy_motion_sensors_rate(joy_id, SDL_GetGamepadSensorDataRate(gamepad, SDL_SENSOR_ACCEL)); + } } // An event for an attached joypad } else if (sdl_event.type >= SDL_EVENT_JOYSTICK_AXIS_MOTION && sdl_event.type < SDL_EVENT_FINGER_DOWN && sdl_instance_id_to_joypad_id.has(sdl_event.jdevice.which)) { @@ -272,6 +278,25 @@ void JoypadSDL::process_events() { } } } + + for (int i = 0; i < Input::JOYPADS_MAX; i++) { + Joypad &joy = joypads[i]; + if (!joy.attached || !joy.supports_motion_sensors) { + continue; + } + SDL_Gamepad *gamepad = SDL_GetGamepadFromID(joy.sdl_instance_idx); + // gamepad should not be NULL since joy.supports_motion_sensors is true here. + + float accel_data[3]; + float gyro_data[3]; + SDL_GetGamepadSensorData(gamepad, SDL_SENSOR_ACCEL, accel_data, 3); + SDL_GetGamepadSensorData(gamepad, SDL_SENSOR_GYRO, gyro_data, 3); + + Input::get_singleton()->joy_motion_sensors( + i, + Vector3(-accel_data[0], -accel_data[1], -accel_data[2]), + Vector3(gyro_data[0], gyro_data[1], gyro_data[2])); + } } void JoypadSDL::close_joypad(int p_pad_idx) { @@ -301,6 +326,16 @@ void JoypadSDL::Joypad::set_joy_light(const Color &p_color) { SDL_SetJoystickLED(get_sdl_joystick(), p_color.get_r8(), p_color.get_g8(), p_color.get_b8()); } +bool JoypadSDL::Joypad::has_joy_motion_sensors() const { + return supports_motion_sensors; +} + +void JoypadSDL::Joypad::set_joy_motion_sensors_enabled(bool p_enable) { + SDL_Gamepad *gamepad = get_sdl_gamepad(); + SDL_SetGamepadSensorEnabled(gamepad, SDL_SENSOR_ACCEL, p_enable); + SDL_SetGamepadSensorEnabled(gamepad, SDL_SENSOR_GYRO, p_enable); +} + SDL_Joystick *JoypadSDL::Joypad::get_sdl_joystick() const { return SDL_GetJoystickFromID(sdl_instance_idx); } diff --git a/drivers/sdl/joypad_sdl.h b/drivers/sdl/joypad_sdl.h index 88aa342bf5c..c3919611fcd 100644 --- a/drivers/sdl/joypad_sdl.h +++ b/drivers/sdl/joypad_sdl.h @@ -56,11 +56,15 @@ private: SDL_JoystickID sdl_instance_idx; bool supports_force_feedback = false; + bool supports_motion_sensors = false; uint64_t ff_effect_timestamp = 0; virtual bool has_joy_light() const override; virtual void set_joy_light(const Color &p_color) override; + virtual bool has_joy_motion_sensors() const override; + virtual void set_joy_motion_sensors_enabled(bool p_enable) override; + SDL_Joystick *get_sdl_joystick() const; SDL_Gamepad *get_sdl_gamepad() const; }; diff --git a/thirdparty/README.md b/thirdparty/README.md index 40bf2af4c2a..977f2919112 100644 --- a/thirdparty/README.md +++ b/thirdparty/README.md @@ -368,6 +368,21 @@ Files extracted from upstream source: - `LICENSE.TXT` and `docs/FTL.TXT` +## gamepadmotionhelpers + +- Upstream: https://github.com/JibbSmart/GamepadMotionHelpers +- Version: 39b578aacf34c3a1c584d8f7f194adc776f88055, 2023 +- License: MIT + +Files extracted from upstream source: + +- `GamepadMotion.hpp` +- `LICENSE.TXT` + +Patches: + +- `0001-fix-warnings.patch` ([GH-111679](https://github.com/godotengine/godot/pull/111679)) + ## glad - Upstream: https://github.com/Dav1dde/glad diff --git a/thirdparty/gamepadmotionhelpers/GamepadMotion.hpp b/thirdparty/gamepadmotionhelpers/GamepadMotion.hpp new file mode 100644 index 00000000000..7d89f86d7f7 --- /dev/null +++ b/thirdparty/gamepadmotionhelpers/GamepadMotion.hpp @@ -0,0 +1,1298 @@ +// Copyright (c) 2020-2023 Julian "Jibb" Smart +// Released under the MIT license. See https://github.com/JibbSmart/GamepadMotionHelpers/blob/main/LICENSE for more info +// Version 9 + +#pragma once + +#define _USE_MATH_DEFINES +#include +#include // std::min, std::max and std::clamp + +// You don't need to look at these. These will just be used internally by the GamepadMotion class declared below. +// You can ignore anything in namespace GamepadMotionHelpers. +class GamepadMotionSettings; +class GamepadMotion; + +namespace GamepadMotionHelpers +{ + struct GyroCalibration + { + float X; + float Y; + float Z; + float AccelMagnitude; + int NumSamples; + }; + + struct Quat + { + float w; + float x; + float y; + float z; + + Quat(); + Quat(float inW, float inX, float inY, float inZ); + void Set(float inW, float inX, float inY, float inZ); + Quat& operator*=(const Quat& rhs); + friend Quat operator*(Quat lhs, const Quat& rhs); + void Normalize(); + Quat Normalized() const; + void Invert(); + Quat Inverse() const; + }; + + struct Vec + { + float x; + float y; + float z; + + Vec(); + Vec(float inValue); + Vec(float inX, float inY, float inZ); + void Set(float inX, float inY, float inZ); + float Length() const; + float LengthSquared() const; + void Normalize(); + Vec Normalized() const; + float Dot(const Vec& other) const; + Vec Cross(const Vec& other) const; + Vec Min(const Vec& other) const; + Vec Max(const Vec& other) const; + Vec Abs() const; + Vec Lerp(const Vec& other, float factor) const; + Vec Lerp(const Vec& other, const Vec& factor) const; + Vec& operator+=(const Vec& rhs); + friend Vec operator+(Vec lhs, const Vec& rhs); + Vec& operator-=(const Vec& rhs); + friend Vec operator-(Vec lhs, const Vec& rhs); + Vec& operator*=(const float rhs); + friend Vec operator*(Vec lhs, const float rhs); + Vec& operator/=(const float rhs); + friend Vec operator/(Vec lhs, const float rhs); + Vec& operator*=(const Quat& rhs); + friend Vec operator*(Vec lhs, const Quat& rhs); + Vec operator-() const; + }; + + struct SensorMinMaxWindow + { + Vec MinGyro; + Vec MaxGyro; + Vec MeanGyro; + Vec MinAccel; + Vec MaxAccel; + Vec MeanAccel; + Vec StartAccel; + int NumSamples = 0; + float TimeSampled = 0.f; + + SensorMinMaxWindow(); + void Reset(float remainder); + void AddSample(const Vec& inGyro, const Vec& inAccel, float deltaTime); + Vec GetMidGyro(); + }; + + struct AutoCalibration + { + SensorMinMaxWindow MinMaxWindow; + Vec SmoothedAngularVelocityGyro; + Vec SmoothedAngularVelocityAccel; + Vec SmoothedPreviousAccel; + Vec PreviousAccel; + + AutoCalibration(); + void Reset(); + bool AddSampleStillness(const Vec& inGyro, const Vec& inAccel, float deltaTime, bool doSensorFusion); + void NoSampleStillness(); + bool AddSampleSensorFusion(const Vec& inGyro, const Vec& inAccel, float deltaTime); + void NoSampleSensorFusion(); + void SetCalibrationData(GyroCalibration* calibrationData); + void SetSettings(GamepadMotionSettings* settings); + + float Confidence = 0.f; + bool IsSteady() { return bIsSteady; } + + private: + Vec MinDeltaGyro = Vec(1.f); + Vec MinDeltaAccel = Vec(0.25f); + float RecalibrateThreshold = 1.f; + float SensorFusionSkippedTime = 0.f; + float TimeSteadySensorFusion = 0.f; + float TimeSteadyStillness = 0.f; + bool bIsSteady = false; + + GyroCalibration* CalibrationData; + GamepadMotionSettings* Settings; + }; + + struct Motion + { + Quat Quaternion; + Vec Accel; + Vec Grav; + + Vec SmoothAccel = Vec(); + float Shakiness = 0.f; + const float ShortSteadinessHalfTime = 0.25f; + const float LongSteadinessHalfTime = 1.f; + + Motion(); + void Reset(); + void Update(float inGyroX, float inGyroY, float inGyroZ, float inAccelX, float inAccelY, float inAccelZ, float gravityLength, float deltaTime); + void SetSettings(GamepadMotionSettings* settings); + + private: + GamepadMotionSettings* Settings; + }; + + enum CalibrationMode + { + Manual = 0, + Stillness = 1, + SensorFusion = 2, + }; + + // https://stackoverflow.com/a/1448478/1130520 + inline CalibrationMode operator|(CalibrationMode a, CalibrationMode b) + { + return static_cast(static_cast(a) | static_cast(b)); + } + + inline CalibrationMode operator&(CalibrationMode a, CalibrationMode b) + { + return static_cast(static_cast(a) & static_cast(b)); + } + + inline CalibrationMode operator~(CalibrationMode a) + { + return static_cast(~static_cast(a)); + } + + // https://stackoverflow.com/a/23152590/1130520 + inline CalibrationMode& operator|=(CalibrationMode& a, CalibrationMode b) + { + return (CalibrationMode&)((int&)(a) |= static_cast(b)); + } + + inline CalibrationMode& operator&=(CalibrationMode& a, CalibrationMode b) + { + return (CalibrationMode&)((int&)(a) &= static_cast(b)); + } +} + +// Note that I'm using a Y-up coordinate system. This is to follow the convention set by the motion sensors in +// PlayStation controllers, which was what I was using when writing in this. But for the record, Z-up is +// better for most games (XY ground-plane in 3D games simplifies using 2D vectors in navigation, for example). + +// Gyro units should be degrees per second. Accelerometer should be g-force (approx. 9.8 m/s^2 = 1 g). If you're using +// radians per second, meters per second squared, etc, conversion should be simple. + +class GamepadMotionSettings +{ +public: + int MinStillnessSamples = 10; + float MinStillnessCollectionTime = 0.5f; + float MinStillnessCorrectionTime = 2.f; + float MaxStillnessError = 2.f; + float StillnessSampleDeteriorationRate = 0.2f; + float StillnessErrorClimbRate = 0.1f; + float StillnessErrorDropOnRecalibrate = 0.1f; + float StillnessCalibrationEaseInTime = 3.f; + float StillnessCalibrationHalfTime = 0.1f; + float StillnessConfidenceRate = 1.f; + + float StillnessGyroDelta = -1.f; + float StillnessAccelDelta = -1.f; + + float SensorFusionCalibrationSmoothingStrength = 2.f; + float SensorFusionAngularAccelerationThreshold = 20.f; + float SensorFusionCalibrationEaseInTime = 3.f; + float SensorFusionCalibrationHalfTime = 0.1f; + float SensorFusionConfidenceRate = 1.f; + + float GravityCorrectionShakinessMaxThreshold = 0.4f; + float GravityCorrectionShakinessMinThreshold = 0.01f; + + float GravityCorrectionStillSpeed = 1.f; + float GravityCorrectionShakySpeed = 0.1f; + + float GravityCorrectionGyroFactor = 0.1f; + float GravityCorrectionGyroMinThreshold = 0.05f; + float GravityCorrectionGyroMaxThreshold = 0.25f; + + float GravityCorrectionMinimumSpeed = 0.01f; +}; + +class GamepadMotion +{ +public: + GamepadMotion(); + + void Reset(); + + void ProcessMotion(float gyroX, float gyroY, float gyroZ, + float accelX, float accelY, float accelZ, float deltaTime); + + // reading the current state + void GetCalibratedGyro(float& x, float& y, float& z); + void GetGravity(float& x, float& y, float& z); + void GetProcessedAcceleration(float& x, float& y, float& z); + void GetOrientation(float& w, float& x, float& y, float& z); + void GetPlayerSpaceGyro(float& x, float& y, const float yawRelaxFactor = 1.41f); + static void CalculatePlayerSpaceGyro(float& x, float& y, const float gyroX, const float gyroY, const float gyroZ, const float gravX, const float gravY, const float gravZ, const float yawRelaxFactor = 1.41f); + void GetWorldSpaceGyro(float& x, float& y, const float sideReductionThreshold = 0.125f); + static void CalculateWorldSpaceGyro(float& x, float& y, const float gyroX, const float gyroY, const float gyroZ, const float gravX, const float gravY, const float gravZ, const float sideReductionThreshold = 0.125f); + + // gyro calibration functions + void StartContinuousCalibration(); + void PauseContinuousCalibration(); + void ResetContinuousCalibration(); + void GetCalibrationOffset(float& xOffset, float& yOffset, float& zOffset); + void SetCalibrationOffset(float xOffset, float yOffset, float zOffset, int weight); + float GetAutoCalibrationConfidence(); + void SetAutoCalibrationConfidence(float newConfidence); + bool GetAutoCalibrationIsSteady(); + + GamepadMotionHelpers::CalibrationMode GetCalibrationMode(); + void SetCalibrationMode(GamepadMotionHelpers::CalibrationMode calibrationMode); + + void ResetMotion(); + + GamepadMotionSettings Settings; + +private: + GamepadMotionHelpers::Vec Gyro; + GamepadMotionHelpers::Vec RawAccel; + GamepadMotionHelpers::Motion Motion; + GamepadMotionHelpers::GyroCalibration GyroCalibration; + GamepadMotionHelpers::AutoCalibration AutoCalibration; + GamepadMotionHelpers::CalibrationMode CurrentCalibrationMode; + + bool IsCalibrating; + void PushSensorSamples(float gyroX, float gyroY, float gyroZ, float accelMagnitude); + void GetCalibratedSensor(float& gyroOffsetX, float& gyroOffsetY, float& gyroOffsetZ, float& accelMagnitude); +}; + +///////////// Everything below here are just implementation details ///////////// + +namespace GamepadMotionHelpers +{ + inline Quat::Quat() + { + w = 1.0f; + x = 0.0f; + y = 0.0f; + z = 0.0f; + } + + inline Quat::Quat(float inW, float inX, float inY, float inZ) + { + w = inW; + x = inX; + y = inY; + z = inZ; + } + + inline static Quat AngleAxis(float inAngle, float inX, float inY, float inZ) + { + const float sinHalfAngle = sinf(inAngle * 0.5f); + Vec inAxis = Vec(inX, inY, inZ); + inAxis.Normalize(); + inAxis *= sinHalfAngle; + Quat result = Quat(cosf(inAngle * 0.5f), inAxis.x, inAxis.y, inAxis.z); + return result; + } + + inline void Quat::Set(float inW, float inX, float inY, float inZ) + { + w = inW; + x = inX; + y = inY; + z = inZ; + } + + inline Quat& Quat::operator*=(const Quat& rhs) + { + Set(w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z, + w * rhs.x + x * rhs.w + y * rhs.z - z * rhs.y, + w * rhs.y - x * rhs.z + y * rhs.w + z * rhs.x, + w * rhs.z + x * rhs.y - y * rhs.x + z * rhs.w); + return *this; + } + + inline Quat operator*(Quat lhs, const Quat& rhs) + { + lhs *= rhs; + return lhs; + } + + inline void Quat::Normalize() + { + const float length = sqrtf(w * w + x * x + y * y + z * z); + const float fixFactor = 1.0f / length; + + w *= fixFactor; + x *= fixFactor; + y *= fixFactor; + z *= fixFactor; + + return; + } + + inline Quat Quat::Normalized() const + { + Quat result = *this; + result.Normalize(); + return result; + } + + inline void Quat::Invert() + { + x = -x; + y = -y; + z = -z; + return; + } + + inline Quat Quat::Inverse() const + { + Quat result = *this; + result.Invert(); + return result; + } + + inline Vec::Vec() + { + x = 0.0f; + y = 0.0f; + z = 0.0f; + } + + inline Vec::Vec(float inValue) + { + x = inValue; + y = inValue; + z = inValue; + } + + inline Vec::Vec(float inX, float inY, float inZ) + { + x = inX; + y = inY; + z = inZ; + } + + inline void Vec::Set(float inX, float inY, float inZ) + { + x = inX; + y = inY; + z = inZ; + } + + inline float Vec::Length() const + { + return sqrtf(x * x + y * y + z * z); + } + + inline float Vec::LengthSquared() const + { + return x * x + y * y + z * z; + } + + inline void Vec::Normalize() + { + const float length = Length(); + if (length == 0.0) + { + return; + } + const float fixFactor = 1.0f / length; + + x *= fixFactor; + y *= fixFactor; + z *= fixFactor; + return; + } + + inline Vec Vec::Normalized() const + { + Vec result = *this; + result.Normalize(); + return result; + } + + inline Vec& Vec::operator+=(const Vec& rhs) + { + Set(x + rhs.x, y + rhs.y, z + rhs.z); + return *this; + } + + inline Vec operator+(Vec lhs, const Vec& rhs) + { + lhs += rhs; + return lhs; + } + + inline Vec& Vec::operator-=(const Vec& rhs) + { + Set(x - rhs.x, y - rhs.y, z - rhs.z); + return *this; + } + + inline Vec operator-(Vec lhs, const Vec& rhs) + { + lhs -= rhs; + return lhs; + } + + inline Vec& Vec::operator*=(const float rhs) + { + Set(x * rhs, y * rhs, z * rhs); + return *this; + } + + inline Vec operator*(Vec lhs, const float rhs) + { + lhs *= rhs; + return lhs; + } + + inline Vec& Vec::operator/=(const float rhs) + { + Set(x / rhs, y / rhs, z / rhs); + return *this; + } + + inline Vec operator/(Vec lhs, const float rhs) + { + lhs /= rhs; + return lhs; + } + + inline Vec& Vec::operator*=(const Quat& rhs) + { + Quat temp = rhs * Quat(0.0f, x, y, z) * rhs.Inverse(); + Set(temp.x, temp.y, temp.z); + return *this; + } + + inline Vec operator*(Vec lhs, const Quat& rhs) + { + lhs *= rhs; + return lhs; + } + + inline Vec Vec::operator-() const + { + Vec result = Vec(-x, -y, -z); + return result; + } + + inline float Vec::Dot(const Vec& other) const + { + return x * other.x + y * other.y + z * other.z; + } + + inline Vec Vec::Cross(const Vec& other) const + { + return Vec(y * other.z - z * other.y, + z * other.x - x * other.z, + x * other.y - y * other.x); + } + + inline Vec Vec::Min(const Vec& other) const + { + return Vec(x < other.x ? x : other.x, + y < other.y ? y : other.y, + z < other.z ? z : other.z); + } + + inline Vec Vec::Max(const Vec& other) const + { + return Vec(x > other.x ? x : other.x, + y > other.y ? y : other.y, + z > other.z ? z : other.z); + } + + inline Vec Vec::Abs() const + { + return Vec(x > 0 ? x : -x, + y > 0 ? y : -y, + z > 0 ? z : -z); + } + + inline Vec Vec::Lerp(const Vec& other, float factor) const + { + return *this + (other - *this) * factor; + } + + inline Vec Vec::Lerp(const Vec& other, const Vec& factor) const + { + return Vec(this->x + (other.x - this->x) * factor.x, + this->y + (other.y - this->y) * factor.y, + this->z + (other.z - this->z) * factor.z); + } + + inline Motion::Motion() + { + Reset(); + } + + inline void Motion::Reset() + { + Quaternion.Set(1.f, 0.f, 0.f, 0.f); + Accel.Set(0.f, 0.f, 0.f); + Grav.Set(0.f, 0.f, 0.f); + SmoothAccel.Set(0.f, 0.f, 0.f); + Shakiness = 0.f; + } + + /// + /// The gyro inputs should be calibrated degrees per second but have no other processing. Acceleration is in G units (1 = approx. 9.8m/s^2) + /// + inline void Motion::Update(float inGyroX, float inGyroY, float inGyroZ, float inAccelX, float inAccelY, float inAccelZ, float gravityLength, float deltaTime) + { + if (!Settings) + { + return; + } + + // get settings + const float gravityCorrectionShakinessMinThreshold = Settings->GravityCorrectionShakinessMinThreshold; + const float gravityCorrectionShakinessMaxThreshold = Settings->GravityCorrectionShakinessMaxThreshold; + const float gravityCorrectionStillSpeed = Settings->GravityCorrectionStillSpeed; + const float gravityCorrectionShakySpeed = Settings->GravityCorrectionShakySpeed; + const float gravityCorrectionGyroFactor = Settings->GravityCorrectionGyroFactor; + const float gravityCorrectionGyroMinThreshold = Settings->GravityCorrectionGyroMinThreshold; + const float gravityCorrectionGyroMaxThreshold = Settings->GravityCorrectionGyroMaxThreshold; + const float gravityCorrectionMinimumSpeed = Settings->GravityCorrectionMinimumSpeed; + + const Vec axis = Vec(inGyroX, inGyroY, inGyroZ); + const Vec accel = Vec(inAccelX, inAccelY, inAccelZ); + const float angleSpeed = axis.Length() * (float)M_PI / 180.0f; + const float angle = angleSpeed * deltaTime; + + // rotate + Quat rotation = AngleAxis(angle, axis.x, axis.y, axis.z); + Quaternion *= rotation; // do it this way because it's a local rotation, not global + + //printf("Quat: %.4f %.4f %.4f %.4f\n", + // Quaternion.w, Quaternion.x, Quaternion.y, Quaternion.z); + float accelMagnitude = accel.Length(); + if (accelMagnitude > 0.0f) + { + const Vec accelNorm = accel / accelMagnitude; + // account for rotation when tracking smoothed acceleration + SmoothAccel *= rotation.Inverse(); + //printf("Absolute Accel: %.4f %.4f %.4f\n", + // absoluteAccel.x, absoluteAccel.y, absoluteAccel.z); + const float smoothFactor = ShortSteadinessHalfTime <= 0.f ? 0.f : exp2f(-deltaTime / ShortSteadinessHalfTime); + Shakiness *= smoothFactor; + Shakiness = std::max(Shakiness, (accel - SmoothAccel).Length()); + SmoothAccel = accel.Lerp(SmoothAccel, smoothFactor); + + //printf("Shakiness: %.4f\n", Shakiness); + + // update grav by rotation + Grav *= rotation.Inverse(); + // we want to close the gap between grav and raw acceleration. What's the difference + const Vec gravToAccel = (accelNorm * -gravityLength) - Grav; + const Vec gravToAccelDir = gravToAccel.Normalized(); + // adjustment rate + float gravCorrectionSpeed; + if (gravityCorrectionShakinessMinThreshold < gravityCorrectionShakinessMaxThreshold) + { + gravCorrectionSpeed = gravityCorrectionStillSpeed + (gravityCorrectionShakySpeed - gravityCorrectionStillSpeed) * std::clamp((Shakiness - gravityCorrectionShakinessMinThreshold) / (gravityCorrectionShakinessMaxThreshold - gravityCorrectionShakinessMinThreshold), 0.f, 1.f); + } + else + { + gravCorrectionSpeed = Shakiness < gravityCorrectionShakinessMaxThreshold ? gravityCorrectionStillSpeed : gravityCorrectionShakySpeed; + } + // we also limit it to be no faster than a given proportion of the gyro rate, or the minimum gravity correction speed + const float gyroGravCorrectionLimit = std::max(angleSpeed * gravityCorrectionGyroFactor, gravityCorrectionMinimumSpeed); + if (gravCorrectionSpeed > gyroGravCorrectionLimit) + { + float closeEnoughFactor; + if (gravityCorrectionGyroMinThreshold < gravityCorrectionGyroMaxThreshold) + { + closeEnoughFactor = std::clamp((gravToAccel.Length() - gravityCorrectionGyroMinThreshold) / (gravityCorrectionGyroMaxThreshold - gravityCorrectionGyroMinThreshold), 0.f, 1.f); + } + else + { + closeEnoughFactor = gravToAccel.Length() < gravityCorrectionGyroMaxThreshold ? 0.f : 1.f; + } + gravCorrectionSpeed = gyroGravCorrectionLimit + (gravCorrectionSpeed - gyroGravCorrectionLimit) * closeEnoughFactor; + } + const Vec gravToAccelDelta = gravToAccelDir * gravCorrectionSpeed * deltaTime; + if (gravToAccelDelta.LengthSquared() < gravToAccel.LengthSquared()) + { + Grav += gravToAccelDelta; + } + else + { + Grav = accelNorm * -gravityLength; + } + + const Vec gravityDirection = Grav.Normalized() * Quaternion.Inverse(); // absolute gravity direction + const float errorAngle = acosf(std::clamp(Vec(0.0f, -1.0f, 0.0f).Dot(gravityDirection), -1.f, 1.f)); + const Vec flattened = Vec(0.0f, -1.0f, 0.0f).Cross(gravityDirection); + Quat correctionQuat = AngleAxis(errorAngle, flattened.x, flattened.y, flattened.z); + Quaternion = Quaternion * correctionQuat; + + Accel = accel + Grav; + } + else + { + Grav *= rotation.Inverse(); + Accel = Grav; + } + Quaternion.Normalize(); + } + + inline void Motion::SetSettings(GamepadMotionSettings* settings) + { + Settings = settings; + } + + inline SensorMinMaxWindow::SensorMinMaxWindow() + { + Reset(0.f); + } + + inline void SensorMinMaxWindow::Reset(float remainder) + { + NumSamples = 0; + TimeSampled = remainder; + } + + inline void SensorMinMaxWindow::AddSample(const Vec& inGyro, const Vec& inAccel, float deltaTime) + { + if (NumSamples == 0) + { + MaxGyro = inGyro; + MinGyro = inGyro; + MeanGyro = inGyro; + MaxAccel = inAccel; + MinAccel = inAccel; + MeanAccel = inAccel; + StartAccel = inAccel; + NumSamples = 1; + TimeSampled += deltaTime; + return; + } + + MaxGyro = MaxGyro.Max(inGyro); + MinGyro = MinGyro.Min(inGyro); + MaxAccel = MaxAccel.Max(inAccel); + MinAccel = MinAccel.Min(inAccel); + + NumSamples++; + TimeSampled += deltaTime; + + Vec delta = inGyro - MeanGyro; + MeanGyro += delta * (1.f / NumSamples); + delta = inAccel - MeanAccel; + MeanAccel += delta * (1.f / NumSamples); + } + + inline Vec SensorMinMaxWindow::GetMidGyro() + { + return MeanGyro; + } + + inline AutoCalibration::AutoCalibration() + { + CalibrationData = nullptr; + Reset(); + } + + inline void AutoCalibration::Reset() + { + MinMaxWindow.Reset(0.f); + Confidence = 0.f; + bIsSteady = false; + MinDeltaGyro = Vec(1.f); + MinDeltaAccel = Vec(0.25f); + RecalibrateThreshold = 1.f; + SensorFusionSkippedTime = 0.f; + TimeSteadySensorFusion = 0.f; + TimeSteadyStillness = 0.f; + } + + inline bool AutoCalibration::AddSampleStillness(const Vec& inGyro, const Vec& inAccel, float deltaTime, bool doSensorFusion) + { + if (inGyro.x == 0.f && inGyro.y == 0.f && inGyro.z == 0.f && + inAccel.x == 0.f && inAccel.y == 0.f && inAccel.z == 0.f) + { + // zeroes are almost certainly not valid inputs + return false; + } + + if (!Settings) + { + return false; + } + + if (!CalibrationData) + { + return false; + } + + // get settings + const int minStillnessSamples = Settings->MinStillnessSamples; + const float minStillnessCollectionTime = Settings->MinStillnessCollectionTime; + const float minStillnessCorrectionTime = Settings->MinStillnessCorrectionTime; + const float maxStillnessError = Settings->MaxStillnessError; + const float stillnessSampleDeteriorationRate = Settings->StillnessSampleDeteriorationRate; + const float stillnessErrorClimbRate = Settings->StillnessErrorClimbRate; + const float stillnessErrorDropOnRecalibrate = Settings->StillnessErrorDropOnRecalibrate; + const float stillnessCalibrationEaseInTime = Settings->StillnessCalibrationEaseInTime; + const float stillnessCalibrationHalfTime = Settings->StillnessCalibrationHalfTime * Confidence; + const float stillnessConfidenceRate = Settings->StillnessConfidenceRate; + const float stillnessGyroDelta = Settings->StillnessGyroDelta; + const float stillnessAccelDelta = Settings->StillnessAccelDelta; + + MinMaxWindow.AddSample(inGyro, inAccel, deltaTime); + // get deltas + const Vec gyroDelta = MinMaxWindow.MaxGyro - MinMaxWindow.MinGyro; + const Vec accelDelta = MinMaxWindow.MaxAccel - MinMaxWindow.MinAccel; + + bool calibrated = false; + bool isSteady = false; + const Vec climbThisTick = Vec(stillnessSampleDeteriorationRate * deltaTime); + if (stillnessGyroDelta < 0.f) + { + if (Confidence < 1.f) + { + MinDeltaGyro += climbThisTick; + } + } + else + { + MinDeltaGyro = Vec(stillnessGyroDelta); + } + if (stillnessAccelDelta < 0.f) + { + if (Confidence < 1.f) + { + MinDeltaAccel += climbThisTick; + } + } + else + { + MinDeltaAccel = Vec(stillnessAccelDelta); + } + + //printf("Deltas: %.4f %.4f %.4f; %.4f %.4f %.4f\n", + // gyroDelta.x, gyroDelta.y, gyroDelta.z, + // accelDelta.x, accelDelta.y, accelDelta.z); + + if (MinMaxWindow.NumSamples >= minStillnessSamples && MinMaxWindow.TimeSampled >= minStillnessCollectionTime) + { + MinDeltaGyro = MinDeltaGyro.Min(gyroDelta); + MinDeltaAccel = MinDeltaAccel.Min(accelDelta); + } + else + { + RecalibrateThreshold = std::min(RecalibrateThreshold + stillnessErrorClimbRate * deltaTime, maxStillnessError); + return false; + } + + // check that all inputs are below appropriate thresholds to be considered "still" + if (gyroDelta.x <= MinDeltaGyro.x * RecalibrateThreshold && + gyroDelta.y <= MinDeltaGyro.y * RecalibrateThreshold && + gyroDelta.z <= MinDeltaGyro.z * RecalibrateThreshold && + accelDelta.x <= MinDeltaAccel.x * RecalibrateThreshold && + accelDelta.y <= MinDeltaAccel.y * RecalibrateThreshold && + accelDelta.z <= MinDeltaAccel.z * RecalibrateThreshold) + { + if (MinMaxWindow.NumSamples >= minStillnessSamples && MinMaxWindow.TimeSampled >= minStillnessCorrectionTime) + { + 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) + { + TimeSteadySensorFusion = 0.f; + //printf("No calibration due to acceleration of %.4f\n", gyroAccelerationMag); + } + else + { + 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/thirdparty/gamepadmotionhelpers/LICENSE.txt b/thirdparty/gamepadmotionhelpers/LICENSE.txt new file mode 100644 index 00000000000..a46396a3c14 --- /dev/null +++ b/thirdparty/gamepadmotionhelpers/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/thirdparty/gamepadmotionhelpers/patches/0001-fix-warnings.patch b/thirdparty/gamepadmotionhelpers/patches/0001-fix-warnings.patch new file mode 100644 index 00000000000..3b3c0ce77b2 --- /dev/null +++ b/thirdparty/gamepadmotionhelpers/patches/0001-fix-warnings.patch @@ -0,0 +1,37 @@ +diff --git a/thirdparty/gamepadmotionhelpers/GamepadMotion.hpp b/thirdparty/gamepadmotionhelpers/GamepadMotion.hpp +index 02497ac953..7d89f86d7f 100644 +--- a/thirdparty/gamepadmotionhelpers/GamepadMotion.hpp ++++ b/thirdparty/gamepadmotionhelpers/GamepadMotion.hpp +@@ -810,11 +810,6 @@ namespace GamepadMotionHelpers + { + 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; + +@@ -973,20 +968,11 @@ namespace GamepadMotionHelpers + // 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);