1.1 --- /dev/null Thu Jan 01 00:00:00 1970 +0000
1.2 +++ b/widget/gonk/libui/VelocityTracker.cpp Wed Dec 31 06:09:35 2014 +0100
1.3 @@ -0,0 +1,929 @@
1.4 +/*
1.5 + * Copyright (C) 2012 The Android Open Source Project
1.6 + *
1.7 + * Licensed under the Apache License, Version 2.0 (the "License");
1.8 + * you may not use this file except in compliance with the License.
1.9 + * You may obtain a copy of the License at
1.10 + *
1.11 + * http://www.apache.org/licenses/LICENSE-2.0
1.12 + *
1.13 + * Unless required by applicable law or agreed to in writing, software
1.14 + * distributed under the License is distributed on an "AS IS" BASIS,
1.15 + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
1.16 + * See the License for the specific language governing permissions and
1.17 + * limitations under the License.
1.18 + */
1.19 +
1.20 +#define LOG_TAG "VelocityTracker"
1.21 +//#define LOG_NDEBUG 0
1.22 +#include "cutils_log.h"
1.23 +
1.24 +// Log debug messages about velocity tracking.
1.25 +#define DEBUG_VELOCITY 0
1.26 +
1.27 +// Log debug messages about the progress of the algorithm itself.
1.28 +#define DEBUG_STRATEGY 0
1.29 +
1.30 +#include <math.h>
1.31 +#include <limits.h>
1.32 +
1.33 +#include "VelocityTracker.h"
1.34 +#include <utils/BitSet.h>
1.35 +#include <utils/String8.h>
1.36 +#include <utils/Timers.h>
1.37 +
1.38 +#include <cutils/properties.h>
1.39 +
1.40 +namespace android {
1.41 +
1.42 +// Nanoseconds per milliseconds.
1.43 +static const nsecs_t NANOS_PER_MS = 1000000;
1.44 +
1.45 +// Threshold for determining that a pointer has stopped moving.
1.46 +// Some input devices do not send ACTION_MOVE events in the case where a pointer has
1.47 +// stopped. We need to detect this case so that we can accurately predict the
1.48 +// velocity after the pointer starts moving again.
1.49 +static const nsecs_t ASSUME_POINTER_STOPPED_TIME = 40 * NANOS_PER_MS;
1.50 +
1.51 +
1.52 +static float vectorDot(const float* a, const float* b, uint32_t m) {
1.53 + float r = 0;
1.54 + while (m--) {
1.55 + r += *(a++) * *(b++);
1.56 + }
1.57 + return r;
1.58 +}
1.59 +
1.60 +static float vectorNorm(const float* a, uint32_t m) {
1.61 + float r = 0;
1.62 + while (m--) {
1.63 + float t = *(a++);
1.64 + r += t * t;
1.65 + }
1.66 + return sqrtf(r);
1.67 +}
1.68 +
1.69 +#if DEBUG_STRATEGY || DEBUG_VELOCITY
1.70 +static String8 vectorToString(const float* a, uint32_t m) {
1.71 + String8 str;
1.72 + str.append("[");
1.73 + while (m--) {
1.74 + str.appendFormat(" %f", *(a++));
1.75 + if (m) {
1.76 + str.append(",");
1.77 + }
1.78 + }
1.79 + str.append(" ]");
1.80 + return str;
1.81 +}
1.82 +
1.83 +static String8 matrixToString(const float* a, uint32_t m, uint32_t n, bool rowMajor) {
1.84 + String8 str;
1.85 + str.append("[");
1.86 + for (size_t i = 0; i < m; i++) {
1.87 + if (i) {
1.88 + str.append(",");
1.89 + }
1.90 + str.append(" [");
1.91 + for (size_t j = 0; j < n; j++) {
1.92 + if (j) {
1.93 + str.append(",");
1.94 + }
1.95 + str.appendFormat(" %f", a[rowMajor ? i * n + j : j * m + i]);
1.96 + }
1.97 + str.append(" ]");
1.98 + }
1.99 + str.append(" ]");
1.100 + return str;
1.101 +}
1.102 +#endif
1.103 +
1.104 +
1.105 +// --- VelocityTracker ---
1.106 +
1.107 +// The default velocity tracker strategy.
1.108 +// Although other strategies are available for testing and comparison purposes,
1.109 +// this is the strategy that applications will actually use. Be very careful
1.110 +// when adjusting the default strategy because it can dramatically affect
1.111 +// (often in a bad way) the user experience.
1.112 +const char* VelocityTracker::DEFAULT_STRATEGY = "lsq2";
1.113 +
1.114 +VelocityTracker::VelocityTracker(const char* strategy) :
1.115 + mLastEventTime(0), mCurrentPointerIdBits(0), mActivePointerId(-1) {
1.116 + char value[PROPERTY_VALUE_MAX];
1.117 +
1.118 + // Allow the default strategy to be overridden using a system property for debugging.
1.119 + if (!strategy) {
1.120 + int length = property_get("debug.velocitytracker.strategy", value, NULL);
1.121 + if (length > 0) {
1.122 + strategy = value;
1.123 + } else {
1.124 + strategy = DEFAULT_STRATEGY;
1.125 + }
1.126 + }
1.127 +
1.128 + // Configure the strategy.
1.129 + if (!configureStrategy(strategy)) {
1.130 + ALOGD("Unrecognized velocity tracker strategy name '%s'.", strategy);
1.131 + if (!configureStrategy(DEFAULT_STRATEGY)) {
1.132 + LOG_ALWAYS_FATAL("Could not create the default velocity tracker strategy '%s'!",
1.133 + strategy);
1.134 + }
1.135 + }
1.136 +}
1.137 +
1.138 +VelocityTracker::~VelocityTracker() {
1.139 + delete mStrategy;
1.140 +}
1.141 +
1.142 +bool VelocityTracker::configureStrategy(const char* strategy) {
1.143 + mStrategy = createStrategy(strategy);
1.144 + return mStrategy != NULL;
1.145 +}
1.146 +
1.147 +VelocityTrackerStrategy* VelocityTracker::createStrategy(const char* strategy) {
1.148 + if (!strcmp("lsq1", strategy)) {
1.149 + // 1st order least squares. Quality: POOR.
1.150 + // Frequently underfits the touch data especially when the finger accelerates
1.151 + // or changes direction. Often underestimates velocity. The direction
1.152 + // is overly influenced by historical touch points.
1.153 + return new LeastSquaresVelocityTrackerStrategy(1);
1.154 + }
1.155 + if (!strcmp("lsq2", strategy)) {
1.156 + // 2nd order least squares. Quality: VERY GOOD.
1.157 + // Pretty much ideal, but can be confused by certain kinds of touch data,
1.158 + // particularly if the panel has a tendency to generate delayed,
1.159 + // duplicate or jittery touch coordinates when the finger is released.
1.160 + return new LeastSquaresVelocityTrackerStrategy(2);
1.161 + }
1.162 + if (!strcmp("lsq3", strategy)) {
1.163 + // 3rd order least squares. Quality: UNUSABLE.
1.164 + // Frequently overfits the touch data yielding wildly divergent estimates
1.165 + // of the velocity when the finger is released.
1.166 + return new LeastSquaresVelocityTrackerStrategy(3);
1.167 + }
1.168 + if (!strcmp("wlsq2-delta", strategy)) {
1.169 + // 2nd order weighted least squares, delta weighting. Quality: EXPERIMENTAL
1.170 + return new LeastSquaresVelocityTrackerStrategy(2,
1.171 + LeastSquaresVelocityTrackerStrategy::WEIGHTING_DELTA);
1.172 + }
1.173 + if (!strcmp("wlsq2-central", strategy)) {
1.174 + // 2nd order weighted least squares, central weighting. Quality: EXPERIMENTAL
1.175 + return new LeastSquaresVelocityTrackerStrategy(2,
1.176 + LeastSquaresVelocityTrackerStrategy::WEIGHTING_CENTRAL);
1.177 + }
1.178 + if (!strcmp("wlsq2-recent", strategy)) {
1.179 + // 2nd order weighted least squares, recent weighting. Quality: EXPERIMENTAL
1.180 + return new LeastSquaresVelocityTrackerStrategy(2,
1.181 + LeastSquaresVelocityTrackerStrategy::WEIGHTING_RECENT);
1.182 + }
1.183 + if (!strcmp("int1", strategy)) {
1.184 + // 1st order integrating filter. Quality: GOOD.
1.185 + // Not as good as 'lsq2' because it cannot estimate acceleration but it is
1.186 + // more tolerant of errors. Like 'lsq1', this strategy tends to underestimate
1.187 + // the velocity of a fling but this strategy tends to respond to changes in
1.188 + // direction more quickly and accurately.
1.189 + return new IntegratingVelocityTrackerStrategy(1);
1.190 + }
1.191 + if (!strcmp("int2", strategy)) {
1.192 + // 2nd order integrating filter. Quality: EXPERIMENTAL.
1.193 + // For comparison purposes only. Unlike 'int1' this strategy can compensate
1.194 + // for acceleration but it typically overestimates the effect.
1.195 + return new IntegratingVelocityTrackerStrategy(2);
1.196 + }
1.197 + if (!strcmp("legacy", strategy)) {
1.198 + // Legacy velocity tracker algorithm. Quality: POOR.
1.199 + // For comparison purposes only. This algorithm is strongly influenced by
1.200 + // old data points, consistently underestimates velocity and takes a very long
1.201 + // time to adjust to changes in direction.
1.202 + return new LegacyVelocityTrackerStrategy();
1.203 + }
1.204 + return NULL;
1.205 +}
1.206 +
1.207 +void VelocityTracker::clear() {
1.208 + mCurrentPointerIdBits.clear();
1.209 + mActivePointerId = -1;
1.210 +
1.211 + mStrategy->clear();
1.212 +}
1.213 +
1.214 +void VelocityTracker::clearPointers(BitSet32 idBits) {
1.215 + BitSet32 remainingIdBits(mCurrentPointerIdBits.value & ~idBits.value);
1.216 + mCurrentPointerIdBits = remainingIdBits;
1.217 +
1.218 + if (mActivePointerId >= 0 && idBits.hasBit(mActivePointerId)) {
1.219 + mActivePointerId = !remainingIdBits.isEmpty() ? remainingIdBits.firstMarkedBit() : -1;
1.220 + }
1.221 +
1.222 + mStrategy->clearPointers(idBits);
1.223 +}
1.224 +
1.225 +void VelocityTracker::addMovement(nsecs_t eventTime, BitSet32 idBits, const Position* positions) {
1.226 + while (idBits.count() > MAX_POINTERS) {
1.227 + idBits.clearLastMarkedBit();
1.228 + }
1.229 +
1.230 + if ((mCurrentPointerIdBits.value & idBits.value)
1.231 + && eventTime >= mLastEventTime + ASSUME_POINTER_STOPPED_TIME) {
1.232 +#if DEBUG_VELOCITY
1.233 + ALOGD("VelocityTracker: stopped for %0.3f ms, clearing state.",
1.234 + (eventTime - mLastEventTime) * 0.000001f);
1.235 +#endif
1.236 + // We have not received any movements for too long. Assume that all pointers
1.237 + // have stopped.
1.238 + mStrategy->clear();
1.239 + }
1.240 + mLastEventTime = eventTime;
1.241 +
1.242 + mCurrentPointerIdBits = idBits;
1.243 + if (mActivePointerId < 0 || !idBits.hasBit(mActivePointerId)) {
1.244 + mActivePointerId = idBits.isEmpty() ? -1 : idBits.firstMarkedBit();
1.245 + }
1.246 +
1.247 + mStrategy->addMovement(eventTime, idBits, positions);
1.248 +
1.249 +#if DEBUG_VELOCITY
1.250 + ALOGD("VelocityTracker: addMovement eventTime=%lld, idBits=0x%08x, activePointerId=%d",
1.251 + eventTime, idBits.value, mActivePointerId);
1.252 + for (BitSet32 iterBits(idBits); !iterBits.isEmpty(); ) {
1.253 + uint32_t id = iterBits.firstMarkedBit();
1.254 + uint32_t index = idBits.getIndexOfBit(id);
1.255 + iterBits.clearBit(id);
1.256 + Estimator estimator;
1.257 + getEstimator(id, &estimator);
1.258 + ALOGD(" %d: position (%0.3f, %0.3f), "
1.259 + "estimator (degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f)",
1.260 + id, positions[index].x, positions[index].y,
1.261 + int(estimator.degree),
1.262 + vectorToString(estimator.xCoeff, estimator.degree + 1).string(),
1.263 + vectorToString(estimator.yCoeff, estimator.degree + 1).string(),
1.264 + estimator.confidence);
1.265 + }
1.266 +#endif
1.267 +}
1.268 +
1.269 +void VelocityTracker::addMovement(const MotionEvent* event) {
1.270 + int32_t actionMasked = event->getActionMasked();
1.271 +
1.272 + switch (actionMasked) {
1.273 + case AMOTION_EVENT_ACTION_DOWN:
1.274 + case AMOTION_EVENT_ACTION_HOVER_ENTER:
1.275 + // Clear all pointers on down before adding the new movement.
1.276 + clear();
1.277 + break;
1.278 + case AMOTION_EVENT_ACTION_POINTER_DOWN: {
1.279 + // Start a new movement trace for a pointer that just went down.
1.280 + // We do this on down instead of on up because the client may want to query the
1.281 + // final velocity for a pointer that just went up.
1.282 + BitSet32 downIdBits;
1.283 + downIdBits.markBit(event->getPointerId(event->getActionIndex()));
1.284 + clearPointers(downIdBits);
1.285 + break;
1.286 + }
1.287 + case AMOTION_EVENT_ACTION_MOVE:
1.288 + case AMOTION_EVENT_ACTION_HOVER_MOVE:
1.289 + break;
1.290 + default:
1.291 + // Ignore all other actions because they do not convey any new information about
1.292 + // pointer movement. We also want to preserve the last known velocity of the pointers.
1.293 + // Note that ACTION_UP and ACTION_POINTER_UP always report the last known position
1.294 + // of the pointers that went up. ACTION_POINTER_UP does include the new position of
1.295 + // pointers that remained down but we will also receive an ACTION_MOVE with this
1.296 + // information if any of them actually moved. Since we don't know how many pointers
1.297 + // will be going up at once it makes sense to just wait for the following ACTION_MOVE
1.298 + // before adding the movement.
1.299 + return;
1.300 + }
1.301 +
1.302 + size_t pointerCount = event->getPointerCount();
1.303 + if (pointerCount > MAX_POINTERS) {
1.304 + pointerCount = MAX_POINTERS;
1.305 + }
1.306 +
1.307 + BitSet32 idBits;
1.308 + for (size_t i = 0; i < pointerCount; i++) {
1.309 + idBits.markBit(event->getPointerId(i));
1.310 + }
1.311 +
1.312 + uint32_t pointerIndex[MAX_POINTERS];
1.313 + for (size_t i = 0; i < pointerCount; i++) {
1.314 + pointerIndex[i] = idBits.getIndexOfBit(event->getPointerId(i));
1.315 + }
1.316 +
1.317 + nsecs_t eventTime;
1.318 + Position positions[pointerCount];
1.319 +
1.320 + size_t historySize = event->getHistorySize();
1.321 + for (size_t h = 0; h < historySize; h++) {
1.322 + eventTime = event->getHistoricalEventTime(h);
1.323 + for (size_t i = 0; i < pointerCount; i++) {
1.324 + uint32_t index = pointerIndex[i];
1.325 + positions[index].x = event->getHistoricalX(i, h);
1.326 + positions[index].y = event->getHistoricalY(i, h);
1.327 + }
1.328 + addMovement(eventTime, idBits, positions);
1.329 + }
1.330 +
1.331 + eventTime = event->getEventTime();
1.332 + for (size_t i = 0; i < pointerCount; i++) {
1.333 + uint32_t index = pointerIndex[i];
1.334 + positions[index].x = event->getX(i);
1.335 + positions[index].y = event->getY(i);
1.336 + }
1.337 + addMovement(eventTime, idBits, positions);
1.338 +}
1.339 +
1.340 +bool VelocityTracker::getVelocity(uint32_t id, float* outVx, float* outVy) const {
1.341 + Estimator estimator;
1.342 + if (getEstimator(id, &estimator) && estimator.degree >= 1) {
1.343 + *outVx = estimator.xCoeff[1];
1.344 + *outVy = estimator.yCoeff[1];
1.345 + return true;
1.346 + }
1.347 + *outVx = 0;
1.348 + *outVy = 0;
1.349 + return false;
1.350 +}
1.351 +
1.352 +bool VelocityTracker::getEstimator(uint32_t id, Estimator* outEstimator) const {
1.353 + return mStrategy->getEstimator(id, outEstimator);
1.354 +}
1.355 +
1.356 +
1.357 +// --- LeastSquaresVelocityTrackerStrategy ---
1.358 +
1.359 +const nsecs_t LeastSquaresVelocityTrackerStrategy::HORIZON;
1.360 +const uint32_t LeastSquaresVelocityTrackerStrategy::HISTORY_SIZE;
1.361 +
1.362 +LeastSquaresVelocityTrackerStrategy::LeastSquaresVelocityTrackerStrategy(
1.363 + uint32_t degree, Weighting weighting) :
1.364 + mDegree(degree), mWeighting(weighting) {
1.365 + clear();
1.366 +}
1.367 +
1.368 +LeastSquaresVelocityTrackerStrategy::~LeastSquaresVelocityTrackerStrategy() {
1.369 +}
1.370 +
1.371 +void LeastSquaresVelocityTrackerStrategy::clear() {
1.372 + mIndex = 0;
1.373 + mMovements[0].idBits.clear();
1.374 +}
1.375 +
1.376 +void LeastSquaresVelocityTrackerStrategy::clearPointers(BitSet32 idBits) {
1.377 + BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value);
1.378 + mMovements[mIndex].idBits = remainingIdBits;
1.379 +}
1.380 +
1.381 +void LeastSquaresVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits,
1.382 + const VelocityTracker::Position* positions) {
1.383 + if (++mIndex == HISTORY_SIZE) {
1.384 + mIndex = 0;
1.385 + }
1.386 +
1.387 + Movement& movement = mMovements[mIndex];
1.388 + movement.eventTime = eventTime;
1.389 + movement.idBits = idBits;
1.390 + uint32_t count = idBits.count();
1.391 + for (uint32_t i = 0; i < count; i++) {
1.392 + movement.positions[i] = positions[i];
1.393 + }
1.394 +}
1.395 +
1.396 +/**
1.397 + * Solves a linear least squares problem to obtain a N degree polynomial that fits
1.398 + * the specified input data as nearly as possible.
1.399 + *
1.400 + * Returns true if a solution is found, false otherwise.
1.401 + *
1.402 + * The input consists of two vectors of data points X and Y with indices 0..m-1
1.403 + * along with a weight vector W of the same size.
1.404 + *
1.405 + * The output is a vector B with indices 0..n that describes a polynomial
1.406 + * that fits the data, such the sum of W[i] * W[i] * abs(Y[i] - (B[0] + B[1] X[i]
1.407 + * + B[2] X[i]^2 ... B[n] X[i]^n)) for all i between 0 and m-1 is minimized.
1.408 + *
1.409 + * Accordingly, the weight vector W should be initialized by the caller with the
1.410 + * reciprocal square root of the variance of the error in each input data point.
1.411 + * In other words, an ideal choice for W would be W[i] = 1 / var(Y[i]) = 1 / stddev(Y[i]).
1.412 + * The weights express the relative importance of each data point. If the weights are
1.413 + * all 1, then the data points are considered to be of equal importance when fitting
1.414 + * the polynomial. It is a good idea to choose weights that diminish the importance
1.415 + * of data points that may have higher than usual error margins.
1.416 + *
1.417 + * Errors among data points are assumed to be independent. W is represented here
1.418 + * as a vector although in the literature it is typically taken to be a diagonal matrix.
1.419 + *
1.420 + * That is to say, the function that generated the input data can be approximated
1.421 + * by y(x) ~= B[0] + B[1] x + B[2] x^2 + ... + B[n] x^n.
1.422 + *
1.423 + * The coefficient of determination (R^2) is also returned to describe the goodness
1.424 + * of fit of the model for the given data. It is a value between 0 and 1, where 1
1.425 + * indicates perfect correspondence.
1.426 + *
1.427 + * This function first expands the X vector to a m by n matrix A such that
1.428 + * A[i][0] = 1, A[i][1] = X[i], A[i][2] = X[i]^2, ..., A[i][n] = X[i]^n, then
1.429 + * multiplies it by w[i]./
1.430 + *
1.431 + * Then it calculates the QR decomposition of A yielding an m by m orthonormal matrix Q
1.432 + * and an m by n upper triangular matrix R. Because R is upper triangular (lower
1.433 + * part is all zeroes), we can simplify the decomposition into an m by n matrix
1.434 + * Q1 and a n by n matrix R1 such that A = Q1 R1.
1.435 + *
1.436 + * Finally we solve the system of linear equations given by R1 B = (Qtranspose W Y)
1.437 + * to find B.
1.438 + *
1.439 + * For efficiency, we lay out A and Q column-wise in memory because we frequently
1.440 + * operate on the column vectors. Conversely, we lay out R row-wise.
1.441 + *
1.442 + * http://en.wikipedia.org/wiki/Numerical_methods_for_linear_least_squares
1.443 + * http://en.wikipedia.org/wiki/Gram-Schmidt
1.444 + */
1.445 +static bool solveLeastSquares(const float* x, const float* y,
1.446 + const float* w, uint32_t m, uint32_t n, float* outB, float* outDet) {
1.447 +#if DEBUG_STRATEGY
1.448 + ALOGD("solveLeastSquares: m=%d, n=%d, x=%s, y=%s, w=%s", int(m), int(n),
1.449 + vectorToString(x, m).string(), vectorToString(y, m).string(),
1.450 + vectorToString(w, m).string());
1.451 +#endif
1.452 +
1.453 + // Expand the X vector to a matrix A, pre-multiplied by the weights.
1.454 + float a[n][m]; // column-major order
1.455 + for (uint32_t h = 0; h < m; h++) {
1.456 + a[0][h] = w[h];
1.457 + for (uint32_t i = 1; i < n; i++) {
1.458 + a[i][h] = a[i - 1][h] * x[h];
1.459 + }
1.460 + }
1.461 +#if DEBUG_STRATEGY
1.462 + ALOGD(" - a=%s", matrixToString(&a[0][0], m, n, false /*rowMajor*/).string());
1.463 +#endif
1.464 +
1.465 + // Apply the Gram-Schmidt process to A to obtain its QR decomposition.
1.466 + float q[n][m]; // orthonormal basis, column-major order
1.467 + float r[n][n]; // upper triangular matrix, row-major order
1.468 + for (uint32_t j = 0; j < n; j++) {
1.469 + for (uint32_t h = 0; h < m; h++) {
1.470 + q[j][h] = a[j][h];
1.471 + }
1.472 + for (uint32_t i = 0; i < j; i++) {
1.473 + float dot = vectorDot(&q[j][0], &q[i][0], m);
1.474 + for (uint32_t h = 0; h < m; h++) {
1.475 + q[j][h] -= dot * q[i][h];
1.476 + }
1.477 + }
1.478 +
1.479 + float norm = vectorNorm(&q[j][0], m);
1.480 + if (norm < 0.000001f) {
1.481 + // vectors are linearly dependent or zero so no solution
1.482 +#if DEBUG_STRATEGY
1.483 + ALOGD(" - no solution, norm=%f", norm);
1.484 +#endif
1.485 + return false;
1.486 + }
1.487 +
1.488 + float invNorm = 1.0f / norm;
1.489 + for (uint32_t h = 0; h < m; h++) {
1.490 + q[j][h] *= invNorm;
1.491 + }
1.492 + for (uint32_t i = 0; i < n; i++) {
1.493 + r[j][i] = i < j ? 0 : vectorDot(&q[j][0], &a[i][0], m);
1.494 + }
1.495 + }
1.496 +#if DEBUG_STRATEGY
1.497 + ALOGD(" - q=%s", matrixToString(&q[0][0], m, n, false /*rowMajor*/).string());
1.498 + ALOGD(" - r=%s", matrixToString(&r[0][0], n, n, true /*rowMajor*/).string());
1.499 +
1.500 + // calculate QR, if we factored A correctly then QR should equal A
1.501 + float qr[n][m];
1.502 + for (uint32_t h = 0; h < m; h++) {
1.503 + for (uint32_t i = 0; i < n; i++) {
1.504 + qr[i][h] = 0;
1.505 + for (uint32_t j = 0; j < n; j++) {
1.506 + qr[i][h] += q[j][h] * r[j][i];
1.507 + }
1.508 + }
1.509 + }
1.510 + ALOGD(" - qr=%s", matrixToString(&qr[0][0], m, n, false /*rowMajor*/).string());
1.511 +#endif
1.512 +
1.513 + // Solve R B = Qt W Y to find B. This is easy because R is upper triangular.
1.514 + // We just work from bottom-right to top-left calculating B's coefficients.
1.515 + float wy[m];
1.516 + for (uint32_t h = 0; h < m; h++) {
1.517 + wy[h] = y[h] * w[h];
1.518 + }
1.519 + for (uint32_t i = n; i-- != 0; ) {
1.520 + outB[i] = vectorDot(&q[i][0], wy, m);
1.521 + for (uint32_t j = n - 1; j > i; j--) {
1.522 + outB[i] -= r[i][j] * outB[j];
1.523 + }
1.524 + outB[i] /= r[i][i];
1.525 + }
1.526 +#if DEBUG_STRATEGY
1.527 + ALOGD(" - b=%s", vectorToString(outB, n).string());
1.528 +#endif
1.529 +
1.530 + // Calculate the coefficient of determination as 1 - (SSerr / SStot) where
1.531 + // SSerr is the residual sum of squares (variance of the error),
1.532 + // and SStot is the total sum of squares (variance of the data) where each
1.533 + // has been weighted.
1.534 + float ymean = 0;
1.535 + for (uint32_t h = 0; h < m; h++) {
1.536 + ymean += y[h];
1.537 + }
1.538 + ymean /= m;
1.539 +
1.540 + float sserr = 0;
1.541 + float sstot = 0;
1.542 + for (uint32_t h = 0; h < m; h++) {
1.543 + float err = y[h] - outB[0];
1.544 + float term = 1;
1.545 + for (uint32_t i = 1; i < n; i++) {
1.546 + term *= x[h];
1.547 + err -= term * outB[i];
1.548 + }
1.549 + sserr += w[h] * w[h] * err * err;
1.550 + float var = y[h] - ymean;
1.551 + sstot += w[h] * w[h] * var * var;
1.552 + }
1.553 + *outDet = sstot > 0.000001f ? 1.0f - (sserr / sstot) : 1;
1.554 +#if DEBUG_STRATEGY
1.555 + ALOGD(" - sserr=%f", sserr);
1.556 + ALOGD(" - sstot=%f", sstot);
1.557 + ALOGD(" - det=%f", *outDet);
1.558 +#endif
1.559 + return true;
1.560 +}
1.561 +
1.562 +bool LeastSquaresVelocityTrackerStrategy::getEstimator(uint32_t id,
1.563 + VelocityTracker::Estimator* outEstimator) const {
1.564 + outEstimator->clear();
1.565 +
1.566 + // Iterate over movement samples in reverse time order and collect samples.
1.567 + float x[HISTORY_SIZE];
1.568 + float y[HISTORY_SIZE];
1.569 + float w[HISTORY_SIZE];
1.570 + float time[HISTORY_SIZE];
1.571 + uint32_t m = 0;
1.572 + uint32_t index = mIndex;
1.573 + const Movement& newestMovement = mMovements[mIndex];
1.574 + do {
1.575 + const Movement& movement = mMovements[index];
1.576 + if (!movement.idBits.hasBit(id)) {
1.577 + break;
1.578 + }
1.579 +
1.580 + nsecs_t age = newestMovement.eventTime - movement.eventTime;
1.581 + if (age > HORIZON) {
1.582 + break;
1.583 + }
1.584 +
1.585 + const VelocityTracker::Position& position = movement.getPosition(id);
1.586 + x[m] = position.x;
1.587 + y[m] = position.y;
1.588 + w[m] = chooseWeight(index);
1.589 + time[m] = -age * 0.000000001f;
1.590 + index = (index == 0 ? HISTORY_SIZE : index) - 1;
1.591 + } while (++m < HISTORY_SIZE);
1.592 +
1.593 + if (m == 0) {
1.594 + return false; // no data
1.595 + }
1.596 +
1.597 + // Calculate a least squares polynomial fit.
1.598 + uint32_t degree = mDegree;
1.599 + if (degree > m - 1) {
1.600 + degree = m - 1;
1.601 + }
1.602 + if (degree >= 1) {
1.603 + float xdet, ydet;
1.604 + uint32_t n = degree + 1;
1.605 + if (solveLeastSquares(time, x, w, m, n, outEstimator->xCoeff, &xdet)
1.606 + && solveLeastSquares(time, y, w, m, n, outEstimator->yCoeff, &ydet)) {
1.607 + outEstimator->time = newestMovement.eventTime;
1.608 + outEstimator->degree = degree;
1.609 + outEstimator->confidence = xdet * ydet;
1.610 +#if DEBUG_STRATEGY
1.611 + ALOGD("estimate: degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f",
1.612 + int(outEstimator->degree),
1.613 + vectorToString(outEstimator->xCoeff, n).string(),
1.614 + vectorToString(outEstimator->yCoeff, n).string(),
1.615 + outEstimator->confidence);
1.616 +#endif
1.617 + return true;
1.618 + }
1.619 + }
1.620 +
1.621 + // No velocity data available for this pointer, but we do have its current position.
1.622 + outEstimator->xCoeff[0] = x[0];
1.623 + outEstimator->yCoeff[0] = y[0];
1.624 + outEstimator->time = newestMovement.eventTime;
1.625 + outEstimator->degree = 0;
1.626 + outEstimator->confidence = 1;
1.627 + return true;
1.628 +}
1.629 +
1.630 +float LeastSquaresVelocityTrackerStrategy::chooseWeight(uint32_t index) const {
1.631 + switch (mWeighting) {
1.632 + case WEIGHTING_DELTA: {
1.633 + // Weight points based on how much time elapsed between them and the next
1.634 + // point so that points that "cover" a shorter time span are weighed less.
1.635 + // delta 0ms: 0.5
1.636 + // delta 10ms: 1.0
1.637 + if (index == mIndex) {
1.638 + return 1.0f;
1.639 + }
1.640 + uint32_t nextIndex = (index + 1) % HISTORY_SIZE;
1.641 + float deltaMillis = (mMovements[nextIndex].eventTime- mMovements[index].eventTime)
1.642 + * 0.000001f;
1.643 + if (deltaMillis < 0) {
1.644 + return 0.5f;
1.645 + }
1.646 + if (deltaMillis < 10) {
1.647 + return 0.5f + deltaMillis * 0.05;
1.648 + }
1.649 + return 1.0f;
1.650 + }
1.651 +
1.652 + case WEIGHTING_CENTRAL: {
1.653 + // Weight points based on their age, weighing very recent and very old points less.
1.654 + // age 0ms: 0.5
1.655 + // age 10ms: 1.0
1.656 + // age 50ms: 1.0
1.657 + // age 60ms: 0.5
1.658 + float ageMillis = (mMovements[mIndex].eventTime - mMovements[index].eventTime)
1.659 + * 0.000001f;
1.660 + if (ageMillis < 0) {
1.661 + return 0.5f;
1.662 + }
1.663 + if (ageMillis < 10) {
1.664 + return 0.5f + ageMillis * 0.05;
1.665 + }
1.666 + if (ageMillis < 50) {
1.667 + return 1.0f;
1.668 + }
1.669 + if (ageMillis < 60) {
1.670 + return 0.5f + (60 - ageMillis) * 0.05;
1.671 + }
1.672 + return 0.5f;
1.673 + }
1.674 +
1.675 + case WEIGHTING_RECENT: {
1.676 + // Weight points based on their age, weighing older points less.
1.677 + // age 0ms: 1.0
1.678 + // age 50ms: 1.0
1.679 + // age 100ms: 0.5
1.680 + float ageMillis = (mMovements[mIndex].eventTime - mMovements[index].eventTime)
1.681 + * 0.000001f;
1.682 + if (ageMillis < 50) {
1.683 + return 1.0f;
1.684 + }
1.685 + if (ageMillis < 100) {
1.686 + return 0.5f + (100 - ageMillis) * 0.01f;
1.687 + }
1.688 + return 0.5f;
1.689 + }
1.690 +
1.691 + case WEIGHTING_NONE:
1.692 + default:
1.693 + return 1.0f;
1.694 + }
1.695 +}
1.696 +
1.697 +
1.698 +// --- IntegratingVelocityTrackerStrategy ---
1.699 +
1.700 +IntegratingVelocityTrackerStrategy::IntegratingVelocityTrackerStrategy(uint32_t degree) :
1.701 + mDegree(degree) {
1.702 +}
1.703 +
1.704 +IntegratingVelocityTrackerStrategy::~IntegratingVelocityTrackerStrategy() {
1.705 +}
1.706 +
1.707 +void IntegratingVelocityTrackerStrategy::clear() {
1.708 + mPointerIdBits.clear();
1.709 +}
1.710 +
1.711 +void IntegratingVelocityTrackerStrategy::clearPointers(BitSet32 idBits) {
1.712 + mPointerIdBits.value &= ~idBits.value;
1.713 +}
1.714 +
1.715 +void IntegratingVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits,
1.716 + const VelocityTracker::Position* positions) {
1.717 + uint32_t index = 0;
1.718 + for (BitSet32 iterIdBits(idBits); !iterIdBits.isEmpty();) {
1.719 + uint32_t id = iterIdBits.clearFirstMarkedBit();
1.720 + State& state = mPointerState[id];
1.721 + const VelocityTracker::Position& position = positions[index++];
1.722 + if (mPointerIdBits.hasBit(id)) {
1.723 + updateState(state, eventTime, position.x, position.y);
1.724 + } else {
1.725 + initState(state, eventTime, position.x, position.y);
1.726 + }
1.727 + }
1.728 +
1.729 + mPointerIdBits = idBits;
1.730 +}
1.731 +
1.732 +bool IntegratingVelocityTrackerStrategy::getEstimator(uint32_t id,
1.733 + VelocityTracker::Estimator* outEstimator) const {
1.734 + outEstimator->clear();
1.735 +
1.736 + if (mPointerIdBits.hasBit(id)) {
1.737 + const State& state = mPointerState[id];
1.738 + populateEstimator(state, outEstimator);
1.739 + return true;
1.740 + }
1.741 +
1.742 + return false;
1.743 +}
1.744 +
1.745 +void IntegratingVelocityTrackerStrategy::initState(State& state,
1.746 + nsecs_t eventTime, float xpos, float ypos) const {
1.747 + state.updateTime = eventTime;
1.748 + state.degree = 0;
1.749 +
1.750 + state.xpos = xpos;
1.751 + state.xvel = 0;
1.752 + state.xaccel = 0;
1.753 + state.ypos = ypos;
1.754 + state.yvel = 0;
1.755 + state.yaccel = 0;
1.756 +}
1.757 +
1.758 +void IntegratingVelocityTrackerStrategy::updateState(State& state,
1.759 + nsecs_t eventTime, float xpos, float ypos) const {
1.760 + const nsecs_t MIN_TIME_DELTA = 2 * NANOS_PER_MS;
1.761 + const float FILTER_TIME_CONSTANT = 0.010f; // 10 milliseconds
1.762 +
1.763 + if (eventTime <= state.updateTime + MIN_TIME_DELTA) {
1.764 + return;
1.765 + }
1.766 +
1.767 + float dt = (eventTime - state.updateTime) * 0.000000001f;
1.768 + state.updateTime = eventTime;
1.769 +
1.770 + float xvel = (xpos - state.xpos) / dt;
1.771 + float yvel = (ypos - state.ypos) / dt;
1.772 + if (state.degree == 0) {
1.773 + state.xvel = xvel;
1.774 + state.yvel = yvel;
1.775 + state.degree = 1;
1.776 + } else {
1.777 + float alpha = dt / (FILTER_TIME_CONSTANT + dt);
1.778 + if (mDegree == 1) {
1.779 + state.xvel += (xvel - state.xvel) * alpha;
1.780 + state.yvel += (yvel - state.yvel) * alpha;
1.781 + } else {
1.782 + float xaccel = (xvel - state.xvel) / dt;
1.783 + float yaccel = (yvel - state.yvel) / dt;
1.784 + if (state.degree == 1) {
1.785 + state.xaccel = xaccel;
1.786 + state.yaccel = yaccel;
1.787 + state.degree = 2;
1.788 + } else {
1.789 + state.xaccel += (xaccel - state.xaccel) * alpha;
1.790 + state.yaccel += (yaccel - state.yaccel) * alpha;
1.791 + }
1.792 + state.xvel += (state.xaccel * dt) * alpha;
1.793 + state.yvel += (state.yaccel * dt) * alpha;
1.794 + }
1.795 + }
1.796 + state.xpos = xpos;
1.797 + state.ypos = ypos;
1.798 +}
1.799 +
1.800 +void IntegratingVelocityTrackerStrategy::populateEstimator(const State& state,
1.801 + VelocityTracker::Estimator* outEstimator) const {
1.802 + outEstimator->time = state.updateTime;
1.803 + outEstimator->confidence = 1.0f;
1.804 + outEstimator->degree = state.degree;
1.805 + outEstimator->xCoeff[0] = state.xpos;
1.806 + outEstimator->xCoeff[1] = state.xvel;
1.807 + outEstimator->xCoeff[2] = state.xaccel / 2;
1.808 + outEstimator->yCoeff[0] = state.ypos;
1.809 + outEstimator->yCoeff[1] = state.yvel;
1.810 + outEstimator->yCoeff[2] = state.yaccel / 2;
1.811 +}
1.812 +
1.813 +
1.814 +// --- LegacyVelocityTrackerStrategy ---
1.815 +
1.816 +const nsecs_t LegacyVelocityTrackerStrategy::HORIZON;
1.817 +const uint32_t LegacyVelocityTrackerStrategy::HISTORY_SIZE;
1.818 +const nsecs_t LegacyVelocityTrackerStrategy::MIN_DURATION;
1.819 +
1.820 +LegacyVelocityTrackerStrategy::LegacyVelocityTrackerStrategy() {
1.821 + clear();
1.822 +}
1.823 +
1.824 +LegacyVelocityTrackerStrategy::~LegacyVelocityTrackerStrategy() {
1.825 +}
1.826 +
1.827 +void LegacyVelocityTrackerStrategy::clear() {
1.828 + mIndex = 0;
1.829 + mMovements[0].idBits.clear();
1.830 +}
1.831 +
1.832 +void LegacyVelocityTrackerStrategy::clearPointers(BitSet32 idBits) {
1.833 + BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value);
1.834 + mMovements[mIndex].idBits = remainingIdBits;
1.835 +}
1.836 +
1.837 +void LegacyVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits,
1.838 + const VelocityTracker::Position* positions) {
1.839 + if (++mIndex == HISTORY_SIZE) {
1.840 + mIndex = 0;
1.841 + }
1.842 +
1.843 + Movement& movement = mMovements[mIndex];
1.844 + movement.eventTime = eventTime;
1.845 + movement.idBits = idBits;
1.846 + uint32_t count = idBits.count();
1.847 + for (uint32_t i = 0; i < count; i++) {
1.848 + movement.positions[i] = positions[i];
1.849 + }
1.850 +}
1.851 +
1.852 +bool LegacyVelocityTrackerStrategy::getEstimator(uint32_t id,
1.853 + VelocityTracker::Estimator* outEstimator) const {
1.854 + outEstimator->clear();
1.855 +
1.856 + const Movement& newestMovement = mMovements[mIndex];
1.857 + if (!newestMovement.idBits.hasBit(id)) {
1.858 + return false; // no data
1.859 + }
1.860 +
1.861 + // Find the oldest sample that contains the pointer and that is not older than HORIZON.
1.862 + nsecs_t minTime = newestMovement.eventTime - HORIZON;
1.863 + uint32_t oldestIndex = mIndex;
1.864 + uint32_t numTouches = 1;
1.865 + do {
1.866 + uint32_t nextOldestIndex = (oldestIndex == 0 ? HISTORY_SIZE : oldestIndex) - 1;
1.867 + const Movement& nextOldestMovement = mMovements[nextOldestIndex];
1.868 + if (!nextOldestMovement.idBits.hasBit(id)
1.869 + || nextOldestMovement.eventTime < minTime) {
1.870 + break;
1.871 + }
1.872 + oldestIndex = nextOldestIndex;
1.873 + } while (++numTouches < HISTORY_SIZE);
1.874 +
1.875 + // Calculate an exponentially weighted moving average of the velocity estimate
1.876 + // at different points in time measured relative to the oldest sample.
1.877 + // This is essentially an IIR filter. Newer samples are weighted more heavily
1.878 + // than older samples. Samples at equal time points are weighted more or less
1.879 + // equally.
1.880 + //
1.881 + // One tricky problem is that the sample data may be poorly conditioned.
1.882 + // Sometimes samples arrive very close together in time which can cause us to
1.883 + // overestimate the velocity at that time point. Most samples might be measured
1.884 + // 16ms apart but some consecutive samples could be only 0.5sm apart because
1.885 + // the hardware or driver reports them irregularly or in bursts.
1.886 + float accumVx = 0;
1.887 + float accumVy = 0;
1.888 + uint32_t index = oldestIndex;
1.889 + uint32_t samplesUsed = 0;
1.890 + const Movement& oldestMovement = mMovements[oldestIndex];
1.891 + const VelocityTracker::Position& oldestPosition = oldestMovement.getPosition(id);
1.892 + nsecs_t lastDuration = 0;
1.893 +
1.894 + while (numTouches-- > 1) {
1.895 + if (++index == HISTORY_SIZE) {
1.896 + index = 0;
1.897 + }
1.898 + const Movement& movement = mMovements[index];
1.899 + nsecs_t duration = movement.eventTime - oldestMovement.eventTime;
1.900 +
1.901 + // If the duration between samples is small, we may significantly overestimate
1.902 + // the velocity. Consequently, we impose a minimum duration constraint on the
1.903 + // samples that we include in the calculation.
1.904 + if (duration >= MIN_DURATION) {
1.905 + const VelocityTracker::Position& position = movement.getPosition(id);
1.906 + float scale = 1000000000.0f / duration; // one over time delta in seconds
1.907 + float vx = (position.x - oldestPosition.x) * scale;
1.908 + float vy = (position.y - oldestPosition.y) * scale;
1.909 + accumVx = (accumVx * lastDuration + vx * duration) / (duration + lastDuration);
1.910 + accumVy = (accumVy * lastDuration + vy * duration) / (duration + lastDuration);
1.911 + lastDuration = duration;
1.912 + samplesUsed += 1;
1.913 + }
1.914 + }
1.915 +
1.916 + // Report velocity.
1.917 + const VelocityTracker::Position& newestPosition = newestMovement.getPosition(id);
1.918 + outEstimator->time = newestMovement.eventTime;
1.919 + outEstimator->confidence = 1;
1.920 + outEstimator->xCoeff[0] = newestPosition.x;
1.921 + outEstimator->yCoeff[0] = newestPosition.y;
1.922 + if (samplesUsed) {
1.923 + outEstimator->xCoeff[1] = accumVx;
1.924 + outEstimator->yCoeff[1] = accumVy;
1.925 + outEstimator->degree = 1;
1.926 + } else {
1.927 + outEstimator->degree = 0;
1.928 + }
1.929 + return true;
1.930 +}
1.931 +
1.932 +} // namespace android
