Skip to content

Commit 9b12057

Browse files
NintorchNintorch
committed
Add support for joypad motion sensors
1 parent ef34c3d commit 9b12057

File tree

5 files changed

+727
-0
lines changed

5 files changed

+727
-0
lines changed

‎core/input/input.cpp‎

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

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

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

0 commit comments

Comments
 (0)