VelocityTracker.cpp revision 13e4bdc8fc3c9cede83a1e99251235006226f67d
19066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project/* 29066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * Copyright (C) 2012 The Android Open Source Project 39066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * 49066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * Licensed under the Apache License, Version 2.0 (the "License"); 59066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * you may not use this file except in compliance with the License. 69066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * You may obtain a copy of the License at 79066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * 89066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * http://www.apache.org/licenses/LICENSE-2.0 99066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * 109066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * Unless required by applicable law or agreed to in writing, software 119066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * distributed under the License is distributed on an "AS IS" BASIS, 129066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 139066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * See the License for the specific language governing permissions and 149066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * limitations under the License. 159066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project */ 169066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 179066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#define LOG_TAG "VelocityTracker" 189066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project//#define LOG_NDEBUG 0 199066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 209b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li// Log debug messages about velocity tracking. 219066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#define DEBUG_VELOCITY 0 229b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 239066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project// Log debug messages about the progress of the algorithm itself. 249066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#define DEBUG_STRATEGY 0 259066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 269066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#include <inttypes.h> 279066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#include <limits.h> 289066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#include <math.h> 29a696f5d667227365da732481770767dcb330dd23Mathias Agopian 309066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#include <android-base/stringprintf.h> 319066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#include <cutils/properties.h> 329066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#include <input/VelocityTracker.h> 339066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#include <utils/BitSet.h> 349066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#include <utils/Timers.h> 359066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 369066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Projectnamespace android { 379066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 389066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project// Nanoseconds per milliseconds. 399066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Projectstatic const nsecs_t NANOS_PER_MS = 1000000; 40df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Main 417478ea6848c0059e65a4089b4ec2ff4158520870Wu-cheng Li// Threshold for determining that a pointer has stopped moving. 427478ea6848c0059e65a4089b4ec2ff4158520870Wu-cheng Li// Some input devices do not send ACTION_MOVE events in the case where a pointer has 43df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Main// stopped. We need to detect this case so that we can accurately predict the 44df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Main// velocity after the pointer starts moving again. 457478ea6848c0059e65a4089b4ec2ff4158520870Wu-cheng Listatic const nsecs_t ASSUME_POINTER_STOPPED_TIME = 40 * NANOS_PER_MS; 467478ea6848c0059e65a4089b4ec2ff4158520870Wu-cheng Li 47df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Main 48df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Mainstatic float vectorDot(const float* a, const float* b, uint32_t m) { 49df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Main float r = 0; 50df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Main for (size_t i = 0; i < m; i++) { 51df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Main r += *(a++) * *(b++); 52df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Main } 53df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Main return r; 54df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Main} 557478ea6848c0059e65a4089b4ec2ff4158520870Wu-cheng Li 56df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Mainstatic float vectorNorm(const float* a, uint32_t m) { 579066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project float r = 0; 589066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project for (size_t i = 0; i < m; i++) { 599066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project float t = *(a++); 609b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li r += t * t; 61d2c2929c94bec68741b85f4174e11307fb65157fWu-cheng Li } 62da83f4674a564007baac03db062a289c8158d940Benny Wong return sqrtf(r); 63da83f4674a564007baac03db062a289c8158d940Benny Wong} 64da83f4674a564007baac03db062a289c8158d940Benny Wong 65da83f4674a564007baac03db062a289c8158d940Benny Wong#if DEBUG_STRATEGY || DEBUG_VELOCITY 66da83f4674a564007baac03db062a289c8158d940Benny Wongstatic std::string vectorToString(const float* a, uint32_t m) { 67da83f4674a564007baac03db062a289c8158d940Benny Wong std::string str; 68da83f4674a564007baac03db062a289c8158d940Benny Wong str += "["; 69da83f4674a564007baac03db062a289c8158d940Benny Wong for (size_t i = 0; i < m; i++) { 70da83f4674a564007baac03db062a289c8158d940Benny Wong if (i) { 71da83f4674a564007baac03db062a289c8158d940Benny Wong str += ","; 729066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 739066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project str += android::base::StringPrintf(" %f", *(a++)); 749066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 759066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project str += " ]"; 769066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return str; 779066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 789066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#endif 79e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks 809066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#if DEBUG_STRATEGY 813f4639a6611222ae1ae5493de49213250d292139Wu-cheng Listatic std::string matrixToString(const float* a, uint32_t m, uint32_t n, bool rowMajor) { 829066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project std::string str; 839066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project str = "["; 8494927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp for (size_t i = 0; i < m; i++) { 859b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li if (i) { 869066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project str += ","; 87e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang } 88e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang str += " ["; 89e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang for (size_t j = 0; j < n; j++) { 90e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang if (j) { 91e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang str += ","; 92e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang } 939066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project str += android::base::StringPrintf(" %f", a[rowMajor ? i * n + j : j * m + i]); 94e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang } 95e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang str += " ]"; 96e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang } 97e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang str += " ]"; 98e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang return str; 99e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang} 100e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang#endif 101e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang 102e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang 103e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang// --- VelocityTracker --- 1049066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 1059b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li// The default velocity tracker strategy. 106e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang// Although other strategies are available for testing and comparison purposes, 1079066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project// this is the strategy that applications will actually use. Be very careful 1089066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project// when adjusting the default strategy because it can dramatically affect 109e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang// (often in a bad way) the user experience. 1109066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Projectconst char* VelocityTracker::DEFAULT_STRATEGY = "impulse"; 1119066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 1129066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source ProjectVelocityTracker::VelocityTracker(const char* strategy) : 1139066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project mLastEventTime(0), mCurrentPointerIdBits(0), mActivePointerId(-1) { 114e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks char value[PROPERTY_VALUE_MAX]; 1153f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li 1169066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // Allow the default strategy to be overridden using a system property for debugging. 1179066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (!strategy) { 1189066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project int length = property_get("debug.velocitytracker.strategy", value, NULL); 1199066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (length > 0) { 1209066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project strategy = value; 1219066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } else { 1229066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project strategy = DEFAULT_STRATEGY; 1239066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 1249066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 1259066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 126e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang // Configure the strategy. 1279066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (!configureStrategy(strategy)) { 1289b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li ALOGD("Unrecognized velocity tracker strategy name '%s'.", strategy); 1299b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li if (!configureStrategy(DEFAULT_STRATEGY)) { 1309b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li LOG_ALWAYS_FATAL("Could not create the default velocity tracker strategy '%s'!", 1319066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project strategy); 1329b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 133e25cc656392d8866e163f78b60c7791455d0fb44Chih-Chung Chang } 1349066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 1359b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 1369066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source ProjectVelocityTracker::~VelocityTracker() { 1379066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project delete mStrategy; 1389066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 1399b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 1409066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Projectbool VelocityTracker::configureStrategy(const char* strategy) { 1419066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project mStrategy = createStrategy(strategy); 1429b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li return mStrategy != NULL; 1439066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 1449066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 1459066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source ProjectVelocityTrackerStrategy* VelocityTracker::createStrategy(const char* strategy) { 1469066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (!strcmp("impulse", strategy)) { 1479066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // Physical model of pushing an object. Quality: VERY GOOD. 1489066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // Works with duplicate coordinates, unclean finger liftoff. 1499b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li return new ImpulseVelocityTrackerStrategy(); 1509066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 1519066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (!strcmp("lsq1", strategy)) { 1529066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // 1st order least squares. Quality: POOR. 1539b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li // Frequently underfits the touch data especially when the finger accelerates 1549066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // or changes direction. Often underestimates velocity. The direction 1559066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // is overly influenced by historical touch points. 1569066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return new LeastSquaresVelocityTrackerStrategy(1); 1579066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 1589b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li if (!strcmp("lsq2", strategy)) { 1599066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // 2nd order least squares. Quality: VERY GOOD. 1609066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // Pretty much ideal, but can be confused by certain kinds of touch data, 1619066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // particularly if the panel has a tendency to generate delayed, 1629066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // duplicate or jittery touch coordinates when the finger is released. 1639066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return new LeastSquaresVelocityTrackerStrategy(2); 1649066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 1659066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (!strcmp("lsq3", strategy)) { 1669066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // 3rd order least squares. Quality: UNUSABLE. 1679066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // Frequently overfits the touch data yielding wildly divergent estimates 168ffe1cf251a4f8469695b8acfa37270684dc1b70cWu-cheng Li // of the velocity when the finger is released. 1699066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return new LeastSquaresVelocityTrackerStrategy(3); 170ffe1cf251a4f8469695b8acfa37270684dc1b70cWu-cheng Li } 1719b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li if (!strcmp("wlsq2-delta", strategy)) { 1729066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // 2nd order weighted least squares, delta weighting. Quality: EXPERIMENTAL 1739b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li return new LeastSquaresVelocityTrackerStrategy(2, 1749066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project LeastSquaresVelocityTrackerStrategy::WEIGHTING_DELTA); 1759066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 1769066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (!strcmp("wlsq2-central", strategy)) { 1779066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // 2nd order weighted least squares, central weighting. Quality: EXPERIMENTAL 178ffe1cf251a4f8469695b8acfa37270684dc1b70cWu-cheng Li return new LeastSquaresVelocityTrackerStrategy(2, 1799066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project LeastSquaresVelocityTrackerStrategy::WEIGHTING_CENTRAL); 180ffe1cf251a4f8469695b8acfa37270684dc1b70cWu-cheng Li } 1819b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li if (!strcmp("wlsq2-recent", strategy)) { 1829066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // 2nd order weighted least squares, recent weighting. Quality: EXPERIMENTAL 1839066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return new LeastSquaresVelocityTrackerStrategy(2, 1849066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project LeastSquaresVelocityTrackerStrategy::WEIGHTING_RECENT); 1859066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 1869b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li if (!strcmp("int1", strategy)) { 1879066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // 1st order integrating filter. Quality: GOOD. 1889066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // Not as good as 'lsq2' because it cannot estimate acceleration but it is 1899066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // more tolerant of errors. Like 'lsq1', this strategy tends to underestimate 1909066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // the velocity of a fling but this strategy tends to respond to changes in 191b8a10fe45657f2dcc50cae8a06805f8438a6937eWu-cheng Li // direction more quickly and accurately. 192b8a10fe45657f2dcc50cae8a06805f8438a6937eWu-cheng Li return new IntegratingVelocityTrackerStrategy(1); 193b8a10fe45657f2dcc50cae8a06805f8438a6937eWu-cheng Li } 194b8a10fe45657f2dcc50cae8a06805f8438a6937eWu-cheng Li if (!strcmp("int2", strategy)) { 195b8a10fe45657f2dcc50cae8a06805f8438a6937eWu-cheng Li // 2nd order integrating filter. Quality: EXPERIMENTAL. 1969066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // For comparison purposes only. Unlike 'int1' this strategy can compensate 1979066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // for acceleration but it typically overestimates the effect. 1989066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return new IntegratingVelocityTrackerStrategy(2); 1999066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 2009066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (!strcmp("legacy", strategy)) { 2019066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // Legacy velocity tracker algorithm. Quality: POOR. 2029066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // For comparison purposes only. This algorithm is strongly influenced by 2039066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // old data points, consistently underestimates velocity and takes a very long 2049066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // time to adjust to changes in direction. 2059066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return new LegacyVelocityTrackerStrategy(); 2069066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 2079066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return NULL; 208df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Main} 209a696f5d667227365da732481770767dcb330dd23Mathias Agopian 210df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Mainvoid VelocityTracker::clear() { 211da0a56df963353a1f1bd1914fa31f870d982dd5aScott Main mCurrentPointerIdBits.clear(); 2129b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li mActivePointerId = -1; 2139b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 2149066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project mStrategy->clear(); 2159066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 2169066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 2179066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Projectvoid VelocityTracker::clearPointers(BitSet32 idBits) { 2189b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li BitSet32 remainingIdBits(mCurrentPointerIdBits.value & ~idBits.value); 2199066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project mCurrentPointerIdBits = remainingIdBits; 2209066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 2219066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (mActivePointerId >= 0 && idBits.hasBit(mActivePointerId)) { 2229066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project mActivePointerId = !remainingIdBits.isEmpty() ? remainingIdBits.firstMarkedBit() : -1; 2239b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 2249066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 2259066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project mStrategy->clearPointers(idBits); 2269066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 2279066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 2289b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Livoid VelocityTracker::addMovement(nsecs_t eventTime, BitSet32 idBits, const Position* positions) { 2299066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project while (idBits.count() > MAX_POINTERS) { 2309066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project idBits.clearLastMarkedBit(); 2319066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 2329066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 2339066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if ((mCurrentPointerIdBits.value & idBits.value) 2349066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project && eventTime >= mLastEventTime + ASSUME_POINTER_STOPPED_TIME) { 2359066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#if DEBUG_VELOCITY 2369b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li ALOGD("VelocityTracker: stopped for %0.3f ms, clearing state.", 2379066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project (eventTime - mLastEventTime) * 0.000001f); 2389066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#endif 2399066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // We have not received any movements for too long. Assume that all pointers 2409066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // have stopped. 2419066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project mStrategy->clear(); 2429066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 2439066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project mLastEventTime = eventTime; 2449066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 2459066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project mCurrentPointerIdBits = idBits; 2469066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (mActivePointerId < 0 || !idBits.hasBit(mActivePointerId)) { 24794927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp mActivePointerId = idBits.isEmpty() ? -1 : idBits.firstMarkedBit(); 248a6118c6383c6f5703a576d08586a340fd71d28a4Dave Sparks } 249a6118c6383c6f5703a576d08586a340fd71d28a4Dave Sparks 25094927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp mStrategy->addMovement(eventTime, idBits, positions); 2519066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 2529066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#if DEBUG_VELOCITY 2539066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project ALOGD("VelocityTracker: addMovement eventTime=%" PRId64 ", idBits=0x%08x, activePointerId=%d", 2549066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project eventTime, idBits.value, mActivePointerId); 2559066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project for (BitSet32 iterBits(idBits); !iterBits.isEmpty(); ) { 2569066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project uint32_t id = iterBits.firstMarkedBit(); 2579066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project uint32_t index = idBits.getIndexOfBit(id); 2589066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project iterBits.clearBit(id); 2599066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project Estimator estimator; 26094927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp getEstimator(id, &estimator); 26194927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp ALOGD(" %d: position (%0.3f, %0.3f), " 26294927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp "estimator (degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f)", 26394927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp id, positions[index].x, positions[index].y, 26494927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp int(estimator.degree), 26594927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp vectorToString(estimator.xCoeff, estimator.degree + 1).c_str(), 26694927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp vectorToString(estimator.yCoeff, estimator.degree + 1).c_str(), 26794927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp estimator.confidence); 26894927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp } 26994927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp#endif 27094927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp} 27194927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp 27294927dffce1626898b59579dfc5af53b5de8cef6Andrew Harpvoid VelocityTracker::addMovement(const MotionEvent* event) { 27394927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp int32_t actionMasked = event->getActionMasked(); 27494927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp 27594927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp switch (actionMasked) { 27694927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp case AMOTION_EVENT_ACTION_DOWN: 27794927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp case AMOTION_EVENT_ACTION_HOVER_ENTER: 27894927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp // Clear all pointers on down before adding the new movement. 27994927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp clear(); 28094927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp break; 28194927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp case AMOTION_EVENT_ACTION_POINTER_DOWN: { 28294927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp // Start a new movement trace for a pointer that just went down. 28394927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp // We do this on down instead of on up because the client may want to query the 28494927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp // final velocity for a pointer that just went up. 28594927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp BitSet32 downIdBits; 2869066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project downIdBits.markBit(event->getPointerId(event->getActionIndex())); 2879066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project clearPointers(downIdBits); 28894927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp break; 2893f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li } 2903f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li case AMOTION_EVENT_ACTION_MOVE: 2913f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li case AMOTION_EVENT_ACTION_HOVER_MOVE: 2923f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li break; 2933f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li default: 2943f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li // Ignore all other actions because they do not convey any new information about 2953f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li // pointer movement. We also want to preserve the last known velocity of the pointers. 296c10275abd6a494c93a025f683dde104a5d4f2793Wu-cheng Li // Note that ACTION_UP and ACTION_POINTER_UP always report the last known position 2973f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li // of the pointers that went up. ACTION_POINTER_UP does include the new position of 2983f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li // pointers that remained down but we will also receive an ACTION_MOVE with this 2993f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li // information if any of them actually moved. Since we don't know how many pointers 3003f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li // will be going up at once it makes sense to just wait for the following ACTION_MOVE 30194927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp // before adding the movement. 30294927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp return; 30394927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp } 30494927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp 30594927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp size_t pointerCount = event->getPointerCount(); 3065b9bcda3a26e9b1f9b1eff28a2be8853d69614f0Wu-cheng Li if (pointerCount > MAX_POINTERS) { 30794927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp pointerCount = MAX_POINTERS; 30894927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp } 3099066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 3109066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project BitSet32 idBits; 3119066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project for (size_t i = 0; i < pointerCount; i++) { 3129066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project idBits.markBit(event->getPointerId(i)); 3139066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 3149066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 3159066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project uint32_t pointerIndex[MAX_POINTERS]; 3169066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project for (size_t i = 0; i < pointerCount; i++) { 3179066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project pointerIndex[i] = idBits.getIndexOfBit(event->getPointerId(i)); 3189066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 3199066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 3209066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project nsecs_t eventTime; 3219066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project Position positions[pointerCount]; 322c62f9bd13327937aa2d2f20b44215397120634c1Dave Sparks 3239066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project size_t historySize = event->getHistorySize(); 3249066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project for (size_t h = 0; h < historySize; h++) { 3259066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project eventTime = event->getHistoricalEventTime(h); 3269066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project for (size_t i = 0; i < pointerCount; i++) { 327c62f9bd13327937aa2d2f20b44215397120634c1Dave Sparks uint32_t index = pointerIndex[i]; 328c62f9bd13327937aa2d2f20b44215397120634c1Dave Sparks positions[index].x = event->getHistoricalX(i, h); 329e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks positions[index].y = event->getHistoricalY(i, h); 3309066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 331e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks addMovement(eventTime, idBits, positions); 3329066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 3339066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 334c62f9bd13327937aa2d2f20b44215397120634c1Dave Sparks eventTime = event->getEventTime(); 335e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks for (size_t i = 0; i < pointerCount; i++) { 3369066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project uint32_t index = pointerIndex[i]; 337e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks positions[index].x = event->getX(i); 3389066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project positions[index].y = event->getY(i); 3399b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 340c62f9bd13327937aa2d2f20b44215397120634c1Dave Sparks addMovement(eventTime, idBits, positions); 3419066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 342a6118c6383c6f5703a576d08586a340fd71d28a4Dave Sparks 3439066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Projectbool VelocityTracker::getVelocity(uint32_t id, float* outVx, float* outVy) const { 344a6118c6383c6f5703a576d08586a340fd71d28a4Dave Sparks Estimator estimator; 345a6118c6383c6f5703a576d08586a340fd71d28a4Dave Sparks if (getEstimator(id, &estimator) && estimator.degree >= 1) { 346a6118c6383c6f5703a576d08586a340fd71d28a4Dave Sparks *outVx = estimator.xCoeff[1]; 3479066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project *outVy = estimator.yCoeff[1]; 34894927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp return true; 349a6118c6383c6f5703a576d08586a340fd71d28a4Dave Sparks } 350a6118c6383c6f5703a576d08586a340fd71d28a4Dave Sparks *outVx = 0; 351a6118c6383c6f5703a576d08586a340fd71d28a4Dave Sparks *outVy = 0; 35294927dffce1626898b59579dfc5af53b5de8cef6Andrew Harp return false; 3539066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 354a6118c6383c6f5703a576d08586a340fd71d28a4Dave Sparks 3559066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Projectbool VelocityTracker::getEstimator(uint32_t id, Estimator* outEstimator) const { 3569066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return mStrategy->getEstimator(id, outEstimator); 3579066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 358e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks 359e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks 360e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks// --- LeastSquaresVelocityTrackerStrategy --- 361e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks 362e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave SparksLeastSquaresVelocityTrackerStrategy::LeastSquaresVelocityTrackerStrategy( 363e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks uint32_t degree, Weighting weighting) : 364c62f9bd13327937aa2d2f20b44215397120634c1Dave Sparks mDegree(degree), mWeighting(weighting) { 365e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks clear(); 3669066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 367e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks 368e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave SparksLeastSquaresVelocityTrackerStrategy::~LeastSquaresVelocityTrackerStrategy() { 369e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks} 370e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks 3713f4639a6611222ae1ae5493de49213250d292139Wu-cheng Livoid LeastSquaresVelocityTrackerStrategy::clear() { 3723f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li mIndex = 0; 373e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks mMovements[0].idBits.clear(); 3749066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 3759066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 376c62f9bd13327937aa2d2f20b44215397120634c1Dave Sparksvoid LeastSquaresVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { 3779066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value); 378e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks mMovements[mIndex].idBits = remainingIdBits; 3799066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 380e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks 3819066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Projectvoid LeastSquaresVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, 3829066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project const VelocityTracker::Position* positions) { 3839066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (++mIndex == HISTORY_SIZE) { 3849066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project mIndex = 0; 3859066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 3869066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 3879066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project Movement& movement = mMovements[mIndex]; 3889066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project movement.eventTime = eventTime; 3899066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project movement.idBits = idBits; 3909066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project uint32_t count = idBits.count(); 3919066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project for (uint32_t i = 0; i < count; i++) { 3929066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project movement.positions[i] = positions[i]; 3939066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 3949066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 3959066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 3969066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project/** 3979066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * Solves a linear least squares problem to obtain a N degree polynomial that fits 3989066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * the specified input data as nearly as possible. 3999066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * 4009066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * Returns true if a solution is found, false otherwise. 4019066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * 4029066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * The input consists of two vectors of data points X and Y with indices 0..m-1 4039066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * along with a weight vector W of the same size. 4049066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * 4057478ea6848c0059e65a4089b4ec2ff4158520870Wu-cheng Li * The output is a vector B with indices 0..n that describes a polynomial 4067478ea6848c0059e65a4089b4ec2ff4158520870Wu-cheng Li * that fits the data, such the sum of W[i] * W[i] * abs(Y[i] - (B[0] + B[1] X[i] 407df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Main * + B[2] X[i]^2 ... B[n] X[i]^n)) for all i between 0 and m-1 is minimized. 408df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Main * 4097478ea6848c0059e65a4089b4ec2ff4158520870Wu-cheng Li * Accordingly, the weight vector W should be initialized by the caller with the 410df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Main * reciprocal square root of the variance of the error in each input data point. 411df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Main * In other words, an ideal choice for W would be W[i] = 1 / var(Y[i]) = 1 / stddev(Y[i]). 4129066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * The weights express the relative importance of each data point. If the weights are 4139066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * all 1, then the data points are considered to be of equal importance when fitting 4149066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * the polynomial. It is a good idea to choose weights that diminish the importance 4159066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * of data points that may have higher than usual error margins. 4169b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * 4179b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * Errors among data points are assumed to be independent. W is represented here 4189b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * as a vector although in the literature it is typically taken to be a diagonal matrix. 4199b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * 4209066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * That is to say, the function that generated the input data can be approximated 4219066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * by y(x) ~= B[0] + B[1] x + B[2] x^2 + ... + B[n] x^n. 4229066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * 4239066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * The coefficient of determination (R^2) is also returned to describe the goodness 4249066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * of fit of the model for the given data. It is a value between 0 and 1, where 1 4259066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * indicates perfect correspondence. 4269066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * 4279b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * This function first expands the X vector to a m by n matrix A such that 42836322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li * A[i][0] = 1, A[i][1] = X[i], A[i][2] = X[i]^2, ..., A[i][n] = X[i]^n, then 42936322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li * multiplies it by w[i]./ 43036322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li * 43136322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li * Then it calculates the QR decomposition of A yielding an m by m orthonormal matrix Q 43236322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li * and an m by n upper triangular matrix R. Because R is upper triangular (lower 43336322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li * part is all zeroes), we can simplify the decomposition into an m by n matrix 434da0a56df963353a1f1bd1914fa31f870d982dd5aScott Main * Q1 and a n by n matrix R1 such that A = Q1 R1. 4357478ea6848c0059e65a4089b4ec2ff4158520870Wu-cheng Li * 4367478ea6848c0059e65a4089b4ec2ff4158520870Wu-cheng Li * Finally we solve the system of linear equations given by R1 B = (Qtranspose W Y) 437df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Main * to find B. 438df4578e8ab7008a7e528d5af2ae761b33cf2bdf4Scott Main * 439068ef42c3ffe1eccec10f97f08541304f679fe67Wu-cheng Li * For efficiency, we lay out A and Q column-wise in memory because we frequently 440068ef42c3ffe1eccec10f97f08541304f679fe67Wu-cheng Li * operate on the column vectors. Conversely, we lay out R row-wise. 441068ef42c3ffe1eccec10f97f08541304f679fe67Wu-cheng Li * 4427478ea6848c0059e65a4089b4ec2ff4158520870Wu-cheng Li * http://en.wikipedia.org/wiki/Numerical_methods_for_linear_least_squares 4439066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * http://en.wikipedia.org/wiki/Gram-Schmidt 4449066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project */ 4459066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Projectstatic bool solveLeastSquares(const float* x, const float* y, 4469066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project const float* w, uint32_t m, uint32_t n, float* outB, float* outDet) { 4479066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#if DEBUG_STRATEGY 4489066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project ALOGD("solveLeastSquares: m=%d, n=%d, x=%s, y=%s, w=%s", int(m), int(n), 4499066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project vectorToString(x, m).c_str(), vectorToString(y, m).c_str(), 4509066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project vectorToString(w, m).c_str()); 4519066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#endif 4529066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 453244f8c26365a303d9dd861bd48a29a4b48578da1Chih-Chung Chang // Expand the X vector to a matrix A, pre-multiplied by the weights. 454244f8c26365a303d9dd861bd48a29a4b48578da1Chih-Chung Chang float a[n][m]; // column-major order 455244f8c26365a303d9dd861bd48a29a4b48578da1Chih-Chung Chang for (uint32_t h = 0; h < m; h++) { 456244f8c26365a303d9dd861bd48a29a4b48578da1Chih-Chung Chang a[0][h] = w[h]; 457244f8c26365a303d9dd861bd48a29a4b48578da1Chih-Chung Chang for (uint32_t i = 1; i < n; i++) { 458244f8c26365a303d9dd861bd48a29a4b48578da1Chih-Chung Chang a[i][h] = a[i - 1][h] * x[h]; 459244f8c26365a303d9dd861bd48a29a4b48578da1Chih-Chung Chang } 460244f8c26365a303d9dd861bd48a29a4b48578da1Chih-Chung Chang } 461244f8c26365a303d9dd861bd48a29a4b48578da1Chih-Chung Chang#if DEBUG_STRATEGY 462244f8c26365a303d9dd861bd48a29a4b48578da1Chih-Chung Chang ALOGD(" - a=%s", matrixToString(&a[0][0], m, n, false /*rowMajor*/).c_str()); 463244f8c26365a303d9dd861bd48a29a4b48578da1Chih-Chung Chang#endif 464244f8c26365a303d9dd861bd48a29a4b48578da1Chih-Chung Chang 465244f8c26365a303d9dd861bd48a29a4b48578da1Chih-Chung Chang // Apply the Gram-Schmidt process to A to obtain its QR decomposition. 4669066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project float q[n][m]; // orthonormal basis, column-major order 4679066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project float r[n][n]; // upper triangular matrix, row-major order 4689066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project for (uint32_t j = 0; j < n; j++) { 4699066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project for (uint32_t h = 0; h < m; h++) { 4709066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project q[j][h] = a[j][h]; 4719066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 4729066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project for (uint32_t i = 0; i < j; i++) { 4739066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project float dot = vectorDot(&q[j][0], &q[i][0], m); 4749066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project for (uint32_t h = 0; h < m; h++) { 4759066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project q[j][h] -= dot * q[i][h]; 4769066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 4779066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 4789066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 4799066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project float norm = vectorNorm(&q[j][0], m); 4809066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (norm < 0.000001f) { 4819066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // vectors are linearly dependent or zero so no solution 4829066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#if DEBUG_STRATEGY 4839b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li ALOGD(" - no solution, norm=%f", norm); 4849066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#endif 4859066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return false; 4869066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 4879066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 4889066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project float invNorm = 1.0f / norm; 4899066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project for (uint32_t h = 0; h < m; h++) { 4909066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project q[j][h] *= invNorm; 4919b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 4929b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li for (uint32_t i = 0; i < n; i++) { 4939b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li r[j][i] = i < j ? 0 : vectorDot(&q[j][0], &a[i][0], m); 4949b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 4959b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 4969b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li#if DEBUG_STRATEGY 4979b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li ALOGD(" - q=%s", matrixToString(&q[0][0], m, n, false /*rowMajor*/).c_str()); 4989b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li ALOGD(" - r=%s", matrixToString(&r[0][0], n, n, true /*rowMajor*/).c_str()); 4999b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 5009b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li // calculate QR, if we factored A correctly then QR should equal A 50140057ce749c8c4d274db0352a2af4344bda92dbaWu-cheng Li float qr[n][m]; 50240057ce749c8c4d274db0352a2af4344bda92dbaWu-cheng Li for (uint32_t h = 0; h < m; h++) { 50340057ce749c8c4d274db0352a2af4344bda92dbaWu-cheng Li for (uint32_t i = 0; i < n; i++) { 50440057ce749c8c4d274db0352a2af4344bda92dbaWu-cheng Li qr[i][h] = 0; 5059066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project for (uint32_t j = 0; j < n; j++) { 5069066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project qr[i][h] += q[j][h] * r[j][i]; 5079066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 5089066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 5099066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 5109066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project ALOGD(" - qr=%s", matrixToString(&qr[0][0], m, n, false /*rowMajor*/).c_str()); 511e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks#endif 512e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks 513e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks // Solve R B = Qt W Y to find B. This is easy because R is upper triangular. 514e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks // We just work from bottom-right to top-left calculating B's coefficients. 515e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks float wy[m]; 5169b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li for (uint32_t h = 0; h < m; h++) { 5179b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li wy[h] = y[h] * w[h]; 5189b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 5199b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li for (uint32_t i = n; i != 0; ) { 5209b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li i--; 5219b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li outB[i] = vectorDot(&q[i][0], wy, m); 5229b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li for (uint32_t j = n - 1; j > i; j--) { 5239b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li outB[i] -= r[i][j] * outB[j]; 5249b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 5259b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li outB[i] /= r[i][i]; 5269b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 527e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks#if DEBUG_STRATEGY 52840057ce749c8c4d274db0352a2af4344bda92dbaWu-cheng Li ALOGD(" - b=%s", vectorToString(outB, n).c_str()); 52940057ce749c8c4d274db0352a2af4344bda92dbaWu-cheng Li#endif 53040057ce749c8c4d274db0352a2af4344bda92dbaWu-cheng Li 53140057ce749c8c4d274db0352a2af4344bda92dbaWu-cheng Li // Calculate the coefficient of determination as 1 - (SSerr / SStot) where 532e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks // SSerr is the residual sum of squares (variance of the error), 533e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks // and SStot is the total sum of squares (variance of the data) where each 534e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks // has been weighted. 535e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks float ymean = 0; 536e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks for (uint32_t h = 0; h < m; h++) { 537e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks ymean += y[h]; 538e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks } 5399066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project ymean /= m; 5409066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 541e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks float sserr = 0; 5429066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project float sstot = 0; 5439066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project for (uint32_t h = 0; h < m; h++) { 5449066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project float err = y[h] - outB[0]; 5459066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project float term = 1; 546e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks for (uint32_t i = 1; i < n; i++) { 5473f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li term *= x[h]; 5483f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li err -= term * outB[i]; 5493f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li } 5503f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li sserr += w[h] * w[h] * err * err; 5513f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li float var = y[h] - ymean; 5523f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li sstot += w[h] * w[h] * var * var; 5533f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li } 5543f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li *outDet = sstot > 0.000001f ? 1.0f - (sserr / sstot) : 1; 5553f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li#if DEBUG_STRATEGY 5560ca25191c663ef229f1f475b17899f2017ed6980Wu-cheng Li ALOGD(" - sserr=%f", sserr); 55736f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li ALOGD(" - sstot=%f", sstot); 55836f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li ALOGD(" - det=%f", *outDet); 55936f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li#endif 5600ca25191c663ef229f1f475b17899f2017ed6980Wu-cheng Li return true; 5610ca25191c663ef229f1f475b17899f2017ed6980Wu-cheng Li} 56236f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li 56336f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li/* 56436f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li * Optimized unweighted second-order least squares fit. About 2x speed improvement compared to 56536f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li * the default implementation 56636f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li */ 5673f4639a6611222ae1ae5493de49213250d292139Wu-cheng Listatic float solveUnweightedLeastSquaresDeg2(const float* x, const float* y, size_t count) { 5683f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li float sxi = 0, sxiyi = 0, syi = 0, sxi2 = 0, sxi3 = 0, sxi2yi = 0, sxi4 = 0; 5698cbb8f5e1f939b03515cb4d5942c3fcb226efb9eWu-cheng Li 5700ca25191c663ef229f1f475b17899f2017ed6980Wu-cheng Li for (size_t i = 0; i < count; i++) { 5710ca25191c663ef229f1f475b17899f2017ed6980Wu-cheng Li float xi = x[i]; 57236f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li float yi = y[i]; 57336f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li float xi2 = xi*xi; 57436f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li float xi3 = xi2*xi; 57536f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li float xi4 = xi3*xi; 576d1d7706fce19a9a0cf71ff9b65f3aba9b89eeb3bChih-Chung Chang float xi2yi = xi2*yi; 577d1d7706fce19a9a0cf71ff9b65f3aba9b89eeb3bChih-Chung Chang float xiyi = xi*yi; 578d1d7706fce19a9a0cf71ff9b65f3aba9b89eeb3bChih-Chung Chang 579d1d7706fce19a9a0cf71ff9b65f3aba9b89eeb3bChih-Chung Chang sxi += xi; 580d1d7706fce19a9a0cf71ff9b65f3aba9b89eeb3bChih-Chung Chang sxi2 += xi2; 581d1d7706fce19a9a0cf71ff9b65f3aba9b89eeb3bChih-Chung Chang sxiyi += xiyi; 582d1d7706fce19a9a0cf71ff9b65f3aba9b89eeb3bChih-Chung Chang sxi2yi += xi2yi; 583d1d7706fce19a9a0cf71ff9b65f3aba9b89eeb3bChih-Chung Chang syi += yi; 584d1d7706fce19a9a0cf71ff9b65f3aba9b89eeb3bChih-Chung Chang sxi3 += xi3; 585e7bd22a9d9441916aa9c67d80ee9f02a2d3e10e5Chih-Chung Chang sxi4 += xi4; 586e7bd22a9d9441916aa9c67d80ee9f02a2d3e10e5Chih-Chung Chang } 587d1d7706fce19a9a0cf71ff9b65f3aba9b89eeb3bChih-Chung Chang 588d1d7706fce19a9a0cf71ff9b65f3aba9b89eeb3bChih-Chung Chang float Sxx = sxi2 - sxi*sxi / count; 589d1d7706fce19a9a0cf71ff9b65f3aba9b89eeb3bChih-Chung Chang float Sxy = sxiyi - sxi*syi / count; 590d1d7706fce19a9a0cf71ff9b65f3aba9b89eeb3bChih-Chung Chang float Sxx2 = sxi3 - sxi*sxi2 / count; 5913f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li float Sx2y = sxi2yi - sxi2*syi / count; 592e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks float Sx2x2 = sxi4 - sxi2*sxi2 / count; 5933f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li 594e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks float numerator = Sxy*Sx2x2 - Sx2y*Sxx2; 595e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks float denominator = Sxx*Sx2x2 - Sxx2*Sxx2; 5963f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li if (denominator == 0) { 59736f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li ALOGW("division by 0 when computing velocity, Sxx=%f, Sx2x2=%f, Sxx2=%f", Sxx, Sx2x2, Sxx2); 59836f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li return 0; 5993f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li } 60036f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li return numerator/denominator; 60136f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li} 60236f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li 603e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparksbool LeastSquaresVelocityTrackerStrategy::getEstimator(uint32_t id, 6048cbb8f5e1f939b03515cb4d5942c3fcb226efb9eWu-cheng Li VelocityTracker::Estimator* outEstimator) const { 605e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks outEstimator->clear(); 6063f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li 607e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks // Iterate over movement samples in reverse time order and collect samples. 608e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks float x[HISTORY_SIZE]; 609e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks float y[HISTORY_SIZE]; 6103f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li float w[HISTORY_SIZE]; 61136f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li float time[HISTORY_SIZE]; 6128cbb8f5e1f939b03515cb4d5942c3fcb226efb9eWu-cheng Li uint32_t m = 0; 6133f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li uint32_t index = mIndex; 6148cbb8f5e1f939b03515cb4d5942c3fcb226efb9eWu-cheng Li const Movement& newestMovement = mMovements[mIndex]; 615e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks do { 6163f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li const Movement& movement = mMovements[index]; 617e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks if (!movement.idBits.hasBit(id)) { 6183f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li break; 619e8b26e197f7c5e4acbdf8a5cd3f014fbc242c8abDave Sparks } 6209b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 621a1b653d41df9a7999e1dba2a508295671ff6771dJames Dong nsecs_t age = newestMovement.eventTime - movement.eventTime; 6229066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (age > HORIZON) { 6239066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project break; 6249066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 6259066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 6269066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project const VelocityTracker::Position& position = movement.getPosition(id); 6279b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li x[m] = position.x; 6289066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project y[m] = position.y; 6299066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project w[m] = chooseWeight(index); 6309066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project time[m] = -age * 0.000000001f; 6319066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project index = (index == 0 ? HISTORY_SIZE : index) - 1; 6329066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } while (++m < HISTORY_SIZE); 6339066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 6349066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (m == 0) { 6359066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return false; // no data 6369066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 6379066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 6389066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // Calculate a least squares polynomial fit. 6399066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project uint32_t degree = mDegree; 6409066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (degree > m - 1) { 6419066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project degree = m - 1; 6429066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 6439066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (degree >= 1) { 6449066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (degree == 2 && mWeighting == WEIGHTING_NONE) { // optimize unweighted, degree=2 fit 6459066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->time = newestMovement.eventTime; 6469066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->degree = 2; 6479066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->confidence = 1; 6489066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->xCoeff[0] = 0; // only slope is calculated, set rest of coefficients = 0 6499066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->yCoeff[0] = 0; 6509066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->xCoeff[1] = solveUnweightedLeastSquaresDeg2(time, x, m); 6519066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->yCoeff[1] = solveUnweightedLeastSquaresDeg2(time, y, m); 6529066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->xCoeff[2] = 0; 6539b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li outEstimator->yCoeff[2] = 0; 6549066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return true; 6559066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 6569066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 6579066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project float xdet, ydet; 6589066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project uint32_t n = degree + 1; 6599b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li if (solveLeastSquares(time, x, w, m, n, outEstimator->xCoeff, &xdet) 6609066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project && solveLeastSquares(time, y, w, m, n, outEstimator->yCoeff, &ydet)) { 6619066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->time = newestMovement.eventTime; 6629066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->degree = degree; 6639066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->confidence = xdet * ydet; 6649066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#if DEBUG_STRATEGY 6659066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project ALOGD("estimate: degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f", 6669066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project int(outEstimator->degree), 6679066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project vectorToString(outEstimator->xCoeff, n).c_str(), 6689066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project vectorToString(outEstimator->yCoeff, n).c_str(), 6699066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->confidence); 6709066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project#endif 6719066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return true; 6729066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 6739066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 6749066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 6759066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // No velocity data available for this pointer, but we do have its current position. 6769066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->xCoeff[0] = x[0]; 6779066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->yCoeff[0] = y[0]; 6789066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->time = newestMovement.eventTime; 6799066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->degree = 0; 6809066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->confidence = 1; 6819066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return true; 6829b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li} 6839066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 6849066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Projectfloat LeastSquaresVelocityTrackerStrategy::chooseWeight(uint32_t index) const { 6859066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project switch (mWeighting) { 6869066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project case WEIGHTING_DELTA: { 6879066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // Weight points based on how much time elapsed between them and the next 6889066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // point so that points that "cover" a shorter time span are weighed less. 6899066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // delta 0ms: 0.5 6904c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li // delta 10ms: 1.0 6914c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li if (index == mIndex) { 6924c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li return 1.0f; 6934c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li } 6944c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li uint32_t nextIndex = (index + 1) % HISTORY_SIZE; 6954c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li float deltaMillis = (mMovements[nextIndex].eventTime- mMovements[index].eventTime) 6964c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li * 0.000001f; 6974c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li if (deltaMillis < 0) { 6984c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li return 0.5f; 6994c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li } 7004c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li if (deltaMillis < 10) { 7014c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li return 0.5f + deltaMillis * 0.05; 7024c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li } 7034c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li return 1.0f; 7044c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li } 7054c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li 7064c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li case WEIGHTING_CENTRAL: { 7074c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li // Weight points based on their age, weighing very recent and very old points less. 7084c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li // age 0ms: 0.5 7099066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // age 10ms: 1.0 7109066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // age 50ms: 1.0 7119066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // age 60ms: 0.5 7129066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project float ageMillis = (mMovements[mIndex].eventTime - mMovements[index].eventTime) 7139066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * 0.000001f; 7149066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (ageMillis < 0) { 7159066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return 0.5f; 7169066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 7179b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li if (ageMillis < 10) { 7189b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li return 0.5f + ageMillis * 0.05; 7199b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 7209b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li if (ageMillis < 50) { 7219b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li return 1.0f; 7229b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 7239b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li if (ageMillis < 60) { 7249b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li return 0.5f + (60 - ageMillis) * 0.05; 7259b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 7269b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li return 0.5f; 7279b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 7289066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 7299066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project case WEIGHTING_RECENT: { 7309b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li // Weight points based on their age, weighing older points less. 7319b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li // age 0ms: 1.0 7329b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li // age 50ms: 1.0 7339b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li // age 100ms: 0.5 7349b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li float ageMillis = (mMovements[mIndex].eventTime - mMovements[index].eventTime) 7359b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * 0.000001f; 7364c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li if (ageMillis < 50) { 7379b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li return 1.0f; 7389b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 7399b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li if (ageMillis < 100) { 7409b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li return 0.5f + (100 - ageMillis) * 0.01f; 7419b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 7429b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li return 0.5f; 7439b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 7449b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 7459b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li case WEIGHTING_NONE: 746055c986ab841f8f758398841730f1e90313b132aRay Chen default: 7479b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li return 1.0f; 7489b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 7499b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li} 7509b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 7519b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 75236322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li// --- IntegratingVelocityTrackerStrategy --- 7536c8d2760736a0753dad96b4bb8f98c7d075e6d54Wu-cheng Li 7546c8d2760736a0753dad96b4bb8f98c7d075e6d54Wu-cheng LiIntegratingVelocityTrackerStrategy::IntegratingVelocityTrackerStrategy(uint32_t degree) : 7556c8d2760736a0753dad96b4bb8f98c7d075e6d54Wu-cheng Li mDegree(degree) { 756ff723b6c43d5a8fd0ae0e0732f5d47012d74e01dWu-cheng Li} 75724b326a8978bf78e3e560723dde221792784325bWu-cheng Li 75824b326a8978bf78e3e560723dde221792784325bWu-cheng LiIntegratingVelocityTrackerStrategy::~IntegratingVelocityTrackerStrategy() { 75924b326a8978bf78e3e560723dde221792784325bWu-cheng Li} 7608cbb8f5e1f939b03515cb4d5942c3fcb226efb9eWu-cheng Li 7618cbb8f5e1f939b03515cb4d5942c3fcb226efb9eWu-cheng Livoid IntegratingVelocityTrackerStrategy::clear() { 7628cbb8f5e1f939b03515cb4d5942c3fcb226efb9eWu-cheng Li mPointerIdBits.clear(); 7638cbb8f5e1f939b03515cb4d5942c3fcb226efb9eWu-cheng Li} 7648cbb8f5e1f939b03515cb4d5942c3fcb226efb9eWu-cheng Li 765e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Livoid IntegratingVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { 766e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Li mPointerIdBits.value &= ~idBits.value; 7679b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li} 7689b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 7699b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Livoid IntegratingVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, 7708cbb8f5e1f939b03515cb4d5942c3fcb226efb9eWu-cheng Li const VelocityTracker::Position* positions) { 7718cbb8f5e1f939b03515cb4d5942c3fcb226efb9eWu-cheng Li uint32_t index = 0; 7729b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li for (BitSet32 iterIdBits(idBits); !iterIdBits.isEmpty();) { 7739b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li uint32_t id = iterIdBits.clearFirstMarkedBit(); 7749b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li State& state = mPointerState[id]; 7759b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li const VelocityTracker::Position& position = positions[index++]; 7769b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li if (mPointerIdBits.hasBit(id)) { 7779b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li updateState(state, eventTime, position.x, position.y); 7789b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } else { 7799b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li initState(state, eventTime, position.x, position.y); 7809b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 7819b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 7829b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 7839b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li mPointerIdBits = idBits; 7849b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li} 7859b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 7869b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Libool IntegratingVelocityTrackerStrategy::getEstimator(uint32_t id, 7879b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li VelocityTracker::Estimator* outEstimator) const { 7889b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li outEstimator->clear(); 7899b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 7909b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li if (mPointerIdBits.hasBit(id)) { 7919b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li const State& state = mPointerState[id]; 7929b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li populateEstimator(state, outEstimator); 7939b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li return true; 7949b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 7959b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 7969b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li return false; 7979b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li} 7989b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 7999b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Livoid IntegratingVelocityTrackerStrategy::initState(State& state, 8009b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li nsecs_t eventTime, float xpos, float ypos) const { 8019b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li state.updateTime = eventTime; 8029b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li state.degree = 0; 8039b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 80436f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li state.xpos = xpos; 8059b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li state.xvel = 0; 806068ef42c3ffe1eccec10f97f08541304f679fe67Wu-cheng Li state.xaccel = 0; 807068ef42c3ffe1eccec10f97f08541304f679fe67Wu-cheng Li state.ypos = ypos; 8089b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li state.yvel = 0; 8099b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li state.yaccel = 0; 81036f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li} 8119b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 812068ef42c3ffe1eccec10f97f08541304f679fe67Wu-cheng Livoid IntegratingVelocityTrackerStrategy::updateState(State& state, 813068ef42c3ffe1eccec10f97f08541304f679fe67Wu-cheng Li nsecs_t eventTime, float xpos, float ypos) const { 8149b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li const nsecs_t MIN_TIME_DELTA = 2 * NANOS_PER_MS; 8159b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li const float FILTER_TIME_CONSTANT = 0.010f; // 10 milliseconds 81636f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li 8179b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li if (eventTime <= state.updateTime + MIN_TIME_DELTA) { 8189b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li return; 8199b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 8209b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 82136f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li float dt = (eventTime - state.updateTime) * 0.000000001f; 82236322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li state.updateTime = eventTime; 823068ef42c3ffe1eccec10f97f08541304f679fe67Wu-cheng Li 824068ef42c3ffe1eccec10f97f08541304f679fe67Wu-cheng Li float xvel = (xpos - state.xpos) / dt; 82536322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li float yvel = (ypos - state.ypos) / dt; 826068ef42c3ffe1eccec10f97f08541304f679fe67Wu-cheng Li if (state.degree == 0) { 8279b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li state.xvel = xvel; 8289b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li state.yvel = yvel; 8299b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li state.degree = 1; 8309b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } else { 8319b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li float alpha = dt / (FILTER_TIME_CONSTANT + dt); 8329b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li if (mDegree == 1) { 8339b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li state.xvel += (xvel - state.xvel) * alpha; 8349b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li state.yvel += (yvel - state.yvel) * alpha; 8359b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } else { 8369b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li float xaccel = (xvel - state.xvel) / dt; 8379b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li float yaccel = (yvel - state.yvel) / dt; 8389b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li if (state.degree == 1) { 8399b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li state.xaccel = xaccel; 8409b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li state.yaccel = yaccel; 8419b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li state.degree = 2; 8429b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } else { 8439b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li state.xaccel += (xaccel - state.xaccel) * alpha; 8449b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li state.yaccel += (yaccel - state.yaccel) * alpha; 845c58b42327df5fbc826e2fcc2674ab6db0edfcd92Wu-cheng Li } 846c58b42327df5fbc826e2fcc2674ab6db0edfcd92Wu-cheng Li state.xvel += (state.xaccel * dt) * alpha; 847c58b42327df5fbc826e2fcc2674ab6db0edfcd92Wu-cheng Li state.yvel += (state.yaccel * dt) * alpha; 848c58b42327df5fbc826e2fcc2674ab6db0edfcd92Wu-cheng Li } 849c58b42327df5fbc826e2fcc2674ab6db0edfcd92Wu-cheng Li } 850c58b42327df5fbc826e2fcc2674ab6db0edfcd92Wu-cheng Li state.xpos = xpos; 85136322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li state.ypos = ypos; 85236322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li} 85336322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li 85436322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Livoid IntegratingVelocityTrackerStrategy::populateEstimator(const State& state, 85536322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li VelocityTracker::Estimator* outEstimator) const { 85636f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li outEstimator->time = state.updateTime; 85736322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li outEstimator->confidence = 1.0f; 85836322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li outEstimator->degree = state.degree; 85936322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li outEstimator->xCoeff[0] = state.xpos; 86036322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li outEstimator->xCoeff[1] = state.xvel; 86136322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li outEstimator->xCoeff[2] = state.xaccel / 2; 86236322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li outEstimator->yCoeff[0] = state.ypos; 86336f68b8f24df906c969581b0b8e1a47f95dc03cbWu-cheng Li outEstimator->yCoeff[1] = state.yvel; 86436322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li outEstimator->yCoeff[2] = state.yaccel / 2; 86536322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li} 86636322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li 86736322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li 86836322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li// --- LegacyVelocityTrackerStrategy --- 86936322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li 87036322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng LiLegacyVelocityTrackerStrategy::LegacyVelocityTrackerStrategy() { 87136322db5752c7ec196f59ba94abe5d5a63cc19f5Wu-cheng Li clear(); 872c58b42327df5fbc826e2fcc2674ab6db0edfcd92Wu-cheng Li} 873c58b42327df5fbc826e2fcc2674ab6db0edfcd92Wu-cheng Li 874c58b42327df5fbc826e2fcc2674ab6db0edfcd92Wu-cheng LiLegacyVelocityTrackerStrategy::~LegacyVelocityTrackerStrategy() { 875c58b42327df5fbc826e2fcc2674ab6db0edfcd92Wu-cheng Li} 876c58b42327df5fbc826e2fcc2674ab6db0edfcd92Wu-cheng Li 877c58b42327df5fbc826e2fcc2674ab6db0edfcd92Wu-cheng Livoid LegacyVelocityTrackerStrategy::clear() { 878c58b42327df5fbc826e2fcc2674ab6db0edfcd92Wu-cheng Li mIndex = 0; 879e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Li mMovements[0].idBits.clear(); 880e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Li} 881e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Li 882e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Livoid LegacyVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { 883e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Li BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value); 884e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Li mMovements[mIndex].idBits = remainingIdBits; 885e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Li} 886e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Li 887e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Livoid LegacyVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, 888e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Li const VelocityTracker::Position* positions) { 889e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Li if (++mIndex == HISTORY_SIZE) { 890e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Li mIndex = 0; 891e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Li } 892e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Li 893e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Li Movement& movement = mMovements[mIndex]; 894e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Li movement.eventTime = eventTime; 895e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Li movement.idBits = idBits; 896e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Li uint32_t count = idBits.count(); 897e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Li for (uint32_t i = 0; i < count; i++) { 898ca099614841bc619f217dfa088da630a7eb1ab65Wu-cheng Li movement.positions[i] = positions[i]; 899ca099614841bc619f217dfa088da630a7eb1ab65Wu-cheng Li } 900ca099614841bc619f217dfa088da630a7eb1ab65Wu-cheng Li} 901ca099614841bc619f217dfa088da630a7eb1ab65Wu-cheng Li 902e339c5edbebedf446581f18ad70214007309bf4bWu-cheng Libool LegacyVelocityTrackerStrategy::getEstimator(uint32_t id, 903ca099614841bc619f217dfa088da630a7eb1ab65Wu-cheng Li VelocityTracker::Estimator* outEstimator) const { 904ca099614841bc619f217dfa088da630a7eb1ab65Wu-cheng Li outEstimator->clear(); 905ca099614841bc619f217dfa088da630a7eb1ab65Wu-cheng Li 906ca099614841bc619f217dfa088da630a7eb1ab65Wu-cheng Li const Movement& newestMovement = mMovements[mIndex]; 907ca099614841bc619f217dfa088da630a7eb1ab65Wu-cheng Li if (!newestMovement.idBits.hasBit(id)) { 908ca099614841bc619f217dfa088da630a7eb1ab65Wu-cheng Li return false; // no data 9099b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 9109b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 9119b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li // Find the oldest sample that contains the pointer and that is not older than HORIZON. 912eb68c46a40c773eb56ef7bcf8e7ece5c6a5a8d23Chih-Chung Chang nsecs_t minTime = newestMovement.eventTime - HORIZON; 9139b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li uint32_t oldestIndex = mIndex; 9149b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li uint32_t numTouches = 1; 9159b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li do { 9169066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project uint32_t nextOldestIndex = (oldestIndex == 0 ? HISTORY_SIZE : oldestIndex) - 1; 9179066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project const Movement& nextOldestMovement = mMovements[nextOldestIndex]; 9189066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (!nextOldestMovement.idBits.hasBit(id) 9199066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project || nextOldestMovement.eventTime < minTime) { 9209066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project break; 9219066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 9229066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project oldestIndex = nextOldestIndex; 9239066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } while (++numTouches < HISTORY_SIZE); 9249066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 9259066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // Calculate an exponentially weighted moving average of the velocity estimate 9269066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // at different points in time measured relative to the oldest sample. 9279066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // This is essentially an IIR filter. Newer samples are weighted more heavily 9289066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // than older samples. Samples at equal time points are weighted more or less 9299066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // equally. 9309066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // 9319066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // One tricky problem is that the sample data may be poorly conditioned. 9329066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // Sometimes samples arrive very close together in time which can cause us to 9339066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // overestimate the velocity at that time point. Most samples might be measured 9349066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // 16ms apart but some consecutive samples could be only 0.5sm apart because 9359066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // the hardware or driver reports them irregularly or in bursts. 9369066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project float accumVx = 0; 9379066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project float accumVy = 0; 9389b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li uint32_t index = oldestIndex; 9399066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project uint32_t samplesUsed = 0; 9409066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project const Movement& oldestMovement = mMovements[oldestIndex]; 9419066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project const VelocityTracker::Position& oldestPosition = oldestMovement.getPosition(id); 9429066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project nsecs_t lastDuration = 0; 9439066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 9449066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project while (numTouches-- > 1) { 9459066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (++index == HISTORY_SIZE) { 9469066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project index = 0; 9479066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 9489066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project const Movement& movement = mMovements[index]; 9499066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project nsecs_t duration = movement.eventTime - oldestMovement.eventTime; 9509066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 9519066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // If the duration between samples is small, we may significantly overestimate 9529066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // the velocity. Consequently, we impose a minimum duration constraint on the 9539066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // samples that we include in the calculation. 9549066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (duration >= MIN_DURATION) { 9559066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project const VelocityTracker::Position& position = movement.getPosition(id); 9569b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li float scale = 1000000000.0f / duration; // one over time delta in seconds 9579066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project float vx = (position.x - oldestPosition.x) * scale; 9589066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project float vy = (position.y - oldestPosition.y) * scale; 9599b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li accumVx = (accumVx * lastDuration + vx * duration) / (duration + lastDuration); 9609b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li accumVy = (accumVy * lastDuration + vy * duration) / (duration + lastDuration); 9619066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project lastDuration = duration; 9629066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project samplesUsed += 1; 9639066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 9649066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 9659b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 9669066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project // Report velocity. 9679066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project const VelocityTracker::Position& newestPosition = newestMovement.getPosition(id); 9689066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->time = newestMovement.eventTime; 9699066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->confidence = 1; 9709066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->xCoeff[0] = newestPosition.x; 9719066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->yCoeff[0] = newestPosition.y; 9729066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (samplesUsed) { 9739066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->xCoeff[1] = accumVx; 9749066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->yCoeff[1] = accumVy; 9759066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->degree = 1; 9769066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } else { 9779066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project outEstimator->degree = 0; 9789b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 9799066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return true; 9809066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 9819066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 9829066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project// --- ImpulseVelocityTrackerStrategy --- 9839066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 9849066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source ProjectImpulseVelocityTrackerStrategy::ImpulseVelocityTrackerStrategy() { 9859b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li clear(); 9869066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 9879066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 9889066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source ProjectImpulseVelocityTrackerStrategy::~ImpulseVelocityTrackerStrategy() { 9899066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 9909066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 9919066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Projectvoid ImpulseVelocityTrackerStrategy::clear() { 9929066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project mIndex = 0; 9939066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project mMovements[0].idBits.clear(); 9949066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 9959066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 9969066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Projectvoid ImpulseVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { 9979066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value); 9989066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project mMovements[mIndex].idBits = remainingIdBits; 9999066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 10009066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 10019066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Projectvoid ImpulseVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, 10029066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project const VelocityTracker::Position* positions) { 10039066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (++mIndex == HISTORY_SIZE) { 10049b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li mIndex = 0; 10059066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 10069066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 10079066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project Movement& movement = mMovements[mIndex]; 10089066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project movement.eventTime = eventTime; 10099066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project movement.idBits = idBits; 10109066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project uint32_t count = idBits.count(); 10119066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project for (uint32_t i = 0; i < count; i++) { 10129066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project movement.positions[i] = positions[i]; 10139066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 10149b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li} 10159066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 10169066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project/** 10179066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * Calculate the total impulse provided to the screen and the resulting velocity. 10189066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * 10199066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * The touchscreen is modeled as a physical object. 10209066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * Initial condition is discussed below, but for now suppose that v(t=0) = 0 10219066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * 10229066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * The kinetic energy of the object at the release is E=0.5*m*v^2 10239066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * Then vfinal = sqrt(2E/m). The goal is to calculate E. 10249b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * 10259066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * The kinetic energy at the release is equal to the total work done on the object by the finger. 10269066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * The total work W is the sum of all dW along the path. 10279066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * 10289066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * dW = F*dx, where dx is the piece of path traveled. 10299066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * Force is change of momentum over time, F = dp/dt = m dv/dt. 10309066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * Then substituting: 10319066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * dW = m (dv/dt) * dx = m * v * dv 10329066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * 10339066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * Summing along the path, we get: 10349b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * W = sum(dW) = sum(m * v * dv) = m * sum(v * dv) 10359066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * Since the mass stays constant, the equation for final velocity is: 10369066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * vfinal = sqrt(2*sum(v * dv)) 10379066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * 10389066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * Here, 10399066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * dv : change of velocity = (v[i+1]-v[i]) 10409b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * dx : change of distance = (x[i+1]-x[i]) 10419066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * dt : change of time = (t[i+1]-t[i]) 10429066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * v : instantaneous velocity = dx/dt 10439066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * 10449066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * The final formula is: 10459b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * vfinal = sqrt(2) * sqrt(sum((v[i]-v[i-1])*|v[i]|)) for all i 10469b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * The absolute value is needed to properly account for the sign. If the velocity over a 10479066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * particular segment descreases, then this indicates braking, which means that negative 10489066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * work was done. So for two positive, but decreasing, velocities, this contribution would be 10499066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * negative and will cause a smaller final velocity. 10509b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * 10519b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * Initial condition 10529b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * There are two ways to deal with initial condition: 10539066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * 1) Assume that v(0) = 0, which would mean that the screen is initially at rest. 10549b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * This is not entirely accurate. We are only taking the past X ms of touch data, where X is 10559b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * currently equal to 100. However, a touch event that created a fling probably lasted for longer 10569b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * than that, which would mean that the user has already been interacting with the touchscreen 10573f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li * and it has probably already been moving. 10589c79938d47a3caa06e5fb956955374f30c55992bWu-cheng Li * 2) Assume that the touchscreen has already been moving at a certain velocity, calculate this 10599b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * initial velocity and the equivalent energy, and start with this initial energy. 10609b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * Consider an example where we have the following data, consisting of 3 points: 10619b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * time: t0, t1, t2 10629b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * x : x0, x1, x2 10639066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * v : 0 , v1, v2 10649066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * Here is what will happen in each of these scenarios: 10659066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * 1) By directly applying the formula above with the v(0) = 0 boundary condition, we will get 10664c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li * vfinal = sqrt(2*(|v1|*(v1-v0) + |v2|*(v2-v1))). This can be simplified since v0=0 10674c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li * vfinal = sqrt(2*(|v1|*v1 + |v2|*(v2-v1))) = sqrt(2*(v1^2 + |v2|*(v2 - v1))) 10684c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li * since velocity is a real number 10699b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * 2) If we treat the screen as already moving, then it must already have an energy (per mass) 10709066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * equal to 1/2*v1^2. Then the initial energy should be 1/2*v1*2, and only the second segment 10719066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * will contribute to the total kinetic energy (since we can effectively consider that v0=v1). 10729b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * This will give the following expression for the final velocity: 10739b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * vfinal = sqrt(2*(1/2*v1^2 + |v2|*(v2-v1))) 10749b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * This analysis can be generalized to an arbitrary number of samples. 10759b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * 10769b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * 10779b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * Comparing the two equations above, we see that the only mathematical difference 10789b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * is the factor of 1/2 in front of the first velocity term. 10799b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * This boundary condition would allow for the "proper" calculation of the case when all of the 10809066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * samples are equally spaced in time and distance, which should suggest a constant velocity. 10819b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * 10829b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li * Note that approach 2) is sensitive to the proper ordering of the data in time, since 10839066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project * the boundary condition must be applied to the oldest sample to be accurate. 10849b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li */ 10859b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Listatic float kineticEnergyToVelocity(float work) { 10869b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li static constexpr float sqrt2 = 1.41421356237; 10879066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return (work < 0 ? -1.0 : 1.0) * sqrtf(fabsf(work)) * sqrt2; 10889066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 10899066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 10904c4300c71229638183d814ab8374e09f722910f5Wu-cheng Listatic float calculateImpulseVelocity(const nsecs_t* t, const float* x, size_t count) { 10914c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li // The input should be in reversed time order (most recent sample at index i=0) 10923f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li // t[i] is in nanoseconds, but due to FP arithmetic, convert to seconds inside this function 10934c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li static constexpr float NANOS_PER_SECOND = 1E-9; 10944c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li 10954c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li if (count < 2) { 10964c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li return 0; // if 0 or 1 points, velocity is zero 10974c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li } 10984c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li if (t[1] > t[0]) { // Algorithm will still work, but not perfectly 10994c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li ALOGE("Samples provided to calculateImpulseVelocity in the wrong order"); 11004c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li } 11014c4300c71229638183d814ab8374e09f722910f5Wu-cheng Li if (count == 2) { // if 2 points, basic linear calculation 11029b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li if (t[1] == t[0]) { 11039066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project ALOGE("Events have identical time stamps t=%" PRId64 ", setting velocity = 0", t[0]); 11049b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li return 0; 11059b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 11069066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return (x[1] - x[0]) / (NANOS_PER_SECOND * (t[1] - t[0])); 11079b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 11089b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li // Guaranteed to have at least 3 points here 11099066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project float work = 0; 11109066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project for (size_t i = count - 1; i > 0 ; i--) { // start with the oldest sample and go forward in time 11119066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (t[i] == t[i-1]) { 11129b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li ALOGE("Events have identical time stamps t=%" PRId64 ", skipping sample", t[i]); 11139066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project continue; 11149b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 11159066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project float vprev = kineticEnergyToVelocity(work); // v[i-1] 11169b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li float vcurr = (x[i] - x[i-1]) / (NANOS_PER_SECOND * (t[i] - t[i-1])); // v[i] 11179b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li work += (vcurr - vprev) * fabsf(vcurr); 11189066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (i == count - 1) { 11199066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project work *= 0.5; // initial condition, case 2) above 11209066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 11219b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li } 11229066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return kineticEnergyToVelocity(work); 11239b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li} 11249b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 11259b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Libool ImpulseVelocityTrackerStrategy::getEstimator(uint32_t id, 11269b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li VelocityTracker::Estimator* outEstimator) const { 11279b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li outEstimator->clear(); 11289b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 11299b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li // Iterate over movement samples in reverse time order and collect samples. 11309b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li float x[HISTORY_SIZE]; 11319b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li float y[HISTORY_SIZE]; 11329b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li nsecs_t time[HISTORY_SIZE]; 11339b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li size_t m = 0; // number of points that will be used for fitting 11349066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project size_t index = mIndex; 11359b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li const Movement& newestMovement = mMovements[mIndex]; 11369b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li do { 11379066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project const Movement& movement = mMovements[index]; 11389066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (!movement.idBits.hasBit(id)) { 11399066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project break; 1140a18e90176a8e2442837d0503fbfd4adb9df0818fWu-cheng Li } 1141a18e90176a8e2442837d0503fbfd4adb9df0818fWu-cheng Li 11429b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li nsecs_t age = newestMovement.eventTime - movement.eventTime; 11439066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (age > HORIZON) { 11449066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project break; 11459066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 11469b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 11479066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project const VelocityTracker::Position& position = movement.getPosition(id); 11489066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project x[m] = position.x; 11499066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project y[m] = position.y; 1150a18e90176a8e2442837d0503fbfd4adb9df0818fWu-cheng Li time[m] = movement.eventTime; 1151a18e90176a8e2442837d0503fbfd4adb9df0818fWu-cheng Li index = (index == 0 ? HISTORY_SIZE : index) - 1; 1152a18e90176a8e2442837d0503fbfd4adb9df0818fWu-cheng Li } while (++m < HISTORY_SIZE); 11539b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li 11549066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project if (m == 0) { 11559066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return false; // no data 11569066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project } 11579b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li outEstimator->xCoeff[0] = 0; 11589b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li outEstimator->yCoeff[0] = 0; 11599b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li outEstimator->xCoeff[1] = calculateImpulseVelocity(time, x, m); 11609b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li outEstimator->yCoeff[1] = calculateImpulseVelocity(time, y, m); 11619b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li outEstimator->xCoeff[2] = 0; 11629b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li outEstimator->yCoeff[2] = 0; 11633f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li outEstimator->time = newestMovement.eventTime; 11643f4639a6611222ae1ae5493de49213250d292139Wu-cheng Li outEstimator->degree = 2; // similar results to 2nd degree fit 11659b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li outEstimator->confidence = 1; 11669b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li#if DEBUG_STRATEGY 11679b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li ALOGD("velocity: (%f, %f)", outEstimator->xCoeff[1], outEstimator->yCoeff[1]); 11689b6a8ab8221f2df20c32711b0f1e4f301165fac2Wu-cheng Li#endif 11699066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project return true; 11709066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project} 11719066cfe9886ac131c34d59ed0e2d287b0e3c0087The Android Open Source Project 11727478ea6848c0059e65a4089b4ec2ff4158520870Wu-cheng Li} // namespace android 1173da0a56df963353a1f1bd1914fa31f870d982dd5aScott Main