VelocityTracker.cpp revision 4cd4009e811d3117d642607fc70485f26ce2bb63
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 <inttypes.h> 27#include <limits.h> 28#include <math.h> 29 30#include <android-base/stringprintf.h> 31#include <cutils/properties.h> 32#include <input/VelocityTracker.h> 33#include <utils/BitSet.h> 34#include <utils/Timers.h> 35 36namespace android { 37 38// Nanoseconds per milliseconds. 39static const nsecs_t NANOS_PER_MS = 1000000; 40 41// Threshold for determining that a pointer has stopped moving. 42// Some input devices do not send ACTION_MOVE events in the case where a pointer has 43// stopped. We need to detect this case so that we can accurately predict the 44// velocity after the pointer starts moving again. 45static const nsecs_t ASSUME_POINTER_STOPPED_TIME = 40 * NANOS_PER_MS; 46 47 48static float vectorDot(const float* a, const float* b, uint32_t m) { 49 float r = 0; 50 for (size_t i = 0; i < m; i++) { 51 r += *(a++) * *(b++); 52 } 53 return r; 54} 55 56static float vectorNorm(const float* a, uint32_t m) { 57 float r = 0; 58 for (size_t i = 0; i < m; i++) { 59 float t = *(a++); 60 r += t * t; 61 } 62 return sqrtf(r); 63} 64 65#if DEBUG_STRATEGY || DEBUG_VELOCITY 66static std::string vectorToString(const float* a, uint32_t m) { 67 std::string str; 68 str += "["; 69 for (size_t i = 0; i < m; i++) { 70 if (i) { 71 str += ","; 72 } 73 str += android::base::StringPrintf(" %f", *(a++)); 74 } 75 str += " ]"; 76 return str; 77} 78 79static std::string matrixToString(const float* a, uint32_t m, uint32_t n, bool rowMajor) { 80 std::string str; 81 str = "["; 82 for (size_t i = 0; i < m; i++) { 83 if (i) { 84 str += ","; 85 } 86 str += " ["; 87 for (size_t j = 0; j < n; j++) { 88 if (j) { 89 str += ","; 90 } 91 str += android::base::StringPrintf(" %f", a[rowMajor ? i * n + j : j * m + i]); 92 } 93 str += " ]"; 94 } 95 str += " ]"; 96 return str; 97} 98#endif 99 100 101// --- VelocityTracker --- 102 103// The default velocity tracker strategy. 104// Although other strategies are available for testing and comparison purposes, 105// this is the strategy that applications will actually use. Be very careful 106// when adjusting the default strategy because it can dramatically affect 107// (often in a bad way) the user experience. 108const char* VelocityTracker::DEFAULT_STRATEGY = "lsq2"; 109 110VelocityTracker::VelocityTracker(const char* strategy) : 111 mLastEventTime(0), mCurrentPointerIdBits(0), mActivePointerId(-1) { 112 char value[PROPERTY_VALUE_MAX]; 113 114 // Allow the default strategy to be overridden using a system property for debugging. 115 if (!strategy) { 116 int length = property_get("debug.velocitytracker.strategy", value, NULL); 117 if (length > 0) { 118 strategy = value; 119 } else { 120 strategy = DEFAULT_STRATEGY; 121 } 122 } 123 124 // Configure the strategy. 125 if (!configureStrategy(strategy)) { 126 ALOGD("Unrecognized velocity tracker strategy name '%s'.", strategy); 127 if (!configureStrategy(DEFAULT_STRATEGY)) { 128 LOG_ALWAYS_FATAL("Could not create the default velocity tracker strategy '%s'!", 129 strategy); 130 } 131 } 132} 133 134VelocityTracker::~VelocityTracker() { 135 delete mStrategy; 136} 137 138bool VelocityTracker::configureStrategy(const char* strategy) { 139 mStrategy = createStrategy(strategy); 140 return mStrategy != NULL; 141} 142 143VelocityTrackerStrategy* VelocityTracker::createStrategy(const char* strategy) { 144 if (!strcmp("impulse", strategy)) { 145 // Physical model of pushing an object. Quality: VERY GOOD. 146 // Works with duplicate coordinates, unclean finger liftoff. 147 return new ImpulseVelocityTrackerStrategy(); 148 } 149 if (!strcmp("lsq1", strategy)) { 150 // 1st order least squares. Quality: POOR. 151 // Frequently underfits the touch data especially when the finger accelerates 152 // or changes direction. Often underestimates velocity. The direction 153 // is overly influenced by historical touch points. 154 return new LeastSquaresVelocityTrackerStrategy(1); 155 } 156 if (!strcmp("lsq2", strategy)) { 157 // 2nd order least squares. Quality: VERY GOOD. 158 // Pretty much ideal, but can be confused by certain kinds of touch data, 159 // particularly if the panel has a tendency to generate delayed, 160 // duplicate or jittery touch coordinates when the finger is released. 161 return new LeastSquaresVelocityTrackerStrategy(2); 162 } 163 if (!strcmp("lsq3", strategy)) { 164 // 3rd order least squares. Quality: UNUSABLE. 165 // Frequently overfits the touch data yielding wildly divergent estimates 166 // of the velocity when the finger is released. 167 return new LeastSquaresVelocityTrackerStrategy(3); 168 } 169 if (!strcmp("wlsq2-delta", strategy)) { 170 // 2nd order weighted least squares, delta weighting. Quality: EXPERIMENTAL 171 return new LeastSquaresVelocityTrackerStrategy(2, 172 LeastSquaresVelocityTrackerStrategy::WEIGHTING_DELTA); 173 } 174 if (!strcmp("wlsq2-central", strategy)) { 175 // 2nd order weighted least squares, central weighting. Quality: EXPERIMENTAL 176 return new LeastSquaresVelocityTrackerStrategy(2, 177 LeastSquaresVelocityTrackerStrategy::WEIGHTING_CENTRAL); 178 } 179 if (!strcmp("wlsq2-recent", strategy)) { 180 // 2nd order weighted least squares, recent weighting. Quality: EXPERIMENTAL 181 return new LeastSquaresVelocityTrackerStrategy(2, 182 LeastSquaresVelocityTrackerStrategy::WEIGHTING_RECENT); 183 } 184 if (!strcmp("int1", strategy)) { 185 // 1st order integrating filter. Quality: GOOD. 186 // Not as good as 'lsq2' because it cannot estimate acceleration but it is 187 // more tolerant of errors. Like 'lsq1', this strategy tends to underestimate 188 // the velocity of a fling but this strategy tends to respond to changes in 189 // direction more quickly and accurately. 190 return new IntegratingVelocityTrackerStrategy(1); 191 } 192 if (!strcmp("int2", strategy)) { 193 // 2nd order integrating filter. Quality: EXPERIMENTAL. 194 // For comparison purposes only. Unlike 'int1' this strategy can compensate 195 // for acceleration but it typically overestimates the effect. 196 return new IntegratingVelocityTrackerStrategy(2); 197 } 198 if (!strcmp("legacy", strategy)) { 199 // Legacy velocity tracker algorithm. Quality: POOR. 200 // For comparison purposes only. This algorithm is strongly influenced by 201 // old data points, consistently underestimates velocity and takes a very long 202 // time to adjust to changes in direction. 203 return new LegacyVelocityTrackerStrategy(); 204 } 205 return NULL; 206} 207 208void VelocityTracker::clear() { 209 mCurrentPointerIdBits.clear(); 210 mActivePointerId = -1; 211 212 mStrategy->clear(); 213} 214 215void VelocityTracker::clearPointers(BitSet32 idBits) { 216 BitSet32 remainingIdBits(mCurrentPointerIdBits.value & ~idBits.value); 217 mCurrentPointerIdBits = remainingIdBits; 218 219 if (mActivePointerId >= 0 && idBits.hasBit(mActivePointerId)) { 220 mActivePointerId = !remainingIdBits.isEmpty() ? remainingIdBits.firstMarkedBit() : -1; 221 } 222 223 mStrategy->clearPointers(idBits); 224} 225 226void VelocityTracker::addMovement(nsecs_t eventTime, BitSet32 idBits, const Position* positions) { 227 while (idBits.count() > MAX_POINTERS) { 228 idBits.clearLastMarkedBit(); 229 } 230 231 if ((mCurrentPointerIdBits.value & idBits.value) 232 && eventTime >= mLastEventTime + ASSUME_POINTER_STOPPED_TIME) { 233#if DEBUG_VELOCITY 234 ALOGD("VelocityTracker: stopped for %0.3f ms, clearing state.", 235 (eventTime - mLastEventTime) * 0.000001f); 236#endif 237 // We have not received any movements for too long. Assume that all pointers 238 // have stopped. 239 mStrategy->clear(); 240 } 241 mLastEventTime = eventTime; 242 243 mCurrentPointerIdBits = idBits; 244 if (mActivePointerId < 0 || !idBits.hasBit(mActivePointerId)) { 245 mActivePointerId = idBits.isEmpty() ? -1 : idBits.firstMarkedBit(); 246 } 247 248 mStrategy->addMovement(eventTime, idBits, positions); 249 250#if DEBUG_VELOCITY 251 ALOGD("VelocityTracker: addMovement eventTime=%" PRId64 ", idBits=0x%08x, activePointerId=%d", 252 eventTime, idBits.value, mActivePointerId); 253 for (BitSet32 iterBits(idBits); !iterBits.isEmpty(); ) { 254 uint32_t id = iterBits.firstMarkedBit(); 255 uint32_t index = idBits.getIndexOfBit(id); 256 iterBits.clearBit(id); 257 Estimator estimator; 258 getEstimator(id, &estimator); 259 ALOGD(" %d: position (%0.3f, %0.3f), " 260 "estimator (degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f)", 261 id, positions[index].x, positions[index].y, 262 int(estimator.degree), 263 vectorToString(estimator.xCoeff, estimator.degree + 1).c_str(), 264 vectorToString(estimator.yCoeff, estimator.degree + 1).c_str(), 265 estimator.confidence); 266 } 267#endif 268} 269 270void VelocityTracker::addMovement(const MotionEvent* event) { 271 int32_t actionMasked = event->getActionMasked(); 272 273 switch (actionMasked) { 274 case AMOTION_EVENT_ACTION_DOWN: 275 case AMOTION_EVENT_ACTION_HOVER_ENTER: 276 // Clear all pointers on down before adding the new movement. 277 clear(); 278 break; 279 case AMOTION_EVENT_ACTION_POINTER_DOWN: { 280 // Start a new movement trace for a pointer that just went down. 281 // We do this on down instead of on up because the client may want to query the 282 // final velocity for a pointer that just went up. 283 BitSet32 downIdBits; 284 downIdBits.markBit(event->getPointerId(event->getActionIndex())); 285 clearPointers(downIdBits); 286 break; 287 } 288 case AMOTION_EVENT_ACTION_MOVE: 289 case AMOTION_EVENT_ACTION_HOVER_MOVE: 290 break; 291 default: 292 // Ignore all other actions because they do not convey any new information about 293 // pointer movement. We also want to preserve the last known velocity of the pointers. 294 // Note that ACTION_UP and ACTION_POINTER_UP always report the last known position 295 // of the pointers that went up. ACTION_POINTER_UP does include the new position of 296 // pointers that remained down but we will also receive an ACTION_MOVE with this 297 // information if any of them actually moved. Since we don't know how many pointers 298 // will be going up at once it makes sense to just wait for the following ACTION_MOVE 299 // before adding the movement. 300 return; 301 } 302 303 size_t pointerCount = event->getPointerCount(); 304 if (pointerCount > MAX_POINTERS) { 305 pointerCount = MAX_POINTERS; 306 } 307 308 BitSet32 idBits; 309 for (size_t i = 0; i < pointerCount; i++) { 310 idBits.markBit(event->getPointerId(i)); 311 } 312 313 uint32_t pointerIndex[MAX_POINTERS]; 314 for (size_t i = 0; i < pointerCount; i++) { 315 pointerIndex[i] = idBits.getIndexOfBit(event->getPointerId(i)); 316 } 317 318 nsecs_t eventTime; 319 Position positions[pointerCount]; 320 321 size_t historySize = event->getHistorySize(); 322 for (size_t h = 0; h < historySize; h++) { 323 eventTime = event->getHistoricalEventTime(h); 324 for (size_t i = 0; i < pointerCount; i++) { 325 uint32_t index = pointerIndex[i]; 326 positions[index].x = event->getHistoricalX(i, h); 327 positions[index].y = event->getHistoricalY(i, h); 328 } 329 addMovement(eventTime, idBits, positions); 330 } 331 332 eventTime = event->getEventTime(); 333 for (size_t i = 0; i < pointerCount; i++) { 334 uint32_t index = pointerIndex[i]; 335 positions[index].x = event->getX(i); 336 positions[index].y = event->getY(i); 337 } 338 addMovement(eventTime, idBits, positions); 339} 340 341bool VelocityTracker::getVelocity(uint32_t id, float* outVx, float* outVy) const { 342 Estimator estimator; 343 if (getEstimator(id, &estimator) && estimator.degree >= 1) { 344 *outVx = estimator.xCoeff[1]; 345 *outVy = estimator.yCoeff[1]; 346 return true; 347 } 348 *outVx = 0; 349 *outVy = 0; 350 return false; 351} 352 353bool VelocityTracker::getEstimator(uint32_t id, Estimator* outEstimator) const { 354 return mStrategy->getEstimator(id, outEstimator); 355} 356 357 358// --- LeastSquaresVelocityTrackerStrategy --- 359 360LeastSquaresVelocityTrackerStrategy::LeastSquaresVelocityTrackerStrategy( 361 uint32_t degree, Weighting weighting) : 362 mDegree(degree), mWeighting(weighting) { 363 clear(); 364} 365 366LeastSquaresVelocityTrackerStrategy::~LeastSquaresVelocityTrackerStrategy() { 367} 368 369void LeastSquaresVelocityTrackerStrategy::clear() { 370 mIndex = 0; 371 mMovements[0].idBits.clear(); 372} 373 374void LeastSquaresVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { 375 BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value); 376 mMovements[mIndex].idBits = remainingIdBits; 377} 378 379void LeastSquaresVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, 380 const VelocityTracker::Position* positions) { 381 if (++mIndex == HISTORY_SIZE) { 382 mIndex = 0; 383 } 384 385 Movement& movement = mMovements[mIndex]; 386 movement.eventTime = eventTime; 387 movement.idBits = idBits; 388 uint32_t count = idBits.count(); 389 for (uint32_t i = 0; i < count; i++) { 390 movement.positions[i] = positions[i]; 391 } 392} 393 394/** 395 * Solves a linear least squares problem to obtain a N degree polynomial that fits 396 * the specified input data as nearly as possible. 397 * 398 * Returns true if a solution is found, false otherwise. 399 * 400 * The input consists of two vectors of data points X and Y with indices 0..m-1 401 * along with a weight vector W of the same size. 402 * 403 * The output is a vector B with indices 0..n that describes a polynomial 404 * that fits the data, such the sum of W[i] * W[i] * abs(Y[i] - (B[0] + B[1] X[i] 405 * + B[2] X[i]^2 ... B[n] X[i]^n)) for all i between 0 and m-1 is minimized. 406 * 407 * Accordingly, the weight vector W should be initialized by the caller with the 408 * reciprocal square root of the variance of the error in each input data point. 409 * In other words, an ideal choice for W would be W[i] = 1 / var(Y[i]) = 1 / stddev(Y[i]). 410 * The weights express the relative importance of each data point. If the weights are 411 * all 1, then the data points are considered to be of equal importance when fitting 412 * the polynomial. It is a good idea to choose weights that diminish the importance 413 * of data points that may have higher than usual error margins. 414 * 415 * Errors among data points are assumed to be independent. W is represented here 416 * as a vector although in the literature it is typically taken to be a diagonal matrix. 417 * 418 * That is to say, the function that generated the input data can be approximated 419 * by y(x) ~= B[0] + B[1] x + B[2] x^2 + ... + B[n] x^n. 420 * 421 * The coefficient of determination (R^2) is also returned to describe the goodness 422 * of fit of the model for the given data. It is a value between 0 and 1, where 1 423 * indicates perfect correspondence. 424 * 425 * This function first expands the X vector to a m by n matrix A such that 426 * A[i][0] = 1, A[i][1] = X[i], A[i][2] = X[i]^2, ..., A[i][n] = X[i]^n, then 427 * multiplies it by w[i]./ 428 * 429 * Then it calculates the QR decomposition of A yielding an m by m orthonormal matrix Q 430 * and an m by n upper triangular matrix R. Because R is upper triangular (lower 431 * part is all zeroes), we can simplify the decomposition into an m by n matrix 432 * Q1 and a n by n matrix R1 such that A = Q1 R1. 433 * 434 * Finally we solve the system of linear equations given by R1 B = (Qtranspose W Y) 435 * to find B. 436 * 437 * For efficiency, we lay out A and Q column-wise in memory because we frequently 438 * operate on the column vectors. Conversely, we lay out R row-wise. 439 * 440 * http://en.wikipedia.org/wiki/Numerical_methods_for_linear_least_squares 441 * http://en.wikipedia.org/wiki/Gram-Schmidt 442 */ 443static bool solveLeastSquares(const float* x, const float* y, 444 const float* w, uint32_t m, uint32_t n, float* outB, float* outDet) { 445#if DEBUG_STRATEGY 446 ALOGD("solveLeastSquares: m=%d, n=%d, x=%s, y=%s, w=%s", int(m), int(n), 447 vectorToString(x, m).c_str(), vectorToString(y, m).c_str(), 448 vectorToString(w, m).c_str()); 449#endif 450 451 // Expand the X vector to a matrix A, pre-multiplied by the weights. 452 float a[n][m]; // column-major order 453 for (uint32_t h = 0; h < m; h++) { 454 a[0][h] = w[h]; 455 for (uint32_t i = 1; i < n; i++) { 456 a[i][h] = a[i - 1][h] * x[h]; 457 } 458 } 459#if DEBUG_STRATEGY 460 ALOGD(" - a=%s", matrixToString(&a[0][0], m, n, false /*rowMajor*/).c_str()); 461#endif 462 463 // Apply the Gram-Schmidt process to A to obtain its QR decomposition. 464 float q[n][m]; // orthonormal basis, column-major order 465 float r[n][n]; // upper triangular matrix, row-major order 466 for (uint32_t j = 0; j < n; j++) { 467 for (uint32_t h = 0; h < m; h++) { 468 q[j][h] = a[j][h]; 469 } 470 for (uint32_t i = 0; i < j; i++) { 471 float dot = vectorDot(&q[j][0], &q[i][0], m); 472 for (uint32_t h = 0; h < m; h++) { 473 q[j][h] -= dot * q[i][h]; 474 } 475 } 476 477 float norm = vectorNorm(&q[j][0], m); 478 if (norm < 0.000001f) { 479 // vectors are linearly dependent or zero so no solution 480#if DEBUG_STRATEGY 481 ALOGD(" - no solution, norm=%f", norm); 482#endif 483 return false; 484 } 485 486 float invNorm = 1.0f / norm; 487 for (uint32_t h = 0; h < m; h++) { 488 q[j][h] *= invNorm; 489 } 490 for (uint32_t i = 0; i < n; i++) { 491 r[j][i] = i < j ? 0 : vectorDot(&q[j][0], &a[i][0], m); 492 } 493 } 494#if DEBUG_STRATEGY 495 ALOGD(" - q=%s", matrixToString(&q[0][0], m, n, false /*rowMajor*/).c_str()); 496 ALOGD(" - r=%s", matrixToString(&r[0][0], n, n, true /*rowMajor*/).c_str()); 497 498 // calculate QR, if we factored A correctly then QR should equal A 499 float qr[n][m]; 500 for (uint32_t h = 0; h < m; h++) { 501 for (uint32_t i = 0; i < n; i++) { 502 qr[i][h] = 0; 503 for (uint32_t j = 0; j < n; j++) { 504 qr[i][h] += q[j][h] * r[j][i]; 505 } 506 } 507 } 508 ALOGD(" - qr=%s", matrixToString(&qr[0][0], m, n, false /*rowMajor*/).c_str()); 509#endif 510 511 // Solve R B = Qt W Y to find B. This is easy because R is upper triangular. 512 // We just work from bottom-right to top-left calculating B's coefficients. 513 float wy[m]; 514 for (uint32_t h = 0; h < m; h++) { 515 wy[h] = y[h] * w[h]; 516 } 517 for (uint32_t i = n; i != 0; ) { 518 i--; 519 outB[i] = vectorDot(&q[i][0], wy, m); 520 for (uint32_t j = n - 1; j > i; j--) { 521 outB[i] -= r[i][j] * outB[j]; 522 } 523 outB[i] /= r[i][i]; 524 } 525#if DEBUG_STRATEGY 526 ALOGD(" - b=%s", vectorToString(outB, n).c_str()); 527#endif 528 529 // Calculate the coefficient of determination as 1 - (SSerr / SStot) where 530 // SSerr is the residual sum of squares (variance of the error), 531 // and SStot is the total sum of squares (variance of the data) where each 532 // has been weighted. 533 float ymean = 0; 534 for (uint32_t h = 0; h < m; h++) { 535 ymean += y[h]; 536 } 537 ymean /= m; 538 539 float sserr = 0; 540 float sstot = 0; 541 for (uint32_t h = 0; h < m; h++) { 542 float err = y[h] - outB[0]; 543 float term = 1; 544 for (uint32_t i = 1; i < n; i++) { 545 term *= x[h]; 546 err -= term * outB[i]; 547 } 548 sserr += w[h] * w[h] * err * err; 549 float var = y[h] - ymean; 550 sstot += w[h] * w[h] * var * var; 551 } 552 *outDet = sstot > 0.000001f ? 1.0f - (sserr / sstot) : 1; 553#if DEBUG_STRATEGY 554 ALOGD(" - sserr=%f", sserr); 555 ALOGD(" - sstot=%f", sstot); 556 ALOGD(" - det=%f", *outDet); 557#endif 558 return true; 559} 560 561/* 562 * Optimized unweighted second-order least squares fit. About 2x speed improvement compared to 563 * the default implementation 564 */ 565static float solveUnweightedLeastSquaresDeg2(const float* x, const float* y, size_t count) { 566 float sxi = 0, sxiyi = 0, syi = 0, sxi2 = 0, sxi3 = 0, sxi2yi = 0, sxi4 = 0; 567 568 for (size_t i = 0; i < count; i++) { 569 float xi = x[i]; 570 float yi = y[i]; 571 float xi2 = xi*xi; 572 float xi3 = xi2*xi; 573 float xi4 = xi3*xi; 574 float xi2yi = xi2*yi; 575 float xiyi = xi*yi; 576 577 sxi += xi; 578 sxi2 += xi2; 579 sxiyi += xiyi; 580 sxi2yi += xi2yi; 581 syi += yi; 582 sxi3 += xi3; 583 sxi4 += xi4; 584 } 585 586 float Sxx = sxi2 - sxi*sxi / count; 587 float Sxy = sxiyi - sxi*syi / count; 588 float Sxx2 = sxi3 - sxi*sxi2 / count; 589 float Sx2y = sxi2yi - sxi2*syi / count; 590 float Sx2x2 = sxi4 - sxi2*sxi2 / count; 591 592 float numerator = Sxy*Sx2x2 - Sx2y*Sxx2; 593 float denominator = Sxx*Sx2x2 - Sxx2*Sxx2; 594 if (denominator == 0) { 595 ALOGW("division by 0 when computing velocity, Sxx=%f, Sx2x2=%f, Sxx2=%f", Sxx, Sx2x2, Sxx2); 596 return 0; 597 } 598 return numerator/denominator; 599} 600 601bool LeastSquaresVelocityTrackerStrategy::getEstimator(uint32_t id, 602 VelocityTracker::Estimator* outEstimator) const { 603 outEstimator->clear(); 604 605 // Iterate over movement samples in reverse time order and collect samples. 606 float x[HISTORY_SIZE]; 607 float y[HISTORY_SIZE]; 608 float w[HISTORY_SIZE]; 609 float time[HISTORY_SIZE]; 610 uint32_t m = 0; 611 uint32_t index = mIndex; 612 const Movement& newestMovement = mMovements[mIndex]; 613 do { 614 const Movement& movement = mMovements[index]; 615 if (!movement.idBits.hasBit(id)) { 616 break; 617 } 618 619 nsecs_t age = newestMovement.eventTime - movement.eventTime; 620 if (age > HORIZON) { 621 break; 622 } 623 624 const VelocityTracker::Position& position = movement.getPosition(id); 625 x[m] = position.x; 626 y[m] = position.y; 627 w[m] = chooseWeight(index); 628 time[m] = -age * 0.000000001f; 629 index = (index == 0 ? HISTORY_SIZE : index) - 1; 630 } while (++m < HISTORY_SIZE); 631 632 if (m == 0) { 633 return false; // no data 634 } 635 636 // Calculate a least squares polynomial fit. 637 uint32_t degree = mDegree; 638 if (degree > m - 1) { 639 degree = m - 1; 640 } 641 if (degree >= 1) { 642 if (degree == 2 && mWeighting == WEIGHTING_NONE) { // optimize unweighted, degree=2 fit 643 outEstimator->time = newestMovement.eventTime; 644 outEstimator->degree = 2; 645 outEstimator->confidence = 1; 646 outEstimator->xCoeff[0] = 0; // only slope is calculated, set rest of coefficients = 0 647 outEstimator->yCoeff[0] = 0; 648 outEstimator->xCoeff[1] = solveUnweightedLeastSquaresDeg2(time, x, m); 649 outEstimator->yCoeff[1] = solveUnweightedLeastSquaresDeg2(time, y, m); 650 outEstimator->xCoeff[2] = 0; 651 outEstimator->yCoeff[2] = 0; 652 return true; 653 } 654 655 float xdet, ydet; 656 uint32_t n = degree + 1; 657 if (solveLeastSquares(time, x, w, m, n, outEstimator->xCoeff, &xdet) 658 && solveLeastSquares(time, y, w, m, n, outEstimator->yCoeff, &ydet)) { 659 outEstimator->time = newestMovement.eventTime; 660 outEstimator->degree = degree; 661 outEstimator->confidence = xdet * ydet; 662#if DEBUG_STRATEGY 663 ALOGD("estimate: degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f", 664 int(outEstimator->degree), 665 vectorToString(outEstimator->xCoeff, n).c_str(), 666 vectorToString(outEstimator->yCoeff, n).c_str(), 667 outEstimator->confidence); 668#endif 669 return true; 670 } 671 } 672 673 // No velocity data available for this pointer, but we do have its current position. 674 outEstimator->xCoeff[0] = x[0]; 675 outEstimator->yCoeff[0] = y[0]; 676 outEstimator->time = newestMovement.eventTime; 677 outEstimator->degree = 0; 678 outEstimator->confidence = 1; 679 return true; 680} 681 682float LeastSquaresVelocityTrackerStrategy::chooseWeight(uint32_t index) const { 683 switch (mWeighting) { 684 case WEIGHTING_DELTA: { 685 // Weight points based on how much time elapsed between them and the next 686 // point so that points that "cover" a shorter time span are weighed less. 687 // delta 0ms: 0.5 688 // delta 10ms: 1.0 689 if (index == mIndex) { 690 return 1.0f; 691 } 692 uint32_t nextIndex = (index + 1) % HISTORY_SIZE; 693 float deltaMillis = (mMovements[nextIndex].eventTime- mMovements[index].eventTime) 694 * 0.000001f; 695 if (deltaMillis < 0) { 696 return 0.5f; 697 } 698 if (deltaMillis < 10) { 699 return 0.5f + deltaMillis * 0.05; 700 } 701 return 1.0f; 702 } 703 704 case WEIGHTING_CENTRAL: { 705 // Weight points based on their age, weighing very recent and very old points less. 706 // age 0ms: 0.5 707 // age 10ms: 1.0 708 // age 50ms: 1.0 709 // age 60ms: 0.5 710 float ageMillis = (mMovements[mIndex].eventTime - mMovements[index].eventTime) 711 * 0.000001f; 712 if (ageMillis < 0) { 713 return 0.5f; 714 } 715 if (ageMillis < 10) { 716 return 0.5f + ageMillis * 0.05; 717 } 718 if (ageMillis < 50) { 719 return 1.0f; 720 } 721 if (ageMillis < 60) { 722 return 0.5f + (60 - ageMillis) * 0.05; 723 } 724 return 0.5f; 725 } 726 727 case WEIGHTING_RECENT: { 728 // Weight points based on their age, weighing older points less. 729 // age 0ms: 1.0 730 // age 50ms: 1.0 731 // age 100ms: 0.5 732 float ageMillis = (mMovements[mIndex].eventTime - mMovements[index].eventTime) 733 * 0.000001f; 734 if (ageMillis < 50) { 735 return 1.0f; 736 } 737 if (ageMillis < 100) { 738 return 0.5f + (100 - ageMillis) * 0.01f; 739 } 740 return 0.5f; 741 } 742 743 case WEIGHTING_NONE: 744 default: 745 return 1.0f; 746 } 747} 748 749 750// --- IntegratingVelocityTrackerStrategy --- 751 752IntegratingVelocityTrackerStrategy::IntegratingVelocityTrackerStrategy(uint32_t degree) : 753 mDegree(degree) { 754} 755 756IntegratingVelocityTrackerStrategy::~IntegratingVelocityTrackerStrategy() { 757} 758 759void IntegratingVelocityTrackerStrategy::clear() { 760 mPointerIdBits.clear(); 761} 762 763void IntegratingVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { 764 mPointerIdBits.value &= ~idBits.value; 765} 766 767void IntegratingVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, 768 const VelocityTracker::Position* positions) { 769 uint32_t index = 0; 770 for (BitSet32 iterIdBits(idBits); !iterIdBits.isEmpty();) { 771 uint32_t id = iterIdBits.clearFirstMarkedBit(); 772 State& state = mPointerState[id]; 773 const VelocityTracker::Position& position = positions[index++]; 774 if (mPointerIdBits.hasBit(id)) { 775 updateState(state, eventTime, position.x, position.y); 776 } else { 777 initState(state, eventTime, position.x, position.y); 778 } 779 } 780 781 mPointerIdBits = idBits; 782} 783 784bool IntegratingVelocityTrackerStrategy::getEstimator(uint32_t id, 785 VelocityTracker::Estimator* outEstimator) const { 786 outEstimator->clear(); 787 788 if (mPointerIdBits.hasBit(id)) { 789 const State& state = mPointerState[id]; 790 populateEstimator(state, outEstimator); 791 return true; 792 } 793 794 return false; 795} 796 797void IntegratingVelocityTrackerStrategy::initState(State& state, 798 nsecs_t eventTime, float xpos, float ypos) const { 799 state.updateTime = eventTime; 800 state.degree = 0; 801 802 state.xpos = xpos; 803 state.xvel = 0; 804 state.xaccel = 0; 805 state.ypos = ypos; 806 state.yvel = 0; 807 state.yaccel = 0; 808} 809 810void IntegratingVelocityTrackerStrategy::updateState(State& state, 811 nsecs_t eventTime, float xpos, float ypos) const { 812 const nsecs_t MIN_TIME_DELTA = 2 * NANOS_PER_MS; 813 const float FILTER_TIME_CONSTANT = 0.010f; // 10 milliseconds 814 815 if (eventTime <= state.updateTime + MIN_TIME_DELTA) { 816 return; 817 } 818 819 float dt = (eventTime - state.updateTime) * 0.000000001f; 820 state.updateTime = eventTime; 821 822 float xvel = (xpos - state.xpos) / dt; 823 float yvel = (ypos - state.ypos) / dt; 824 if (state.degree == 0) { 825 state.xvel = xvel; 826 state.yvel = yvel; 827 state.degree = 1; 828 } else { 829 float alpha = dt / (FILTER_TIME_CONSTANT + dt); 830 if (mDegree == 1) { 831 state.xvel += (xvel - state.xvel) * alpha; 832 state.yvel += (yvel - state.yvel) * alpha; 833 } else { 834 float xaccel = (xvel - state.xvel) / dt; 835 float yaccel = (yvel - state.yvel) / dt; 836 if (state.degree == 1) { 837 state.xaccel = xaccel; 838 state.yaccel = yaccel; 839 state.degree = 2; 840 } else { 841 state.xaccel += (xaccel - state.xaccel) * alpha; 842 state.yaccel += (yaccel - state.yaccel) * alpha; 843 } 844 state.xvel += (state.xaccel * dt) * alpha; 845 state.yvel += (state.yaccel * dt) * alpha; 846 } 847 } 848 state.xpos = xpos; 849 state.ypos = ypos; 850} 851 852void IntegratingVelocityTrackerStrategy::populateEstimator(const State& state, 853 VelocityTracker::Estimator* outEstimator) const { 854 outEstimator->time = state.updateTime; 855 outEstimator->confidence = 1.0f; 856 outEstimator->degree = state.degree; 857 outEstimator->xCoeff[0] = state.xpos; 858 outEstimator->xCoeff[1] = state.xvel; 859 outEstimator->xCoeff[2] = state.xaccel / 2; 860 outEstimator->yCoeff[0] = state.ypos; 861 outEstimator->yCoeff[1] = state.yvel; 862 outEstimator->yCoeff[2] = state.yaccel / 2; 863} 864 865 866// --- LegacyVelocityTrackerStrategy --- 867 868LegacyVelocityTrackerStrategy::LegacyVelocityTrackerStrategy() { 869 clear(); 870} 871 872LegacyVelocityTrackerStrategy::~LegacyVelocityTrackerStrategy() { 873} 874 875void LegacyVelocityTrackerStrategy::clear() { 876 mIndex = 0; 877 mMovements[0].idBits.clear(); 878} 879 880void LegacyVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { 881 BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value); 882 mMovements[mIndex].idBits = remainingIdBits; 883} 884 885void LegacyVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, 886 const VelocityTracker::Position* positions) { 887 if (++mIndex == HISTORY_SIZE) { 888 mIndex = 0; 889 } 890 891 Movement& movement = mMovements[mIndex]; 892 movement.eventTime = eventTime; 893 movement.idBits = idBits; 894 uint32_t count = idBits.count(); 895 for (uint32_t i = 0; i < count; i++) { 896 movement.positions[i] = positions[i]; 897 } 898} 899 900bool LegacyVelocityTrackerStrategy::getEstimator(uint32_t id, 901 VelocityTracker::Estimator* outEstimator) const { 902 outEstimator->clear(); 903 904 const Movement& newestMovement = mMovements[mIndex]; 905 if (!newestMovement.idBits.hasBit(id)) { 906 return false; // no data 907 } 908 909 // Find the oldest sample that contains the pointer and that is not older than HORIZON. 910 nsecs_t minTime = newestMovement.eventTime - HORIZON; 911 uint32_t oldestIndex = mIndex; 912 uint32_t numTouches = 1; 913 do { 914 uint32_t nextOldestIndex = (oldestIndex == 0 ? HISTORY_SIZE : oldestIndex) - 1; 915 const Movement& nextOldestMovement = mMovements[nextOldestIndex]; 916 if (!nextOldestMovement.idBits.hasBit(id) 917 || nextOldestMovement.eventTime < minTime) { 918 break; 919 } 920 oldestIndex = nextOldestIndex; 921 } while (++numTouches < HISTORY_SIZE); 922 923 // Calculate an exponentially weighted moving average of the velocity estimate 924 // at different points in time measured relative to the oldest sample. 925 // This is essentially an IIR filter. Newer samples are weighted more heavily 926 // than older samples. Samples at equal time points are weighted more or less 927 // equally. 928 // 929 // One tricky problem is that the sample data may be poorly conditioned. 930 // Sometimes samples arrive very close together in time which can cause us to 931 // overestimate the velocity at that time point. Most samples might be measured 932 // 16ms apart but some consecutive samples could be only 0.5sm apart because 933 // the hardware or driver reports them irregularly or in bursts. 934 float accumVx = 0; 935 float accumVy = 0; 936 uint32_t index = oldestIndex; 937 uint32_t samplesUsed = 0; 938 const Movement& oldestMovement = mMovements[oldestIndex]; 939 const VelocityTracker::Position& oldestPosition = oldestMovement.getPosition(id); 940 nsecs_t lastDuration = 0; 941 942 while (numTouches-- > 1) { 943 if (++index == HISTORY_SIZE) { 944 index = 0; 945 } 946 const Movement& movement = mMovements[index]; 947 nsecs_t duration = movement.eventTime - oldestMovement.eventTime; 948 949 // If the duration between samples is small, we may significantly overestimate 950 // the velocity. Consequently, we impose a minimum duration constraint on the 951 // samples that we include in the calculation. 952 if (duration >= MIN_DURATION) { 953 const VelocityTracker::Position& position = movement.getPosition(id); 954 float scale = 1000000000.0f / duration; // one over time delta in seconds 955 float vx = (position.x - oldestPosition.x) * scale; 956 float vy = (position.y - oldestPosition.y) * scale; 957 accumVx = (accumVx * lastDuration + vx * duration) / (duration + lastDuration); 958 accumVy = (accumVy * lastDuration + vy * duration) / (duration + lastDuration); 959 lastDuration = duration; 960 samplesUsed += 1; 961 } 962 } 963 964 // Report velocity. 965 const VelocityTracker::Position& newestPosition = newestMovement.getPosition(id); 966 outEstimator->time = newestMovement.eventTime; 967 outEstimator->confidence = 1; 968 outEstimator->xCoeff[0] = newestPosition.x; 969 outEstimator->yCoeff[0] = newestPosition.y; 970 if (samplesUsed) { 971 outEstimator->xCoeff[1] = accumVx; 972 outEstimator->yCoeff[1] = accumVy; 973 outEstimator->degree = 1; 974 } else { 975 outEstimator->degree = 0; 976 } 977 return true; 978} 979 980// --- ImpulseVelocityTrackerStrategy --- 981 982ImpulseVelocityTrackerStrategy::ImpulseVelocityTrackerStrategy() { 983 clear(); 984} 985 986ImpulseVelocityTrackerStrategy::~ImpulseVelocityTrackerStrategy() { 987} 988 989void ImpulseVelocityTrackerStrategy::clear() { 990 mIndex = 0; 991 mMovements[0].idBits.clear(); 992} 993 994void ImpulseVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { 995 BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value); 996 mMovements[mIndex].idBits = remainingIdBits; 997} 998 999void ImpulseVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, 1000 const VelocityTracker::Position* positions) { 1001 if (++mIndex == HISTORY_SIZE) { 1002 mIndex = 0; 1003 } 1004 1005 Movement& movement = mMovements[mIndex]; 1006 movement.eventTime = eventTime; 1007 movement.idBits = idBits; 1008 uint32_t count = idBits.count(); 1009 for (uint32_t i = 0; i < count; i++) { 1010 movement.positions[i] = positions[i]; 1011 } 1012} 1013 1014/** 1015 * Calculate the total impulse provided to the screen and the resulting velocity. 1016 * 1017 * The touchscreen is modeled as a physical object. 1018 * Initial condition is discussed below, but for now suppose that v(t=0) = 0 1019 * 1020 * The kinetic energy of the object at the release is E=0.5*m*v^2 1021 * Then vfinal = sqrt(2E/m). The goal is to calculate E. 1022 * 1023 * The kinetic energy at the release is equal to the total work done on the object by the finger. 1024 * The total work W is the sum of all dW along the path. 1025 * 1026 * dW = F*dx, where dx is the piece of path traveled. 1027 * Force is change of momentum over time, F = dp/dt = m dv/dt. 1028 * Then substituting: 1029 * dW = m (dv/dt) * dx = m * v * dv 1030 * 1031 * Summing along the path, we get: 1032 * W = sum(dW) = sum(m * v * dv) = m * sum(v * dv) 1033 * Since the mass stays constant, the equation for final velocity is: 1034 * vfinal = sqrt(2*sum(v * dv)) 1035 * 1036 * Here, 1037 * dv : change of velocity = (v[i+1]-v[i]) 1038 * dx : change of distance = (x[i+1]-x[i]) 1039 * dt : change of time = (t[i+1]-t[i]) 1040 * v : instantaneous velocity = dx/dt 1041 * 1042 * The final formula is: 1043 * vfinal = sqrt(2) * sqrt(sum((v[i]-v[i-1])*|v[i]|)) for all i 1044 * The absolute value is needed to properly account for the sign. If the velocity over a 1045 * particular segment descreases, then this indicates braking, which means that negative 1046 * work was done. So for two positive, but decreasing, velocities, this contribution would be 1047 * negative and will cause a smaller final velocity. 1048 * 1049 * Initial condition 1050 * There are two ways to deal with initial condition: 1051 * 1) Assume that v(0) = 0, which would mean that the screen is initially at rest. 1052 * This is not entirely accurate. We are only taking the past X ms of touch data, where X is 1053 * currently equal to 100. However, a touch event that created a fling probably lasted for longer 1054 * than that, which would mean that the user has already been interacting with the touchscreen 1055 * and it has probably already been moving. 1056 * 2) Assume that the touchscreen has already been moving at a certain velocity, calculate this 1057 * initial velocity and the equivalent energy, and start with this initial energy. 1058 * Consider an example where we have the following data, consisting of 3 points: 1059 * time: t0, t1, t2 1060 * x : x0, x1, x2 1061 * v : 0 , v1, v2 1062 * Here is what will happen in each of these scenarios: 1063 * 1) By directly applying the formula above with the v(0) = 0 boundary condition, we will get 1064 * vfinal = sqrt(2*(|v1|*(v1-v0) + |v2|*(v2-v1))). This can be simplified since v0=0 1065 * vfinal = sqrt(2*(|v1|*v1 + |v2|*(v2-v1))) = sqrt(2*(v1^2 + |v2|*(v2 - v1))) 1066 * since velocity is a real number 1067 * 2) If we treat the screen as already moving, then it must already have an energy (per mass) 1068 * equal to 1/2*v1^2. Then the initial energy should be 1/2*v1*2, and only the second segment 1069 * will contribute to the total kinetic energy (since we can effectively consider that v0=v1). 1070 * This will give the following expression for the final velocity: 1071 * vfinal = sqrt(2*(1/2*v1^2 + |v2|*(v2-v1))) 1072 * This analysis can be generalized to an arbitrary number of samples. 1073 * 1074 * 1075 * Comparing the two equations above, we see that the only mathematical difference 1076 * is the factor of 1/2 in front of the first velocity term. 1077 * This boundary condition would allow for the "proper" calculation of the case when all of the 1078 * samples are equally spaced in time and distance, which should suggest a constant velocity. 1079 * 1080 * Note that approach 2) is sensitive to the proper ordering of the data in time, since 1081 * the boundary condition must be applied to the oldest sample to be accurate. 1082 */ 1083static float calculateImpulseVelocity(const nsecs_t* t, const float* x, size_t count) { 1084 // The input should be in reversed time order (most recent sample at index i=0) 1085 // t[i] is in nanoseconds, but due to FP arithmetic, convert to seconds inside this function 1086 static constexpr float NANOS_PER_SECOND = 1E-9; 1087 static constexpr float sqrt2 = 1.41421356237; 1088 1089 if (count < 2) { 1090 return 0; // if 0 or 1 points, velocity is zero 1091 } 1092 if (t[1] > t[0]) { // Algorithm will still work, but not perfectly 1093 ALOGE("Samples provided to calculateImpulseVelocity in the wrong order"); 1094 } 1095 if (count == 2) { // if 2 points, basic linear calculation 1096 if (t[1] == t[0]) { 1097 ALOGE("Events have identical time stamps t=%" PRId64 ", setting velocity = 0", t[0]); 1098 return 0; 1099 } 1100 return (x[1] - x[0]) / (NANOS_PER_SECOND * (t[1] - t[0])); 1101 } 1102 // Guaranteed to have at least 3 points here 1103 float work = 0; 1104 float vprev, vcurr; // v[i-1] and v[i], respectively 1105 vprev = 0; 1106 for (size_t i = count - 1; i > 0 ; i--) { // start with the oldest sample and go forward in time 1107 if (t[i] == t[i-1]) { 1108 ALOGE("Events have identical time stamps t=%" PRId64 ", skipping sample", t[i]); 1109 continue; 1110 } 1111 vcurr = (x[i] - x[i-1]) / (NANOS_PER_SECOND * (t[i] - t[i-1])); 1112 work += (vcurr - vprev) * fabsf(vcurr); 1113 if (i == count - 1) { 1114 work *= 0.5; // initial condition, case 2) above 1115 } 1116 vprev = vcurr; 1117 } 1118 return (work < 0 ? -1.0 : 1.0) * sqrtf(fabsf(work)) * sqrt2; 1119} 1120 1121bool ImpulseVelocityTrackerStrategy::getEstimator(uint32_t id, 1122 VelocityTracker::Estimator* outEstimator) const { 1123 outEstimator->clear(); 1124 1125 // Iterate over movement samples in reverse time order and collect samples. 1126 float x[HISTORY_SIZE]; 1127 float y[HISTORY_SIZE]; 1128 nsecs_t time[HISTORY_SIZE]; 1129 size_t m = 0; // number of points that will be used for fitting 1130 size_t index = mIndex; 1131 const Movement& newestMovement = mMovements[mIndex]; 1132 do { 1133 const Movement& movement = mMovements[index]; 1134 if (!movement.idBits.hasBit(id)) { 1135 break; 1136 } 1137 1138 nsecs_t age = newestMovement.eventTime - movement.eventTime; 1139 if (age > HORIZON) { 1140 break; 1141 } 1142 1143 const VelocityTracker::Position& position = movement.getPosition(id); 1144 x[m] = position.x; 1145 y[m] = position.y; 1146 time[m] = movement.eventTime; 1147 index = (index == 0 ? HISTORY_SIZE : index) - 1; 1148 } while (++m < HISTORY_SIZE); 1149 1150 if (m == 0) { 1151 return false; // no data 1152 } 1153 outEstimator->xCoeff[0] = 0; 1154 outEstimator->yCoeff[0] = 0; 1155 outEstimator->xCoeff[1] = calculateImpulseVelocity(time, x, m); 1156 outEstimator->yCoeff[1] = calculateImpulseVelocity(time, y, m); 1157 outEstimator->xCoeff[2] = 0; 1158 outEstimator->yCoeff[2] = 0; 1159 outEstimator->time = newestMovement.eventTime; 1160 outEstimator->degree = 2; // similar results to 2nd degree fit 1161 outEstimator->confidence = 1; 1162#if DEBUG_STRATEGY 1163 ALOGD("velocity: (%f, %f)", outEstimator->xCoeff[1], outEstimator->yCoeff[1]); 1164#endif 1165 return true; 1166} 1167 1168} // namespace android 1169