Skip to content

Commit bcac074

Browse files
NintorchNintorch
committed
Add motion sensors feature to joypads
1 parent 36b9212 commit bcac074

File tree

10 files changed

+1222
-3
lines changed

10 files changed

+1222
-3
lines changed

‎core/input/input.cpp‎

Lines changed: 289 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -147,10 +147,27 @@ 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("start_joy_motion_calibration", "device"), &Input::start_joy_motion_calibration);
156+
ClassDB::bind_method(D_METHOD("step_joy_motion_calibration", "device"), &Input::step_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);
167+
ClassDB::bind_method(D_METHOD("set_joy_accelerometer_enabled", "device", "enable"), &Input::set_joy_accelerometer_enabled);
168+
ClassDB::bind_method(D_METHOD("set_joy_gyroscope_enabled", "device", "enable"), &Input::set_joy_gyroscope_enabled);
169+
ClassDB::bind_method(D_METHOD("has_joy_accelerometer", "device"), &Input::has_joy_accelerometer);
170+
ClassDB::bind_method(D_METHOD("has_joy_gyroscope", "device"), &Input::has_joy_gyroscope);
154171
ClassDB::bind_method(D_METHOD("get_last_mouse_velocity"), &Input::get_last_mouse_velocity);
155172
ClassDB::bind_method(D_METHOD("get_last_mouse_screen_velocity"), &Input::get_last_mouse_screen_velocity);
156173
ClassDB::bind_method(D_METHOD("get_mouse_button_mask"), &Input::get_mouse_button_mask);
@@ -731,6 +748,202 @@ Vector3 Input::get_gyroscope() const {
731748
return gyroscope;
732749
}
733750

751+
bool Input::is_joy_accelerometer_enabled(int p_device) const {
752+
_THREAD_SAFE_METHOD_
753+
return joy_motion.has(p_device) && joy_motion[p_device].accelerometer_enabled;
754+
}
755+
756+
bool Input::is_joy_gyroscope_enabled(int p_device) const {
757+
_THREAD_SAFE_METHOD_
758+
return joy_motion.has(p_device) && joy_motion[p_device].gyroscope_enabled;
759+
}
760+
761+
Vector3 Input::get_joy_accelerometer(int p_device) const {
762+
_THREAD_SAFE_METHOD_
763+
if (!joy_motion.has(p_device)) {
764+
return Vector3();
765+
}
766+
767+
if (!joy_motion[p_device].calibration.calibrated) {
768+
return joy_motion[p_device].accelerometer;
769+
}
770+
771+
const MotionInfo &motion = joy_motion[p_device];
772+
// Calibrated accelerometer doesn't include gravity
773+
Vector3 value = (motion.accelerometer - motion.gravity) - motion.calibration.accelerometer_offset;
774+
if (std::abs(value.x) < motion.calibration.accelerometer_deadzone) {
775+
value.x = 0;
776+
}
777+
if (std::abs(value.y) < motion.calibration.accelerometer_deadzone) {
778+
value.y = 0;
779+
}
780+
if (std::abs(value.z) < motion.calibration.accelerometer_deadzone) {
781+
value.z = 0;
782+
}
783+
return value;
784+
}
785+
786+
Vector3 Input::get_joy_gravity(int p_device) const {
787+
_THREAD_SAFE_METHOD_
788+
if (!joy_motion.has(p_device)) {
789+
return Vector3();
790+
}
791+
// Does gravity need calibration?
792+
return joy_motion[p_device].gravity;
793+
}
794+
795+
Vector3 Input::get_joy_gyroscope(int p_device) const {
796+
_THREAD_SAFE_METHOD_
797+
if (!joy_motion.has(p_device)) {
798+
return Vector3();
799+
}
800+
801+
if (!joy_motion[p_device].calibration.calibrated) {
802+
return joy_motion[p_device].gyroscope;
803+
}
804+
805+
const MotionInfo &motion = joy_motion[p_device];
806+
Vector3 value = motion.gyroscope - motion.calibration.accelerometer_offset;
807+
if (std::abs(value.x) < motion.calibration.gyroscope_deadzone) {
808+
value.x = 0;
809+
}
810+
if (std::abs(value.y) < motion.calibration.gyroscope_deadzone) {
811+
value.y = 0;
812+
}
813+
if (std::abs(value.z) < motion.calibration.gyroscope_deadzone) {
814+
value.z = 0;
815+
}
816+
return value;
817+
}
818+
819+
#define CALIBRATION_SETUP \
820+
if (!joy_motion.has(p_device)) { \
821+
return; \
822+
} \
823+
auto &calibration = joy_motion[p_device].calibration;
824+
825+
#define CALIBRATION_SETUP_RETURN(m_return) \
826+
if (!joy_motion.has(p_device)) { \
827+
return m_return; \
828+
} \
829+
auto &calibration = joy_motion[p_device].calibration;
830+
831+
#define CALIBRATE_SENSOR(m_sensor) \
832+
vector_sum = Vector3(); \
833+
for (Vector3 step : calibration.m_sensor##_steps) { \
834+
vector_sum += step; \
835+
} \
836+
vector_sum /= calibration.m_sensor##_steps.size(); \
837+
\
838+
deadzone = 0.0f; \
839+
for (Vector3 step : calibration.m_sensor##_steps) { \
840+
deadzone = MAX(deadzone, (step - vector_sum).length()); \
841+
} \
842+
\
843+
calibration.m_sensor##_offset = vector_sum; \
844+
calibration.m_sensor##_deadzone = deadzone; \
845+
calibration.m_sensor##_steps.clear();
846+
847+
void Input::start_joy_motion_calibration(int p_device) {
848+
_THREAD_SAFE_METHOD_
849+
CALIBRATION_SETUP;
850+
851+
ERR_FAIL_COND_MSG(calibration.in_progress, "Calibration already in progress.");
852+
853+
clear_joy_motion_calibration(p_device);
854+
calibration.in_progress = true;
855+
}
856+
857+
void Input::step_joy_motion_calibration(int p_device) {
858+
_THREAD_SAFE_METHOD_
859+
CALIBRATION_SETUP;
860+
861+
ERR_FAIL_COND_MSG(!calibration.in_progress, "Calibration hasn't been started.");
862+
863+
const MotionInfo &motion = joy_motion[p_device];
864+
calibration.accelerometer_steps.push_back(motion.accelerometer - motion.gravity);
865+
calibration.gyroscope_steps.push_back(motion.gyroscope);
866+
}
867+
868+
void Input::stop_joy_motion_calibration(int p_device) {
869+
_THREAD_SAFE_METHOD_
870+
CALIBRATION_SETUP;
871+
872+
ERR_FAIL_COND_MSG(!calibration.in_progress, "Calibration hasn't been started.");
873+
874+
if (calibration.accelerometer_steps.size() < 10) {
875+
WARN_PRINT_ED("Not enough joypad motion sensors calibration steps, "
876+
"you should call Input.step_joy_motion_calibration() at least 10 times.");
877+
}
878+
879+
Vector3 vector_sum;
880+
float deadzone = 0.0f;
881+
882+
CALIBRATE_SENSOR(accelerometer);
883+
CALIBRATE_SENSOR(gyroscope);
884+
885+
calibration.in_progress = false;
886+
calibration.calibrated = true;
887+
}
888+
889+
void Input::clear_joy_motion_calibration(int p_device) {
890+
_THREAD_SAFE_METHOD_
891+
CALIBRATION_SETUP;
892+
893+
// Calibration might be in progress and the user might want to reset the progress,
894+
// so no need to stop the calibration.
895+
//calibration.in_progress = false;
896+
calibration.accelerometer_steps.clear();
897+
calibration.gyroscope_steps.clear();
898+
calibration.accelerometer_offset = Vector3();
899+
calibration.gyroscope_offset = Vector3();
900+
calibration.accelerometer_deadzone = 0.0f;
901+
calibration.gyroscope_deadzone = 0.0f;
902+
calibration.calibrated = false;
903+
}
904+
905+
Dictionary Input::get_joy_motion_calibration(int p_device) const {
906+
_THREAD_SAFE_METHOD_
907+
CALIBRATION_SETUP_RETURN({});
908+
909+
if (!calibration.calibrated) {
910+
return {};
911+
}
912+
913+
Dictionary result;
914+
result["accelerometer_offset"] = calibration.accelerometer_offset;
915+
result["accelerometer_deadzone"] = calibration.accelerometer_deadzone;
916+
result["gyroscope_offset"] = calibration.gyroscope_offset;
917+
result["gyroscope_deadzone"] = calibration.gyroscope_deadzone;
918+
return result;
919+
}
920+
921+
void Input::set_joy_motion_calibration(int p_device, Dictionary p_calibration_info) {
922+
_THREAD_SAFE_METHOD_
923+
CALIBRATION_SETUP;
924+
925+
ERR_FAIL_COND_MSG(calibration.in_progress, "Calibration already in progress.");
926+
927+
calibration.accelerometer_offset = p_calibration_info["accelerometer_offset"];
928+
calibration.accelerometer_deadzone = p_calibration_info["accelerometer_deadzone"];
929+
calibration.gyroscope_offset = p_calibration_info["gyroscope_offset"];
930+
calibration.gyroscope_deadzone = p_calibration_info["gyroscope_deadzone"];
931+
calibration.in_progress = false;
932+
calibration.calibrated = true;
933+
}
934+
935+
bool Input::is_joy_motion_calibrated(int p_device) const {
936+
_THREAD_SAFE_METHOD_
937+
CALIBRATION_SETUP_RETURN(false);
938+
return calibration.calibrated;
939+
}
940+
941+
bool Input::is_joy_motion_calibrating(int p_device) const {
942+
_THREAD_SAFE_METHOD_
943+
CALIBRATION_SETUP_RETURN(false);
944+
return calibration.in_progress;
945+
}
946+
734947
void Input::_parse_input_event_impl(const Ref<InputEvent> &p_event, bool p_is_emulated) {
735948
// This function does the final delivery of the input event to user land.
736949
// Regardless where the event came from originally, this has to happen on the main thread.
@@ -986,6 +1199,70 @@ void Input::set_joy_axis(int p_device, JoyAxis p_axis, float p_value) {
9861199
_joy_axis[c] = p_value;
9871200
}
9881201

1202+
void Input::set_joy_features(int p_device, JoypadFeatures *p_features) {
1203+
if (!joy_names.has(p_device)) {
1204+
return;
1205+
}
1206+
joy_names[p_device].features = p_features;
1207+
_update_joypad_features(p_device);
1208+
}
1209+
1210+
bool Input::set_joy_accelerometer_enabled(int p_device, bool p_enable) {
1211+
_THREAD_SAFE_METHOD_
1212+
if (!joy_names.has(p_device) || joy_names[p_device].features == nullptr) {
1213+
return false;
1214+
}
1215+
bool enabled = joy_names[p_device].features->set_joy_accelerometer_enabled(p_enable);
1216+
joy_motion[p_device].accelerometer = Vector3();
1217+
joy_motion[p_device].accelerometer_enabled = enabled;
1218+
return enabled;
1219+
}
1220+
1221+
bool Input::set_joy_gyroscope_enabled(int p_device, bool p_enable) {
1222+
_THREAD_SAFE_METHOD_
1223+
if (!joy_names.has(p_device) || joy_names[p_device].features == nullptr) {
1224+
return false;
1225+
}
1226+
bool enabled = joy_names[p_device].features->set_joy_gyroscope_enabled(p_enable);
1227+
joy_motion[p_device].gyroscope = Vector3();
1228+
joy_motion[p_device].gyroscope_enabled = enabled;
1229+
return enabled;
1230+
}
1231+
1232+
void Input::set_joy_accelerometer(int p_device, const Vector3 &p_value) {
1233+
_THREAD_SAFE_METHOD_
1234+
if (!joy_motion.has(p_device)) {
1235+
return;
1236+
}
1237+
joy_motion[p_device].accelerometer = p_value;
1238+
}
1239+
1240+
void Input::set_joy_gravity(int p_device, const Vector3 &p_value) {
1241+
_THREAD_SAFE_METHOD_
1242+
if (!joy_motion.has(p_device)) {
1243+
return;
1244+
}
1245+
joy_motion[p_device].gravity = p_value;
1246+
}
1247+
1248+
void Input::set_joy_gyroscope(int p_device, const Vector3 &p_value) {
1249+
_THREAD_SAFE_METHOD_
1250+
if (!joy_motion.has(p_device)) {
1251+
return;
1252+
}
1253+
joy_motion[p_device].gyroscope = p_value;
1254+
}
1255+
1256+
bool Input::has_joy_accelerometer(int p_device) const {
1257+
_THREAD_SAFE_METHOD_
1258+
return joy_motion.has(p_device) && joy_motion[p_device].has_accelerometer;
1259+
}
1260+
1261+
bool Input::has_joy_gyroscope(int p_device) const {
1262+
_THREAD_SAFE_METHOD_
1263+
return joy_motion.has(p_device) && joy_motion[p_device].has_gyroscope;
1264+
}
1265+
9891266
void Input::start_joy_vibration(int p_device, float p_weak_magnitude, float p_strong_magnitude, float p_duration) {
9901267
_THREAD_SAFE_METHOD_
9911268
if (p_weak_magnitude < 0.f || p_weak_magnitude > 1.f || p_strong_magnitude < 0.f || p_strong_magnitude > 1.f) {
@@ -1478,6 +1755,18 @@ void Input::_update_action_cache(const StringName &p_action_name, ActionState &r
14781755
}
14791756
}
14801757

1758+
void Input::_update_joypad_features(int p_device) {
1759+
if (!joy_names.has(p_device) || joy_names[p_device].features == nullptr) {
1760+
return;
1761+
}
1762+
if (joy_names[p_device].features->has_joy_accelerometer()) {
1763+
joy_motion[p_device].has_accelerometer = true;
1764+
}
1765+
if (joy_names[p_device].features->has_joy_gyroscope()) {
1766+
joy_motion[p_device].has_gyroscope = true;
1767+
}
1768+
}
1769+
14811770
Input::JoyEvent Input::_get_mapped_button_event(const JoyDeviceMapping &mapping, JoyButton p_button) {
14821771
JoyEvent event;
14831772

0 commit comments

Comments
 (0)