michael@0: /* michael@0: * Copyright (C) 2012 The Android Open Source Project michael@0: * michael@0: * Licensed under the Apache License, Version 2.0 (the "License"); michael@0: * you may not use this file except in compliance with the License. michael@0: * You may obtain a copy of the License at michael@0: * michael@0: * http://www.apache.org/licenses/LICENSE-2.0 michael@0: * michael@0: * Unless required by applicable law or agreed to in writing, software michael@0: * distributed under the License is distributed on an "AS IS" BASIS, michael@0: * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. michael@0: * See the License for the specific language governing permissions and michael@0: * limitations under the License. michael@0: */ michael@0: michael@0: #define LOG_TAG "VelocityTracker" michael@0: //#define LOG_NDEBUG 0 michael@0: #include "cutils_log.h" michael@0: michael@0: // Log debug messages about velocity tracking. michael@0: #define DEBUG_VELOCITY 0 michael@0: michael@0: // Log debug messages about the progress of the algorithm itself. michael@0: #define DEBUG_STRATEGY 0 michael@0: michael@0: #include michael@0: #include michael@0: michael@0: #include "VelocityTracker.h" michael@0: #include michael@0: #include michael@0: #include michael@0: michael@0: #include michael@0: michael@0: namespace android { michael@0: michael@0: // Nanoseconds per milliseconds. michael@0: static const nsecs_t NANOS_PER_MS = 1000000; michael@0: michael@0: // Threshold for determining that a pointer has stopped moving. michael@0: // Some input devices do not send ACTION_MOVE events in the case where a pointer has michael@0: // stopped. We need to detect this case so that we can accurately predict the michael@0: // velocity after the pointer starts moving again. michael@0: static const nsecs_t ASSUME_POINTER_STOPPED_TIME = 40 * NANOS_PER_MS; michael@0: michael@0: michael@0: static float vectorDot(const float* a, const float* b, uint32_t m) { michael@0: float r = 0; michael@0: while (m--) { michael@0: r += *(a++) * *(b++); michael@0: } michael@0: return r; michael@0: } michael@0: michael@0: static float vectorNorm(const float* a, uint32_t m) { michael@0: float r = 0; michael@0: while (m--) { michael@0: float t = *(a++); michael@0: r += t * t; michael@0: } michael@0: return sqrtf(r); michael@0: } michael@0: michael@0: #if DEBUG_STRATEGY || DEBUG_VELOCITY michael@0: static String8 vectorToString(const float* a, uint32_t m) { michael@0: String8 str; michael@0: str.append("["); michael@0: while (m--) { michael@0: str.appendFormat(" %f", *(a++)); michael@0: if (m) { michael@0: str.append(","); michael@0: } michael@0: } michael@0: str.append(" ]"); michael@0: return str; michael@0: } michael@0: michael@0: static String8 matrixToString(const float* a, uint32_t m, uint32_t n, bool rowMajor) { michael@0: String8 str; michael@0: str.append("["); michael@0: for (size_t i = 0; i < m; i++) { michael@0: if (i) { michael@0: str.append(","); michael@0: } michael@0: str.append(" ["); michael@0: for (size_t j = 0; j < n; j++) { michael@0: if (j) { michael@0: str.append(","); michael@0: } michael@0: str.appendFormat(" %f", a[rowMajor ? i * n + j : j * m + i]); michael@0: } michael@0: str.append(" ]"); michael@0: } michael@0: str.append(" ]"); michael@0: return str; michael@0: } michael@0: #endif michael@0: michael@0: michael@0: // --- VelocityTracker --- michael@0: michael@0: // The default velocity tracker strategy. michael@0: // Although other strategies are available for testing and comparison purposes, michael@0: // this is the strategy that applications will actually use. Be very careful michael@0: // when adjusting the default strategy because it can dramatically affect michael@0: // (often in a bad way) the user experience. michael@0: const char* VelocityTracker::DEFAULT_STRATEGY = "lsq2"; michael@0: michael@0: VelocityTracker::VelocityTracker(const char* strategy) : michael@0: mLastEventTime(0), mCurrentPointerIdBits(0), mActivePointerId(-1) { michael@0: char value[PROPERTY_VALUE_MAX]; michael@0: michael@0: // Allow the default strategy to be overridden using a system property for debugging. michael@0: if (!strategy) { michael@0: int length = property_get("debug.velocitytracker.strategy", value, NULL); michael@0: if (length > 0) { michael@0: strategy = value; michael@0: } else { michael@0: strategy = DEFAULT_STRATEGY; michael@0: } michael@0: } michael@0: michael@0: // Configure the strategy. michael@0: if (!configureStrategy(strategy)) { michael@0: ALOGD("Unrecognized velocity tracker strategy name '%s'.", strategy); michael@0: if (!configureStrategy(DEFAULT_STRATEGY)) { michael@0: LOG_ALWAYS_FATAL("Could not create the default velocity tracker strategy '%s'!", michael@0: strategy); michael@0: } michael@0: } michael@0: } michael@0: michael@0: VelocityTracker::~VelocityTracker() { michael@0: delete mStrategy; michael@0: } michael@0: michael@0: bool VelocityTracker::configureStrategy(const char* strategy) { michael@0: mStrategy = createStrategy(strategy); michael@0: return mStrategy != NULL; michael@0: } michael@0: michael@0: VelocityTrackerStrategy* VelocityTracker::createStrategy(const char* strategy) { michael@0: if (!strcmp("lsq1", strategy)) { michael@0: // 1st order least squares. Quality: POOR. michael@0: // Frequently underfits the touch data especially when the finger accelerates michael@0: // or changes direction. Often underestimates velocity. The direction michael@0: // is overly influenced by historical touch points. michael@0: return new LeastSquaresVelocityTrackerStrategy(1); michael@0: } michael@0: if (!strcmp("lsq2", strategy)) { michael@0: // 2nd order least squares. Quality: VERY GOOD. michael@0: // Pretty much ideal, but can be confused by certain kinds of touch data, michael@0: // particularly if the panel has a tendency to generate delayed, michael@0: // duplicate or jittery touch coordinates when the finger is released. michael@0: return new LeastSquaresVelocityTrackerStrategy(2); michael@0: } michael@0: if (!strcmp("lsq3", strategy)) { michael@0: // 3rd order least squares. Quality: UNUSABLE. michael@0: // Frequently overfits the touch data yielding wildly divergent estimates michael@0: // of the velocity when the finger is released. michael@0: return new LeastSquaresVelocityTrackerStrategy(3); michael@0: } michael@0: if (!strcmp("wlsq2-delta", strategy)) { michael@0: // 2nd order weighted least squares, delta weighting. Quality: EXPERIMENTAL michael@0: return new LeastSquaresVelocityTrackerStrategy(2, michael@0: LeastSquaresVelocityTrackerStrategy::WEIGHTING_DELTA); michael@0: } michael@0: if (!strcmp("wlsq2-central", strategy)) { michael@0: // 2nd order weighted least squares, central weighting. Quality: EXPERIMENTAL michael@0: return new LeastSquaresVelocityTrackerStrategy(2, michael@0: LeastSquaresVelocityTrackerStrategy::WEIGHTING_CENTRAL); michael@0: } michael@0: if (!strcmp("wlsq2-recent", strategy)) { michael@0: // 2nd order weighted least squares, recent weighting. Quality: EXPERIMENTAL michael@0: return new LeastSquaresVelocityTrackerStrategy(2, michael@0: LeastSquaresVelocityTrackerStrategy::WEIGHTING_RECENT); michael@0: } michael@0: if (!strcmp("int1", strategy)) { michael@0: // 1st order integrating filter. Quality: GOOD. michael@0: // Not as good as 'lsq2' because it cannot estimate acceleration but it is michael@0: // more tolerant of errors. Like 'lsq1', this strategy tends to underestimate michael@0: // the velocity of a fling but this strategy tends to respond to changes in michael@0: // direction more quickly and accurately. michael@0: return new IntegratingVelocityTrackerStrategy(1); michael@0: } michael@0: if (!strcmp("int2", strategy)) { michael@0: // 2nd order integrating filter. Quality: EXPERIMENTAL. michael@0: // For comparison purposes only. Unlike 'int1' this strategy can compensate michael@0: // for acceleration but it typically overestimates the effect. michael@0: return new IntegratingVelocityTrackerStrategy(2); michael@0: } michael@0: if (!strcmp("legacy", strategy)) { michael@0: // Legacy velocity tracker algorithm. Quality: POOR. michael@0: // For comparison purposes only. This algorithm is strongly influenced by michael@0: // old data points, consistently underestimates velocity and takes a very long michael@0: // time to adjust to changes in direction. michael@0: return new LegacyVelocityTrackerStrategy(); michael@0: } michael@0: return NULL; michael@0: } michael@0: michael@0: void VelocityTracker::clear() { michael@0: mCurrentPointerIdBits.clear(); michael@0: mActivePointerId = -1; michael@0: michael@0: mStrategy->clear(); michael@0: } michael@0: michael@0: void VelocityTracker::clearPointers(BitSet32 idBits) { michael@0: BitSet32 remainingIdBits(mCurrentPointerIdBits.value & ~idBits.value); michael@0: mCurrentPointerIdBits = remainingIdBits; michael@0: michael@0: if (mActivePointerId >= 0 && idBits.hasBit(mActivePointerId)) { michael@0: mActivePointerId = !remainingIdBits.isEmpty() ? remainingIdBits.firstMarkedBit() : -1; michael@0: } michael@0: michael@0: mStrategy->clearPointers(idBits); michael@0: } michael@0: michael@0: void VelocityTracker::addMovement(nsecs_t eventTime, BitSet32 idBits, const Position* positions) { michael@0: while (idBits.count() > MAX_POINTERS) { michael@0: idBits.clearLastMarkedBit(); michael@0: } michael@0: michael@0: if ((mCurrentPointerIdBits.value & idBits.value) michael@0: && eventTime >= mLastEventTime + ASSUME_POINTER_STOPPED_TIME) { michael@0: #if DEBUG_VELOCITY michael@0: ALOGD("VelocityTracker: stopped for %0.3f ms, clearing state.", michael@0: (eventTime - mLastEventTime) * 0.000001f); michael@0: #endif michael@0: // We have not received any movements for too long. Assume that all pointers michael@0: // have stopped. michael@0: mStrategy->clear(); michael@0: } michael@0: mLastEventTime = eventTime; michael@0: michael@0: mCurrentPointerIdBits = idBits; michael@0: if (mActivePointerId < 0 || !idBits.hasBit(mActivePointerId)) { michael@0: mActivePointerId = idBits.isEmpty() ? -1 : idBits.firstMarkedBit(); michael@0: } michael@0: michael@0: mStrategy->addMovement(eventTime, idBits, positions); michael@0: michael@0: #if DEBUG_VELOCITY michael@0: ALOGD("VelocityTracker: addMovement eventTime=%lld, idBits=0x%08x, activePointerId=%d", michael@0: eventTime, idBits.value, mActivePointerId); michael@0: for (BitSet32 iterBits(idBits); !iterBits.isEmpty(); ) { michael@0: uint32_t id = iterBits.firstMarkedBit(); michael@0: uint32_t index = idBits.getIndexOfBit(id); michael@0: iterBits.clearBit(id); michael@0: Estimator estimator; michael@0: getEstimator(id, &estimator); michael@0: ALOGD(" %d: position (%0.3f, %0.3f), " michael@0: "estimator (degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f)", michael@0: id, positions[index].x, positions[index].y, michael@0: int(estimator.degree), michael@0: vectorToString(estimator.xCoeff, estimator.degree + 1).string(), michael@0: vectorToString(estimator.yCoeff, estimator.degree + 1).string(), michael@0: estimator.confidence); michael@0: } michael@0: #endif michael@0: } michael@0: michael@0: void VelocityTracker::addMovement(const MotionEvent* event) { michael@0: int32_t actionMasked = event->getActionMasked(); michael@0: michael@0: switch (actionMasked) { michael@0: case AMOTION_EVENT_ACTION_DOWN: michael@0: case AMOTION_EVENT_ACTION_HOVER_ENTER: michael@0: // Clear all pointers on down before adding the new movement. michael@0: clear(); michael@0: break; michael@0: case AMOTION_EVENT_ACTION_POINTER_DOWN: { michael@0: // Start a new movement trace for a pointer that just went down. michael@0: // We do this on down instead of on up because the client may want to query the michael@0: // final velocity for a pointer that just went up. michael@0: BitSet32 downIdBits; michael@0: downIdBits.markBit(event->getPointerId(event->getActionIndex())); michael@0: clearPointers(downIdBits); michael@0: break; michael@0: } michael@0: case AMOTION_EVENT_ACTION_MOVE: michael@0: case AMOTION_EVENT_ACTION_HOVER_MOVE: michael@0: break; michael@0: default: michael@0: // Ignore all other actions because they do not convey any new information about michael@0: // pointer movement. We also want to preserve the last known velocity of the pointers. michael@0: // Note that ACTION_UP and ACTION_POINTER_UP always report the last known position michael@0: // of the pointers that went up. ACTION_POINTER_UP does include the new position of michael@0: // pointers that remained down but we will also receive an ACTION_MOVE with this michael@0: // information if any of them actually moved. Since we don't know how many pointers michael@0: // will be going up at once it makes sense to just wait for the following ACTION_MOVE michael@0: // before adding the movement. michael@0: return; michael@0: } michael@0: michael@0: size_t pointerCount = event->getPointerCount(); michael@0: if (pointerCount > MAX_POINTERS) { michael@0: pointerCount = MAX_POINTERS; michael@0: } michael@0: michael@0: BitSet32 idBits; michael@0: for (size_t i = 0; i < pointerCount; i++) { michael@0: idBits.markBit(event->getPointerId(i)); michael@0: } michael@0: michael@0: uint32_t pointerIndex[MAX_POINTERS]; michael@0: for (size_t i = 0; i < pointerCount; i++) { michael@0: pointerIndex[i] = idBits.getIndexOfBit(event->getPointerId(i)); michael@0: } michael@0: michael@0: nsecs_t eventTime; michael@0: Position positions[pointerCount]; michael@0: michael@0: size_t historySize = event->getHistorySize(); michael@0: for (size_t h = 0; h < historySize; h++) { michael@0: eventTime = event->getHistoricalEventTime(h); michael@0: for (size_t i = 0; i < pointerCount; i++) { michael@0: uint32_t index = pointerIndex[i]; michael@0: positions[index].x = event->getHistoricalX(i, h); michael@0: positions[index].y = event->getHistoricalY(i, h); michael@0: } michael@0: addMovement(eventTime, idBits, positions); michael@0: } michael@0: michael@0: eventTime = event->getEventTime(); michael@0: for (size_t i = 0; i < pointerCount; i++) { michael@0: uint32_t index = pointerIndex[i]; michael@0: positions[index].x = event->getX(i); michael@0: positions[index].y = event->getY(i); michael@0: } michael@0: addMovement(eventTime, idBits, positions); michael@0: } michael@0: michael@0: bool VelocityTracker::getVelocity(uint32_t id, float* outVx, float* outVy) const { michael@0: Estimator estimator; michael@0: if (getEstimator(id, &estimator) && estimator.degree >= 1) { michael@0: *outVx = estimator.xCoeff[1]; michael@0: *outVy = estimator.yCoeff[1]; michael@0: return true; michael@0: } michael@0: *outVx = 0; michael@0: *outVy = 0; michael@0: return false; michael@0: } michael@0: michael@0: bool VelocityTracker::getEstimator(uint32_t id, Estimator* outEstimator) const { michael@0: return mStrategy->getEstimator(id, outEstimator); michael@0: } michael@0: michael@0: michael@0: // --- LeastSquaresVelocityTrackerStrategy --- michael@0: michael@0: const nsecs_t LeastSquaresVelocityTrackerStrategy::HORIZON; michael@0: const uint32_t LeastSquaresVelocityTrackerStrategy::HISTORY_SIZE; michael@0: michael@0: LeastSquaresVelocityTrackerStrategy::LeastSquaresVelocityTrackerStrategy( michael@0: uint32_t degree, Weighting weighting) : michael@0: mDegree(degree), mWeighting(weighting) { michael@0: clear(); michael@0: } michael@0: michael@0: LeastSquaresVelocityTrackerStrategy::~LeastSquaresVelocityTrackerStrategy() { michael@0: } michael@0: michael@0: void LeastSquaresVelocityTrackerStrategy::clear() { michael@0: mIndex = 0; michael@0: mMovements[0].idBits.clear(); michael@0: } michael@0: michael@0: void LeastSquaresVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { michael@0: BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value); michael@0: mMovements[mIndex].idBits = remainingIdBits; michael@0: } michael@0: michael@0: void LeastSquaresVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, michael@0: const VelocityTracker::Position* positions) { michael@0: if (++mIndex == HISTORY_SIZE) { michael@0: mIndex = 0; michael@0: } michael@0: michael@0: Movement& movement = mMovements[mIndex]; michael@0: movement.eventTime = eventTime; michael@0: movement.idBits = idBits; michael@0: uint32_t count = idBits.count(); michael@0: for (uint32_t i = 0; i < count; i++) { michael@0: movement.positions[i] = positions[i]; michael@0: } michael@0: } michael@0: michael@0: /** michael@0: * Solves a linear least squares problem to obtain a N degree polynomial that fits michael@0: * the specified input data as nearly as possible. michael@0: * michael@0: * Returns true if a solution is found, false otherwise. michael@0: * michael@0: * The input consists of two vectors of data points X and Y with indices 0..m-1 michael@0: * along with a weight vector W of the same size. michael@0: * michael@0: * The output is a vector B with indices 0..n that describes a polynomial michael@0: * that fits the data, such the sum of W[i] * W[i] * abs(Y[i] - (B[0] + B[1] X[i] michael@0: * + B[2] X[i]^2 ... B[n] X[i]^n)) for all i between 0 and m-1 is minimized. michael@0: * michael@0: * Accordingly, the weight vector W should be initialized by the caller with the michael@0: * reciprocal square root of the variance of the error in each input data point. michael@0: * In other words, an ideal choice for W would be W[i] = 1 / var(Y[i]) = 1 / stddev(Y[i]). michael@0: * The weights express the relative importance of each data point. If the weights are michael@0: * all 1, then the data points are considered to be of equal importance when fitting michael@0: * the polynomial. It is a good idea to choose weights that diminish the importance michael@0: * of data points that may have higher than usual error margins. michael@0: * michael@0: * Errors among data points are assumed to be independent. W is represented here michael@0: * as a vector although in the literature it is typically taken to be a diagonal matrix. michael@0: * michael@0: * That is to say, the function that generated the input data can be approximated michael@0: * by y(x) ~= B[0] + B[1] x + B[2] x^2 + ... + B[n] x^n. michael@0: * michael@0: * The coefficient of determination (R^2) is also returned to describe the goodness michael@0: * of fit of the model for the given data. It is a value between 0 and 1, where 1 michael@0: * indicates perfect correspondence. michael@0: * michael@0: * This function first expands the X vector to a m by n matrix A such that michael@0: * A[i][0] = 1, A[i][1] = X[i], A[i][2] = X[i]^2, ..., A[i][n] = X[i]^n, then michael@0: * multiplies it by w[i]./ michael@0: * michael@0: * Then it calculates the QR decomposition of A yielding an m by m orthonormal matrix Q michael@0: * and an m by n upper triangular matrix R. Because R is upper triangular (lower michael@0: * part is all zeroes), we can simplify the decomposition into an m by n matrix michael@0: * Q1 and a n by n matrix R1 such that A = Q1 R1. michael@0: * michael@0: * Finally we solve the system of linear equations given by R1 B = (Qtranspose W Y) michael@0: * to find B. michael@0: * michael@0: * For efficiency, we lay out A and Q column-wise in memory because we frequently michael@0: * operate on the column vectors. Conversely, we lay out R row-wise. michael@0: * michael@0: * http://en.wikipedia.org/wiki/Numerical_methods_for_linear_least_squares michael@0: * http://en.wikipedia.org/wiki/Gram-Schmidt michael@0: */ michael@0: static bool solveLeastSquares(const float* x, const float* y, michael@0: const float* w, uint32_t m, uint32_t n, float* outB, float* outDet) { michael@0: #if DEBUG_STRATEGY michael@0: ALOGD("solveLeastSquares: m=%d, n=%d, x=%s, y=%s, w=%s", int(m), int(n), michael@0: vectorToString(x, m).string(), vectorToString(y, m).string(), michael@0: vectorToString(w, m).string()); michael@0: #endif michael@0: michael@0: // Expand the X vector to a matrix A, pre-multiplied by the weights. michael@0: float a[n][m]; // column-major order michael@0: for (uint32_t h = 0; h < m; h++) { michael@0: a[0][h] = w[h]; michael@0: for (uint32_t i = 1; i < n; i++) { michael@0: a[i][h] = a[i - 1][h] * x[h]; michael@0: } michael@0: } michael@0: #if DEBUG_STRATEGY michael@0: ALOGD(" - a=%s", matrixToString(&a[0][0], m, n, false /*rowMajor*/).string()); michael@0: #endif michael@0: michael@0: // Apply the Gram-Schmidt process to A to obtain its QR decomposition. michael@0: float q[n][m]; // orthonormal basis, column-major order michael@0: float r[n][n]; // upper triangular matrix, row-major order michael@0: for (uint32_t j = 0; j < n; j++) { michael@0: for (uint32_t h = 0; h < m; h++) { michael@0: q[j][h] = a[j][h]; michael@0: } michael@0: for (uint32_t i = 0; i < j; i++) { michael@0: float dot = vectorDot(&q[j][0], &q[i][0], m); michael@0: for (uint32_t h = 0; h < m; h++) { michael@0: q[j][h] -= dot * q[i][h]; michael@0: } michael@0: } michael@0: michael@0: float norm = vectorNorm(&q[j][0], m); michael@0: if (norm < 0.000001f) { michael@0: // vectors are linearly dependent or zero so no solution michael@0: #if DEBUG_STRATEGY michael@0: ALOGD(" - no solution, norm=%f", norm); michael@0: #endif michael@0: return false; michael@0: } michael@0: michael@0: float invNorm = 1.0f / norm; michael@0: for (uint32_t h = 0; h < m; h++) { michael@0: q[j][h] *= invNorm; michael@0: } michael@0: for (uint32_t i = 0; i < n; i++) { michael@0: r[j][i] = i < j ? 0 : vectorDot(&q[j][0], &a[i][0], m); michael@0: } michael@0: } michael@0: #if DEBUG_STRATEGY michael@0: ALOGD(" - q=%s", matrixToString(&q[0][0], m, n, false /*rowMajor*/).string()); michael@0: ALOGD(" - r=%s", matrixToString(&r[0][0], n, n, true /*rowMajor*/).string()); michael@0: michael@0: // calculate QR, if we factored A correctly then QR should equal A michael@0: float qr[n][m]; michael@0: for (uint32_t h = 0; h < m; h++) { michael@0: for (uint32_t i = 0; i < n; i++) { michael@0: qr[i][h] = 0; michael@0: for (uint32_t j = 0; j < n; j++) { michael@0: qr[i][h] += q[j][h] * r[j][i]; michael@0: } michael@0: } michael@0: } michael@0: ALOGD(" - qr=%s", matrixToString(&qr[0][0], m, n, false /*rowMajor*/).string()); michael@0: #endif michael@0: michael@0: // Solve R B = Qt W Y to find B. This is easy because R is upper triangular. michael@0: // We just work from bottom-right to top-left calculating B's coefficients. michael@0: float wy[m]; michael@0: for (uint32_t h = 0; h < m; h++) { michael@0: wy[h] = y[h] * w[h]; michael@0: } michael@0: for (uint32_t i = n; i-- != 0; ) { michael@0: outB[i] = vectorDot(&q[i][0], wy, m); michael@0: for (uint32_t j = n - 1; j > i; j--) { michael@0: outB[i] -= r[i][j] * outB[j]; michael@0: } michael@0: outB[i] /= r[i][i]; michael@0: } michael@0: #if DEBUG_STRATEGY michael@0: ALOGD(" - b=%s", vectorToString(outB, n).string()); michael@0: #endif michael@0: michael@0: // Calculate the coefficient of determination as 1 - (SSerr / SStot) where michael@0: // SSerr is the residual sum of squares (variance of the error), michael@0: // and SStot is the total sum of squares (variance of the data) where each michael@0: // has been weighted. michael@0: float ymean = 0; michael@0: for (uint32_t h = 0; h < m; h++) { michael@0: ymean += y[h]; michael@0: } michael@0: ymean /= m; michael@0: michael@0: float sserr = 0; michael@0: float sstot = 0; michael@0: for (uint32_t h = 0; h < m; h++) { michael@0: float err = y[h] - outB[0]; michael@0: float term = 1; michael@0: for (uint32_t i = 1; i < n; i++) { michael@0: term *= x[h]; michael@0: err -= term * outB[i]; michael@0: } michael@0: sserr += w[h] * w[h] * err * err; michael@0: float var = y[h] - ymean; michael@0: sstot += w[h] * w[h] * var * var; michael@0: } michael@0: *outDet = sstot > 0.000001f ? 1.0f - (sserr / sstot) : 1; michael@0: #if DEBUG_STRATEGY michael@0: ALOGD(" - sserr=%f", sserr); michael@0: ALOGD(" - sstot=%f", sstot); michael@0: ALOGD(" - det=%f", *outDet); michael@0: #endif michael@0: return true; michael@0: } michael@0: michael@0: bool LeastSquaresVelocityTrackerStrategy::getEstimator(uint32_t id, michael@0: VelocityTracker::Estimator* outEstimator) const { michael@0: outEstimator->clear(); michael@0: michael@0: // Iterate over movement samples in reverse time order and collect samples. michael@0: float x[HISTORY_SIZE]; michael@0: float y[HISTORY_SIZE]; michael@0: float w[HISTORY_SIZE]; michael@0: float time[HISTORY_SIZE]; michael@0: uint32_t m = 0; michael@0: uint32_t index = mIndex; michael@0: const Movement& newestMovement = mMovements[mIndex]; michael@0: do { michael@0: const Movement& movement = mMovements[index]; michael@0: if (!movement.idBits.hasBit(id)) { michael@0: break; michael@0: } michael@0: michael@0: nsecs_t age = newestMovement.eventTime - movement.eventTime; michael@0: if (age > HORIZON) { michael@0: break; michael@0: } michael@0: michael@0: const VelocityTracker::Position& position = movement.getPosition(id); michael@0: x[m] = position.x; michael@0: y[m] = position.y; michael@0: w[m] = chooseWeight(index); michael@0: time[m] = -age * 0.000000001f; michael@0: index = (index == 0 ? HISTORY_SIZE : index) - 1; michael@0: } while (++m < HISTORY_SIZE); michael@0: michael@0: if (m == 0) { michael@0: return false; // no data michael@0: } michael@0: michael@0: // Calculate a least squares polynomial fit. michael@0: uint32_t degree = mDegree; michael@0: if (degree > m - 1) { michael@0: degree = m - 1; michael@0: } michael@0: if (degree >= 1) { michael@0: float xdet, ydet; michael@0: uint32_t n = degree + 1; michael@0: if (solveLeastSquares(time, x, w, m, n, outEstimator->xCoeff, &xdet) michael@0: && solveLeastSquares(time, y, w, m, n, outEstimator->yCoeff, &ydet)) { michael@0: outEstimator->time = newestMovement.eventTime; michael@0: outEstimator->degree = degree; michael@0: outEstimator->confidence = xdet * ydet; michael@0: #if DEBUG_STRATEGY michael@0: ALOGD("estimate: degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f", michael@0: int(outEstimator->degree), michael@0: vectorToString(outEstimator->xCoeff, n).string(), michael@0: vectorToString(outEstimator->yCoeff, n).string(), michael@0: outEstimator->confidence); michael@0: #endif michael@0: return true; michael@0: } michael@0: } michael@0: michael@0: // No velocity data available for this pointer, but we do have its current position. michael@0: outEstimator->xCoeff[0] = x[0]; michael@0: outEstimator->yCoeff[0] = y[0]; michael@0: outEstimator->time = newestMovement.eventTime; michael@0: outEstimator->degree = 0; michael@0: outEstimator->confidence = 1; michael@0: return true; michael@0: } michael@0: michael@0: float LeastSquaresVelocityTrackerStrategy::chooseWeight(uint32_t index) const { michael@0: switch (mWeighting) { michael@0: case WEIGHTING_DELTA: { michael@0: // Weight points based on how much time elapsed between them and the next michael@0: // point so that points that "cover" a shorter time span are weighed less. michael@0: // delta 0ms: 0.5 michael@0: // delta 10ms: 1.0 michael@0: if (index == mIndex) { michael@0: return 1.0f; michael@0: } michael@0: uint32_t nextIndex = (index + 1) % HISTORY_SIZE; michael@0: float deltaMillis = (mMovements[nextIndex].eventTime- mMovements[index].eventTime) michael@0: * 0.000001f; michael@0: if (deltaMillis < 0) { michael@0: return 0.5f; michael@0: } michael@0: if (deltaMillis < 10) { michael@0: return 0.5f + deltaMillis * 0.05; michael@0: } michael@0: return 1.0f; michael@0: } michael@0: michael@0: case WEIGHTING_CENTRAL: { michael@0: // Weight points based on their age, weighing very recent and very old points less. michael@0: // age 0ms: 0.5 michael@0: // age 10ms: 1.0 michael@0: // age 50ms: 1.0 michael@0: // age 60ms: 0.5 michael@0: float ageMillis = (mMovements[mIndex].eventTime - mMovements[index].eventTime) michael@0: * 0.000001f; michael@0: if (ageMillis < 0) { michael@0: return 0.5f; michael@0: } michael@0: if (ageMillis < 10) { michael@0: return 0.5f + ageMillis * 0.05; michael@0: } michael@0: if (ageMillis < 50) { michael@0: return 1.0f; michael@0: } michael@0: if (ageMillis < 60) { michael@0: return 0.5f + (60 - ageMillis) * 0.05; michael@0: } michael@0: return 0.5f; michael@0: } michael@0: michael@0: case WEIGHTING_RECENT: { michael@0: // Weight points based on their age, weighing older points less. michael@0: // age 0ms: 1.0 michael@0: // age 50ms: 1.0 michael@0: // age 100ms: 0.5 michael@0: float ageMillis = (mMovements[mIndex].eventTime - mMovements[index].eventTime) michael@0: * 0.000001f; michael@0: if (ageMillis < 50) { michael@0: return 1.0f; michael@0: } michael@0: if (ageMillis < 100) { michael@0: return 0.5f + (100 - ageMillis) * 0.01f; michael@0: } michael@0: return 0.5f; michael@0: } michael@0: michael@0: case WEIGHTING_NONE: michael@0: default: michael@0: return 1.0f; michael@0: } michael@0: } michael@0: michael@0: michael@0: // --- IntegratingVelocityTrackerStrategy --- michael@0: michael@0: IntegratingVelocityTrackerStrategy::IntegratingVelocityTrackerStrategy(uint32_t degree) : michael@0: mDegree(degree) { michael@0: } michael@0: michael@0: IntegratingVelocityTrackerStrategy::~IntegratingVelocityTrackerStrategy() { michael@0: } michael@0: michael@0: void IntegratingVelocityTrackerStrategy::clear() { michael@0: mPointerIdBits.clear(); michael@0: } michael@0: michael@0: void IntegratingVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { michael@0: mPointerIdBits.value &= ~idBits.value; michael@0: } michael@0: michael@0: void IntegratingVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, michael@0: const VelocityTracker::Position* positions) { michael@0: uint32_t index = 0; michael@0: for (BitSet32 iterIdBits(idBits); !iterIdBits.isEmpty();) { michael@0: uint32_t id = iterIdBits.clearFirstMarkedBit(); michael@0: State& state = mPointerState[id]; michael@0: const VelocityTracker::Position& position = positions[index++]; michael@0: if (mPointerIdBits.hasBit(id)) { michael@0: updateState(state, eventTime, position.x, position.y); michael@0: } else { michael@0: initState(state, eventTime, position.x, position.y); michael@0: } michael@0: } michael@0: michael@0: mPointerIdBits = idBits; michael@0: } michael@0: michael@0: bool IntegratingVelocityTrackerStrategy::getEstimator(uint32_t id, michael@0: VelocityTracker::Estimator* outEstimator) const { michael@0: outEstimator->clear(); michael@0: michael@0: if (mPointerIdBits.hasBit(id)) { michael@0: const State& state = mPointerState[id]; michael@0: populateEstimator(state, outEstimator); michael@0: return true; michael@0: } michael@0: michael@0: return false; michael@0: } michael@0: michael@0: void IntegratingVelocityTrackerStrategy::initState(State& state, michael@0: nsecs_t eventTime, float xpos, float ypos) const { michael@0: state.updateTime = eventTime; michael@0: state.degree = 0; michael@0: michael@0: state.xpos = xpos; michael@0: state.xvel = 0; michael@0: state.xaccel = 0; michael@0: state.ypos = ypos; michael@0: state.yvel = 0; michael@0: state.yaccel = 0; michael@0: } michael@0: michael@0: void IntegratingVelocityTrackerStrategy::updateState(State& state, michael@0: nsecs_t eventTime, float xpos, float ypos) const { michael@0: const nsecs_t MIN_TIME_DELTA = 2 * NANOS_PER_MS; michael@0: const float FILTER_TIME_CONSTANT = 0.010f; // 10 milliseconds michael@0: michael@0: if (eventTime <= state.updateTime + MIN_TIME_DELTA) { michael@0: return; michael@0: } michael@0: michael@0: float dt = (eventTime - state.updateTime) * 0.000000001f; michael@0: state.updateTime = eventTime; michael@0: michael@0: float xvel = (xpos - state.xpos) / dt; michael@0: float yvel = (ypos - state.ypos) / dt; michael@0: if (state.degree == 0) { michael@0: state.xvel = xvel; michael@0: state.yvel = yvel; michael@0: state.degree = 1; michael@0: } else { michael@0: float alpha = dt / (FILTER_TIME_CONSTANT + dt); michael@0: if (mDegree == 1) { michael@0: state.xvel += (xvel - state.xvel) * alpha; michael@0: state.yvel += (yvel - state.yvel) * alpha; michael@0: } else { michael@0: float xaccel = (xvel - state.xvel) / dt; michael@0: float yaccel = (yvel - state.yvel) / dt; michael@0: if (state.degree == 1) { michael@0: state.xaccel = xaccel; michael@0: state.yaccel = yaccel; michael@0: state.degree = 2; michael@0: } else { michael@0: state.xaccel += (xaccel - state.xaccel) * alpha; michael@0: state.yaccel += (yaccel - state.yaccel) * alpha; michael@0: } michael@0: state.xvel += (state.xaccel * dt) * alpha; michael@0: state.yvel += (state.yaccel * dt) * alpha; michael@0: } michael@0: } michael@0: state.xpos = xpos; michael@0: state.ypos = ypos; michael@0: } michael@0: michael@0: void IntegratingVelocityTrackerStrategy::populateEstimator(const State& state, michael@0: VelocityTracker::Estimator* outEstimator) const { michael@0: outEstimator->time = state.updateTime; michael@0: outEstimator->confidence = 1.0f; michael@0: outEstimator->degree = state.degree; michael@0: outEstimator->xCoeff[0] = state.xpos; michael@0: outEstimator->xCoeff[1] = state.xvel; michael@0: outEstimator->xCoeff[2] = state.xaccel / 2; michael@0: outEstimator->yCoeff[0] = state.ypos; michael@0: outEstimator->yCoeff[1] = state.yvel; michael@0: outEstimator->yCoeff[2] = state.yaccel / 2; michael@0: } michael@0: michael@0: michael@0: // --- LegacyVelocityTrackerStrategy --- michael@0: michael@0: const nsecs_t LegacyVelocityTrackerStrategy::HORIZON; michael@0: const uint32_t LegacyVelocityTrackerStrategy::HISTORY_SIZE; michael@0: const nsecs_t LegacyVelocityTrackerStrategy::MIN_DURATION; michael@0: michael@0: LegacyVelocityTrackerStrategy::LegacyVelocityTrackerStrategy() { michael@0: clear(); michael@0: } michael@0: michael@0: LegacyVelocityTrackerStrategy::~LegacyVelocityTrackerStrategy() { michael@0: } michael@0: michael@0: void LegacyVelocityTrackerStrategy::clear() { michael@0: mIndex = 0; michael@0: mMovements[0].idBits.clear(); michael@0: } michael@0: michael@0: void LegacyVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { michael@0: BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value); michael@0: mMovements[mIndex].idBits = remainingIdBits; michael@0: } michael@0: michael@0: void LegacyVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, michael@0: const VelocityTracker::Position* positions) { michael@0: if (++mIndex == HISTORY_SIZE) { michael@0: mIndex = 0; michael@0: } michael@0: michael@0: Movement& movement = mMovements[mIndex]; michael@0: movement.eventTime = eventTime; michael@0: movement.idBits = idBits; michael@0: uint32_t count = idBits.count(); michael@0: for (uint32_t i = 0; i < count; i++) { michael@0: movement.positions[i] = positions[i]; michael@0: } michael@0: } michael@0: michael@0: bool LegacyVelocityTrackerStrategy::getEstimator(uint32_t id, michael@0: VelocityTracker::Estimator* outEstimator) const { michael@0: outEstimator->clear(); michael@0: michael@0: const Movement& newestMovement = mMovements[mIndex]; michael@0: if (!newestMovement.idBits.hasBit(id)) { michael@0: return false; // no data michael@0: } michael@0: michael@0: // Find the oldest sample that contains the pointer and that is not older than HORIZON. michael@0: nsecs_t minTime = newestMovement.eventTime - HORIZON; michael@0: uint32_t oldestIndex = mIndex; michael@0: uint32_t numTouches = 1; michael@0: do { michael@0: uint32_t nextOldestIndex = (oldestIndex == 0 ? HISTORY_SIZE : oldestIndex) - 1; michael@0: const Movement& nextOldestMovement = mMovements[nextOldestIndex]; michael@0: if (!nextOldestMovement.idBits.hasBit(id) michael@0: || nextOldestMovement.eventTime < minTime) { michael@0: break; michael@0: } michael@0: oldestIndex = nextOldestIndex; michael@0: } while (++numTouches < HISTORY_SIZE); michael@0: michael@0: // Calculate an exponentially weighted moving average of the velocity estimate michael@0: // at different points in time measured relative to the oldest sample. michael@0: // This is essentially an IIR filter. Newer samples are weighted more heavily michael@0: // than older samples. Samples at equal time points are weighted more or less michael@0: // equally. michael@0: // michael@0: // One tricky problem is that the sample data may be poorly conditioned. michael@0: // Sometimes samples arrive very close together in time which can cause us to michael@0: // overestimate the velocity at that time point. Most samples might be measured michael@0: // 16ms apart but some consecutive samples could be only 0.5sm apart because michael@0: // the hardware or driver reports them irregularly or in bursts. michael@0: float accumVx = 0; michael@0: float accumVy = 0; michael@0: uint32_t index = oldestIndex; michael@0: uint32_t samplesUsed = 0; michael@0: const Movement& oldestMovement = mMovements[oldestIndex]; michael@0: const VelocityTracker::Position& oldestPosition = oldestMovement.getPosition(id); michael@0: nsecs_t lastDuration = 0; michael@0: michael@0: while (numTouches-- > 1) { michael@0: if (++index == HISTORY_SIZE) { michael@0: index = 0; michael@0: } michael@0: const Movement& movement = mMovements[index]; michael@0: nsecs_t duration = movement.eventTime - oldestMovement.eventTime; michael@0: michael@0: // If the duration between samples is small, we may significantly overestimate michael@0: // the velocity. Consequently, we impose a minimum duration constraint on the michael@0: // samples that we include in the calculation. michael@0: if (duration >= MIN_DURATION) { michael@0: const VelocityTracker::Position& position = movement.getPosition(id); michael@0: float scale = 1000000000.0f / duration; // one over time delta in seconds michael@0: float vx = (position.x - oldestPosition.x) * scale; michael@0: float vy = (position.y - oldestPosition.y) * scale; michael@0: accumVx = (accumVx * lastDuration + vx * duration) / (duration + lastDuration); michael@0: accumVy = (accumVy * lastDuration + vy * duration) / (duration + lastDuration); michael@0: lastDuration = duration; michael@0: samplesUsed += 1; michael@0: } michael@0: } michael@0: michael@0: // Report velocity. michael@0: const VelocityTracker::Position& newestPosition = newestMovement.getPosition(id); michael@0: outEstimator->time = newestMovement.eventTime; michael@0: outEstimator->confidence = 1; michael@0: outEstimator->xCoeff[0] = newestPosition.x; michael@0: outEstimator->yCoeff[0] = newestPosition.y; michael@0: if (samplesUsed) { michael@0: outEstimator->xCoeff[1] = accumVx; michael@0: outEstimator->yCoeff[1] = accumVy; michael@0: outEstimator->degree = 1; michael@0: } else { michael@0: outEstimator->degree = 0; michael@0: } michael@0: return true; michael@0: } michael@0: michael@0: } // namespace android