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