Skip to content

Commit 9422c91

Browse files
NintorchNintorch
committed
Add support for joypad motion sensors
1 parent ef34c3d commit 9422c91

File tree

5 files changed

+728
-0
lines changed

5 files changed

+728
-0
lines changed

‎core/input/input.cpp‎

Lines changed: 311 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -147,12 +147,29 @@ void Input::_bind_methods() {
147147
ClassDB::bind_method(D_METHOD("get_accelerometer"), &Input::get_accelerometer);
148148
ClassDB::bind_method(D_METHOD("get_magnetometer"), &Input::get_magnetometer);
149149
ClassDB::bind_method(D_METHOD("get_gyroscope"), &Input::get_gyroscope);
150+
ClassDB::bind_method(D_METHOD("is_joy_accelerometer_enabled", "device"), &Input::is_joy_accelerometer_enabled);
151+
ClassDB::bind_method(D_METHOD("is_joy_gyroscope_enabled", "device"), &Input::is_joy_gyroscope_enabled);
152+
ClassDB::bind_method(D_METHOD("get_joy_accelerometer", "device"), &Input::get_joy_accelerometer);
153+
ClassDB::bind_method(D_METHOD("get_joy_gravity", "device"), &Input::get_joy_gravity);
154+
ClassDB::bind_method(D_METHOD("get_joy_gyroscope", "device"), &Input::get_joy_gyroscope);
155+
ClassDB::bind_method(D_METHOD("get_joy_sensor_rate", "device"), &Input::get_joy_sensor_rate);
156+
ClassDB::bind_method(D_METHOD("start_joy_motion_calibration", "device"), &Input::start_joy_motion_calibration);
157+
ClassDB::bind_method(D_METHOD("stop_joy_motion_calibration", "device"), &Input::stop_joy_motion_calibration);
158+
ClassDB::bind_method(D_METHOD("clear_joy_motion_calibration", "device"), &Input::clear_joy_motion_calibration);
159+
ClassDB::bind_method(D_METHOD("get_joy_motion_calibration", "device"), &Input::get_joy_motion_calibration);
160+
ClassDB::bind_method(D_METHOD("set_joy_motion_calibration", "device", "calibration_info"), &Input::set_joy_motion_calibration);
161+
ClassDB::bind_method(D_METHOD("is_joy_motion_calibrated", "device"), &Input::is_joy_motion_calibrated);
162+
ClassDB::bind_method(D_METHOD("is_joy_motion_calibrating", "device"), &Input::is_joy_motion_calibrating);
150163
ClassDB::bind_method(D_METHOD("set_gravity", "value"), &Input::set_gravity);
151164
ClassDB::bind_method(D_METHOD("set_accelerometer", "value"), &Input::set_accelerometer);
152165
ClassDB::bind_method(D_METHOD("set_magnetometer", "value"), &Input::set_magnetometer);
153166
ClassDB::bind_method(D_METHOD("set_gyroscope", "value"), &Input::set_gyroscope);
154167
ClassDB::bind_method(D_METHOD("set_joy_light", "device", "color"), &Input::set_joy_light);
155168
ClassDB::bind_method(D_METHOD("has_joy_light", "device"), &Input::has_joy_light);
169+
ClassDB::bind_method(D_METHOD("set_joy_accelerometer_enabled", "device", "enable"), &Input::set_joy_accelerometer_enabled);
170+
ClassDB::bind_method(D_METHOD("set_joy_gyroscope_enabled", "device", "enable"), &Input::set_joy_gyroscope_enabled);
171+
ClassDB::bind_method(D_METHOD("has_joy_accelerometer", "device"), &Input::has_joy_accelerometer);
172+
ClassDB::bind_method(D_METHOD("has_joy_gyroscope", "device"), &Input::has_joy_gyroscope);
156173
ClassDB::bind_method(D_METHOD("get_last_mouse_velocity"), &Input::get_last_mouse_velocity);
157174
ClassDB::bind_method(D_METHOD("get_last_mouse_screen_velocity"), &Input::get_last_mouse_screen_velocity);
158175
ClassDB::bind_method(D_METHOD("get_mouse_button_mask"), &Input::get_mouse_button_mask);
@@ -684,6 +701,7 @@ void Input::joy_connection_changed(int p_idx, bool p_connected, const String &p_
684701
for (int i = 0; i < (int)JoyAxis::MAX; i++) {
685702
set_joy_axis(p_idx, (JoyAxis)i, 0.0f);
686703
}
704+
joy_motion.erase(p_idx);
687705
}
688706
joy_names[p_idx] = js;
689707

@@ -739,6 +757,199 @@ Vector3 Input::get_gyroscope() const {
739757
return gyroscope;
740758
}
741759

760+
bool Input::is_joy_accelerometer_enabled(int p_device) const {
761+
_THREAD_SAFE_METHOD_
762+
const MotionInfo *motion_info = joy_motion.getptr(p_device);
763+
return motion_info && motion_info->accelerometer_enabled;
764+
}
765+
766+
bool Input::is_joy_gyroscope_enabled(int p_device) const {
767+
_THREAD_SAFE_METHOD_
768+
const MotionInfo *motion_info = joy_motion.getptr(p_device);
769+
return motion_info && motion_info->gyroscope_enabled;
770+
}
771+
772+
Vector3 Input::get_joy_accelerometer(int p_device) const {
773+
_THREAD_SAFE_METHOD_
774+
const MotionInfo *motion_info = joy_motion.getptr(p_device);
775+
if (motion_info == nullptr) {
776+
return Vector3();
777+
}
778+
779+
if (!motion_info->calibration.calibrated) {
780+
return motion_info->accelerometer;
781+
}
782+
783+
// Calibrated accelerometer doesn't include gravity.
784+
Vector3 value = (motion_info->accelerometer - motion_info->gravity) - motion_info->calibration.accelerometer_offset;
785+
if (Math::abs(value.x) < motion_info->calibration.accelerometer_deadzone) {
786+
value.x = 0;
787+
}
788+
if (Math::abs(value.y) < motion_info->calibration.accelerometer_deadzone) {
789+
value.y = 0;
790+
}
791+
if (Math::abs(value.z) < motion_info->calibration.accelerometer_deadzone) {
792+
value.z = 0;
793+
}
794+
return value;
795+
}
796+
797+
Vector3 Input::get_joy_gravity(int p_device) const {
798+
_THREAD_SAFE_METHOD_
799+
if (!joy_motion.has(p_device)) {
800+
return Vector3();
801+
}
802+
// Gravity does not need calibration.
803+
return joy_motion[p_device].gravity;
804+
}
805+
806+
Vector3 Input::get_joy_gyroscope(int p_device) const {
807+
_THREAD_SAFE_METHOD_
808+
const MotionInfo *motion_info = joy_motion.getptr(p_device);
809+
if (motion_info == nullptr) {
810+
return Vector3();
811+
}
812+
813+
if (!motion_info->calibration.calibrated) {
814+
return motion_info->gyroscope;
815+
}
816+
817+
Vector3 value = motion_info->gyroscope - motion_info->calibration.gyroscope_offset;
818+
if (Math::abs(value.x) < motion_info->calibration.gyroscope_deadzone) {
819+
value.x = 0;
820+
}
821+
if (Math::abs(value.y) < motion_info->calibration.gyroscope_deadzone) {
822+
value.y = 0;
823+
}
824+
if (Math::abs(value.z) < motion_info->calibration.gyroscope_deadzone) {
825+
value.z = 0;
826+
}
827+
return value;
828+
}
829+
830+
float Input::get_joy_sensor_rate(int p_device) const {
831+
_THREAD_SAFE_METHOD_
832+
const MotionInfo *motion_info = joy_motion.getptr(p_device);
833+
if (motion_info == nullptr) {
834+
return 0.0f;
835+
}
836+
return motion_info->sensor_data_rate;
837+
}
838+
839+
void Input::start_joy_motion_calibration(int p_device) {
840+
_THREAD_SAFE_METHOD_
841+
if (!joy_motion.has(p_device)) {
842+
return;
843+
}
844+
MotionCalibrationInfo &calibration = joy_motion[p_device].calibration;
845+
846+
ERR_FAIL_COND_MSG(calibration.in_progress, "Calibration already in progress.");
847+
848+
clear_joy_motion_calibration(p_device);
849+
calibration.in_progress = true;
850+
}
851+
852+
#define JOY_CALIBRATE_SENSOR(m_sensor) \
853+
vector_sum = Vector3(); \
854+
for (Vector3 step : calibration.m_sensor##_steps) { \
855+
vector_sum += step; \
856+
} \
857+
vector_sum /= calibration.m_sensor##_steps.size(); \
858+
\
859+
deadzone = 0.0f; \
860+
for (Vector3 step : calibration.m_sensor##_steps) { \
861+
deadzone = MAX(deadzone, (step - vector_sum).length()); \
862+
} \
863+
\
864+
calibration.m_sensor##_offset = vector_sum; \
865+
calibration.m_sensor##_deadzone = deadzone; \
866+
calibration.m_sensor##_steps.clear();
867+
868+
void Input::stop_joy_motion_calibration(int p_device) {
869+
_THREAD_SAFE_METHOD_
870+
if (!joy_motion.has(p_device)) {
871+
return;
872+
}
873+
MotionCalibrationInfo &calibration = joy_motion[p_device].calibration;
874+
875+
ERR_FAIL_COND_MSG(!calibration.in_progress, "Calibration hasn't been started.");
876+
877+
Vector3 vector_sum;
878+
float deadzone = 0.0f;
879+
880+
JOY_CALIBRATE_SENSOR(accelerometer);
881+
JOY_CALIBRATE_SENSOR(gyroscope);
882+
883+
calibration.in_progress = false;
884+
calibration.calibrated = true;
885+
}
886+
887+
void Input::clear_joy_motion_calibration(int p_device) {
888+
_THREAD_SAFE_METHOD_
889+
if (!joy_motion.has(p_device)) {
890+
return;
891+
}
892+
MotionCalibrationInfo &calibration = joy_motion[p_device].calibration;
893+
894+
// Calibration might be in progress and the developer or the user might want to reset it,
895+
// so no need to stop the calibration.
896+
bool in_progress = calibration.in_progress;
897+
calibration = MotionCalibrationInfo();
898+
calibration.in_progress = in_progress;
899+
}
900+
901+
Dictionary Input::get_joy_motion_calibration(int p_device) const {
902+
_THREAD_SAFE_METHOD_
903+
if (!joy_motion.has(p_device)) {
904+
return Dictionary();
905+
}
906+
const MotionCalibrationInfo &calibration = joy_motion[p_device].calibration;
907+
908+
if (!calibration.calibrated) {
909+
return Dictionary();
910+
}
911+
912+
Dictionary result;
913+
result["accelerometer_offset"] = calibration.accelerometer_offset;
914+
result["accelerometer_deadzone"] = calibration.accelerometer_deadzone;
915+
result["gyroscope_offset"] = calibration.gyroscope_offset;
916+
result["gyroscope_deadzone"] = calibration.gyroscope_deadzone;
917+
return result;
918+
}
919+
920+
void Input::set_joy_motion_calibration(int p_device, const Dictionary &p_calibration_info) {
921+
_THREAD_SAFE_METHOD_
922+
if (!joy_motion.has(p_device)) {
923+
return;
924+
}
925+
MotionCalibrationInfo &calibration = joy_motion[p_device].calibration;
926+
927+
ERR_FAIL_COND_MSG(calibration.in_progress, "Calibration is currently in progress.");
928+
929+
calibration.accelerometer_offset = p_calibration_info["accelerometer_offset"];
930+
calibration.accelerometer_deadzone = p_calibration_info["accelerometer_deadzone"];
931+
calibration.gyroscope_offset = p_calibration_info["gyroscope_offset"];
932+
calibration.gyroscope_deadzone = p_calibration_info["gyroscope_deadzone"];
933+
calibration.in_progress = false;
934+
calibration.calibrated = true;
935+
}
936+
937+
bool Input::is_joy_motion_calibrated(int p_device) const {
938+
_THREAD_SAFE_METHOD_
939+
if (!joy_motion.has(p_device)) {
940+
return false;
941+
}
942+
return joy_motion[p_device].calibration.calibrated;
943+
}
944+
945+
bool Input::is_joy_motion_calibrating(int p_device) const {
946+
_THREAD_SAFE_METHOD_
947+
if (!joy_motion.has(p_device)) {
948+
return false;
949+
}
950+
return joy_motion[p_device].calibration.in_progress;
951+
}
952+
742953
void Input::_parse_input_event_impl(const Ref<InputEvent> &p_event, bool p_is_emulated) {
743954
// This function does the final delivery of the input event to user land.
744955
// Regardless where the event came from originally, this has to happen on the main thread.
@@ -1016,6 +1227,100 @@ bool Input::has_joy_light(int p_device) const {
10161227
return joypad && joypad->has_light;
10171228
}
10181229

1230+
bool Input::set_joy_accelerometer_enabled(int p_device, bool p_enable) {
1231+
_THREAD_SAFE_METHOD_
1232+
Joypad *joypad = joy_names.getptr(p_device);
1233+
if (joypad == nullptr || joypad->features == nullptr) {
1234+
return false;
1235+
}
1236+
1237+
MotionInfo *motion = joy_motion.getptr(p_device);
1238+
if (motion == nullptr) {
1239+
return false;
1240+
}
1241+
1242+
bool enabled = joypad->features->set_joy_accelerometer_enabled(p_enable);
1243+
motion->accelerometer = Vector3();
1244+
motion->accelerometer_enabled = enabled;
1245+
return enabled;
1246+
}
1247+
1248+
bool Input::set_joy_gyroscope_enabled(int p_device, bool p_enable) {
1249+
_THREAD_SAFE_METHOD_
1250+
Joypad *joypad = joy_names.getptr(p_device);
1251+
if (joypad == nullptr || joypad->features == nullptr) {
1252+
return false;
1253+
}
1254+
1255+
MotionInfo *motion = joy_motion.getptr(p_device);
1256+
if (motion == nullptr) {
1257+
return false;
1258+
}
1259+
1260+
bool enabled = joypad->features->set_joy_gyroscope_enabled(p_enable);
1261+
motion->gyroscope = Vector3();
1262+
motion->gyroscope_enabled = enabled;
1263+
return enabled;
1264+
}
1265+
1266+
void Input::set_joy_accelerometer(int p_device, const Vector3 &p_value) {
1267+
_THREAD_SAFE_METHOD_
1268+
MotionInfo *motion = joy_motion.getptr(p_device);
1269+
if (motion == nullptr) {
1270+
return;
1271+
}
1272+
1273+
motion->accelerometer = p_value - motion->gravity;
1274+
1275+
if (motion->calibration.in_progress) {
1276+
motion->calibration.accelerometer_steps.push_back(motion->accelerometer);
1277+
}
1278+
}
1279+
1280+
void Input::set_joy_gravity(int p_device, const Vector3 &p_value) {
1281+
_THREAD_SAFE_METHOD_
1282+
MotionInfo *motion = joy_motion.getptr(p_device);
1283+
if (motion == nullptr) {
1284+
return;
1285+
}
1286+
1287+
motion->gravity = p_value;
1288+
}
1289+
1290+
void Input::set_joy_gyroscope(int p_device, const Vector3 &p_value) {
1291+
_THREAD_SAFE_METHOD_
1292+
MotionInfo *motion = joy_motion.getptr(p_device);
1293+
if (motion == nullptr) {
1294+
return;
1295+
}
1296+
1297+
motion->accelerometer = p_value;
1298+
1299+
if (motion->calibration.in_progress) {
1300+
motion->calibration.gyroscope_steps.push_back(p_value);
1301+
}
1302+
}
1303+
1304+
void Input::set_joy_sensor_rate(int p_device, float p_rate) {
1305+
_THREAD_SAFE_METHOD_
1306+
MotionInfo *motion = joy_motion.getptr(p_device);
1307+
if (motion == nullptr) {
1308+
return;
1309+
}
1310+
1311+
motion->sensor_data_rate = p_rate;
1312+
}
1313+
1314+
bool Input::has_joy_accelerometer(int p_device) const {
1315+
_THREAD_SAFE_METHOD_
1316+
return joy_motion.has(p_device) && joy_motion[p_device].has_accelerometer;
1317+
}
1318+
1319+
bool Input::has_joy_gyroscope(int p_device) const {
1320+
_THREAD_SAFE_METHOD_
1321+
return joy_motion.has(p_device) && joy_motion[p_device].has_gyroscope;
1322+
}
1323+
10191324
void Input::start_joy_vibration(int p_device, float p_weak_magnitude, float p_strong_magnitude, float p_duration) {
10201325
_THREAD_SAFE_METHOD_
10211326
if (p_weak_magnitude < 0.f || p_weak_magnitude > 1.f || p_strong_magnitude < 0.f || p_strong_magnitude > 1.f) {
@@ -1516,6 +1821,12 @@ void Input::_update_joypad_features(int p_device) {
15161821
if (joypad->features->has_joy_light()) {
15171822
joypad->has_light = true;
15181823
}
1824+
if (joypad->features->has_joy_accelerometer()) {
1825+
joy_motion[p_device].has_accelerometer = true;
1826+
}
1827+
if (joypad->features->has_joy_gyroscope()) {
1828+
joy_motion[p_device].has_gyroscope = true;
1829+
}
15191830
}
15201831

15211832
Input::JoyEvent Input::_get_mapped_button_event(const JoyDeviceMapping &mapping, JoyButton p_button) {

0 commit comments

Comments
 (0)