1 | // Copyright (C) 2016 Jolla Ltd, author: <gunnar.sletta@jollamobile.com> |
2 | // SPDX-License-Identifier: LicenseRef-Qt-Commercial OR LGPL-3.0-only OR GPL-2.0-only OR GPL-3.0-only |
3 | |
4 | #include <private/qglobal_p.h> |
5 | |
6 | // |
7 | // W A R N I N G |
8 | // ------------- |
9 | // |
10 | // This file is not part of the Qt API. It exists purely as an |
11 | // implementation detail. This header file may change from version to |
12 | // version without notice, or even be removed. |
13 | // |
14 | // We mean it. |
15 | // |
16 | |
17 | QT_BEGIN_NAMESPACE |
18 | |
19 | struct QEvdevTouchFilter |
20 | { |
21 | QEvdevTouchFilter(); |
22 | |
23 | void initialize(float pos, float velocity); |
24 | void update(float pos, float velocity, float timeDelta); |
25 | |
26 | float position() const { return x.x; } |
27 | float velocity() const { return x.y; } |
28 | |
29 | private: |
30 | struct vec2 { |
31 | vec2(float x = 0.0f, float y = 0.0f) : x(x), y(y) { } |
32 | float x, y; |
33 | |
34 | vec2 operator-(vec2 v) { |
35 | return vec2(x - v.x, y - v.y); |
36 | } |
37 | |
38 | vec2 operator+(vec2 v) { |
39 | return vec2(x + v.x, y + v.y); |
40 | } |
41 | }; |
42 | |
43 | struct mat2 { |
44 | float a, b, c, d; |
45 | mat2(float a = 1.0f, float b = 0.0f, float c = 0.0f, float d = 1.0f) |
46 | : a(a) |
47 | , b(b) |
48 | , c(c) |
49 | , d(d) |
50 | { |
51 | } |
52 | |
53 | mat2 transposed() const { |
54 | return mat2(a, c, |
55 | b, d); |
56 | } |
57 | |
58 | mat2 inverted() const { |
59 | float det = 1.0f / (a * d - b * c); |
60 | return mat2( d * det, -b * det, |
61 | -c * det, a * det); |
62 | } |
63 | |
64 | mat2 operator+(mat2 m) const { |
65 | return mat2(a + m.a, b + m.b, |
66 | c + m.c, d + m.d); |
67 | } |
68 | |
69 | mat2 operator-(mat2 m) const { |
70 | return mat2(a - m.a, b - m.b, |
71 | c - m.c, d - m.d); |
72 | } |
73 | |
74 | vec2 operator*(vec2 v) const { |
75 | return vec2(a * v.x + b * v.y, |
76 | c * v.x + d * v.y); |
77 | } |
78 | |
79 | mat2 operator*(mat2 M) const { |
80 | return mat2(a * M.a + b * M.c, |
81 | a * M.b + b * M.d, |
82 | c * M.a + d * M.c, |
83 | c * M.b + d * M.d); |
84 | } |
85 | }; |
86 | |
87 | vec2 x; |
88 | mat2 A; |
89 | mat2 P; |
90 | mat2 Q; |
91 | mat2 R; |
92 | mat2 H; |
93 | }; |
94 | |
95 | inline QEvdevTouchFilter::QEvdevTouchFilter() |
96 | { |
97 | } |
98 | |
99 | inline void QEvdevTouchFilter::initialize(float pos, float velocity) |
100 | { |
101 | x = vec2(pos, velocity); |
102 | |
103 | P = mat2(0.0f, 0.0f, |
104 | 0.0f, 0.0f); |
105 | |
106 | Q = mat2(0.0f, 0.0f, |
107 | 0.0f, 0.1f); |
108 | R = mat2(0.1f, 0.0f, |
109 | 0.0f, 0.1f); |
110 | } |
111 | |
112 | inline void QEvdevTouchFilter::update(float pos, float velocity, float dT) |
113 | { |
114 | A.b = dT; |
115 | |
116 | // Prediction setp |
117 | x = A * x; |
118 | P = A * P * A.transposed() + Q; |
119 | |
120 | // Correction step (complete with H) |
121 | // mat2 S = H * P * H.transposed() + R; |
122 | // mat2 K = P * H.transposed() * S.inverted(); |
123 | // vec2 m(pos, velocity); |
124 | // vec2 y = m - H * x; |
125 | // x = x + K * y; |
126 | // P = (mat2() - K * H) * P; |
127 | |
128 | // Correction step (without H as H is currently set to I, so we can ignore |
129 | // it in the calculations...) |
130 | mat2 S = P + R; |
131 | mat2 K = P * S.inverted(); |
132 | vec2 m(pos, velocity); |
133 | vec2 y = m - x; |
134 | x = x + K * y; |
135 | P = (mat2() - K) * P; |
136 | |
137 | } |
138 | |
139 | QT_END_NAMESPACE |
140 | |