1/*
2 * Copyright (C) 2012 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 *      http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17#define LOG_TAG "VelocityTracker"
18//#define LOG_NDEBUG 0
19
20// Log debug messages about velocity tracking.
21#define DEBUG_VELOCITY 0
22
23// Log debug messages about the progress of the algorithm itself.
24#define DEBUG_STRATEGY 0
25
26#include <math.h>
27#include <limits.h>
28
29#include <cutils/properties.h>
30#include <input/VelocityTracker.h>
31#include <utils/BitSet.h>
32#include <utils/String8.h>
33#include <utils/Timers.h>
34
35namespace android {
36
37// Nanoseconds per milliseconds.
38static const nsecs_t NANOS_PER_MS = 1000000;
39
40// Threshold for determining that a pointer has stopped moving.
41// Some input devices do not send ACTION_MOVE events in the case where a pointer has
42// stopped.  We need to detect this case so that we can accurately predict the
43// velocity after the pointer starts moving again.
44static const nsecs_t ASSUME_POINTER_STOPPED_TIME = 40 * NANOS_PER_MS;
45
46
47static float vectorDot(const float* a, const float* b, uint32_t m) {
48    float r = 0;
49    while (m) {
50        m--;
51        r += *(a++) * *(b++);
52    }
53    return r;
54}
55
56static float vectorNorm(const float* a, uint32_t m) {
57    float r = 0;
58    while (m) {
59        m--;
60        float t = *(a++);
61        r += t * t;
62    }
63    return sqrtf(r);
64}
65
66#if DEBUG_STRATEGY || DEBUG_VELOCITY
67static String8 vectorToString(const float* a, uint32_t m) {
68    String8 str;
69    str.append("[");
70    while (m--) {
71        str.appendFormat(" %f", *(a++));
72        if (m) {
73            str.append(",");
74        }
75    }
76    str.append(" ]");
77    return str;
78}
79
80static String8 matrixToString(const float* a, uint32_t m, uint32_t n, bool rowMajor) {
81    String8 str;
82    str.append("[");
83    for (size_t i = 0; i < m; i++) {
84        if (i) {
85            str.append(",");
86        }
87        str.append(" [");
88        for (size_t j = 0; j < n; j++) {
89            if (j) {
90                str.append(",");
91            }
92            str.appendFormat(" %f", a[rowMajor ? i * n + j : j * m + i]);
93        }
94        str.append(" ]");
95    }
96    str.append(" ]");
97    return str;
98}
99#endif
100
101
102// --- VelocityTracker ---
103
104// The default velocity tracker strategy.
105// Although other strategies are available for testing and comparison purposes,
106// this is the strategy that applications will actually use.  Be very careful
107// when adjusting the default strategy because it can dramatically affect
108// (often in a bad way) the user experience.
109const char* VelocityTracker::DEFAULT_STRATEGY = "lsq2";
110
111VelocityTracker::VelocityTracker(const char* strategy) :
112        mLastEventTime(0), mCurrentPointerIdBits(0), mActivePointerId(-1) {
113    char value[PROPERTY_VALUE_MAX];
114
115    // Allow the default strategy to be overridden using a system property for debugging.
116    if (!strategy) {
117        int length = property_get("debug.velocitytracker.strategy", value, NULL);
118        if (length > 0) {
119            strategy = value;
120        } else {
121            strategy = DEFAULT_STRATEGY;
122        }
123    }
124
125    // Configure the strategy.
126    if (!configureStrategy(strategy)) {
127        ALOGD("Unrecognized velocity tracker strategy name '%s'.", strategy);
128        if (!configureStrategy(DEFAULT_STRATEGY)) {
129            LOG_ALWAYS_FATAL("Could not create the default velocity tracker strategy '%s'!",
130                    strategy);
131        }
132    }
133}
134
135VelocityTracker::~VelocityTracker() {
136    delete mStrategy;
137}
138
139bool VelocityTracker::configureStrategy(const char* strategy) {
140    mStrategy = createStrategy(strategy);
141    return mStrategy != NULL;
142}
143
144VelocityTrackerStrategy* VelocityTracker::createStrategy(const char* strategy) {
145    if (!strcmp("lsq1", strategy)) {
146        // 1st order least squares.  Quality: POOR.
147        // Frequently underfits the touch data especially when the finger accelerates
148        // or changes direction.  Often underestimates velocity.  The direction
149        // is overly influenced by historical touch points.
150        return new LeastSquaresVelocityTrackerStrategy(1);
151    }
152    if (!strcmp("lsq2", strategy)) {
153        // 2nd order least squares.  Quality: VERY GOOD.
154        // Pretty much ideal, but can be confused by certain kinds of touch data,
155        // particularly if the panel has a tendency to generate delayed,
156        // duplicate or jittery touch coordinates when the finger is released.
157        return new LeastSquaresVelocityTrackerStrategy(2);
158    }
159    if (!strcmp("lsq3", strategy)) {
160        // 3rd order least squares.  Quality: UNUSABLE.
161        // Frequently overfits the touch data yielding wildly divergent estimates
162        // of the velocity when the finger is released.
163        return new LeastSquaresVelocityTrackerStrategy(3);
164    }
165    if (!strcmp("wlsq2-delta", strategy)) {
166        // 2nd order weighted least squares, delta weighting.  Quality: EXPERIMENTAL
167        return new LeastSquaresVelocityTrackerStrategy(2,
168                LeastSquaresVelocityTrackerStrategy::WEIGHTING_DELTA);
169    }
170    if (!strcmp("wlsq2-central", strategy)) {
171        // 2nd order weighted least squares, central weighting.  Quality: EXPERIMENTAL
172        return new LeastSquaresVelocityTrackerStrategy(2,
173                LeastSquaresVelocityTrackerStrategy::WEIGHTING_CENTRAL);
174    }
175    if (!strcmp("wlsq2-recent", strategy)) {
176        // 2nd order weighted least squares, recent weighting.  Quality: EXPERIMENTAL
177        return new LeastSquaresVelocityTrackerStrategy(2,
178                LeastSquaresVelocityTrackerStrategy::WEIGHTING_RECENT);
179    }
180    if (!strcmp("int1", strategy)) {
181        // 1st order integrating filter.  Quality: GOOD.
182        // Not as good as 'lsq2' because it cannot estimate acceleration but it is
183        // more tolerant of errors.  Like 'lsq1', this strategy tends to underestimate
184        // the velocity of a fling but this strategy tends to respond to changes in
185        // direction more quickly and accurately.
186        return new IntegratingVelocityTrackerStrategy(1);
187    }
188    if (!strcmp("int2", strategy)) {
189        // 2nd order integrating filter.  Quality: EXPERIMENTAL.
190        // For comparison purposes only.  Unlike 'int1' this strategy can compensate
191        // for acceleration but it typically overestimates the effect.
192        return new IntegratingVelocityTrackerStrategy(2);
193    }
194    if (!strcmp("legacy", strategy)) {
195        // Legacy velocity tracker algorithm.  Quality: POOR.
196        // For comparison purposes only.  This algorithm is strongly influenced by
197        // old data points, consistently underestimates velocity and takes a very long
198        // time to adjust to changes in direction.
199        return new LegacyVelocityTrackerStrategy();
200    }
201    return NULL;
202}
203
204void VelocityTracker::clear() {
205    mCurrentPointerIdBits.clear();
206    mActivePointerId = -1;
207
208    mStrategy->clear();
209}
210
211void VelocityTracker::clearPointers(BitSet32 idBits) {
212    BitSet32 remainingIdBits(mCurrentPointerIdBits.value & ~idBits.value);
213    mCurrentPointerIdBits = remainingIdBits;
214
215    if (mActivePointerId >= 0 && idBits.hasBit(mActivePointerId)) {
216        mActivePointerId = !remainingIdBits.isEmpty() ? remainingIdBits.firstMarkedBit() : -1;
217    }
218
219    mStrategy->clearPointers(idBits);
220}
221
222void VelocityTracker::addMovement(nsecs_t eventTime, BitSet32 idBits, const Position* positions) {
223    while (idBits.count() > MAX_POINTERS) {
224        idBits.clearLastMarkedBit();
225    }
226
227    if ((mCurrentPointerIdBits.value & idBits.value)
228            && eventTime >= mLastEventTime + ASSUME_POINTER_STOPPED_TIME) {
229#if DEBUG_VELOCITY
230        ALOGD("VelocityTracker: stopped for %0.3f ms, clearing state.",
231                (eventTime - mLastEventTime) * 0.000001f);
232#endif
233        // We have not received any movements for too long.  Assume that all pointers
234        // have stopped.
235        mStrategy->clear();
236    }
237    mLastEventTime = eventTime;
238
239    mCurrentPointerIdBits = idBits;
240    if (mActivePointerId < 0 || !idBits.hasBit(mActivePointerId)) {
241        mActivePointerId = idBits.isEmpty() ? -1 : idBits.firstMarkedBit();
242    }
243
244    mStrategy->addMovement(eventTime, idBits, positions);
245
246#if DEBUG_VELOCITY
247    ALOGD("VelocityTracker: addMovement eventTime=%lld, idBits=0x%08x, activePointerId=%d",
248            eventTime, idBits.value, mActivePointerId);
249    for (BitSet32 iterBits(idBits); !iterBits.isEmpty(); ) {
250        uint32_t id = iterBits.firstMarkedBit();
251        uint32_t index = idBits.getIndexOfBit(id);
252        iterBits.clearBit(id);
253        Estimator estimator;
254        getEstimator(id, &estimator);
255        ALOGD("  %d: position (%0.3f, %0.3f), "
256                "estimator (degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f)",
257                id, positions[index].x, positions[index].y,
258                int(estimator.degree),
259                vectorToString(estimator.xCoeff, estimator.degree + 1).string(),
260                vectorToString(estimator.yCoeff, estimator.degree + 1).string(),
261                estimator.confidence);
262    }
263#endif
264}
265
266void VelocityTracker::addMovement(const MotionEvent* event) {
267    int32_t actionMasked = event->getActionMasked();
268
269    switch (actionMasked) {
270    case AMOTION_EVENT_ACTION_DOWN:
271    case AMOTION_EVENT_ACTION_HOVER_ENTER:
272        // Clear all pointers on down before adding the new movement.
273        clear();
274        break;
275    case AMOTION_EVENT_ACTION_POINTER_DOWN: {
276        // Start a new movement trace for a pointer that just went down.
277        // We do this on down instead of on up because the client may want to query the
278        // final velocity for a pointer that just went up.
279        BitSet32 downIdBits;
280        downIdBits.markBit(event->getPointerId(event->getActionIndex()));
281        clearPointers(downIdBits);
282        break;
283    }
284    case AMOTION_EVENT_ACTION_MOVE:
285    case AMOTION_EVENT_ACTION_HOVER_MOVE:
286        break;
287    default:
288        // Ignore all other actions because they do not convey any new information about
289        // pointer movement.  We also want to preserve the last known velocity of the pointers.
290        // Note that ACTION_UP and ACTION_POINTER_UP always report the last known position
291        // of the pointers that went up.  ACTION_POINTER_UP does include the new position of
292        // pointers that remained down but we will also receive an ACTION_MOVE with this
293        // information if any of them actually moved.  Since we don't know how many pointers
294        // will be going up at once it makes sense to just wait for the following ACTION_MOVE
295        // before adding the movement.
296        return;
297    }
298
299    size_t pointerCount = event->getPointerCount();
300    if (pointerCount > MAX_POINTERS) {
301        pointerCount = MAX_POINTERS;
302    }
303
304    BitSet32 idBits;
305    for (size_t i = 0; i < pointerCount; i++) {
306        idBits.markBit(event->getPointerId(i));
307    }
308
309    uint32_t pointerIndex[MAX_POINTERS];
310    for (size_t i = 0; i < pointerCount; i++) {
311        pointerIndex[i] = idBits.getIndexOfBit(event->getPointerId(i));
312    }
313
314    nsecs_t eventTime;
315    Position positions[pointerCount];
316
317    size_t historySize = event->getHistorySize();
318    for (size_t h = 0; h < historySize; h++) {
319        eventTime = event->getHistoricalEventTime(h);
320        for (size_t i = 0; i < pointerCount; i++) {
321            uint32_t index = pointerIndex[i];
322            positions[index].x = event->getHistoricalX(i, h);
323            positions[index].y = event->getHistoricalY(i, h);
324        }
325        addMovement(eventTime, idBits, positions);
326    }
327
328    eventTime = event->getEventTime();
329    for (size_t i = 0; i < pointerCount; i++) {
330        uint32_t index = pointerIndex[i];
331        positions[index].x = event->getX(i);
332        positions[index].y = event->getY(i);
333    }
334    addMovement(eventTime, idBits, positions);
335}
336
337bool VelocityTracker::getVelocity(uint32_t id, float* outVx, float* outVy) const {
338    Estimator estimator;
339    if (getEstimator(id, &estimator) && estimator.degree >= 1) {
340        *outVx = estimator.xCoeff[1];
341        *outVy = estimator.yCoeff[1];
342        return true;
343    }
344    *outVx = 0;
345    *outVy = 0;
346    return false;
347}
348
349bool VelocityTracker::getEstimator(uint32_t id, Estimator* outEstimator) const {
350    return mStrategy->getEstimator(id, outEstimator);
351}
352
353
354// --- LeastSquaresVelocityTrackerStrategy ---
355
356const nsecs_t LeastSquaresVelocityTrackerStrategy::HORIZON;
357const uint32_t LeastSquaresVelocityTrackerStrategy::HISTORY_SIZE;
358
359LeastSquaresVelocityTrackerStrategy::LeastSquaresVelocityTrackerStrategy(
360        uint32_t degree, Weighting weighting) :
361        mDegree(degree), mWeighting(weighting) {
362    clear();
363}
364
365LeastSquaresVelocityTrackerStrategy::~LeastSquaresVelocityTrackerStrategy() {
366}
367
368void LeastSquaresVelocityTrackerStrategy::clear() {
369    mIndex = 0;
370    mMovements[0].idBits.clear();
371}
372
373void LeastSquaresVelocityTrackerStrategy::clearPointers(BitSet32 idBits) {
374    BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value);
375    mMovements[mIndex].idBits = remainingIdBits;
376}
377
378void LeastSquaresVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits,
379        const VelocityTracker::Position* positions) {
380    if (++mIndex == HISTORY_SIZE) {
381        mIndex = 0;
382    }
383
384    Movement& movement = mMovements[mIndex];
385    movement.eventTime = eventTime;
386    movement.idBits = idBits;
387    uint32_t count = idBits.count();
388    for (uint32_t i = 0; i < count; i++) {
389        movement.positions[i] = positions[i];
390    }
391}
392
393/**
394 * Solves a linear least squares problem to obtain a N degree polynomial that fits
395 * the specified input data as nearly as possible.
396 *
397 * Returns true if a solution is found, false otherwise.
398 *
399 * The input consists of two vectors of data points X and Y with indices 0..m-1
400 * along with a weight vector W of the same size.
401 *
402 * The output is a vector B with indices 0..n that describes a polynomial
403 * that fits the data, such the sum of W[i] * W[i] * abs(Y[i] - (B[0] + B[1] X[i]
404 * + B[2] X[i]^2 ... B[n] X[i]^n)) for all i between 0 and m-1 is minimized.
405 *
406 * Accordingly, the weight vector W should be initialized by the caller with the
407 * reciprocal square root of the variance of the error in each input data point.
408 * In other words, an ideal choice for W would be W[i] = 1 / var(Y[i]) = 1 / stddev(Y[i]).
409 * The weights express the relative importance of each data point.  If the weights are
410 * all 1, then the data points are considered to be of equal importance when fitting
411 * the polynomial.  It is a good idea to choose weights that diminish the importance
412 * of data points that may have higher than usual error margins.
413 *
414 * Errors among data points are assumed to be independent.  W is represented here
415 * as a vector although in the literature it is typically taken to be a diagonal matrix.
416 *
417 * That is to say, the function that generated the input data can be approximated
418 * by y(x) ~= B[0] + B[1] x + B[2] x^2 + ... + B[n] x^n.
419 *
420 * The coefficient of determination (R^2) is also returned to describe the goodness
421 * of fit of the model for the given data.  It is a value between 0 and 1, where 1
422 * indicates perfect correspondence.
423 *
424 * This function first expands the X vector to a m by n matrix A such that
425 * A[i][0] = 1, A[i][1] = X[i], A[i][2] = X[i]^2, ..., A[i][n] = X[i]^n, then
426 * multiplies it by w[i]./
427 *
428 * Then it calculates the QR decomposition of A yielding an m by m orthonormal matrix Q
429 * and an m by n upper triangular matrix R.  Because R is upper triangular (lower
430 * part is all zeroes), we can simplify the decomposition into an m by n matrix
431 * Q1 and a n by n matrix R1 such that A = Q1 R1.
432 *
433 * Finally we solve the system of linear equations given by R1 B = (Qtranspose W Y)
434 * to find B.
435 *
436 * For efficiency, we lay out A and Q column-wise in memory because we frequently
437 * operate on the column vectors.  Conversely, we lay out R row-wise.
438 *
439 * http://en.wikipedia.org/wiki/Numerical_methods_for_linear_least_squares
440 * http://en.wikipedia.org/wiki/Gram-Schmidt
441 */
442static bool solveLeastSquares(const float* x, const float* y,
443        const float* w, uint32_t m, uint32_t n, float* outB, float* outDet) {
444#if DEBUG_STRATEGY
445    ALOGD("solveLeastSquares: m=%d, n=%d, x=%s, y=%s, w=%s", int(m), int(n),
446            vectorToString(x, m).string(), vectorToString(y, m).string(),
447            vectorToString(w, m).string());
448#endif
449
450    // Expand the X vector to a matrix A, pre-multiplied by the weights.
451    float a[n][m]; // column-major order
452    for (uint32_t h = 0; h < m; h++) {
453        a[0][h] = w[h];
454        for (uint32_t i = 1; i < n; i++) {
455            a[i][h] = a[i - 1][h] * x[h];
456        }
457    }
458#if DEBUG_STRATEGY
459    ALOGD("  - a=%s", matrixToString(&a[0][0], m, n, false /*rowMajor*/).string());
460#endif
461
462    // Apply the Gram-Schmidt process to A to obtain its QR decomposition.
463    float q[n][m]; // orthonormal basis, column-major order
464    float r[n][n]; // upper triangular matrix, row-major order
465    for (uint32_t j = 0; j < n; j++) {
466        for (uint32_t h = 0; h < m; h++) {
467            q[j][h] = a[j][h];
468        }
469        for (uint32_t i = 0; i < j; i++) {
470            float dot = vectorDot(&q[j][0], &q[i][0], m);
471            for (uint32_t h = 0; h < m; h++) {
472                q[j][h] -= dot * q[i][h];
473            }
474        }
475
476        float norm = vectorNorm(&q[j][0], m);
477        if (norm < 0.000001f) {
478            // vectors are linearly dependent or zero so no solution
479#if DEBUG_STRATEGY
480            ALOGD("  - no solution, norm=%f", norm);
481#endif
482            return false;
483        }
484
485        float invNorm = 1.0f / norm;
486        for (uint32_t h = 0; h < m; h++) {
487            q[j][h] *= invNorm;
488        }
489        for (uint32_t i = 0; i < n; i++) {
490            r[j][i] = i < j ? 0 : vectorDot(&q[j][0], &a[i][0], m);
491        }
492    }
493#if DEBUG_STRATEGY
494    ALOGD("  - q=%s", matrixToString(&q[0][0], m, n, false /*rowMajor*/).string());
495    ALOGD("  - r=%s", matrixToString(&r[0][0], n, n, true /*rowMajor*/).string());
496
497    // calculate QR, if we factored A correctly then QR should equal A
498    float qr[n][m];
499    for (uint32_t h = 0; h < m; h++) {
500        for (uint32_t i = 0; i < n; i++) {
501            qr[i][h] = 0;
502            for (uint32_t j = 0; j < n; j++) {
503                qr[i][h] += q[j][h] * r[j][i];
504            }
505        }
506    }
507    ALOGD("  - qr=%s", matrixToString(&qr[0][0], m, n, false /*rowMajor*/).string());
508#endif
509
510    // Solve R B = Qt W Y to find B.  This is easy because R is upper triangular.
511    // We just work from bottom-right to top-left calculating B's coefficients.
512    float wy[m];
513    for (uint32_t h = 0; h < m; h++) {
514        wy[h] = y[h] * w[h];
515    }
516    for (uint32_t i = n; i != 0; ) {
517        i--;
518        outB[i] = vectorDot(&q[i][0], wy, m);
519        for (uint32_t j = n - 1; j > i; j--) {
520            outB[i] -= r[i][j] * outB[j];
521        }
522        outB[i] /= r[i][i];
523    }
524#if DEBUG_STRATEGY
525    ALOGD("  - b=%s", vectorToString(outB, n).string());
526#endif
527
528    // Calculate the coefficient of determination as 1 - (SSerr / SStot) where
529    // SSerr is the residual sum of squares (variance of the error),
530    // and SStot is the total sum of squares (variance of the data) where each
531    // has been weighted.
532    float ymean = 0;
533    for (uint32_t h = 0; h < m; h++) {
534        ymean += y[h];
535    }
536    ymean /= m;
537
538    float sserr = 0;
539    float sstot = 0;
540    for (uint32_t h = 0; h < m; h++) {
541        float err = y[h] - outB[0];
542        float term = 1;
543        for (uint32_t i = 1; i < n; i++) {
544            term *= x[h];
545            err -= term * outB[i];
546        }
547        sserr += w[h] * w[h] * err * err;
548        float var = y[h] - ymean;
549        sstot += w[h] * w[h] * var * var;
550    }
551    *outDet = sstot > 0.000001f ? 1.0f - (sserr / sstot) : 1;
552#if DEBUG_STRATEGY
553    ALOGD("  - sserr=%f", sserr);
554    ALOGD("  - sstot=%f", sstot);
555    ALOGD("  - det=%f", *outDet);
556#endif
557    return true;
558}
559
560bool LeastSquaresVelocityTrackerStrategy::getEstimator(uint32_t id,
561        VelocityTracker::Estimator* outEstimator) const {
562    outEstimator->clear();
563
564    // Iterate over movement samples in reverse time order and collect samples.
565    float x[HISTORY_SIZE];
566    float y[HISTORY_SIZE];
567    float w[HISTORY_SIZE];
568    float time[HISTORY_SIZE];
569    uint32_t m = 0;
570    uint32_t index = mIndex;
571    const Movement& newestMovement = mMovements[mIndex];
572    do {
573        const Movement& movement = mMovements[index];
574        if (!movement.idBits.hasBit(id)) {
575            break;
576        }
577
578        nsecs_t age = newestMovement.eventTime - movement.eventTime;
579        if (age > HORIZON) {
580            break;
581        }
582
583        const VelocityTracker::Position& position = movement.getPosition(id);
584        x[m] = position.x;
585        y[m] = position.y;
586        w[m] = chooseWeight(index);
587        time[m] = -age * 0.000000001f;
588        index = (index == 0 ? HISTORY_SIZE : index) - 1;
589    } while (++m < HISTORY_SIZE);
590
591    if (m == 0) {
592        return false; // no data
593    }
594
595    // Calculate a least squares polynomial fit.
596    uint32_t degree = mDegree;
597    if (degree > m - 1) {
598        degree = m - 1;
599    }
600    if (degree >= 1) {
601        float xdet, ydet;
602        uint32_t n = degree + 1;
603        if (solveLeastSquares(time, x, w, m, n, outEstimator->xCoeff, &xdet)
604                && solveLeastSquares(time, y, w, m, n, outEstimator->yCoeff, &ydet)) {
605            outEstimator->time = newestMovement.eventTime;
606            outEstimator->degree = degree;
607            outEstimator->confidence = xdet * ydet;
608#if DEBUG_STRATEGY
609            ALOGD("estimate: degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f",
610                    int(outEstimator->degree),
611                    vectorToString(outEstimator->xCoeff, n).string(),
612                    vectorToString(outEstimator->yCoeff, n).string(),
613                    outEstimator->confidence);
614#endif
615            return true;
616        }
617    }
618
619    // No velocity data available for this pointer, but we do have its current position.
620    outEstimator->xCoeff[0] = x[0];
621    outEstimator->yCoeff[0] = y[0];
622    outEstimator->time = newestMovement.eventTime;
623    outEstimator->degree = 0;
624    outEstimator->confidence = 1;
625    return true;
626}
627
628float LeastSquaresVelocityTrackerStrategy::chooseWeight(uint32_t index) const {
629    switch (mWeighting) {
630    case WEIGHTING_DELTA: {
631        // Weight points based on how much time elapsed between them and the next
632        // point so that points that "cover" a shorter time span are weighed less.
633        //   delta  0ms: 0.5
634        //   delta 10ms: 1.0
635        if (index == mIndex) {
636            return 1.0f;
637        }
638        uint32_t nextIndex = (index + 1) % HISTORY_SIZE;
639        float deltaMillis = (mMovements[nextIndex].eventTime- mMovements[index].eventTime)
640                * 0.000001f;
641        if (deltaMillis < 0) {
642            return 0.5f;
643        }
644        if (deltaMillis < 10) {
645            return 0.5f + deltaMillis * 0.05;
646        }
647        return 1.0f;
648    }
649
650    case WEIGHTING_CENTRAL: {
651        // Weight points based on their age, weighing very recent and very old points less.
652        //   age  0ms: 0.5
653        //   age 10ms: 1.0
654        //   age 50ms: 1.0
655        //   age 60ms: 0.5
656        float ageMillis = (mMovements[mIndex].eventTime - mMovements[index].eventTime)
657                * 0.000001f;
658        if (ageMillis < 0) {
659            return 0.5f;
660        }
661        if (ageMillis < 10) {
662            return 0.5f + ageMillis * 0.05;
663        }
664        if (ageMillis < 50) {
665            return 1.0f;
666        }
667        if (ageMillis < 60) {
668            return 0.5f + (60 - ageMillis) * 0.05;
669        }
670        return 0.5f;
671    }
672
673    case WEIGHTING_RECENT: {
674        // Weight points based on their age, weighing older points less.
675        //   age   0ms: 1.0
676        //   age  50ms: 1.0
677        //   age 100ms: 0.5
678        float ageMillis = (mMovements[mIndex].eventTime - mMovements[index].eventTime)
679                * 0.000001f;
680        if (ageMillis < 50) {
681            return 1.0f;
682        }
683        if (ageMillis < 100) {
684            return 0.5f + (100 - ageMillis) * 0.01f;
685        }
686        return 0.5f;
687    }
688
689    case WEIGHTING_NONE:
690    default:
691        return 1.0f;
692    }
693}
694
695
696// --- IntegratingVelocityTrackerStrategy ---
697
698IntegratingVelocityTrackerStrategy::IntegratingVelocityTrackerStrategy(uint32_t degree) :
699        mDegree(degree) {
700}
701
702IntegratingVelocityTrackerStrategy::~IntegratingVelocityTrackerStrategy() {
703}
704
705void IntegratingVelocityTrackerStrategy::clear() {
706    mPointerIdBits.clear();
707}
708
709void IntegratingVelocityTrackerStrategy::clearPointers(BitSet32 idBits) {
710    mPointerIdBits.value &= ~idBits.value;
711}
712
713void IntegratingVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits,
714        const VelocityTracker::Position* positions) {
715    uint32_t index = 0;
716    for (BitSet32 iterIdBits(idBits); !iterIdBits.isEmpty();) {
717        uint32_t id = iterIdBits.clearFirstMarkedBit();
718        State& state = mPointerState[id];
719        const VelocityTracker::Position& position = positions[index++];
720        if (mPointerIdBits.hasBit(id)) {
721            updateState(state, eventTime, position.x, position.y);
722        } else {
723            initState(state, eventTime, position.x, position.y);
724        }
725    }
726
727    mPointerIdBits = idBits;
728}
729
730bool IntegratingVelocityTrackerStrategy::getEstimator(uint32_t id,
731        VelocityTracker::Estimator* outEstimator) const {
732    outEstimator->clear();
733
734    if (mPointerIdBits.hasBit(id)) {
735        const State& state = mPointerState[id];
736        populateEstimator(state, outEstimator);
737        return true;
738    }
739
740    return false;
741}
742
743void IntegratingVelocityTrackerStrategy::initState(State& state,
744        nsecs_t eventTime, float xpos, float ypos) const {
745    state.updateTime = eventTime;
746    state.degree = 0;
747
748    state.xpos = xpos;
749    state.xvel = 0;
750    state.xaccel = 0;
751    state.ypos = ypos;
752    state.yvel = 0;
753    state.yaccel = 0;
754}
755
756void IntegratingVelocityTrackerStrategy::updateState(State& state,
757        nsecs_t eventTime, float xpos, float ypos) const {
758    const nsecs_t MIN_TIME_DELTA = 2 * NANOS_PER_MS;
759    const float FILTER_TIME_CONSTANT = 0.010f; // 10 milliseconds
760
761    if (eventTime <= state.updateTime + MIN_TIME_DELTA) {
762        return;
763    }
764
765    float dt = (eventTime - state.updateTime) * 0.000000001f;
766    state.updateTime = eventTime;
767
768    float xvel = (xpos - state.xpos) / dt;
769    float yvel = (ypos - state.ypos) / dt;
770    if (state.degree == 0) {
771        state.xvel = xvel;
772        state.yvel = yvel;
773        state.degree = 1;
774    } else {
775        float alpha = dt / (FILTER_TIME_CONSTANT + dt);
776        if (mDegree == 1) {
777            state.xvel += (xvel - state.xvel) * alpha;
778            state.yvel += (yvel - state.yvel) * alpha;
779        } else {
780            float xaccel = (xvel - state.xvel) / dt;
781            float yaccel = (yvel - state.yvel) / dt;
782            if (state.degree == 1) {
783                state.xaccel = xaccel;
784                state.yaccel = yaccel;
785                state.degree = 2;
786            } else {
787                state.xaccel += (xaccel - state.xaccel) * alpha;
788                state.yaccel += (yaccel - state.yaccel) * alpha;
789            }
790            state.xvel += (state.xaccel * dt) * alpha;
791            state.yvel += (state.yaccel * dt) * alpha;
792        }
793    }
794    state.xpos = xpos;
795    state.ypos = ypos;
796}
797
798void IntegratingVelocityTrackerStrategy::populateEstimator(const State& state,
799        VelocityTracker::Estimator* outEstimator) const {
800    outEstimator->time = state.updateTime;
801    outEstimator->confidence = 1.0f;
802    outEstimator->degree = state.degree;
803    outEstimator->xCoeff[0] = state.xpos;
804    outEstimator->xCoeff[1] = state.xvel;
805    outEstimator->xCoeff[2] = state.xaccel / 2;
806    outEstimator->yCoeff[0] = state.ypos;
807    outEstimator->yCoeff[1] = state.yvel;
808    outEstimator->yCoeff[2] = state.yaccel / 2;
809}
810
811
812// --- LegacyVelocityTrackerStrategy ---
813
814const nsecs_t LegacyVelocityTrackerStrategy::HORIZON;
815const uint32_t LegacyVelocityTrackerStrategy::HISTORY_SIZE;
816const nsecs_t LegacyVelocityTrackerStrategy::MIN_DURATION;
817
818LegacyVelocityTrackerStrategy::LegacyVelocityTrackerStrategy() {
819    clear();
820}
821
822LegacyVelocityTrackerStrategy::~LegacyVelocityTrackerStrategy() {
823}
824
825void LegacyVelocityTrackerStrategy::clear() {
826    mIndex = 0;
827    mMovements[0].idBits.clear();
828}
829
830void LegacyVelocityTrackerStrategy::clearPointers(BitSet32 idBits) {
831    BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value);
832    mMovements[mIndex].idBits = remainingIdBits;
833}
834
835void LegacyVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits,
836        const VelocityTracker::Position* positions) {
837    if (++mIndex == HISTORY_SIZE) {
838        mIndex = 0;
839    }
840
841    Movement& movement = mMovements[mIndex];
842    movement.eventTime = eventTime;
843    movement.idBits = idBits;
844    uint32_t count = idBits.count();
845    for (uint32_t i = 0; i < count; i++) {
846        movement.positions[i] = positions[i];
847    }
848}
849
850bool LegacyVelocityTrackerStrategy::getEstimator(uint32_t id,
851        VelocityTracker::Estimator* outEstimator) const {
852    outEstimator->clear();
853
854    const Movement& newestMovement = mMovements[mIndex];
855    if (!newestMovement.idBits.hasBit(id)) {
856        return false; // no data
857    }
858
859    // Find the oldest sample that contains the pointer and that is not older than HORIZON.
860    nsecs_t minTime = newestMovement.eventTime - HORIZON;
861    uint32_t oldestIndex = mIndex;
862    uint32_t numTouches = 1;
863    do {
864        uint32_t nextOldestIndex = (oldestIndex == 0 ? HISTORY_SIZE : oldestIndex) - 1;
865        const Movement& nextOldestMovement = mMovements[nextOldestIndex];
866        if (!nextOldestMovement.idBits.hasBit(id)
867                || nextOldestMovement.eventTime < minTime) {
868            break;
869        }
870        oldestIndex = nextOldestIndex;
871    } while (++numTouches < HISTORY_SIZE);
872
873    // Calculate an exponentially weighted moving average of the velocity estimate
874    // at different points in time measured relative to the oldest sample.
875    // This is essentially an IIR filter.  Newer samples are weighted more heavily
876    // than older samples.  Samples at equal time points are weighted more or less
877    // equally.
878    //
879    // One tricky problem is that the sample data may be poorly conditioned.
880    // Sometimes samples arrive very close together in time which can cause us to
881    // overestimate the velocity at that time point.  Most samples might be measured
882    // 16ms apart but some consecutive samples could be only 0.5sm apart because
883    // the hardware or driver reports them irregularly or in bursts.
884    float accumVx = 0;
885    float accumVy = 0;
886    uint32_t index = oldestIndex;
887    uint32_t samplesUsed = 0;
888    const Movement& oldestMovement = mMovements[oldestIndex];
889    const VelocityTracker::Position& oldestPosition = oldestMovement.getPosition(id);
890    nsecs_t lastDuration = 0;
891
892    while (numTouches-- > 1) {
893        if (++index == HISTORY_SIZE) {
894            index = 0;
895        }
896        const Movement& movement = mMovements[index];
897        nsecs_t duration = movement.eventTime - oldestMovement.eventTime;
898
899        // If the duration between samples is small, we may significantly overestimate
900        // the velocity.  Consequently, we impose a minimum duration constraint on the
901        // samples that we include in the calculation.
902        if (duration >= MIN_DURATION) {
903            const VelocityTracker::Position& position = movement.getPosition(id);
904            float scale = 1000000000.0f / duration; // one over time delta in seconds
905            float vx = (position.x - oldestPosition.x) * scale;
906            float vy = (position.y - oldestPosition.y) * scale;
907            accumVx = (accumVx * lastDuration + vx * duration) / (duration + lastDuration);
908            accumVy = (accumVy * lastDuration + vy * duration) / (duration + lastDuration);
909            lastDuration = duration;
910            samplesUsed += 1;
911        }
912    }
913
914    // Report velocity.
915    const VelocityTracker::Position& newestPosition = newestMovement.getPosition(id);
916    outEstimator->time = newestMovement.eventTime;
917    outEstimator->confidence = 1;
918    outEstimator->xCoeff[0] = newestPosition.x;
919    outEstimator->yCoeff[0] = newestPosition.y;
920    if (samplesUsed) {
921        outEstimator->xCoeff[1] = accumVx;
922        outEstimator->yCoeff[1] = accumVy;
923        outEstimator->degree = 1;
924    } else {
925        outEstimator->degree = 0;
926    }
927    return true;
928}
929
930} // namespace android
931