| /**************************************************************************** |
| ** |
| ** 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 |