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
17QT_BEGIN_NAMESPACE
18
19struct 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
29private:
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
95inline QEvdevTouchFilter::QEvdevTouchFilter()
96{
97}
98
99inline 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
112inline 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
139QT_END_NAMESPACE
140

source code of qtbase/src/platformsupport/input/evdevtouch/qevdevtouchfilter_p.h