diff --git a/src/common/quaternion.h b/src/common/quaternion.h index da44f35cdc..4d0871eb46 100644 --- a/src/common/quaternion.h +++ b/src/common/quaternion.h @@ -36,6 +36,36 @@ public: T length = std::sqrt(xyz.Length2() + w * w); return {xyz / length, w / length}; } + + [[nodiscard]] std::array ToMatrix() const { + const T x2 = xyz[0] * xyz[0]; + const T y2 = xyz[1] * xyz[1]; + const T z2 = xyz[2] * xyz[2]; + + const T xy = xyz[0] * xyz[1]; + const T wz = w * xyz[2]; + const T xz = xyz[0] * xyz[2]; + const T wy = w * xyz[1]; + const T yz = xyz[1] * xyz[2]; + const T wx = w * xyz[0]; + + return {1.0f - 2.0f * (y2 + z2), + 2.0f * (xy + wz), + 2.0f * (xz - wy), + 0.0f, + 2.0f * (xy - wz), + 1.0f - 2.0f * (x2 + z2), + 2.0f * (yz + wx), + 0.0f, + 2.0f * (xz + wy), + 2.0f * (yz - wx), + 1.0f - 2.0f * (x2 + y2), + 0.0f, + 0.0f, + 0.0f, + 0.0f, + 1.0f}; + } }; template diff --git a/src/input_common/CMakeLists.txt b/src/input_common/CMakeLists.txt index 56267c8a81..c673b83890 100644 --- a/src/input_common/CMakeLists.txt +++ b/src/input_common/CMakeLists.txt @@ -7,6 +7,8 @@ add_library(input_common STATIC main.h motion_emu.cpp motion_emu.h + motion_input.cpp + motion_input.h settings.cpp settings.h gcadapter/gc_adapter.cpp diff --git a/src/input_common/motion_input.cpp b/src/input_common/motion_input.cpp new file mode 100644 index 0000000000..e3aec04e19 --- /dev/null +++ b/src/input_common/motion_input.cpp @@ -0,0 +1,185 @@ +#include "input_common/motion_input.h" + +namespace InputCommon { + +MotionInput::MotionInput(f32 new_kp, f32 new_ki, f32 new_kd) : kp(new_kp), ki(new_ki), kd(new_kd) { + accel = {}; + gyro = {}; + gyro_drift = {}; + gyro_threshold = 0; + rotations = {}; + + quat.w = 0; + quat.xyz[0] = 0; + quat.xyz[1] = 0; + quat.xyz[2] = -1; + + real_error = {}; + integral_error = {}; + derivative_error = {}; + + reset_counter = 0; + reset_enabled = true; +} + +void MotionInput::SetAcceleration(Common::Vec3f acceleration) { + accel = acceleration; +} + +void MotionInput::SetGyroscope(Common::Vec3f gyroscope) { + gyro = gyroscope - gyro_drift; + if (gyro.Length2() < gyro_threshold) { + gyro = {}; + } +} + +void MotionInput::SetQuaternion(Common::Quaternion quaternion) { + quat = quaternion; +} + +void MotionInput::SetGyroDrift(Common::Vec3f drift) { + drift = gyro_drift; +} + +void MotionInput::SetGyroThreshold(f32 threshold) { + gyro_threshold = threshold; +} + +void MotionInput::EnableReset(bool reset) { + reset_enabled = reset; +} + +void MotionInput::ResetRotations() { + rotations = {}; +} + +bool MotionInput::IsMoving(f32 sensitivity) { + return gyro.Length2() >= sensitivity || accel.Length() <= 0.9f || accel.Length() >= 1.1f; +} + +bool MotionInput::IsCalibrated(f32 sensitivity) { + return real_error.Length() > sensitivity; +} + +void MotionInput::UpdateRotation(u64 elapsed_time) { + rotations += gyro * elapsed_time; +} + +void MotionInput::UpdateOrientation(u64 elapsed_time) { + // Short name local variable for readability + f32 q1 = quat.w, q2 = quat.xyz[0], q3 = quat.xyz[1], q4 = quat.xyz[2]; + f32 sample_period = elapsed_time / 1000000.0f; + + auto normal_accel = accel.Normalized(); + auto rad_gyro = gyro * 3.1415926535f; + rad_gyro.z = -rad_gyro.z; + + // Ignore drift correction if acceleration is not present + if (normal_accel.Length() == 1.0f) { + f32 ax = -normal_accel.x; + f32 ay = normal_accel.y; + f32 az = -normal_accel.z; + f32 vx, vy, vz; + Common::Vec3f new_real_error; + + // Estimated direction of gravity + vx = 2.0f * (q2 * q4 - q1 * q3); + vy = 2.0f * (q1 * q2 + q3 * q4); + vz = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4; + + // Error is cross product between estimated direction and measured direction of gravity + new_real_error.x = ay * vz - az * vy; + new_real_error.y = az * vx - ax * vz; + new_real_error.x = ax * vy - ay * vx; + + derivative_error = new_real_error - real_error; + real_error = new_real_error; + + // Prevent integral windup + if (ki != 0.0f) { + integral_error += real_error; + } else { + integral_error = {}; + } + + // Apply feedback terms + rad_gyro += kp * real_error; + rad_gyro += ki * integral_error; + rad_gyro += kd * derivative_error; + } + + f32 gx = rad_gyro.y; + f32 gy = rad_gyro.x; + f32 gz = rad_gyro.z; + + // Integrate rate of change of quaternion + f32 pa, pb, pc; + pa = q2; + pb = q3; + pc = q4; + q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * sample_period); + q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * sample_period); + q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * sample_period); + q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * sample_period); + + quat.w = q1; + quat.xyz[0] = q2; + quat.xyz[1] = q3; + quat.xyz[2] = q4; + quat = quat.Normalized(); +} + +std::array MotionInput::GetOrientation() { + std::array orientation = {}; + Common::Quaternion quad; + + quad.w = -quat.xyz[2]; + quad.xyz[0] = -quat.xyz[1]; + quad.xyz[1] = -quat.xyz[0]; + quad.xyz[2] = -quat.w; + + std::array matrix4x4 = quad.ToMatrix(); + + orientation[0] = Common::Vec3f(matrix4x4[0], matrix4x4[1], matrix4x4[2]); + orientation[1] = Common::Vec3f(matrix4x4[4], matrix4x4[5], matrix4x4[6]); + orientation[2] = Common::Vec3f(matrix4x4[8], matrix4x4[9], matrix4x4[10]); + + return orientation; +} + +Common::Vec3f MotionInput::GetAcceleration() { + return accel; +} + +Common::Vec3f MotionInput::GetGyroscope() { + return gyro; +} + +Common::Quaternion MotionInput::GetQuaternion() { + return quat; +} + +Common::Vec3f MotionInput::GetRotations() { + return rotations; +} + +void MotionInput::resetOrientation() { + if (!reset_enabled) { + return; + } + if (!IsMoving(0.5f) && accel.z <= -0.9f) { + ++reset_counter; + if (reset_counter > 900) { + // TODO: calculate quaternion from gravity vector + quat.w = 0; + quat.xyz[0] = 0; + quat.xyz[1] = 0; + quat.xyz[2] = -1; + integral_error = {}; + reset_counter = 0; + } + } else { + reset_counter = 0; + } +} +} // namespace InputCommon \ No newline at end of file diff --git a/src/input_common/motion_input.h b/src/input_common/motion_input.h new file mode 100644 index 0000000000..4b8093d8c5 --- /dev/null +++ b/src/input_common/motion_input.h @@ -0,0 +1,62 @@ +// Copyright 2014 Dolphin Emulator Project +// Licensed under GPLv2+ +// Refer to the license.txt file included. + +#pragma once + +#include "common/common_types.h" +#include "common/quaternion.h" +#include "common/vector_math.h" + +namespace InputCommon { + +class MotionInput { +public: + MotionInput(f32 new_kp, f32 new_ki, f32 new_kd); + + void SetAcceleration(Common::Vec3f acceleration); + void SetGyroscope(Common::Vec3f acceleration); + void SetQuaternion(Common::Quaternion quaternion); + void SetGyroDrift(Common::Vec3f drift); + void SetGyroThreshold(f32 threshold); + + void EnableReset(bool reset); + void ResetRotations(); + + void UpdateRotation(u64 elapsed_time); + void UpdateOrientation(u64 elapsed_time); + + std::array GetOrientation(); + Common::Vec3f GetAcceleration(); + Common::Vec3f GetGyroscope(); + Common::Vec3f GetRotations(); + Common::Quaternion GetQuaternion(); + + bool IsMoving(f32 sensitivity); + bool IsCalibrated(f32 sensitivity); + + // PID constants + const f32 kp; + const f32 ki; + const f32 kd; + +private: + void resetOrientation(); + + // PID errors + Common::Vec3f real_error; + Common::Vec3f integral_error; + Common::Vec3f derivative_error; + + Common::Quaternion quat; + Common::Vec3f rotations; + Common::Vec3f accel; + Common::Vec3f gyro; + Common::Vec3f gyro_drift; + + f32 gyro_threshold; + f32 reset_counter; + bool reset_enabled; +}; + +} // namespace InputCommon