widget/gonk/libui/VelocityTracker.cpp@fc2d59ddac77
widget/gonk/libui/VelocityTracker.cpp
Wed, 31 Dec 2014 07:22:50 +0100
- author
- Michael Schloh von Bennewitz <michael@schloh.com>
- date
- Wed, 31 Dec 2014 07:22:50 +0100
- branch
- TOR_BUG_3246
- changeset 4
- fc2d59ddac77
- permissions
- -rw-r--r--
Correct previous dual key logic pending first delivery installment.
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 */
17 #define LOG_TAG "VelocityTracker"
18 //#define LOG_NDEBUG 0
19 #include "cutils_log.h"
21 // Log debug messages about velocity tracking.
22 #define DEBUG_VELOCITY 0
24 // Log debug messages about the progress of the algorithm itself.
25 #define DEBUG_STRATEGY 0
27 #include <math.h>
28 #include <limits.h>
30 #include "VelocityTracker.h"
31 #include <utils/BitSet.h>
32 #include <utils/String8.h>
33 #include <utils/Timers.h>
35 #include <cutils/properties.h>
37 namespace android {
39 // Nanoseconds per milliseconds.
40 static const nsecs_t NANOS_PER_MS = 1000000;
42 // Threshold for determining that a pointer has stopped moving.
43 // Some input devices do not send ACTION_MOVE events in the case where a pointer has
44 // stopped. We need to detect this case so that we can accurately predict the
45 // velocity after the pointer starts moving again.
46 static const nsecs_t ASSUME_POINTER_STOPPED_TIME = 40 * NANOS_PER_MS;
49 static float vectorDot(const float* a, const float* b, uint32_t m) {
50 float r = 0;
51 while (m--) {
52 r += *(a++) * *(b++);
53 }
54 return r;
55 }
57 static float vectorNorm(const float* a, uint32_t m) {
58 float r = 0;
59 while (m--) {
60 float t = *(a++);
61 r += t * t;
62 }
63 return sqrtf(r);
64 }
66 #if DEBUG_STRATEGY || DEBUG_VELOCITY
67 static 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 }
80 static 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
102 // --- VelocityTracker ---
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.
109 const char* VelocityTracker::DEFAULT_STRATEGY = "lsq2";
111 VelocityTracker::VelocityTracker(const char* strategy) :
112 mLastEventTime(0), mCurrentPointerIdBits(0), mActivePointerId(-1) {
113 char value[PROPERTY_VALUE_MAX];
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 }
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 }
135 VelocityTracker::~VelocityTracker() {
136 delete mStrategy;
137 }
139 bool VelocityTracker::configureStrategy(const char* strategy) {
140 mStrategy = createStrategy(strategy);
141 return mStrategy != NULL;
142 }
144 VelocityTrackerStrategy* 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 }
204 void VelocityTracker::clear() {
205 mCurrentPointerIdBits.clear();
206 mActivePointerId = -1;
208 mStrategy->clear();
209 }
211 void VelocityTracker::clearPointers(BitSet32 idBits) {
212 BitSet32 remainingIdBits(mCurrentPointerIdBits.value & ~idBits.value);
213 mCurrentPointerIdBits = remainingIdBits;
215 if (mActivePointerId >= 0 && idBits.hasBit(mActivePointerId)) {
216 mActivePointerId = !remainingIdBits.isEmpty() ? remainingIdBits.firstMarkedBit() : -1;
217 }
219 mStrategy->clearPointers(idBits);
220 }
222 void VelocityTracker::addMovement(nsecs_t eventTime, BitSet32 idBits, const Position* positions) {
223 while (idBits.count() > MAX_POINTERS) {
224 idBits.clearLastMarkedBit();
225 }
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;
239 mCurrentPointerIdBits = idBits;
240 if (mActivePointerId < 0 || !idBits.hasBit(mActivePointerId)) {
241 mActivePointerId = idBits.isEmpty() ? -1 : idBits.firstMarkedBit();
242 }
244 mStrategy->addMovement(eventTime, idBits, positions);
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 }
266 void VelocityTracker::addMovement(const MotionEvent* event) {
267 int32_t actionMasked = event->getActionMasked();
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 }
299 size_t pointerCount = event->getPointerCount();
300 if (pointerCount > MAX_POINTERS) {
301 pointerCount = MAX_POINTERS;
302 }
304 BitSet32 idBits;
305 for (size_t i = 0; i < pointerCount; i++) {
306 idBits.markBit(event->getPointerId(i));
307 }
309 uint32_t pointerIndex[MAX_POINTERS];
310 for (size_t i = 0; i < pointerCount; i++) {
311 pointerIndex[i] = idBits.getIndexOfBit(event->getPointerId(i));
312 }
314 nsecs_t eventTime;
315 Position positions[pointerCount];
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 }
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 }
337 bool 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 }
349 bool VelocityTracker::getEstimator(uint32_t id, Estimator* outEstimator) const {
350 return mStrategy->getEstimator(id, outEstimator);
351 }
354 // --- LeastSquaresVelocityTrackerStrategy ---
356 const nsecs_t LeastSquaresVelocityTrackerStrategy::HORIZON;
357 const uint32_t LeastSquaresVelocityTrackerStrategy::HISTORY_SIZE;
359 LeastSquaresVelocityTrackerStrategy::LeastSquaresVelocityTrackerStrategy(
360 uint32_t degree, Weighting weighting) :
361 mDegree(degree), mWeighting(weighting) {
362 clear();
363 }
365 LeastSquaresVelocityTrackerStrategy::~LeastSquaresVelocityTrackerStrategy() {
366 }
368 void LeastSquaresVelocityTrackerStrategy::clear() {
369 mIndex = 0;
370 mMovements[0].idBits.clear();
371 }
373 void LeastSquaresVelocityTrackerStrategy::clearPointers(BitSet32 idBits) {
374 BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value);
375 mMovements[mIndex].idBits = remainingIdBits;
376 }
378 void LeastSquaresVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits,
379 const VelocityTracker::Position* positions) {
380 if (++mIndex == HISTORY_SIZE) {
381 mIndex = 0;
382 }
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 }
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 */
442 static 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
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
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 }
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 }
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());
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
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 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
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;
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 }
559 bool LeastSquaresVelocityTrackerStrategy::getEstimator(uint32_t id,
560 VelocityTracker::Estimator* outEstimator) const {
561 outEstimator->clear();
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 }
577 nsecs_t age = newestMovement.eventTime - movement.eventTime;
578 if (age > HORIZON) {
579 break;
580 }
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);
590 if (m == 0) {
591 return false; // no data
592 }
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 }
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 }
627 float 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 }
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 }
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 }
688 case WEIGHTING_NONE:
689 default:
690 return 1.0f;
691 }
692 }
695 // --- IntegratingVelocityTrackerStrategy ---
697 IntegratingVelocityTrackerStrategy::IntegratingVelocityTrackerStrategy(uint32_t degree) :
698 mDegree(degree) {
699 }
701 IntegratingVelocityTrackerStrategy::~IntegratingVelocityTrackerStrategy() {
702 }
704 void IntegratingVelocityTrackerStrategy::clear() {
705 mPointerIdBits.clear();
706 }
708 void IntegratingVelocityTrackerStrategy::clearPointers(BitSet32 idBits) {
709 mPointerIdBits.value &= ~idBits.value;
710 }
712 void 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 }
726 mPointerIdBits = idBits;
727 }
729 bool IntegratingVelocityTrackerStrategy::getEstimator(uint32_t id,
730 VelocityTracker::Estimator* outEstimator) const {
731 outEstimator->clear();
733 if (mPointerIdBits.hasBit(id)) {
734 const State& state = mPointerState[id];
735 populateEstimator(state, outEstimator);
736 return true;
737 }
739 return false;
740 }
742 void IntegratingVelocityTrackerStrategy::initState(State& state,
743 nsecs_t eventTime, float xpos, float ypos) const {
744 state.updateTime = eventTime;
745 state.degree = 0;
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 }
755 void 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
760 if (eventTime <= state.updateTime + MIN_TIME_DELTA) {
761 return;
762 }
764 float dt = (eventTime - state.updateTime) * 0.000000001f;
765 state.updateTime = eventTime;
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 }
797 void 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 }
811 // --- LegacyVelocityTrackerStrategy ---
813 const nsecs_t LegacyVelocityTrackerStrategy::HORIZON;
814 const uint32_t LegacyVelocityTrackerStrategy::HISTORY_SIZE;
815 const nsecs_t LegacyVelocityTrackerStrategy::MIN_DURATION;
817 LegacyVelocityTrackerStrategy::LegacyVelocityTrackerStrategy() {
818 clear();
819 }
821 LegacyVelocityTrackerStrategy::~LegacyVelocityTrackerStrategy() {
822 }
824 void LegacyVelocityTrackerStrategy::clear() {
825 mIndex = 0;
826 mMovements[0].idBits.clear();
827 }
829 void LegacyVelocityTrackerStrategy::clearPointers(BitSet32 idBits) {
830 BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value);
831 mMovements[mIndex].idBits = remainingIdBits;
832 }
834 void LegacyVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits,
835 const VelocityTracker::Position* positions) {
836 if (++mIndex == HISTORY_SIZE) {
837 mIndex = 0;
838 }
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 }
849 bool LegacyVelocityTrackerStrategy::getEstimator(uint32_t id,
850 VelocityTracker::Estimator* outEstimator) const {
851 outEstimator->clear();
853 const Movement& newestMovement = mMovements[mIndex];
854 if (!newestMovement.idBits.hasBit(id)) {
855 return false; // no data
856 }
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);
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;
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;
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 }
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 }
929 } // namespace android
