Implement an integrating VelocityTracker strategy.

This algorithm better tolerates certain kinds of errors in the
touch input than the least squares strategy but it may underestimate
the velocity of accelerating movements.  This algorithm is
mainly of interest as a baseline for testing and comparison with the
least squares algorithm, which remains the default.

Bug: 6413587
Change-Id: I8ddb50084e44875e234717907e5b06d03f59788c

Author: Jeff Brown

include/androidfw/VelocityTracker.h

@@ -172,6 +172,39 @@ class LeastSquaresVelocityTrackerStrategy : public VelocityTrackerStrategy {
     Movement mMovements[HISTORY_SIZE];
 };
 
+
+/*
+ * Velocity tracker algorithm that uses an IIR filter.
+ */
+class IntegratingVelocityTrackerStrategy : public VelocityTrackerStrategy {
+public:
+    IntegratingVelocityTrackerStrategy();
+    ~IntegratingVelocityTrackerStrategy();
+
+    virtual void clear();
+    virtual void clearPointers(BitSet32 idBits);
+    virtual void addMovement(nsecs_t eventTime, BitSet32 idBits,
+            const VelocityTracker::Position* positions);
+    virtual bool getEstimator(uint32_t id, VelocityTracker::Estimator* outEstimator) const;
+
+private:
+    // Current state estimate for a particular pointer.
+    struct State {
+        nsecs_t updateTime;
+        bool first;
+
+        float xpos, xvel;
+        float ypos, yvel;
+    };
+
+    BitSet32 mPointerIdBits;
+    State mPointerState[MAX_POINTER_ID + 1];
+
+    static void initState(State& state, nsecs_t eventTime, float xpos, float ypos);
+    static void updateState(State& state, nsecs_t eventTime, float xpos, float ypos);
+    static void populateEstimator(const State& state, VelocityTracker::Estimator* outEstimator);
+};
+
 } // namespace android
 
 #endif // _ANDROIDFW_VELOCITY_TRACKER_H

libs/androidfw/VelocityTracker.cpp

@@ -161,6 +161,14 @@ VelocityTrackerStrategy* VelocityTracker::createStrategy(const char* strategy) {
         // of the velocity when the finger is released.
         return new LeastSquaresVelocityTrackerStrategy(3);
     }
+    if (!strcmp("int1", strategy)) {
+        // 1st order integrating filter.  Quality: GOOD.
+        // Not as good as 'lsq2' because it cannot estimate acceleration but it is
+        // more tolerant of errors.  Like 'lsq1', this strategy tends to underestimate
+        // the velocity of a fling but this strategy tends to respond to changes in
+        // direction more quickly and accurately.
+        return new IntegratingVelocityTrackerStrategy();
+    }
     return NULL;
 }
 
@@ -564,4 +572,100 @@ bool LeastSquaresVelocityTrackerStrategy::getEstimator(uint32_t id,
     return true;
 }
 
+
+// --- IntegratingVelocityTrackerStrategy ---
+
+IntegratingVelocityTrackerStrategy::IntegratingVelocityTrackerStrategy() {
+}
+
+IntegratingVelocityTrackerStrategy::~IntegratingVelocityTrackerStrategy() {
+}
+
+void IntegratingVelocityTrackerStrategy::clear() {
+    mPointerIdBits.clear();
+}
+
+void IntegratingVelocityTrackerStrategy::clearPointers(BitSet32 idBits) {
+    mPointerIdBits.value &= ~idBits.value;
+}
+
+void IntegratingVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits,
+        const VelocityTracker::Position* positions) {
+    uint32_t index = 0;
+    for (BitSet32 iterIdBits(idBits); !iterIdBits.isEmpty();) {
+        uint32_t id = iterIdBits.clearFirstMarkedBit();
+        State& state = mPointerState[id];
+        const VelocityTracker::Position& position = positions[index++];
+        if (mPointerIdBits.hasBit(id)) {
+            updateState(state, eventTime, position.x, position.y);
+        } else {
+            initState(state, eventTime, position.x, position.y);
+        }
+    }
+
+    mPointerIdBits = idBits;
+}
+
+bool IntegratingVelocityTrackerStrategy::getEstimator(uint32_t id,
+        VelocityTracker::Estimator* outEstimator) const {
+    outEstimator->clear();
+
+    if (mPointerIdBits.hasBit(id)) {
+        const State& state = mPointerState[id];
+        populateEstimator(state, outEstimator);
+        return true;
+    }
+
+    return false;
+}
+
+void IntegratingVelocityTrackerStrategy::initState(State& state,
+        nsecs_t eventTime, float xpos, float ypos) {
+    state.updateTime = eventTime;
+    state.first = true;
+
+    state.xpos = xpos;
+    state.xvel = 0;
+    state.ypos = ypos;
+    state.yvel = 0;
+}
+
+void IntegratingVelocityTrackerStrategy::updateState(State& state,
+        nsecs_t eventTime, float xpos, float ypos) {
+    const nsecs_t MIN_TIME_DELTA = 2 * NANOS_PER_MS;
+    const float FILTER_TIME_CONSTANT = 0.010f; // 10 milliseconds
+
+    if (eventTime <= state.updateTime + MIN_TIME_DELTA) {
+        return;
+    }
+
+    float dt = (eventTime - state.updateTime) * 0.000000001f;
+    state.updateTime = eventTime;
+
+    float xvel = (xpos - state.xpos) / dt;
+    float yvel = (ypos - state.ypos) / dt;
+    if (state.first) {
+        state.xvel = xvel;
+        state.yvel = yvel;
+        state.first = false;
+    } else {
+        float alpha = dt / (FILTER_TIME_CONSTANT + dt);
+        state.xvel += (xvel - state.xvel) * alpha;
+        state.yvel += (yvel - state.yvel) * alpha;
+    }
+    state.xpos = xpos;
+    state.ypos = ypos;
+}
+
+void IntegratingVelocityTrackerStrategy::populateEstimator(const State& state,
+        VelocityTracker::Estimator* outEstimator) {
+    outEstimator->time = state.updateTime;
+    outEstimator->degree = 1;
+    outEstimator->confidence = 1.0f;
+    outEstimator->xCoeff[0] = state.xpos;
+    outEstimator->xCoeff[1] = state.xvel;
+    outEstimator->yCoeff[0] = state.ypos;
+    outEstimator->yCoeff[1] = state.yvel;
+}
+
 } // namespace android