VelocityTracker.cpp revision 7b9d189574fdf530df9c2e30e4fd799c9a25e6b4
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 <cutils/properties.h> 31#include <input/VelocityTracker.h> 32#include <utils/BitSet.h> 33#include <utils/String8.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 String8 vectorToString(const float* a, uint32_t m) { 67 String8 str; 68 str.append("["); 69 for (size_t i = 0; i < m; i++) { 70 if (i) { 71 str.append(","); 72 } 73 str.appendFormat(" %f", *(a++)); 74 } 75 str.append(" ]"); 76 return str; 77} 78 79static 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. 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("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 203void VelocityTracker::clear() { 204 mCurrentPointerIdBits.clear(); 205 mActivePointerId = -1; 206 207 mStrategy->clear(); 208} 209 210void 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 221void 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=%" PRId64 ", 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 265void 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 336bool 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 348bool VelocityTracker::getEstimator(uint32_t id, Estimator* outEstimator) const { 349 return mStrategy->getEstimator(id, outEstimator); 350} 351 352 353// --- LeastSquaresVelocityTrackerStrategy --- 354 355const nsecs_t LeastSquaresVelocityTrackerStrategy::HORIZON; 356const uint32_t LeastSquaresVelocityTrackerStrategy::HISTORY_SIZE; 357 358LeastSquaresVelocityTrackerStrategy::LeastSquaresVelocityTrackerStrategy( 359 uint32_t degree, Weighting weighting) : 360 mDegree(degree), mWeighting(weighting) { 361 clear(); 362} 363 364LeastSquaresVelocityTrackerStrategy::~LeastSquaresVelocityTrackerStrategy() { 365} 366 367void LeastSquaresVelocityTrackerStrategy::clear() { 368 mIndex = 0; 369 mMovements[0].idBits.clear(); 370} 371 372void LeastSquaresVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { 373 BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value); 374 mMovements[mIndex].idBits = remainingIdBits; 375} 376 377void 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 */ 441static 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 i--; 517 outB[i] = vectorDot(&q[i][0], wy, m); 518 for (uint32_t j = n - 1; j > i; j--) { 519 outB[i] -= r[i][j] * outB[j]; 520 } 521 outB[i] /= r[i][i]; 522 } 523#if DEBUG_STRATEGY 524 ALOGD(" - b=%s", vectorToString(outB, n).string()); 525#endif 526 527 // Calculate the coefficient of determination as 1 - (SSerr / SStot) where 528 // SSerr is the residual sum of squares (variance of the error), 529 // and SStot is the total sum of squares (variance of the data) where each 530 // has been weighted. 531 float ymean = 0; 532 for (uint32_t h = 0; h < m; h++) { 533 ymean += y[h]; 534 } 535 ymean /= m; 536 537 float sserr = 0; 538 float sstot = 0; 539 for (uint32_t h = 0; h < m; h++) { 540 float err = y[h] - outB[0]; 541 float term = 1; 542 for (uint32_t i = 1; i < n; i++) { 543 term *= x[h]; 544 err -= term * outB[i]; 545 } 546 sserr += w[h] * w[h] * err * err; 547 float var = y[h] - ymean; 548 sstot += w[h] * w[h] * var * var; 549 } 550 *outDet = sstot > 0.000001f ? 1.0f - (sserr / sstot) : 1; 551#if DEBUG_STRATEGY 552 ALOGD(" - sserr=%f", sserr); 553 ALOGD(" - sstot=%f", sstot); 554 ALOGD(" - det=%f", *outDet); 555#endif 556 return true; 557} 558 559bool LeastSquaresVelocityTrackerStrategy::getEstimator(uint32_t id, 560 VelocityTracker::Estimator* outEstimator) const { 561 outEstimator->clear(); 562 563 // Iterate over movement samples in reverse time order and collect samples. 564 float x[HISTORY_SIZE]; 565 float y[HISTORY_SIZE]; 566 float w[HISTORY_SIZE]; 567 float time[HISTORY_SIZE]; 568 uint32_t m = 0; 569 uint32_t index = mIndex; 570 const Movement& newestMovement = mMovements[mIndex]; 571 do { 572 const Movement& movement = mMovements[index]; 573 if (!movement.idBits.hasBit(id)) { 574 break; 575 } 576 577 nsecs_t age = newestMovement.eventTime - movement.eventTime; 578 if (age > HORIZON) { 579 break; 580 } 581 582 const VelocityTracker::Position& position = movement.getPosition(id); 583 x[m] = position.x; 584 y[m] = position.y; 585 w[m] = chooseWeight(index); 586 time[m] = -age * 0.000000001f; 587 index = (index == 0 ? HISTORY_SIZE : index) - 1; 588 } while (++m < HISTORY_SIZE); 589 590 if (m == 0) { 591 return false; // no data 592 } 593 594 // Calculate a least squares polynomial fit. 595 uint32_t degree = mDegree; 596 if (degree > m - 1) { 597 degree = m - 1; 598 } 599 if (degree >= 1) { 600 float xdet, ydet; 601 uint32_t n = degree + 1; 602 if (solveLeastSquares(time, x, w, m, n, outEstimator->xCoeff, &xdet) 603 && solveLeastSquares(time, y, w, m, n, outEstimator->yCoeff, &ydet)) { 604 outEstimator->time = newestMovement.eventTime; 605 outEstimator->degree = degree; 606 outEstimator->confidence = xdet * ydet; 607#if DEBUG_STRATEGY 608 ALOGD("estimate: degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f", 609 int(outEstimator->degree), 610 vectorToString(outEstimator->xCoeff, n).string(), 611 vectorToString(outEstimator->yCoeff, n).string(), 612 outEstimator->confidence); 613#endif 614 return true; 615 } 616 } 617 618 // No velocity data available for this pointer, but we do have its current position. 619 outEstimator->xCoeff[0] = x[0]; 620 outEstimator->yCoeff[0] = y[0]; 621 outEstimator->time = newestMovement.eventTime; 622 outEstimator->degree = 0; 623 outEstimator->confidence = 1; 624 return true; 625} 626 627float LeastSquaresVelocityTrackerStrategy::chooseWeight(uint32_t index) const { 628 switch (mWeighting) { 629 case WEIGHTING_DELTA: { 630 // Weight points based on how much time elapsed between them and the next 631 // point so that points that "cover" a shorter time span are weighed less. 632 // delta 0ms: 0.5 633 // delta 10ms: 1.0 634 if (index == mIndex) { 635 return 1.0f; 636 } 637 uint32_t nextIndex = (index + 1) % HISTORY_SIZE; 638 float deltaMillis = (mMovements[nextIndex].eventTime- mMovements[index].eventTime) 639 * 0.000001f; 640 if (deltaMillis < 0) { 641 return 0.5f; 642 } 643 if (deltaMillis < 10) { 644 return 0.5f + deltaMillis * 0.05; 645 } 646 return 1.0f; 647 } 648 649 case WEIGHTING_CENTRAL: { 650 // Weight points based on their age, weighing very recent and very old points less. 651 // age 0ms: 0.5 652 // age 10ms: 1.0 653 // age 50ms: 1.0 654 // age 60ms: 0.5 655 float ageMillis = (mMovements[mIndex].eventTime - mMovements[index].eventTime) 656 * 0.000001f; 657 if (ageMillis < 0) { 658 return 0.5f; 659 } 660 if (ageMillis < 10) { 661 return 0.5f + ageMillis * 0.05; 662 } 663 if (ageMillis < 50) { 664 return 1.0f; 665 } 666 if (ageMillis < 60) { 667 return 0.5f + (60 - ageMillis) * 0.05; 668 } 669 return 0.5f; 670 } 671 672 case WEIGHTING_RECENT: { 673 // Weight points based on their age, weighing older points less. 674 // age 0ms: 1.0 675 // age 50ms: 1.0 676 // age 100ms: 0.5 677 float ageMillis = (mMovements[mIndex].eventTime - mMovements[index].eventTime) 678 * 0.000001f; 679 if (ageMillis < 50) { 680 return 1.0f; 681 } 682 if (ageMillis < 100) { 683 return 0.5f + (100 - ageMillis) * 0.01f; 684 } 685 return 0.5f; 686 } 687 688 case WEIGHTING_NONE: 689 default: 690 return 1.0f; 691 } 692} 693 694 695// --- IntegratingVelocityTrackerStrategy --- 696 697IntegratingVelocityTrackerStrategy::IntegratingVelocityTrackerStrategy(uint32_t degree) : 698 mDegree(degree) { 699} 700 701IntegratingVelocityTrackerStrategy::~IntegratingVelocityTrackerStrategy() { 702} 703 704void IntegratingVelocityTrackerStrategy::clear() { 705 mPointerIdBits.clear(); 706} 707 708void IntegratingVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { 709 mPointerIdBits.value &= ~idBits.value; 710} 711 712void IntegratingVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, 713 const VelocityTracker::Position* positions) { 714 uint32_t index = 0; 715 for (BitSet32 iterIdBits(idBits); !iterIdBits.isEmpty();) { 716 uint32_t id = iterIdBits.clearFirstMarkedBit(); 717 State& state = mPointerState[id]; 718 const VelocityTracker::Position& position = positions[index++]; 719 if (mPointerIdBits.hasBit(id)) { 720 updateState(state, eventTime, position.x, position.y); 721 } else { 722 initState(state, eventTime, position.x, position.y); 723 } 724 } 725 726 mPointerIdBits = idBits; 727} 728 729bool IntegratingVelocityTrackerStrategy::getEstimator(uint32_t id, 730 VelocityTracker::Estimator* outEstimator) const { 731 outEstimator->clear(); 732 733 if (mPointerIdBits.hasBit(id)) { 734 const State& state = mPointerState[id]; 735 populateEstimator(state, outEstimator); 736 return true; 737 } 738 739 return false; 740} 741 742void IntegratingVelocityTrackerStrategy::initState(State& state, 743 nsecs_t eventTime, float xpos, float ypos) const { 744 state.updateTime = eventTime; 745 state.degree = 0; 746 747 state.xpos = xpos; 748 state.xvel = 0; 749 state.xaccel = 0; 750 state.ypos = ypos; 751 state.yvel = 0; 752 state.yaccel = 0; 753} 754 755void IntegratingVelocityTrackerStrategy::updateState(State& state, 756 nsecs_t eventTime, float xpos, float ypos) const { 757 const nsecs_t MIN_TIME_DELTA = 2 * NANOS_PER_MS; 758 const float FILTER_TIME_CONSTANT = 0.010f; // 10 milliseconds 759 760 if (eventTime <= state.updateTime + MIN_TIME_DELTA) { 761 return; 762 } 763 764 float dt = (eventTime - state.updateTime) * 0.000000001f; 765 state.updateTime = eventTime; 766 767 float xvel = (xpos - state.xpos) / dt; 768 float yvel = (ypos - state.ypos) / dt; 769 if (state.degree == 0) { 770 state.xvel = xvel; 771 state.yvel = yvel; 772 state.degree = 1; 773 } else { 774 float alpha = dt / (FILTER_TIME_CONSTANT + dt); 775 if (mDegree == 1) { 776 state.xvel += (xvel - state.xvel) * alpha; 777 state.yvel += (yvel - state.yvel) * alpha; 778 } else { 779 float xaccel = (xvel - state.xvel) / dt; 780 float yaccel = (yvel - state.yvel) / dt; 781 if (state.degree == 1) { 782 state.xaccel = xaccel; 783 state.yaccel = yaccel; 784 state.degree = 2; 785 } else { 786 state.xaccel += (xaccel - state.xaccel) * alpha; 787 state.yaccel += (yaccel - state.yaccel) * alpha; 788 } 789 state.xvel += (state.xaccel * dt) * alpha; 790 state.yvel += (state.yaccel * dt) * alpha; 791 } 792 } 793 state.xpos = xpos; 794 state.ypos = ypos; 795} 796 797void IntegratingVelocityTrackerStrategy::populateEstimator(const State& state, 798 VelocityTracker::Estimator* outEstimator) const { 799 outEstimator->time = state.updateTime; 800 outEstimator->confidence = 1.0f; 801 outEstimator->degree = state.degree; 802 outEstimator->xCoeff[0] = state.xpos; 803 outEstimator->xCoeff[1] = state.xvel; 804 outEstimator->xCoeff[2] = state.xaccel / 2; 805 outEstimator->yCoeff[0] = state.ypos; 806 outEstimator->yCoeff[1] = state.yvel; 807 outEstimator->yCoeff[2] = state.yaccel / 2; 808} 809 810 811// --- LegacyVelocityTrackerStrategy --- 812 813const nsecs_t LegacyVelocityTrackerStrategy::HORIZON; 814const uint32_t LegacyVelocityTrackerStrategy::HISTORY_SIZE; 815const nsecs_t LegacyVelocityTrackerStrategy::MIN_DURATION; 816 817LegacyVelocityTrackerStrategy::LegacyVelocityTrackerStrategy() { 818 clear(); 819} 820 821LegacyVelocityTrackerStrategy::~LegacyVelocityTrackerStrategy() { 822} 823 824void LegacyVelocityTrackerStrategy::clear() { 825 mIndex = 0; 826 mMovements[0].idBits.clear(); 827} 828 829void LegacyVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { 830 BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value); 831 mMovements[mIndex].idBits = remainingIdBits; 832} 833 834void LegacyVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, 835 const VelocityTracker::Position* positions) { 836 if (++mIndex == HISTORY_SIZE) { 837 mIndex = 0; 838 } 839 840 Movement& movement = mMovements[mIndex]; 841 movement.eventTime = eventTime; 842 movement.idBits = idBits; 843 uint32_t count = idBits.count(); 844 for (uint32_t i = 0; i < count; i++) { 845 movement.positions[i] = positions[i]; 846 } 847} 848 849bool LegacyVelocityTrackerStrategy::getEstimator(uint32_t id, 850 VelocityTracker::Estimator* outEstimator) const { 851 outEstimator->clear(); 852 853 const Movement& newestMovement = mMovements[mIndex]; 854 if (!newestMovement.idBits.hasBit(id)) { 855 return false; // no data 856 } 857 858 // Find the oldest sample that contains the pointer and that is not older than HORIZON. 859 nsecs_t minTime = newestMovement.eventTime - HORIZON; 860 uint32_t oldestIndex = mIndex; 861 uint32_t numTouches = 1; 862 do { 863 uint32_t nextOldestIndex = (oldestIndex == 0 ? HISTORY_SIZE : oldestIndex) - 1; 864 const Movement& nextOldestMovement = mMovements[nextOldestIndex]; 865 if (!nextOldestMovement.idBits.hasBit(id) 866 || nextOldestMovement.eventTime < minTime) { 867 break; 868 } 869 oldestIndex = nextOldestIndex; 870 } while (++numTouches < HISTORY_SIZE); 871 872 // Calculate an exponentially weighted moving average of the velocity estimate 873 // at different points in time measured relative to the oldest sample. 874 // This is essentially an IIR filter. Newer samples are weighted more heavily 875 // than older samples. Samples at equal time points are weighted more or less 876 // equally. 877 // 878 // One tricky problem is that the sample data may be poorly conditioned. 879 // Sometimes samples arrive very close together in time which can cause us to 880 // overestimate the velocity at that time point. Most samples might be measured 881 // 16ms apart but some consecutive samples could be only 0.5sm apart because 882 // the hardware or driver reports them irregularly or in bursts. 883 float accumVx = 0; 884 float accumVy = 0; 885 uint32_t index = oldestIndex; 886 uint32_t samplesUsed = 0; 887 const Movement& oldestMovement = mMovements[oldestIndex]; 888 const VelocityTracker::Position& oldestPosition = oldestMovement.getPosition(id); 889 nsecs_t lastDuration = 0; 890 891 while (numTouches-- > 1) { 892 if (++index == HISTORY_SIZE) { 893 index = 0; 894 } 895 const Movement& movement = mMovements[index]; 896 nsecs_t duration = movement.eventTime - oldestMovement.eventTime; 897 898 // If the duration between samples is small, we may significantly overestimate 899 // the velocity. Consequently, we impose a minimum duration constraint on the 900 // samples that we include in the calculation. 901 if (duration >= MIN_DURATION) { 902 const VelocityTracker::Position& position = movement.getPosition(id); 903 float scale = 1000000000.0f / duration; // one over time delta in seconds 904 float vx = (position.x - oldestPosition.x) * scale; 905 float vy = (position.y - oldestPosition.y) * scale; 906 accumVx = (accumVx * lastDuration + vx * duration) / (duration + lastDuration); 907 accumVy = (accumVy * lastDuration + vy * duration) / (duration + lastDuration); 908 lastDuration = duration; 909 samplesUsed += 1; 910 } 911 } 912 913 // Report velocity. 914 const VelocityTracker::Position& newestPosition = newestMovement.getPosition(id); 915 outEstimator->time = newestMovement.eventTime; 916 outEstimator->confidence = 1; 917 outEstimator->xCoeff[0] = newestPosition.x; 918 outEstimator->yCoeff[0] = newestPosition.y; 919 if (samplesUsed) { 920 outEstimator->xCoeff[1] = accumVx; 921 outEstimator->yCoeff[1] = accumVy; 922 outEstimator->degree = 1; 923 } else { 924 outEstimator->degree = 0; 925 } 926 return true; 927} 928 929} // namespace android 930