1 | /**************************************************************************** |
2 | ** |
3 | ** Copyright (C) 2016 Jolla Ltd, author: <gunnar.sletta@jollamobile.com> |
4 | ** Contact: https://www.qt.io/licensing/ |
5 | ** |
6 | ** This file is part of the plugins module of the Qt Toolkit. |
7 | ** |
8 | ** $QT_BEGIN_LICENSE:LGPL$ |
9 | ** Commercial License Usage |
10 | ** Licensees holding valid commercial Qt licenses may use this file in |
11 | ** accordance with the commercial license agreement provided with the |
12 | ** Software or, alternatively, in accordance with the terms contained in |
13 | ** a written agreement between you and The Qt Company. For licensing terms |
14 | ** and conditions see https://www.qt.io/terms-conditions. For further |
15 | ** information use the contact form at https://www.qt.io/contact-us. |
16 | ** |
17 | ** GNU Lesser General Public License Usage |
18 | ** Alternatively, this file may be used under the terms of the GNU Lesser |
19 | ** General Public License version 3 as published by the Free Software |
20 | ** Foundation and appearing in the file LICENSE.LGPL3 included in the |
21 | ** packaging of this file. Please review the following information to |
22 | ** ensure the GNU Lesser General Public License version 3 requirements |
23 | ** will be met: https://www.gnu.org/licenses/lgpl-3.0.html. |
24 | ** |
25 | ** GNU General Public License Usage |
26 | ** Alternatively, this file may be used under the terms of the GNU |
27 | ** General Public License version 2.0 or (at your option) the GNU General |
28 | ** Public license version 3 or any later version approved by the KDE Free |
29 | ** Qt Foundation. The licenses are as published by the Free Software |
30 | ** Foundation and appearing in the file LICENSE.GPL2 and LICENSE.GPL3 |
31 | ** included in the packaging of this file. Please review the following |
32 | ** information to ensure the GNU General Public License requirements will |
33 | ** be met: https://www.gnu.org/licenses/gpl-2.0.html and |
34 | ** https://www.gnu.org/licenses/gpl-3.0.html. |
35 | ** |
36 | ** $QT_END_LICENSE$ |
37 | ** |
38 | ****************************************************************************/ |
39 | |
40 | #include <qglobal.h> |
41 | |
42 | // |
43 | // W A R N I N G |
44 | // ------------- |
45 | // |
46 | // This file is not part of the Qt API. It exists purely as an |
47 | // implementation detail. This header file may change from version to |
48 | // version without notice, or even be removed. |
49 | // |
50 | // We mean it. |
51 | // |
52 | |
53 | QT_BEGIN_NAMESPACE |
54 | |
55 | struct QEvdevTouchFilter |
56 | { |
57 | QEvdevTouchFilter(); |
58 | |
59 | void initialize(float pos, float velocity); |
60 | void update(float pos, float velocity, float timeDelta); |
61 | |
62 | float position() const { return x.x; } |
63 | float velocity() const { return x.y; } |
64 | |
65 | private: |
66 | struct vec2 { |
67 | vec2(float x = 0.0f, float y = 0.0f) : x(x), y(y) { } |
68 | float x, y; |
69 | |
70 | vec2 operator-(vec2 v) { |
71 | return vec2(x - v.x, y - v.y); |
72 | } |
73 | |
74 | vec2 operator+(vec2 v) { |
75 | return vec2(x + v.x, y + v.y); |
76 | } |
77 | }; |
78 | |
79 | struct mat2 { |
80 | float a, b, c, d; |
81 | mat2(float a = 1.0f, float b = 0.0f, float c = 0.0f, float d = 1.0f) |
82 | : a(a) |
83 | , b(b) |
84 | , c(c) |
85 | , d(d) |
86 | { |
87 | } |
88 | |
89 | mat2 transposed() const { |
90 | return mat2(a, c, |
91 | b, d); |
92 | } |
93 | |
94 | mat2 inverted() const { |
95 | float det = 1.0f / (a * d - b * c); |
96 | return mat2( d * det, -b * det, |
97 | -c * det, a * det); |
98 | } |
99 | |
100 | mat2 operator+(mat2 m) const { |
101 | return mat2(a + m.a, b + m.b, |
102 | c + m.c, d + m.d); |
103 | } |
104 | |
105 | mat2 operator-(mat2 m) const { |
106 | return mat2(a - m.a, b - m.b, |
107 | c - m.c, d - m.d); |
108 | } |
109 | |
110 | vec2 operator*(vec2 v) const { |
111 | return vec2(a * v.x + b * v.y, |
112 | c * v.x + d * v.y); |
113 | } |
114 | |
115 | mat2 operator*(mat2 M) const { |
116 | return mat2(a * M.a + b * M.c, |
117 | a * M.b + b * M.d, |
118 | c * M.a + d * M.c, |
119 | c * M.b + d * M.d); |
120 | } |
121 | }; |
122 | |
123 | vec2 x; |
124 | mat2 A; |
125 | mat2 P; |
126 | mat2 Q; |
127 | mat2 R; |
128 | mat2 H; |
129 | }; |
130 | |
131 | inline QEvdevTouchFilter::QEvdevTouchFilter() |
132 | { |
133 | } |
134 | |
135 | inline void QEvdevTouchFilter::initialize(float pos, float velocity) |
136 | { |
137 | x = vec2(pos, velocity); |
138 | |
139 | P = mat2(0.0f, 0.0f, |
140 | 0.0f, 0.0f); |
141 | |
142 | Q = mat2(0.0f, 0.0f, |
143 | 0.0f, 0.1f); |
144 | R = mat2(0.1f, 0.0f, |
145 | 0.0f, 0.1f); |
146 | } |
147 | |
148 | inline void QEvdevTouchFilter::update(float pos, float velocity, float dT) |
149 | { |
150 | A.b = dT; |
151 | |
152 | // Prediction setp |
153 | x = A * x; |
154 | P = A * P * A.transposed() + Q; |
155 | |
156 | // Correction step (complete with H) |
157 | // mat2 S = H * P * H.transposed() + R; |
158 | // mat2 K = P * H.transposed() * S.inverted(); |
159 | // vec2 m(pos, velocity); |
160 | // vec2 y = m - H * x; |
161 | // x = x + K * y; |
162 | // P = (mat2() - K * H) * P; |
163 | |
164 | // Correction step (without H as H is currently set to I, so we can ignore |
165 | // it in the calculations...) |
166 | mat2 S = P + R; |
167 | mat2 K = P * S.inverted(); |
168 | vec2 m(pos, velocity); |
169 | vec2 y = m - x; |
170 | x = x + K * y; |
171 | P = (mat2() - K) * P; |
172 | |
173 | } |
174 | |
175 | QT_END_NAMESPACE |
176 | |