summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/common/quaternion.h30
-rw-r--r--src/input_common/CMakeLists.txt2
-rw-r--r--src/input_common/motion_input.cpp185
-rw-r--r--src/input_common/motion_input.h62
4 files changed, 279 insertions, 0 deletions
diff --git a/src/common/quaternion.h b/src/common/quaternion.h
index da44f35cd..4d0871eb4 100644
--- a/src/common/quaternion.h
+++ b/src/common/quaternion.h
@@ -36,6 +36,36 @@ public:
36 T length = std::sqrt(xyz.Length2() + w * w); 36 T length = std::sqrt(xyz.Length2() + w * w);
37 return {xyz / length, w / length}; 37 return {xyz / length, w / length};
38 } 38 }
39
40 [[nodiscard]] std::array<decltype(-T{}), 16> ToMatrix() const {
41 const T x2 = xyz[0] * xyz[0];
42 const T y2 = xyz[1] * xyz[1];
43 const T z2 = xyz[2] * xyz[2];
44
45 const T xy = xyz[0] * xyz[1];
46 const T wz = w * xyz[2];
47 const T xz = xyz[0] * xyz[2];
48 const T wy = w * xyz[1];
49 const T yz = xyz[1] * xyz[2];
50 const T wx = w * xyz[0];
51
52 return {1.0f - 2.0f * (y2 + z2),
53 2.0f * (xy + wz),
54 2.0f * (xz - wy),
55 0.0f,
56 2.0f * (xy - wz),
57 1.0f - 2.0f * (x2 + z2),
58 2.0f * (yz + wx),
59 0.0f,
60 2.0f * (xz + wy),
61 2.0f * (yz - wx),
62 1.0f - 2.0f * (x2 + y2),
63 0.0f,
64 0.0f,
65 0.0f,
66 0.0f,
67 1.0f};
68 }
39}; 69};
40 70
41template <typename T> 71template <typename T>
diff --git a/src/input_common/CMakeLists.txt b/src/input_common/CMakeLists.txt
index 56267c8a8..c673b8389 100644
--- a/src/input_common/CMakeLists.txt
+++ b/src/input_common/CMakeLists.txt
@@ -7,6 +7,8 @@ add_library(input_common STATIC
7 main.h 7 main.h
8 motion_emu.cpp 8 motion_emu.cpp
9 motion_emu.h 9 motion_emu.h
10 motion_input.cpp
11 motion_input.h
10 settings.cpp 12 settings.cpp
11 settings.h 13 settings.h
12 gcadapter/gc_adapter.cpp 14 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 000000000..e3aec04e1
--- /dev/null
+++ b/src/input_common/motion_input.cpp
@@ -0,0 +1,185 @@
1#include "input_common/motion_input.h"
2
3namespace InputCommon {
4
5MotionInput::MotionInput(f32 new_kp, f32 new_ki, f32 new_kd) : kp(new_kp), ki(new_ki), kd(new_kd) {
6 accel = {};
7 gyro = {};
8 gyro_drift = {};
9 gyro_threshold = 0;
10 rotations = {};
11
12 quat.w = 0;
13 quat.xyz[0] = 0;
14 quat.xyz[1] = 0;
15 quat.xyz[2] = -1;
16
17 real_error = {};
18 integral_error = {};
19 derivative_error = {};
20
21 reset_counter = 0;
22 reset_enabled = true;
23}
24
25void MotionInput::SetAcceleration(Common::Vec3f acceleration) {
26 accel = acceleration;
27}
28
29void MotionInput::SetGyroscope(Common::Vec3f gyroscope) {
30 gyro = gyroscope - gyro_drift;
31 if (gyro.Length2() < gyro_threshold) {
32 gyro = {};
33 }
34}
35
36void MotionInput::SetQuaternion(Common::Quaternion<f32> quaternion) {
37 quat = quaternion;
38}
39
40void MotionInput::SetGyroDrift(Common::Vec3f drift) {
41 drift = gyro_drift;
42}
43
44void MotionInput::SetGyroThreshold(f32 threshold) {
45 gyro_threshold = threshold;
46}
47
48void MotionInput::EnableReset(bool reset) {
49 reset_enabled = reset;
50}
51
52void MotionInput::ResetRotations() {
53 rotations = {};
54}
55
56bool MotionInput::IsMoving(f32 sensitivity) {
57 return gyro.Length2() >= sensitivity || accel.Length() <= 0.9f || accel.Length() >= 1.1f;
58}
59
60bool MotionInput::IsCalibrated(f32 sensitivity) {
61 return real_error.Length() > sensitivity;
62}
63
64void MotionInput::UpdateRotation(u64 elapsed_time) {
65 rotations += gyro * elapsed_time;
66}
67
68void MotionInput::UpdateOrientation(u64 elapsed_time) {
69 // Short name local variable for readability
70 f32 q1 = quat.w, q2 = quat.xyz[0], q3 = quat.xyz[1], q4 = quat.xyz[2];
71 f32 sample_period = elapsed_time / 1000000.0f;
72
73 auto normal_accel = accel.Normalized();
74 auto rad_gyro = gyro * 3.1415926535f;
75 rad_gyro.z = -rad_gyro.z;
76
77 // Ignore drift correction if acceleration is not present
78 if (normal_accel.Length() == 1.0f) {
79 f32 ax = -normal_accel.x;
80 f32 ay = normal_accel.y;
81 f32 az = -normal_accel.z;
82 f32 vx, vy, vz;
83 Common::Vec3f new_real_error;
84
85 // Estimated direction of gravity
86 vx = 2.0f * (q2 * q4 - q1 * q3);
87 vy = 2.0f * (q1 * q2 + q3 * q4);
88 vz = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4;
89
90 // Error is cross product between estimated direction and measured direction of gravity
91 new_real_error.x = ay * vz - az * vy;
92 new_real_error.y = az * vx - ax * vz;
93 new_real_error.x = ax * vy - ay * vx;
94
95 derivative_error = new_real_error - real_error;
96 real_error = new_real_error;
97
98 // Prevent integral windup
99 if (ki != 0.0f) {
100 integral_error += real_error;
101 } else {
102 integral_error = {};
103 }
104
105 // Apply feedback terms
106 rad_gyro += kp * real_error;
107 rad_gyro += ki * integral_error;
108 rad_gyro += kd * derivative_error;
109 }
110
111 f32 gx = rad_gyro.y;
112 f32 gy = rad_gyro.x;
113 f32 gz = rad_gyro.z;
114
115 // Integrate rate of change of quaternion
116 f32 pa, pb, pc;
117 pa = q2;
118 pb = q3;
119 pc = q4;
120 q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * sample_period);
121 q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * sample_period);
122 q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * sample_period);
123 q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * sample_period);
124
125 quat.w = q1;
126 quat.xyz[0] = q2;
127 quat.xyz[1] = q3;
128 quat.xyz[2] = q4;
129 quat = quat.Normalized();
130}
131
132std::array<Common::Vec3f, 3> MotionInput::GetOrientation() {
133 std::array<Common::Vec3f, 3> orientation = {};
134 Common::Quaternion<float> quad;
135
136 quad.w = -quat.xyz[2];
137 quad.xyz[0] = -quat.xyz[1];
138 quad.xyz[1] = -quat.xyz[0];
139 quad.xyz[2] = -quat.w;
140
141 std::array<float, 16> matrix4x4 = quad.ToMatrix();
142
143 orientation[0] = Common::Vec3f(matrix4x4[0], matrix4x4[1], matrix4x4[2]);
144 orientation[1] = Common::Vec3f(matrix4x4[4], matrix4x4[5], matrix4x4[6]);
145 orientation[2] = Common::Vec3f(matrix4x4[8], matrix4x4[9], matrix4x4[10]);
146
147 return orientation;
148}
149
150Common::Vec3f MotionInput::GetAcceleration() {
151 return accel;
152}
153
154Common::Vec3f MotionInput::GetGyroscope() {
155 return gyro;
156}
157
158Common::Quaternion<f32> MotionInput::GetQuaternion() {
159 return quat;
160}
161
162Common::Vec3f MotionInput::GetRotations() {
163 return rotations;
164}
165
166void MotionInput::resetOrientation() {
167 if (!reset_enabled) {
168 return;
169 }
170 if (!IsMoving(0.5f) && accel.z <= -0.9f) {
171 ++reset_counter;
172 if (reset_counter > 900) {
173 // TODO: calculate quaternion from gravity vector
174 quat.w = 0;
175 quat.xyz[0] = 0;
176 quat.xyz[1] = 0;
177 quat.xyz[2] = -1;
178 integral_error = {};
179 reset_counter = 0;
180 }
181 } else {
182 reset_counter = 0;
183 }
184}
185} // 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 000000000..4b8093d8c
--- /dev/null
+++ b/src/input_common/motion_input.h
@@ -0,0 +1,62 @@
1// Copyright 2014 Dolphin Emulator Project
2// Licensed under GPLv2+
3// Refer to the license.txt file included.
4
5#pragma once
6
7#include "common/common_types.h"
8#include "common/quaternion.h"
9#include "common/vector_math.h"
10
11namespace InputCommon {
12
13class MotionInput {
14public:
15 MotionInput(f32 new_kp, f32 new_ki, f32 new_kd);
16
17 void SetAcceleration(Common::Vec3f acceleration);
18 void SetGyroscope(Common::Vec3f acceleration);
19 void SetQuaternion(Common::Quaternion<f32> quaternion);
20 void SetGyroDrift(Common::Vec3f drift);
21 void SetGyroThreshold(f32 threshold);
22
23 void EnableReset(bool reset);
24 void ResetRotations();
25
26 void UpdateRotation(u64 elapsed_time);
27 void UpdateOrientation(u64 elapsed_time);
28
29 std::array<Common::Vec3f, 3> GetOrientation();
30 Common::Vec3f GetAcceleration();
31 Common::Vec3f GetGyroscope();
32 Common::Vec3f GetRotations();
33 Common::Quaternion<f32> GetQuaternion();
34
35 bool IsMoving(f32 sensitivity);
36 bool IsCalibrated(f32 sensitivity);
37
38 // PID constants
39 const f32 kp;
40 const f32 ki;
41 const f32 kd;
42
43private:
44 void resetOrientation();
45
46 // PID errors
47 Common::Vec3f real_error;
48 Common::Vec3f integral_error;
49 Common::Vec3f derivative_error;
50
51 Common::Quaternion<f32> quat;
52 Common::Vec3f rotations;
53 Common::Vec3f accel;
54 Common::Vec3f gyro;
55 Common::Vec3f gyro_drift;
56
57 f32 gyro_threshold;
58 f32 reset_counter;
59 bool reset_enabled;
60};
61
62} // namespace InputCommon
HIC SVNT DRACONES