diff --git a/src/input_common/motion_input.cpp b/src/input_common/motion_input.cpp index e3aec04e19..c86a53106f 100644 --- a/src/input_common/motion_input.cpp +++ b/src/input_common/motion_input.cpp @@ -2,43 +2,26 @@ 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 = {}; +MotionInput::MotionInput(f32 new_kp, f32 new_ki, f32 new_kd) + : kp(new_kp), ki(new_ki), kd(new_kd), quat{{0, 0, -1}, 0} {} - 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) { +void MotionInput::SetAcceleration(const Common::Vec3f& acceleration) { accel = acceleration; } -void MotionInput::SetGyroscope(Common::Vec3f gyroscope) { +void MotionInput::SetGyroscope(const Common::Vec3f& gyroscope) { gyro = gyroscope - gyro_drift; if (gyro.Length2() < gyro_threshold) { gyro = {}; } } -void MotionInput::SetQuaternion(Common::Quaternion quaternion) { +void MotionInput::SetQuaternion(const Common::Quaternion& quaternion) { quat = quaternion; } -void MotionInput::SetGyroDrift(Common::Vec3f drift) { - drift = gyro_drift; +void MotionInput::SetGyroDrift(const Common::Vec3f& drift) { + gyro_drift = drift; } void MotionInput::SetGyroThreshold(f32 threshold) { @@ -53,11 +36,11 @@ void MotionInput::ResetRotations() { rotations = {}; } -bool MotionInput::IsMoving(f32 sensitivity) { +bool MotionInput::IsMoving(f32 sensitivity) const { return gyro.Length2() >= sensitivity || accel.Length() <= 0.9f || accel.Length() >= 1.1f; } -bool MotionInput::IsCalibrated(f32 sensitivity) { +bool MotionInput::IsCalibrated(f32 sensitivity) const { return real_error.Length() > sensitivity; } @@ -67,30 +50,30 @@ void MotionInput::UpdateRotation(u64 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; + f32 q1 = quat.w; + f32 q2 = quat.xyz[0]; + f32 q3 = quat.xyz[1]; + f32 q4 = quat.xyz[2]; + const f32 sample_period = elapsed_time / 1000000.0f; - auto normal_accel = accel.Normalized(); + const 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; + const f32 ax = -normal_accel.x; + const f32 ay = normal_accel.y; + const f32 az = -normal_accel.z; // 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; + const f32 vx = 2.0f * (q2 * q4 - q1 * q3); + const f32 vy = 2.0f * (q1 * q2 + q3 * q4); + const f32 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; + const Common::Vec3f new_real_error = {ay * vz - az * vy, az * vx - ax * vz, + ax * vy - ay * vx}; derivative_error = new_real_error - real_error; real_error = new_real_error; @@ -108,15 +91,14 @@ void MotionInput::UpdateOrientation(u64 elapsed_time) { rad_gyro += kd * derivative_error; } - f32 gx = rad_gyro.y; - f32 gy = rad_gyro.x; - f32 gz = rad_gyro.z; + const f32 gx = rad_gyro.y; + const f32 gy = rad_gyro.x; + const f32 gz = rad_gyro.z; // Integrate rate of change of quaternion - f32 pa, pb, pc; - pa = q2; - pb = q3; - pc = q4; + const f32 pa = q2; + const f32 pb = q3; + const f32 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); @@ -129,41 +111,33 @@ void MotionInput::UpdateOrientation(u64 elapsed_time) { quat = quat.Normalized(); } -std::array MotionInput::GetOrientation() { - std::array orientation = {}; - Common::Quaternion quad; +std::array MotionInput::GetOrientation() const { + const Common::Quaternion quad{.xyz = {-quat.xyz[1], -quat.xyz[0], -quat.w}, + .w = -quat.xyz[2]}; + const std::array matrix4x4 = quad.ToMatrix(); - 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; + return {Common::Vec3f(matrix4x4[0], matrix4x4[1], matrix4x4[2]), + Common::Vec3f(matrix4x4[4], matrix4x4[5], matrix4x4[6]), + Common::Vec3f(matrix4x4[8], matrix4x4[9], matrix4x4[10])}; } -Common::Vec3f MotionInput::GetAcceleration() { +Common::Vec3f MotionInput::GetAcceleration() const { return accel; } -Common::Vec3f MotionInput::GetGyroscope() { +Common::Vec3f MotionInput::GetGyroscope() const { return gyro; } -Common::Quaternion MotionInput::GetQuaternion() { +Common::Quaternion MotionInput::GetQuaternion() const { return quat; } -Common::Vec3f MotionInput::GetRotations() { +Common::Vec3f MotionInput::GetRotations() const { return rotations; } -void MotionInput::resetOrientation() { +void MotionInput::ResetOrientation() { if (!reset_enabled) { return; } @@ -182,4 +156,4 @@ void MotionInput::resetOrientation() { reset_counter = 0; } } -} // namespace InputCommon \ No newline at end of file +} // namespace InputCommon diff --git a/src/input_common/motion_input.h b/src/input_common/motion_input.h index 4b8093d8c5..d1a7a9e139 100644 --- a/src/input_common/motion_input.h +++ b/src/input_common/motion_input.h @@ -14,10 +14,16 @@ 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); + MotionInput(const MotionInput&) = default; + MotionInput& operator=(const MotionInput&) = default; + + MotionInput(MotionInput&&) = default; + MotionInput& operator=(MotionInput&&) = default; + + void SetAcceleration(const Common::Vec3f& acceleration); + void SetGyroscope(const Common::Vec3f& acceleration); + void SetQuaternion(const Common::Quaternion& quaternion); + void SetGyroDrift(const Common::Vec3f& drift); void SetGyroThreshold(f32 threshold); void EnableReset(bool reset); @@ -26,23 +32,23 @@ public: 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(); + std::array GetOrientation() const; + Common::Vec3f GetAcceleration() const; + Common::Vec3f GetGyroscope() const; + Common::Vec3f GetRotations() const; + Common::Quaternion GetQuaternion() const; - bool IsMoving(f32 sensitivity); - bool IsCalibrated(f32 sensitivity); + bool IsMoving(f32 sensitivity) const; + bool IsCalibrated(f32 sensitivity) const; + +private: + void ResetOrientation(); // PID constants const f32 kp; const f32 ki; const f32 kd; -private: - void resetOrientation(); - // PID errors Common::Vec3f real_error; Common::Vec3f integral_error; @@ -54,9 +60,9 @@ private: Common::Vec3f gyro; Common::Vec3f gyro_drift; - f32 gyro_threshold; - f32 reset_counter; - bool reset_enabled; + f32 gyro_threshold = 0.0f; + f32 reset_counter = 0.0f; + bool reset_enabled = true; }; } // namespace InputCommon