diff options
Diffstat (limited to 'src/platformsupport/input/evdevtouch/qevdevtouchfilter_p.h')
| -rw-r--r-- | src/platformsupport/input/evdevtouch/qevdevtouchfilter_p.h | 175 |
1 files changed, 175 insertions, 0 deletions
diff --git a/src/platformsupport/input/evdevtouch/qevdevtouchfilter_p.h b/src/platformsupport/input/evdevtouch/qevdevtouchfilter_p.h new file mode 100644 index 0000000000..ff6085d725 --- /dev/null +++ b/src/platformsupport/input/evdevtouch/qevdevtouchfilter_p.h @@ -0,0 +1,175 @@ +/**************************************************************************** +** +** Copyright (C) 2016 Jolla Ltd, author: <gunnar.sletta@jollamobile.com> +** Contact: https://www.qt.io/licensing/ +** +** This file is part of the plugins module of the Qt Toolkit. +** +** $QT_BEGIN_LICENSE:LGPL$ +** Commercial License Usage +** Licensees holding valid commercial Qt licenses may use this file in +** accordance with the commercial license agreement provided with the +** Software or, alternatively, in accordance with the terms contained in +** a written agreement between you and The Qt Company. For licensing terms +** and conditions see https://www.qt.io/terms-conditions. For further +** information use the contact form at https://www.qt.io/contact-us. +** +** GNU Lesser General Public License Usage +** Alternatively, this file may be used under the terms of the GNU Lesser +** General Public License version 3 as published by the Free Software +** Foundation and appearing in the file LICENSE.LGPL3 included in the +** packaging of this file. Please review the following information to +** ensure the GNU Lesser General Public License version 3 requirements +** will be met: https://www.gnu.org/licenses/lgpl-3.0.html. +** +** GNU General Public License Usage +** Alternatively, this file may be used under the terms of the GNU +** General Public License version 2.0 or (at your option) the GNU General +** Public license version 3 or any later version approved by the KDE Free +** Qt Foundation. The licenses are as published by the Free Software +** Foundation and appearing in the file LICENSE.GPL2 and LICENSE.GPL3 +** included in the packaging of this file. Please review the following +** information to ensure the GNU General Public License requirements will +** be met: https://www.gnu.org/licenses/gpl-2.0.html and +** https://www.gnu.org/licenses/gpl-3.0.html. +** +** $QT_END_LICENSE$ +** +****************************************************************************/ + +#include <qglobal.h> + +// +// W A R N I N G +// ------------- +// +// This file is not part of the Qt API. It exists purely as an +// implementation detail. This header file may change from version to +// version without notice, or even be removed. +// +// We mean it. +// + +QT_BEGIN_NAMESPACE + +struct QEvdevTouchFilter +{ + QEvdevTouchFilter(); + + void initialize(float pos, float velocity); + void update(float pos, float velocity, float timeDelta); + + float position() const { return x.x; } + float velocity() const { return x.y; } + +private: + struct vec2 { + vec2(float x = 0.0f, float y = 0.0f) : x(x), y(y) { } + float x, y; + + vec2 operator-(vec2 v) { + return vec2(x - v.x, y - v.y); + } + + vec2 operator+(vec2 v) { + return vec2(x + v.x, y + v.y); + } + }; + + struct mat2 { + float a, b, c, d; + mat2(float a = 1.0f, float b = 0.0f, float c = 0.0f, float d = 1.0f) + : a(a) + , b(b) + , c(c) + , d(d) + { + } + + mat2 transposed() const { + return mat2(a, c, + b, d); + } + + mat2 inverted() const { + float det = 1.0f / (a * d - b * c); + return mat2( d * det, -b * det, + -c * det, a * det); + } + + mat2 operator+(mat2 m) const { + return mat2(a + m.a, b + m.b, + c + m.c, d + m.d); + } + + mat2 operator-(mat2 m) const { + return mat2(a - m.a, b - m.b, + c - m.c, d - m.d); + } + + vec2 operator*(vec2 v) const { + return vec2(a * v.x + b * v.y, + c * v.x + d * v.y); + } + + mat2 operator*(mat2 M) const { + return mat2(a * M.a + b * M.c, + a * M.b + b * M.d, + c * M.a + d * M.c, + c * M.b + d * M.d); + } + }; + + vec2 x; + mat2 A; + mat2 P; + mat2 Q; + mat2 R; + mat2 H; +}; + +inline QEvdevTouchFilter::QEvdevTouchFilter() +{ +} + +inline void QEvdevTouchFilter::initialize(float pos, float velocity) +{ + x = vec2(pos, velocity); + + P = mat2(0.0f, 0.0f, + 0.0f, 0.0f); + + Q = mat2(0.0f, 0.0f, + 0.0f, 0.1f); + R = mat2(0.1f, 0.0f, + 0.0f, 0.1f); +} + +inline void QEvdevTouchFilter::update(float pos, float velocity, float dT) +{ + A.b = dT; + + // Prediction setp + x = A * x; + P = A * P * A.transposed() + Q; + + // Correction step (complete with H) + // mat2 S = H * P * H.transposed() + R; + // mat2 K = P * H.transposed() * S.inverted(); + // vec2 m(pos, velocity); + // vec2 y = m - H * x; + // x = x + K * y; + // P = (mat2() - K * H) * P; + + // Correction step (without H as H is currently set to I, so we can ignore + // it in the calculations...) + mat2 S = P + R; + mat2 K = P * S.inverted(); + vec2 m(pos, velocity); + vec2 y = m - x; + x = x + K * y; + P = (mat2() - K) * P; + +} + +QT_END_NAMESPACE |