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