Implement low pass filtering of axis values

Change-Id: Ie9a7677db6c86cfd51732d0e009e5de6442ee931
Reviewed-by: Paul Lemire <paul.lemire@kdab.com>

4 files changed, 38 insertions, 7 deletions

diff --git a/src/input/backend/movingaverage_p.h b/src/input/backend/movingaverage_p.h
index ac80f6e50..8c5ae7bcb 100644
--- a/src/input/backend/movingaverage_p.h
+++ b/src/input/backend/movingaverage_p.h
@@ -66,7 +66,7 @@ public:
     float average() const;
 
 private:
-    const unsigned int m_maxSampleCount;
+    unsigned int m_maxSampleCount;
     unsigned int m_sampleCount;
     unsigned int m_currentSample;
     float m_total;
diff --git a/src/input/backend/qabstractphysicaldevicebackendnode.cpp b/src/input/backend/qabstractphysicaldevicebackendnode.cpp
index f1a668282..336ab0186 100644
--- a/src/input/backend/qabstractphysicaldevicebackendnode.cpp
+++ b/src/input/backend/qabstractphysicaldevicebackendnode.cpp
@@ -106,6 +106,21 @@ void QAbstractPhysicalDeviceBackendNodePrivate::removeAxisSetting(Qt3DCore::QNod
     }
 }
 
+Input::MovingAverage &QAbstractPhysicalDeviceBackendNodePrivate::getOrCreateFilter(int axisIdentifier)
+{
+    QVector<Input::AxisIdFilter>::iterator it;
+    QVector<Input::AxisIdFilter>::iterator end = m_axisFilters.end();
+    for (it = m_axisFilters.begin(); it != end; ++it) {
+        if (it->m_axisIdentifier == axisIdentifier)
+            return it->m_filter;
+    }
+
+    Input::AxisIdFilter axisIdFilter;
+    axisIdFilter.m_axisIdentifier = axisIdentifier;
+    m_axisFilters.push_back(axisIdFilter);
+    return m_axisFilters.last().m_filter;
+}
+
 Input::AxisSetting *QAbstractPhysicalDeviceBackendNodePrivate::getAxisSetting(Qt3DCore::QNodeId axisSettingId) const
 {
     Q_Q(const QAbstractPhysicalDeviceBackendNode);
@@ -143,6 +158,7 @@ void QAbstractPhysicalDeviceBackendNode::cleanup()
     Q_D(QAbstractPhysicalDeviceBackendNode);
     d->m_enabled = false;
     d->m_axisSettings.clear();
+    d->m_axisFilters.clear();
     d->m_inputAspect = Q_NULLPTR;
 }
 
@@ -180,10 +196,10 @@ QInputAspect *QAbstractPhysicalDeviceBackendNode::inputAspect() const
     return d->m_inputAspect;
 }
 
-float QAbstractPhysicalDeviceBackendNode::processedAxisValue(int axisIdentifier) const
+float QAbstractPhysicalDeviceBackendNode::processedAxisValue(int axisIdentifier)
 {
     // Find axis settings for this axis (if any)
-    Q_D(const QAbstractPhysicalDeviceBackendNode);
+    Q_D(QAbstractPhysicalDeviceBackendNode);
     Qt3DCore::QNodeId axisSettingId;
     QVector<Input::AxisIdSetting>::const_iterator it;
     QVector<Input::AxisIdSetting>::const_iterator end = d->m_axisSettings.cend();
@@ -204,14 +220,19 @@ float QAbstractPhysicalDeviceBackendNode::processedAxisValue(int axisIdentifier)
         Q_ASSERT(axisSetting);
         float val = rawAxisValue;
 
+        // Low pass filtering
+        if (axisSetting->isFilterEnabled()) {
+            // Get the filter corresponding to this axis
+            Input::MovingAverage &filter = d->getOrCreateFilter(axisIdentifier);
+            filter.addSample(val);
+            val = filter.average();
+        }
+
         // Deadzone handling
         if (!qFuzzyIsNull(axisSetting->deadZone())) {
             if (std::abs(val) <= axisSetting->deadZone())
                 val = 0.0f;
         }
-        qDebug() << "Using axis settings. rawValue =" << rawAxisValue << "processedValue =" << val;
-
-        // TODO: Low pass filtering
 
         return val;
     }
diff --git a/src/input/backend/qabstractphysicaldevicebackendnode.h b/src/input/backend/qabstractphysicaldevicebackendnode.h
index 7555dec71..1d7ef21bd 100644
--- a/src/input/backend/qabstractphysicaldevicebackendnode.h
+++ b/src/input/backend/qabstractphysicaldevicebackendnode.h
@@ -62,7 +62,7 @@ public:
     void setInputAspect(QInputAspect *aspect);
     QInputAspect *inputAspect() const;
 
-    float processedAxisValue(int axisIdentifier) const;
+    float processedAxisValue(int axisIdentifier);
     virtual float axisValue(int axisIdentifier) const = 0;
     virtual bool isButtonPressed(int buttonIdentifier) const = 0;
 
diff --git a/src/input/backend/qabstractphysicaldevicebackendnode_p.h b/src/input/backend/qabstractphysicaldevicebackendnode_p.h
index eac9a2255..52853550b 100644
--- a/src/input/backend/qabstractphysicaldevicebackendnode_p.h
+++ b/src/input/backend/qabstractphysicaldevicebackendnode_p.h
@@ -52,6 +52,7 @@
 #include <Qt3DCore/qnodeid.h>
 #include <QtCore/qvector.h>
 #include <Qt3DInput/private/qt3dinput_global_p.h>
+#include <Qt3DInput/private/movingaverage_p.h>
 
 QT_BEGIN_NAMESPACE
 
@@ -68,6 +69,12 @@ struct AxisIdSetting
     Qt3DCore::QNodeId m_axisSettingsId;
 };
 
+struct AxisIdFilter
+{
+    int m_axisIdentifier;
+    MovingAverage m_filter;
+};
+
 class AxisSetting;
 
 }
@@ -82,9 +89,12 @@ public:
     void addAxisSetting(int axisIdentifier, Qt3DCore::QNodeId axisSettingId);
     void removeAxisSetting(Qt3DCore::QNodeId axisSettingsId);
 
+    Input::MovingAverage &getOrCreateFilter(int axisIdentifier);
+
     Input::AxisSetting *getAxisSetting(Qt3DCore::QNodeId axisSettingId) const;
 
     QVector<Input::AxisIdSetting> m_axisSettings;
+    QVector<Input::AxisIdFilter> m_axisFilters;
     QInputAspect *m_inputAspect;
     bool m_enabled;
 };