15912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown/* 25912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * Copyright (C) 2012 The Android Open Source Project 35912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * 45912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * Licensed under the Apache License, Version 2.0 (the "License"); 55912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * you may not use this file except in compliance with the License. 65912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * You may obtain a copy of the License at 75912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * 85912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * http://www.apache.org/licenses/LICENSE-2.0 95912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * 105912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * Unless required by applicable law or agreed to in writing, software 115912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * distributed under the License is distributed on an "AS IS" BASIS, 125912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 135912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * See the License for the specific language governing permissions and 145912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * limitations under the License. 155912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown */ 165912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 175912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#define LOG_TAG "VelocityTracker" 185912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown//#define LOG_NDEBUG 0 195912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 205912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown// Log debug messages about velocity tracking. 215912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#define DEBUG_VELOCITY 0 225912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 235912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown// Log debug messages about the progress of the algorithm itself. 245912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#define DEBUG_STRATEGY 0 255912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 265912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#include <math.h> 275912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#include <limits.h> 285912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 295912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#include <cutils/properties.h> 305912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#include <input/VelocityTracker.h> 315912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#include <utils/BitSet.h> 325912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#include <utils/String8.h> 335912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#include <utils/Timers.h> 345912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 355912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownnamespace android { 365912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 375912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown// Nanoseconds per milliseconds. 385912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownstatic const nsecs_t NANOS_PER_MS = 1000000; 395912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 405912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown// Threshold for determining that a pointer has stopped moving. 415912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown// Some input devices do not send ACTION_MOVE events in the case where a pointer has 425912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown// stopped. We need to detect this case so that we can accurately predict the 435912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown// velocity after the pointer starts moving again. 445912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownstatic const nsecs_t ASSUME_POINTER_STOPPED_TIME = 40 * NANOS_PER_MS; 455912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 465912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 475912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownstatic float vectorDot(const float* a, const float* b, uint32_t m) { 485912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float r = 0; 495912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown while (m--) { 505912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown r += *(a++) * *(b++); 515912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 525912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return r; 535912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 545912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 555912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownstatic float vectorNorm(const float* a, uint32_t m) { 565912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float r = 0; 575912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown while (m--) { 585912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float t = *(a++); 595912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown r += t * t; 605912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 615912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return sqrtf(r); 625912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 635912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 645912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#if DEBUG_STRATEGY || DEBUG_VELOCITY 655912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownstatic String8 vectorToString(const float* a, uint32_t m) { 665912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown String8 str; 675912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown str.append("["); 685912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown while (m--) { 695912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown str.appendFormat(" %f", *(a++)); 705912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (m) { 715912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown str.append(","); 725912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 735912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 745912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown str.append(" ]"); 755912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return str; 765912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 775912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 785912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownstatic String8 matrixToString(const float* a, uint32_t m, uint32_t n, bool rowMajor) { 795912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown String8 str; 805912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown str.append("["); 815912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (size_t i = 0; i < m; i++) { 825912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (i) { 835912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown str.append(","); 845912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 855912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown str.append(" ["); 865912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (size_t j = 0; j < n; j++) { 875912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (j) { 885912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown str.append(","); 895912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 905912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown str.appendFormat(" %f", a[rowMajor ? i * n + j : j * m + i]); 915912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 925912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown str.append(" ]"); 935912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 945912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown str.append(" ]"); 955912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return str; 965912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 975912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#endif 985912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 995912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 1005912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown// --- VelocityTracker --- 1015912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 1025912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown// The default velocity tracker strategy. 1035912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown// Although other strategies are available for testing and comparison purposes, 1045912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown// this is the strategy that applications will actually use. Be very careful 1055912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown// when adjusting the default strategy because it can dramatically affect 1065912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown// (often in a bad way) the user experience. 1075912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownconst char* VelocityTracker::DEFAULT_STRATEGY = "lsq2"; 1085912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 1095912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff BrownVelocityTracker::VelocityTracker(const char* strategy) : 1105912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mLastEventTime(0), mCurrentPointerIdBits(0), mActivePointerId(-1) { 1115912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown char value[PROPERTY_VALUE_MAX]; 1125912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 1135912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Allow the default strategy to be overridden using a system property for debugging. 1145912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (!strategy) { 1155912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown int length = property_get("debug.velocitytracker.strategy", value, NULL); 1165912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (length > 0) { 1175912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown strategy = value; 1185912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } else { 1195912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown strategy = DEFAULT_STRATEGY; 1205912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 1215912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 1225912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 1235912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Configure the strategy. 1245912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (!configureStrategy(strategy)) { 1255912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown ALOGD("Unrecognized velocity tracker strategy name '%s'.", strategy); 1265912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (!configureStrategy(DEFAULT_STRATEGY)) { 1275912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown LOG_ALWAYS_FATAL("Could not create the default velocity tracker strategy '%s'!", 1285912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown strategy); 1295912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 1305912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 1315912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 1325912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 1335912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff BrownVelocityTracker::~VelocityTracker() { 1345912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown delete mStrategy; 1355912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 1365912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 1375912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownbool VelocityTracker::configureStrategy(const char* strategy) { 1385912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mStrategy = createStrategy(strategy); 1395912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return mStrategy != NULL; 1405912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 1415912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 1425912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff BrownVelocityTrackerStrategy* VelocityTracker::createStrategy(const char* strategy) { 1435912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (!strcmp("lsq1", strategy)) { 1445912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // 1st order least squares. Quality: POOR. 1455912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Frequently underfits the touch data especially when the finger accelerates 1465912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // or changes direction. Often underestimates velocity. The direction 1475912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // is overly influenced by historical touch points. 1485912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return new LeastSquaresVelocityTrackerStrategy(1); 1495912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 1505912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (!strcmp("lsq2", strategy)) { 1515912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // 2nd order least squares. Quality: VERY GOOD. 1525912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Pretty much ideal, but can be confused by certain kinds of touch data, 1535912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // particularly if the panel has a tendency to generate delayed, 1545912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // duplicate or jittery touch coordinates when the finger is released. 1555912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return new LeastSquaresVelocityTrackerStrategy(2); 1565912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 1575912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (!strcmp("lsq3", strategy)) { 1585912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // 3rd order least squares. Quality: UNUSABLE. 1595912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Frequently overfits the touch data yielding wildly divergent estimates 1605912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // of the velocity when the finger is released. 1615912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return new LeastSquaresVelocityTrackerStrategy(3); 1625912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 1635912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (!strcmp("wlsq2-delta", strategy)) { 1645912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // 2nd order weighted least squares, delta weighting. Quality: EXPERIMENTAL 1655912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return new LeastSquaresVelocityTrackerStrategy(2, 1665912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown LeastSquaresVelocityTrackerStrategy::WEIGHTING_DELTA); 1675912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 1685912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (!strcmp("wlsq2-central", strategy)) { 1695912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // 2nd order weighted least squares, central weighting. Quality: EXPERIMENTAL 1705912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return new LeastSquaresVelocityTrackerStrategy(2, 1715912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown LeastSquaresVelocityTrackerStrategy::WEIGHTING_CENTRAL); 1725912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 1735912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (!strcmp("wlsq2-recent", strategy)) { 1745912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // 2nd order weighted least squares, recent weighting. Quality: EXPERIMENTAL 1755912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return new LeastSquaresVelocityTrackerStrategy(2, 1765912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown LeastSquaresVelocityTrackerStrategy::WEIGHTING_RECENT); 1775912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 1785912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (!strcmp("int1", strategy)) { 1795912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // 1st order integrating filter. Quality: GOOD. 1805912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Not as good as 'lsq2' because it cannot estimate acceleration but it is 1815912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // more tolerant of errors. Like 'lsq1', this strategy tends to underestimate 1825912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // the velocity of a fling but this strategy tends to respond to changes in 1835912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // direction more quickly and accurately. 1845912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return new IntegratingVelocityTrackerStrategy(1); 1855912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 1865912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (!strcmp("int2", strategy)) { 1875912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // 2nd order integrating filter. Quality: EXPERIMENTAL. 1885912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // For comparison purposes only. Unlike 'int1' this strategy can compensate 1895912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // for acceleration but it typically overestimates the effect. 1905912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return new IntegratingVelocityTrackerStrategy(2); 1915912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 1925912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (!strcmp("legacy", strategy)) { 1935912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Legacy velocity tracker algorithm. Quality: POOR. 1945912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // For comparison purposes only. This algorithm is strongly influenced by 1955912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // old data points, consistently underestimates velocity and takes a very long 1965912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // time to adjust to changes in direction. 1975912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return new LegacyVelocityTrackerStrategy(); 1985912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 1995912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return NULL; 2005912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 2015912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 2025912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownvoid VelocityTracker::clear() { 2035912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mCurrentPointerIdBits.clear(); 2045912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mActivePointerId = -1; 2055912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 2065912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mStrategy->clear(); 2075912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 2085912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 2095912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownvoid VelocityTracker::clearPointers(BitSet32 idBits) { 2105912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown BitSet32 remainingIdBits(mCurrentPointerIdBits.value & ~idBits.value); 2115912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mCurrentPointerIdBits = remainingIdBits; 2125912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 2135912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (mActivePointerId >= 0 && idBits.hasBit(mActivePointerId)) { 2145912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mActivePointerId = !remainingIdBits.isEmpty() ? remainingIdBits.firstMarkedBit() : -1; 2155912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 2165912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 2175912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mStrategy->clearPointers(idBits); 2185912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 2195912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 2205912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownvoid VelocityTracker::addMovement(nsecs_t eventTime, BitSet32 idBits, const Position* positions) { 2215912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown while (idBits.count() > MAX_POINTERS) { 2225912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown idBits.clearLastMarkedBit(); 2235912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 2245912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 2255912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if ((mCurrentPointerIdBits.value & idBits.value) 2265912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown && eventTime >= mLastEventTime + ASSUME_POINTER_STOPPED_TIME) { 2275912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#if DEBUG_VELOCITY 2285912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown ALOGD("VelocityTracker: stopped for %0.3f ms, clearing state.", 2295912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown (eventTime - mLastEventTime) * 0.000001f); 2305912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#endif 2315912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // We have not received any movements for too long. Assume that all pointers 2325912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // have stopped. 2335912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mStrategy->clear(); 2345912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 2355912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mLastEventTime = eventTime; 2365912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 2375912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mCurrentPointerIdBits = idBits; 2385912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (mActivePointerId < 0 || !idBits.hasBit(mActivePointerId)) { 2395912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mActivePointerId = idBits.isEmpty() ? -1 : idBits.firstMarkedBit(); 2405912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 2415912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 2425912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mStrategy->addMovement(eventTime, idBits, positions); 2435912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 2445912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#if DEBUG_VELOCITY 2455912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown ALOGD("VelocityTracker: addMovement eventTime=%lld, idBits=0x%08x, activePointerId=%d", 2465912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown eventTime, idBits.value, mActivePointerId); 2475912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (BitSet32 iterBits(idBits); !iterBits.isEmpty(); ) { 2485912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t id = iterBits.firstMarkedBit(); 2495912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t index = idBits.getIndexOfBit(id); 2505912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown iterBits.clearBit(id); 2515912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown Estimator estimator; 2525912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown getEstimator(id, &estimator); 2535912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown ALOGD(" %d: position (%0.3f, %0.3f), " 2545912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown "estimator (degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f)", 2555912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown id, positions[index].x, positions[index].y, 2565912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown int(estimator.degree), 2575912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown vectorToString(estimator.xCoeff, estimator.degree + 1).string(), 2585912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown vectorToString(estimator.yCoeff, estimator.degree + 1).string(), 2595912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown estimator.confidence); 2605912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 2615912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#endif 2625912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 2635912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 2645912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownvoid VelocityTracker::addMovement(const MotionEvent* event) { 2655912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown int32_t actionMasked = event->getActionMasked(); 2665912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 2675912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown switch (actionMasked) { 2685912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown case AMOTION_EVENT_ACTION_DOWN: 2695912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown case AMOTION_EVENT_ACTION_HOVER_ENTER: 2705912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Clear all pointers on down before adding the new movement. 2715912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown clear(); 2725912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown break; 2735912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown case AMOTION_EVENT_ACTION_POINTER_DOWN: { 2745912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Start a new movement trace for a pointer that just went down. 2755912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // We do this on down instead of on up because the client may want to query the 2765912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // final velocity for a pointer that just went up. 2775912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown BitSet32 downIdBits; 2785912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown downIdBits.markBit(event->getPointerId(event->getActionIndex())); 2795912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown clearPointers(downIdBits); 2805912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown break; 2815912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 2825912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown case AMOTION_EVENT_ACTION_MOVE: 2835912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown case AMOTION_EVENT_ACTION_HOVER_MOVE: 2845912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown break; 2855912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown default: 2865912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Ignore all other actions because they do not convey any new information about 2875912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // pointer movement. We also want to preserve the last known velocity of the pointers. 2885912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Note that ACTION_UP and ACTION_POINTER_UP always report the last known position 2895912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // of the pointers that went up. ACTION_POINTER_UP does include the new position of 2905912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // pointers that remained down but we will also receive an ACTION_MOVE with this 2915912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // information if any of them actually moved. Since we don't know how many pointers 2925912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // will be going up at once it makes sense to just wait for the following ACTION_MOVE 2935912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // before adding the movement. 2945912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return; 2955912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 2965912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 2975912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown size_t pointerCount = event->getPointerCount(); 2985912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (pointerCount > MAX_POINTERS) { 2995912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown pointerCount = MAX_POINTERS; 3005912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 3015912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 3025912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown BitSet32 idBits; 3035912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (size_t i = 0; i < pointerCount; i++) { 3045912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown idBits.markBit(event->getPointerId(i)); 3055912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 3065912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 3075912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t pointerIndex[MAX_POINTERS]; 3085912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (size_t i = 0; i < pointerCount; i++) { 3095912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown pointerIndex[i] = idBits.getIndexOfBit(event->getPointerId(i)); 3105912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 3115912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 3125912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown nsecs_t eventTime; 3135912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown Position positions[pointerCount]; 3145912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 3155912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown size_t historySize = event->getHistorySize(); 3165912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (size_t h = 0; h < historySize; h++) { 3175912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown eventTime = event->getHistoricalEventTime(h); 3185912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (size_t i = 0; i < pointerCount; i++) { 3195912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t index = pointerIndex[i]; 3205912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown positions[index].x = event->getHistoricalX(i, h); 3215912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown positions[index].y = event->getHistoricalY(i, h); 3225912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 3235912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown addMovement(eventTime, idBits, positions); 3245912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 3255912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 3265912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown eventTime = event->getEventTime(); 3275912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (size_t i = 0; i < pointerCount; i++) { 3285912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t index = pointerIndex[i]; 3295912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown positions[index].x = event->getX(i); 3305912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown positions[index].y = event->getY(i); 3315912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 3325912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown addMovement(eventTime, idBits, positions); 3335912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 3345912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 3355912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownbool VelocityTracker::getVelocity(uint32_t id, float* outVx, float* outVy) const { 3365912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown Estimator estimator; 3375912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (getEstimator(id, &estimator) && estimator.degree >= 1) { 3385912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown *outVx = estimator.xCoeff[1]; 3395912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown *outVy = estimator.yCoeff[1]; 3405912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return true; 3415912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 3425912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown *outVx = 0; 3435912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown *outVy = 0; 3445912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return false; 3455912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 3465912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 3475912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownbool VelocityTracker::getEstimator(uint32_t id, Estimator* outEstimator) const { 3485912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return mStrategy->getEstimator(id, outEstimator); 3495912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 3505912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 3515912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 3525912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown// --- LeastSquaresVelocityTrackerStrategy --- 3535912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 3545912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownconst nsecs_t LeastSquaresVelocityTrackerStrategy::HORIZON; 3555912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownconst uint32_t LeastSquaresVelocityTrackerStrategy::HISTORY_SIZE; 3565912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 3575912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff BrownLeastSquaresVelocityTrackerStrategy::LeastSquaresVelocityTrackerStrategy( 3585912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t degree, Weighting weighting) : 3595912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mDegree(degree), mWeighting(weighting) { 3605912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown clear(); 3615912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 3625912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 3635912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff BrownLeastSquaresVelocityTrackerStrategy::~LeastSquaresVelocityTrackerStrategy() { 3645912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 3655912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 3665912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownvoid LeastSquaresVelocityTrackerStrategy::clear() { 3675912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mIndex = 0; 3685912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mMovements[0].idBits.clear(); 3695912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 3705912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 3715912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownvoid LeastSquaresVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { 3725912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value); 3735912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mMovements[mIndex].idBits = remainingIdBits; 3745912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 3755912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 3765912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownvoid LeastSquaresVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, 3775912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown const VelocityTracker::Position* positions) { 3785912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (++mIndex == HISTORY_SIZE) { 3795912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mIndex = 0; 3805912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 3815912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 3825912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown Movement& movement = mMovements[mIndex]; 3835912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown movement.eventTime = eventTime; 3845912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown movement.idBits = idBits; 3855912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t count = idBits.count(); 3865912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t i = 0; i < count; i++) { 3875912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown movement.positions[i] = positions[i]; 3885912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 3895912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 3905912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 3915912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown/** 3925912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * Solves a linear least squares problem to obtain a N degree polynomial that fits 3935912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * the specified input data as nearly as possible. 3945912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * 3955912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * Returns true if a solution is found, false otherwise. 3965912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * 3975912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * The input consists of two vectors of data points X and Y with indices 0..m-1 3985912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * along with a weight vector W of the same size. 3995912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * 4005912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * The output is a vector B with indices 0..n that describes a polynomial 4015912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * that fits the data, such the sum of W[i] * W[i] * abs(Y[i] - (B[0] + B[1] X[i] 4025912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * + B[2] X[i]^2 ... B[n] X[i]^n)) for all i between 0 and m-1 is minimized. 4035912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * 4045912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * Accordingly, the weight vector W should be initialized by the caller with the 4055912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * reciprocal square root of the variance of the error in each input data point. 4065912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * In other words, an ideal choice for W would be W[i] = 1 / var(Y[i]) = 1 / stddev(Y[i]). 4075912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * The weights express the relative importance of each data point. If the weights are 4085912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * all 1, then the data points are considered to be of equal importance when fitting 4095912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * the polynomial. It is a good idea to choose weights that diminish the importance 4105912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * of data points that may have higher than usual error margins. 4115912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * 4125912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * Errors among data points are assumed to be independent. W is represented here 4135912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * as a vector although in the literature it is typically taken to be a diagonal matrix. 4145912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * 4155912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * That is to say, the function that generated the input data can be approximated 4165912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * by y(x) ~= B[0] + B[1] x + B[2] x^2 + ... + B[n] x^n. 4175912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * 4185912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * The coefficient of determination (R^2) is also returned to describe the goodness 4195912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * of fit of the model for the given data. It is a value between 0 and 1, where 1 4205912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * indicates perfect correspondence. 4215912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * 4225912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * This function first expands the X vector to a m by n matrix A such that 4235912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * A[i][0] = 1, A[i][1] = X[i], A[i][2] = X[i]^2, ..., A[i][n] = X[i]^n, then 4245912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * multiplies it by w[i]./ 4255912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * 4265912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * Then it calculates the QR decomposition of A yielding an m by m orthonormal matrix Q 4275912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * and an m by n upper triangular matrix R. Because R is upper triangular (lower 4285912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * part is all zeroes), we can simplify the decomposition into an m by n matrix 4295912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * Q1 and a n by n matrix R1 such that A = Q1 R1. 4305912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * 4315912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * Finally we solve the system of linear equations given by R1 B = (Qtranspose W Y) 4325912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * to find B. 4335912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * 4345912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * For efficiency, we lay out A and Q column-wise in memory because we frequently 4355912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * operate on the column vectors. Conversely, we lay out R row-wise. 4365912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * 4375912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * http://en.wikipedia.org/wiki/Numerical_methods_for_linear_least_squares 4385912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * http://en.wikipedia.org/wiki/Gram-Schmidt 4395912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown */ 4405912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownstatic bool solveLeastSquares(const float* x, const float* y, 4415912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown const float* w, uint32_t m, uint32_t n, float* outB, float* outDet) { 4425912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#if DEBUG_STRATEGY 4435912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown ALOGD("solveLeastSquares: m=%d, n=%d, x=%s, y=%s, w=%s", int(m), int(n), 4445912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown vectorToString(x, m).string(), vectorToString(y, m).string(), 4455912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown vectorToString(w, m).string()); 4465912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#endif 4475912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 4485912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Expand the X vector to a matrix A, pre-multiplied by the weights. 4495912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float a[n][m]; // column-major order 4505912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t h = 0; h < m; h++) { 4515912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown a[0][h] = w[h]; 4525912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t i = 1; i < n; i++) { 4535912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown a[i][h] = a[i - 1][h] * x[h]; 4545912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 4555912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 4565912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#if DEBUG_STRATEGY 4575912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown ALOGD(" - a=%s", matrixToString(&a[0][0], m, n, false /*rowMajor*/).string()); 4585912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#endif 4595912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 4605912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Apply the Gram-Schmidt process to A to obtain its QR decomposition. 4615912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float q[n][m]; // orthonormal basis, column-major order 4625912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float r[n][n]; // upper triangular matrix, row-major order 4635912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t j = 0; j < n; j++) { 4645912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t h = 0; h < m; h++) { 4655912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown q[j][h] = a[j][h]; 4665912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 4675912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t i = 0; i < j; i++) { 4685912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float dot = vectorDot(&q[j][0], &q[i][0], m); 4695912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t h = 0; h < m; h++) { 4705912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown q[j][h] -= dot * q[i][h]; 4715912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 4725912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 4735912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 4745912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float norm = vectorNorm(&q[j][0], m); 4755912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (norm < 0.000001f) { 4765912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // vectors are linearly dependent or zero so no solution 4775912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#if DEBUG_STRATEGY 4785912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown ALOGD(" - no solution, norm=%f", norm); 4795912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#endif 4805912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return false; 4815912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 4825912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 4835912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float invNorm = 1.0f / norm; 4845912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t h = 0; h < m; h++) { 4855912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown q[j][h] *= invNorm; 4865912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 4875912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t i = 0; i < n; i++) { 4885912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown r[j][i] = i < j ? 0 : vectorDot(&q[j][0], &a[i][0], m); 4895912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 4905912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 4915912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#if DEBUG_STRATEGY 4925912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown ALOGD(" - q=%s", matrixToString(&q[0][0], m, n, false /*rowMajor*/).string()); 4935912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown ALOGD(" - r=%s", matrixToString(&r[0][0], n, n, true /*rowMajor*/).string()); 4945912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 4955912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // calculate QR, if we factored A correctly then QR should equal A 4965912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float qr[n][m]; 4975912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t h = 0; h < m; h++) { 4985912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t i = 0; i < n; i++) { 4995912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown qr[i][h] = 0; 5005912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t j = 0; j < n; j++) { 5015912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown qr[i][h] += q[j][h] * r[j][i]; 5025912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 5035912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 5045912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 5055912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown ALOGD(" - qr=%s", matrixToString(&qr[0][0], m, n, false /*rowMajor*/).string()); 5065912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#endif 5075912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 5085912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Solve R B = Qt W Y to find B. This is easy because R is upper triangular. 5095912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // We just work from bottom-right to top-left calculating B's coefficients. 5105912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float wy[m]; 5115912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t h = 0; h < m; h++) { 5125912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown wy[h] = y[h] * w[h]; 5135912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 5145912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t i = n; i-- != 0; ) { 5155912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outB[i] = vectorDot(&q[i][0], wy, m); 5165912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t j = n - 1; j > i; j--) { 5175912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outB[i] -= r[i][j] * outB[j]; 5185912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 5195912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outB[i] /= r[i][i]; 5205912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 5215912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#if DEBUG_STRATEGY 5225912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown ALOGD(" - b=%s", vectorToString(outB, n).string()); 5235912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#endif 5245912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 5255912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Calculate the coefficient of determination as 1 - (SSerr / SStot) where 5265912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // SSerr is the residual sum of squares (variance of the error), 5275912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // and SStot is the total sum of squares (variance of the data) where each 5285912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // has been weighted. 5295912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float ymean = 0; 5305912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t h = 0; h < m; h++) { 5315912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown ymean += y[h]; 5325912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 5335912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown ymean /= m; 5345912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 5355912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float sserr = 0; 5365912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float sstot = 0; 5375912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t h = 0; h < m; h++) { 5385912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float err = y[h] - outB[0]; 5395912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float term = 1; 5405912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t i = 1; i < n; i++) { 5415912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown term *= x[h]; 5425912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown err -= term * outB[i]; 5435912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 5445912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown sserr += w[h] * w[h] * err * err; 5455912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float var = y[h] - ymean; 5465912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown sstot += w[h] * w[h] * var * var; 5475912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 5485912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown *outDet = sstot > 0.000001f ? 1.0f - (sserr / sstot) : 1; 5495912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#if DEBUG_STRATEGY 5505912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown ALOGD(" - sserr=%f", sserr); 5515912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown ALOGD(" - sstot=%f", sstot); 5525912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown ALOGD(" - det=%f", *outDet); 5535912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#endif 5545912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return true; 5555912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 5565912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 5575912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownbool LeastSquaresVelocityTrackerStrategy::getEstimator(uint32_t id, 5585912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown VelocityTracker::Estimator* outEstimator) const { 5595912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->clear(); 5605912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 5615912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Iterate over movement samples in reverse time order and collect samples. 5625912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float x[HISTORY_SIZE]; 5635912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float y[HISTORY_SIZE]; 5645912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float w[HISTORY_SIZE]; 5655912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float time[HISTORY_SIZE]; 5665912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t m = 0; 5675912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t index = mIndex; 5685912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown const Movement& newestMovement = mMovements[mIndex]; 5695912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown do { 5705912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown const Movement& movement = mMovements[index]; 5715912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (!movement.idBits.hasBit(id)) { 5725912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown break; 5735912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 5745912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 5755912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown nsecs_t age = newestMovement.eventTime - movement.eventTime; 5765912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (age > HORIZON) { 5775912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown break; 5785912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 5795912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 5805912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown const VelocityTracker::Position& position = movement.getPosition(id); 5815912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown x[m] = position.x; 5825912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown y[m] = position.y; 5835912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown w[m] = chooseWeight(index); 5845912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown time[m] = -age * 0.000000001f; 5855912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown index = (index == 0 ? HISTORY_SIZE : index) - 1; 5865912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } while (++m < HISTORY_SIZE); 5875912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 5885912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (m == 0) { 5895912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return false; // no data 5905912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 5915912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 5925912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Calculate a least squares polynomial fit. 5935912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t degree = mDegree; 5945912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (degree > m - 1) { 5955912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown degree = m - 1; 5965912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 5975912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (degree >= 1) { 5985912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float xdet, ydet; 5995912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t n = degree + 1; 6005912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (solveLeastSquares(time, x, w, m, n, outEstimator->xCoeff, &xdet) 6015912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown && solveLeastSquares(time, y, w, m, n, outEstimator->yCoeff, &ydet)) { 6025912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->time = newestMovement.eventTime; 6035912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->degree = degree; 6045912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->confidence = xdet * ydet; 6055912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#if DEBUG_STRATEGY 6065912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown ALOGD("estimate: degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f", 6075912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown int(outEstimator->degree), 6085912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown vectorToString(outEstimator->xCoeff, n).string(), 6095912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown vectorToString(outEstimator->yCoeff, n).string(), 6105912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->confidence); 6115912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown#endif 6125912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return true; 6135912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 6145912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 6155912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 6165912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // No velocity data available for this pointer, but we do have its current position. 6175912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->xCoeff[0] = x[0]; 6185912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->yCoeff[0] = y[0]; 6195912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->time = newestMovement.eventTime; 6205912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->degree = 0; 6215912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->confidence = 1; 6225912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return true; 6235912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 6245912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 6255912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownfloat LeastSquaresVelocityTrackerStrategy::chooseWeight(uint32_t index) const { 6265912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown switch (mWeighting) { 6275912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown case WEIGHTING_DELTA: { 6285912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Weight points based on how much time elapsed between them and the next 6295912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // point so that points that "cover" a shorter time span are weighed less. 6305912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // delta 0ms: 0.5 6315912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // delta 10ms: 1.0 6325912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (index == mIndex) { 6335912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return 1.0f; 6345912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 6355912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t nextIndex = (index + 1) % HISTORY_SIZE; 6365912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float deltaMillis = (mMovements[nextIndex].eventTime- mMovements[index].eventTime) 6375912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * 0.000001f; 6385912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (deltaMillis < 0) { 6395912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return 0.5f; 6405912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 6415912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (deltaMillis < 10) { 6425912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return 0.5f + deltaMillis * 0.05; 6435912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 6445912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return 1.0f; 6455912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 6465912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 6475912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown case WEIGHTING_CENTRAL: { 6485912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Weight points based on their age, weighing very recent and very old points less. 6495912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // age 0ms: 0.5 6505912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // age 10ms: 1.0 6515912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // age 50ms: 1.0 6525912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // age 60ms: 0.5 6535912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float ageMillis = (mMovements[mIndex].eventTime - mMovements[index].eventTime) 6545912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * 0.000001f; 6555912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (ageMillis < 0) { 6565912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return 0.5f; 6575912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 6585912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (ageMillis < 10) { 6595912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return 0.5f + ageMillis * 0.05; 6605912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 6615912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (ageMillis < 50) { 6625912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return 1.0f; 6635912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 6645912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (ageMillis < 60) { 6655912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return 0.5f + (60 - ageMillis) * 0.05; 6665912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 6675912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return 0.5f; 6685912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 6695912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 6705912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown case WEIGHTING_RECENT: { 6715912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Weight points based on their age, weighing older points less. 6725912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // age 0ms: 1.0 6735912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // age 50ms: 1.0 6745912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // age 100ms: 0.5 6755912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float ageMillis = (mMovements[mIndex].eventTime - mMovements[index].eventTime) 6765912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown * 0.000001f; 6775912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (ageMillis < 50) { 6785912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return 1.0f; 6795912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 6805912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (ageMillis < 100) { 6815912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return 0.5f + (100 - ageMillis) * 0.01f; 6825912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 6835912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return 0.5f; 6845912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 6855912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 6865912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown case WEIGHTING_NONE: 6875912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown default: 6885912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return 1.0f; 6895912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 6905912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 6915912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 6925912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 6935912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown// --- IntegratingVelocityTrackerStrategy --- 6945912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 6955912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff BrownIntegratingVelocityTrackerStrategy::IntegratingVelocityTrackerStrategy(uint32_t degree) : 6965912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mDegree(degree) { 6975912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 6985912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 6995912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff BrownIntegratingVelocityTrackerStrategy::~IntegratingVelocityTrackerStrategy() { 7005912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 7015912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 7025912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownvoid IntegratingVelocityTrackerStrategy::clear() { 7035912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mPointerIdBits.clear(); 7045912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 7055912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 7065912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownvoid IntegratingVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { 7075912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mPointerIdBits.value &= ~idBits.value; 7085912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 7095912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 7105912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownvoid IntegratingVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, 7115912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown const VelocityTracker::Position* positions) { 7125912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t index = 0; 7135912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (BitSet32 iterIdBits(idBits); !iterIdBits.isEmpty();) { 7145912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t id = iterIdBits.clearFirstMarkedBit(); 7155912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown State& state = mPointerState[id]; 7165912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown const VelocityTracker::Position& position = positions[index++]; 7175912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (mPointerIdBits.hasBit(id)) { 7185912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown updateState(state, eventTime, position.x, position.y); 7195912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } else { 7205912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown initState(state, eventTime, position.x, position.y); 7215912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 7225912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 7235912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 7245912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mPointerIdBits = idBits; 7255912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 7265912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 7275912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownbool IntegratingVelocityTrackerStrategy::getEstimator(uint32_t id, 7285912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown VelocityTracker::Estimator* outEstimator) const { 7295912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->clear(); 7305912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 7315912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (mPointerIdBits.hasBit(id)) { 7325912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown const State& state = mPointerState[id]; 7335912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown populateEstimator(state, outEstimator); 7345912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return true; 7355912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 7365912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 7375912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return false; 7385912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 7395912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 7405912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownvoid IntegratingVelocityTrackerStrategy::initState(State& state, 7415912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown nsecs_t eventTime, float xpos, float ypos) const { 7425912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.updateTime = eventTime; 7435912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.degree = 0; 7445912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 7455912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.xpos = xpos; 7465912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.xvel = 0; 7475912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.xaccel = 0; 7485912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.ypos = ypos; 7495912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.yvel = 0; 7505912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.yaccel = 0; 7515912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 7525912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 7535912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownvoid IntegratingVelocityTrackerStrategy::updateState(State& state, 7545912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown nsecs_t eventTime, float xpos, float ypos) const { 7555912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown const nsecs_t MIN_TIME_DELTA = 2 * NANOS_PER_MS; 7565912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown const float FILTER_TIME_CONSTANT = 0.010f; // 10 milliseconds 7575912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 7585912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (eventTime <= state.updateTime + MIN_TIME_DELTA) { 7595912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return; 7605912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 7615912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 7625912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float dt = (eventTime - state.updateTime) * 0.000000001f; 7635912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.updateTime = eventTime; 7645912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 7655912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float xvel = (xpos - state.xpos) / dt; 7665912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float yvel = (ypos - state.ypos) / dt; 7675912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (state.degree == 0) { 7685912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.xvel = xvel; 7695912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.yvel = yvel; 7705912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.degree = 1; 7715912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } else { 7725912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float alpha = dt / (FILTER_TIME_CONSTANT + dt); 7735912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (mDegree == 1) { 7745912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.xvel += (xvel - state.xvel) * alpha; 7755912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.yvel += (yvel - state.yvel) * alpha; 7765912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } else { 7775912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float xaccel = (xvel - state.xvel) / dt; 7785912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float yaccel = (yvel - state.yvel) / dt; 7795912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (state.degree == 1) { 7805912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.xaccel = xaccel; 7815912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.yaccel = yaccel; 7825912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.degree = 2; 7835912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } else { 7845912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.xaccel += (xaccel - state.xaccel) * alpha; 7855912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.yaccel += (yaccel - state.yaccel) * alpha; 7865912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 7875912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.xvel += (state.xaccel * dt) * alpha; 7885912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.yvel += (state.yaccel * dt) * alpha; 7895912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 7905912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 7915912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.xpos = xpos; 7925912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown state.ypos = ypos; 7935912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 7945912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 7955912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownvoid IntegratingVelocityTrackerStrategy::populateEstimator(const State& state, 7965912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown VelocityTracker::Estimator* outEstimator) const { 7975912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->time = state.updateTime; 7985912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->confidence = 1.0f; 7995912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->degree = state.degree; 8005912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->xCoeff[0] = state.xpos; 8015912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->xCoeff[1] = state.xvel; 8025912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->xCoeff[2] = state.xaccel / 2; 8035912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->yCoeff[0] = state.ypos; 8045912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->yCoeff[1] = state.yvel; 8055912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->yCoeff[2] = state.yaccel / 2; 8065912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 8075912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 8085912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 8095912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown// --- LegacyVelocityTrackerStrategy --- 8105912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 8115912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownconst nsecs_t LegacyVelocityTrackerStrategy::HORIZON; 8125912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownconst uint32_t LegacyVelocityTrackerStrategy::HISTORY_SIZE; 8135912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownconst nsecs_t LegacyVelocityTrackerStrategy::MIN_DURATION; 8145912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 8155912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff BrownLegacyVelocityTrackerStrategy::LegacyVelocityTrackerStrategy() { 8165912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown clear(); 8175912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 8185912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 8195912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff BrownLegacyVelocityTrackerStrategy::~LegacyVelocityTrackerStrategy() { 8205912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 8215912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 8225912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownvoid LegacyVelocityTrackerStrategy::clear() { 8235912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mIndex = 0; 8245912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mMovements[0].idBits.clear(); 8255912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 8265912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 8275912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownvoid LegacyVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { 8285912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value); 8295912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mMovements[mIndex].idBits = remainingIdBits; 8305912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 8315912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 8325912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownvoid LegacyVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, 8335912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown const VelocityTracker::Position* positions) { 8345912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (++mIndex == HISTORY_SIZE) { 8355912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown mIndex = 0; 8365912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 8375912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 8385912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown Movement& movement = mMovements[mIndex]; 8395912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown movement.eventTime = eventTime; 8405912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown movement.idBits = idBits; 8415912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t count = idBits.count(); 8425912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown for (uint32_t i = 0; i < count; i++) { 8435912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown movement.positions[i] = positions[i]; 8445912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 8455912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 8465912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 8475912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brownbool LegacyVelocityTrackerStrategy::getEstimator(uint32_t id, 8485912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown VelocityTracker::Estimator* outEstimator) const { 8495912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->clear(); 8505912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 8515912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown const Movement& newestMovement = mMovements[mIndex]; 8525912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (!newestMovement.idBits.hasBit(id)) { 8535912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return false; // no data 8545912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 8555912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 8565912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Find the oldest sample that contains the pointer and that is not older than HORIZON. 8575912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown nsecs_t minTime = newestMovement.eventTime - HORIZON; 8585912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t oldestIndex = mIndex; 8595912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t numTouches = 1; 8605912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown do { 8615912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t nextOldestIndex = (oldestIndex == 0 ? HISTORY_SIZE : oldestIndex) - 1; 8625912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown const Movement& nextOldestMovement = mMovements[nextOldestIndex]; 8635912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (!nextOldestMovement.idBits.hasBit(id) 8645912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown || nextOldestMovement.eventTime < minTime) { 8655912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown break; 8665912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 8675912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown oldestIndex = nextOldestIndex; 8685912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } while (++numTouches < HISTORY_SIZE); 8695912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 8705912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Calculate an exponentially weighted moving average of the velocity estimate 8715912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // at different points in time measured relative to the oldest sample. 8725912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // This is essentially an IIR filter. Newer samples are weighted more heavily 8735912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // than older samples. Samples at equal time points are weighted more or less 8745912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // equally. 8755912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // 8765912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // One tricky problem is that the sample data may be poorly conditioned. 8775912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Sometimes samples arrive very close together in time which can cause us to 8785912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // overestimate the velocity at that time point. Most samples might be measured 8795912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // 16ms apart but some consecutive samples could be only 0.5sm apart because 8805912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // the hardware or driver reports them irregularly or in bursts. 8815912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float accumVx = 0; 8825912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float accumVy = 0; 8835912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t index = oldestIndex; 8845912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown uint32_t samplesUsed = 0; 8855912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown const Movement& oldestMovement = mMovements[oldestIndex]; 8865912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown const VelocityTracker::Position& oldestPosition = oldestMovement.getPosition(id); 8875912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown nsecs_t lastDuration = 0; 8885912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 8895912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown while (numTouches-- > 1) { 8905912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (++index == HISTORY_SIZE) { 8915912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown index = 0; 8925912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 8935912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown const Movement& movement = mMovements[index]; 8945912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown nsecs_t duration = movement.eventTime - oldestMovement.eventTime; 8955912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 8965912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // If the duration between samples is small, we may significantly overestimate 8975912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // the velocity. Consequently, we impose a minimum duration constraint on the 8985912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // samples that we include in the calculation. 8995912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (duration >= MIN_DURATION) { 9005912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown const VelocityTracker::Position& position = movement.getPosition(id); 9015912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float scale = 1000000000.0f / duration; // one over time delta in seconds 9025912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float vx = (position.x - oldestPosition.x) * scale; 9035912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown float vy = (position.y - oldestPosition.y) * scale; 9045912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown accumVx = (accumVx * lastDuration + vx * duration) / (duration + lastDuration); 9055912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown accumVy = (accumVy * lastDuration + vy * duration) / (duration + lastDuration); 9065912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown lastDuration = duration; 9075912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown samplesUsed += 1; 9085912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 9095912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 9105912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 9115912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown // Report velocity. 9125912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown const VelocityTracker::Position& newestPosition = newestMovement.getPosition(id); 9135912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->time = newestMovement.eventTime; 9145912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->confidence = 1; 9155912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->xCoeff[0] = newestPosition.x; 9165912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->yCoeff[0] = newestPosition.y; 9175912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown if (samplesUsed) { 9185912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->xCoeff[1] = accumVx; 9195912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->yCoeff[1] = accumVy; 9205912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->degree = 1; 9215912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } else { 9225912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown outEstimator->degree = 0; 9235912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown } 9245912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown return true; 9255912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} 9265912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown 9275912f95d26f77d2b6df13e1f2672e48e3f9b871cJeff Brown} // namespace android 928