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