New impulse-based VelocityTracker strategy.

New velocity calculation strategy for VelocityTracker.
The strategy models the phone screen as a physical object
that gets pushed by the finger. Upon liftoff, the total
work done on the object (past 100ms of data and at most
20 most recent samples) is considered to be kinetic energy,
which gets converted into equivalent velocity.
Works well with "bad" data - unclean touch liftoff,
repeated coordinates. Time-shift invariant. Performance
otherwise similar to the current default strategy,
quadratic unweighted least squares.

Bug: 35412046
Test: Recorded bad scroll event on swordfish, this fixes the
fling in the wrong direction. Also tested common usecase
scenarios on sailfish, no regressions observed. Similar
velocity values to lsq2 strategy.

Change-Id: Ib439db0ce3b4fe84f59cf66683eae0b5df7776eb
diff --git a/libs/input/VelocityTracker.cpp b/libs/input/VelocityTracker.cpp
index 62acea3..cc74b9b 100644
--- a/libs/input/VelocityTracker.cpp
+++ b/libs/input/VelocityTracker.cpp
@@ -105,7 +105,7 @@
 // this is the strategy that applications will actually use.  Be very careful
 // when adjusting the default strategy because it can dramatically affect
 // (often in a bad way) the user experience.
-const char* VelocityTracker::DEFAULT_STRATEGY = "lsq2";
+const char* VelocityTracker::DEFAULT_STRATEGY = "impulse";
 
 VelocityTracker::VelocityTracker(const char* strategy) :
         mLastEventTime(0), mCurrentPointerIdBits(0), mActivePointerId(-1) {
@@ -141,6 +141,11 @@
 }
 
 VelocityTrackerStrategy* VelocityTracker::createStrategy(const char* strategy) {
+    if (!strcmp("impulse", strategy)) {
+        // Physical model of pushing an object.  Quality: VERY GOOD.
+        // Works with duplicate coordinates, unclean finger liftoff.
+        return new ImpulseVelocityTrackerStrategy();
+    }
     if (!strcmp("lsq1", strategy)) {
         // 1st order least squares.  Quality: POOR.
         // Frequently underfits the touch data especially when the finger accelerates
@@ -352,9 +357,6 @@
 
 // --- LeastSquaresVelocityTrackerStrategy ---
 
-const nsecs_t LeastSquaresVelocityTrackerStrategy::HORIZON;
-const uint32_t LeastSquaresVelocityTrackerStrategy::HISTORY_SIZE;
-
 LeastSquaresVelocityTrackerStrategy::LeastSquaresVelocityTrackerStrategy(
         uint32_t degree, Weighting weighting) :
         mDegree(degree), mWeighting(weighting) {
@@ -863,10 +865,6 @@
 
 // --- LegacyVelocityTrackerStrategy ---
 
-const nsecs_t LegacyVelocityTrackerStrategy::HORIZON;
-const uint32_t LegacyVelocityTrackerStrategy::HISTORY_SIZE;
-const nsecs_t LegacyVelocityTrackerStrategy::MIN_DURATION;
-
 LegacyVelocityTrackerStrategy::LegacyVelocityTrackerStrategy() {
     clear();
 }
@@ -979,4 +977,192 @@
     return true;
 }
 
+// --- ImpulseVelocityTrackerStrategy ---
+
+ImpulseVelocityTrackerStrategy::ImpulseVelocityTrackerStrategy() {
+    clear();
+}
+
+ImpulseVelocityTrackerStrategy::~ImpulseVelocityTrackerStrategy() {
+}
+
+void ImpulseVelocityTrackerStrategy::clear() {
+    mIndex = 0;
+    mMovements[0].idBits.clear();
+}
+
+void ImpulseVelocityTrackerStrategy::clearPointers(BitSet32 idBits) {
+    BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value);
+    mMovements[mIndex].idBits = remainingIdBits;
+}
+
+void ImpulseVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits,
+        const VelocityTracker::Position* positions) {
+    if (++mIndex == HISTORY_SIZE) {
+        mIndex = 0;
+    }
+
+    Movement& movement = mMovements[mIndex];
+    movement.eventTime = eventTime;
+    movement.idBits = idBits;
+    uint32_t count = idBits.count();
+    for (uint32_t i = 0; i < count; i++) {
+        movement.positions[i] = positions[i];
+    }
+}
+
+/**
+ * Calculate the total impulse provided to the screen and the resulting velocity.
+ *
+ * The touchscreen is modeled as a physical object.
+ * Initial condition is discussed below, but for now suppose that v(t=0) = 0
+ *
+ * The kinetic energy of the object at the release is E=0.5*m*v^2
+ * Then vfinal = sqrt(2E/m). The goal is to calculate E.
+ *
+ * The kinetic energy at the release is equal to the total work done on the object by the finger.
+ * The total work W is the sum of all dW along the path.
+ *
+ * dW = F*dx, where dx is the piece of path traveled.
+ * Force is change of momentum over time, F = dp/dt = m dv/dt.
+ * Then substituting:
+ * dW = m (dv/dt) * dx = m * v * dv
+ *
+ * Summing along the path, we get:
+ * W = sum(dW) = sum(m * v * dv) = m * sum(v * dv)
+ * Since the mass stays constant, the equation for final velocity is:
+ * vfinal = sqrt(2*sum(v * dv))
+ *
+ * Here,
+ * dv : change of velocity = (v[i+1]-v[i])
+ * dx : change of distance = (x[i+1]-x[i])
+ * dt : change of time = (t[i+1]-t[i])
+ * v : instantaneous velocity = dx/dt
+ *
+ * The final formula is:
+ * vfinal = sqrt(2) * sqrt(sum((v[i]-v[i-1])*|v[i]|)) for all i
+ * The absolute value is needed to properly account for the sign. If the velocity over a
+ * particular segment descreases, then this indicates braking, which means that negative
+ * work was done. So for two positive, but decreasing, velocities, this contribution would be
+ * negative and will cause a smaller final velocity.
+ *
+ * Initial condition
+ * There are two ways to deal with initial condition:
+ * 1) Assume that v(0) = 0, which would mean that the screen is initially at rest.
+ * This is not entirely accurate. We are only taking the past X ms of touch data, where X is
+ * currently equal to 100. However, a touch event that created a fling probably lasted for longer
+ * than that, which would mean that the user has already been interacting with the touchscreen
+ * and it has probably already been moving.
+ * 2) Assume that the touchscreen has already been moving at a certain velocity, calculate this
+ * initial velocity and the equivalent energy, and start with this initial energy.
+ * Consider an example where we have the following data, consisting of 3 points:
+ *                 time: t0, t1, t2
+ *                 x   : x0, x1, x2
+ *                 v   : 0 , v1, v2
+ * Here is what will happen in each of these scenarios:
+ * 1) By directly applying the formula above with the v(0) = 0 boundary condition, we will get
+ * vfinal = sqrt(2*(|v1|*(v1-v0) + |v2|*(v2-v1))). This can be simplified since v0=0
+ * vfinal = sqrt(2*(|v1|*v1 + |v2|*(v2-v1))) = sqrt(2*(v1^2 + |v2|*(v2 - v1)))
+ * since velocity is a real number
+ * 2) If we treat the screen as already moving, then it must already have an energy (per mass)
+ * equal to 1/2*v1^2. Then the initial energy should be 1/2*v1*2, and only the second segment
+ * will contribute to the total kinetic energy (since we can effectively consider that v0=v1).
+ * This will give the following expression for the final velocity:
+ * vfinal = sqrt(2*(1/2*v1^2 + |v2|*(v2-v1)))
+ * This analysis can be generalized to an arbitrary number of samples.
+ *
+ *
+ * Comparing the two equations above, we see that the only mathematical difference
+ * is the factor of 1/2 in front of the first velocity term.
+ * This boundary condition would allow for the "proper" calculation of the case when all of the
+ * samples are equally spaced in time and distance, which should suggest a constant velocity.
+ *
+ * Note that approach 2) is sensitive to the proper ordering of the data in time, since
+ * the boundary condition must be applied to the oldest sample to be accurate.
+ */
+static float calculateImpulseVelocity(const nsecs_t* t, const float* x, size_t count) {
+    // The input should be in reversed time order (most recent sample at index i=0)
+    // t[i] is in nanoseconds, but due to FP arithmetic, convert to seconds inside this function
+    static constexpr float NANOS_PER_SECOND = 1E-9;
+    static constexpr float sqrt2 = 1.41421356237;
+
+    if (count < 2) {
+        return 0; // if 0 or 1 points, velocity is zero
+    }
+    if (t[1] > t[0]) { // Algorithm will still work, but not perfectly
+        ALOGE("Samples provided to calculateImpulseVelocity in the wrong order");
+    }
+    if (count == 2) { // if 2 points, basic linear calculation
+        if (t[1] == t[0]) {
+            ALOGE("Events have identical time stamps t=%" PRId64 ", setting velocity = 0", t[0]);
+            return 0;
+        }
+        return (x[1] - x[0]) / (NANOS_PER_SECOND * (t[1] - t[0]));
+    }
+    // Guaranteed to have at least 3 points here
+    float work = 0;
+    float vprev, vcurr; // v[i-1] and v[i], respectively
+    vprev = 0;
+    for (size_t i = count - 1; i > 0 ; i--) { // start with the oldest sample and go forward in time
+        if (t[i] == t[i-1]) {
+            ALOGE("Events have identical time stamps t=%" PRId64 ", skipping sample", t[i]);
+            continue;
+        }
+        vcurr = (x[i] - x[i-1]) / (NANOS_PER_SECOND * (t[i] - t[i-1]));
+        work += (vcurr - vprev) * fabsf(vcurr);
+        if (i == count - 1) {
+            work *= 0.5; // initial condition, case 2) above
+        }
+        vprev = vcurr;
+    }
+    return (work < 0 ? -1.0 : 1.0) * sqrtf(fabsf(work)) * sqrt2;
+}
+
+bool ImpulseVelocityTrackerStrategy::getEstimator(uint32_t id,
+        VelocityTracker::Estimator* outEstimator) const {
+    outEstimator->clear();
+
+    // Iterate over movement samples in reverse time order and collect samples.
+    float x[HISTORY_SIZE];
+    float y[HISTORY_SIZE];
+    nsecs_t time[HISTORY_SIZE];
+    size_t m = 0; // number of points that will be used for fitting
+    size_t index = mIndex;
+    const Movement& newestMovement = mMovements[mIndex];
+    do {
+        const Movement& movement = mMovements[index];
+        if (!movement.idBits.hasBit(id)) {
+            break;
+        }
+
+        nsecs_t age = newestMovement.eventTime - movement.eventTime;
+        if (age > HORIZON) {
+            break;
+        }
+
+        const VelocityTracker::Position& position = movement.getPosition(id);
+        x[m] = position.x;
+        y[m] = position.y;
+        time[m] = movement.eventTime;
+        index = (index == 0 ? HISTORY_SIZE : index) - 1;
+    } while (++m < HISTORY_SIZE);
+
+    if (m == 0) {
+        return false; // no data
+    }
+    outEstimator->xCoeff[0] = 0;
+    outEstimator->yCoeff[0] = 0;
+    outEstimator->xCoeff[1] = calculateImpulseVelocity(time, x, m);
+    outEstimator->yCoeff[1] = calculateImpulseVelocity(time, y, m);
+    outEstimator->xCoeff[2] = 0;
+    outEstimator->yCoeff[2] = 0;
+    outEstimator->time = newestMovement.eventTime;
+    outEstimator->degree = 2; // similar results to 2nd degree fit
+    outEstimator->confidence = 1;
+#if DEBUG_STRATEGY
+    ALOGD("velocity: (%f, %f)", outEstimator->xCoeff[1], outEstimator->yCoeff[1]);
+#endif
+    return true;
+}
+
 } // namespace android