Add support for joypad motion sensors

2026-02-08 03:38:29 +00:00 · 2025-12-05 17:47:31 +05:00
parent 481f36ed20
commit a3eb202b25
10 changed files with 1940 additions and 0 deletions
--- 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<HatMask> 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<InputEventJoypadButton> 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) {