18a90e6e3174083f274538567d851f98478fc83e9Jeff Brown/* 28a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * Copyright (C) 2012 The Android Open Source Project 38a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * 48a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * Licensed under the Apache License, Version 2.0 (the "License"); 58a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * you may not use this file except in compliance with the License. 68a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * You may obtain a copy of the License at 78a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * 88a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * http://www.apache.org/licenses/LICENSE-2.0 98a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * 108a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * Unless required by applicable law or agreed to in writing, software 118a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * distributed under the License is distributed on an "AS IS" BASIS, 128a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 138a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * See the License for the specific language governing permissions and 148a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * limitations under the License. 158a90e6e3174083f274538567d851f98478fc83e9Jeff Brown */ 168a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 178a90e6e3174083f274538567d851f98478fc83e9Jeff Brown#define LOG_TAG "VelocityTracker" 188a90e6e3174083f274538567d851f98478fc83e9Jeff Brown//#define LOG_NDEBUG 0 198a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 208a90e6e3174083f274538567d851f98478fc83e9Jeff Brown// Log debug messages about velocity tracking. 218a90e6e3174083f274538567d851f98478fc83e9Jeff Brown#define DEBUG_VELOCITY 0 228a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 239eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown// Log debug messages about the progress of the algorithm itself. 249eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown#define DEBUG_STRATEGY 0 258a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 268a90e6e3174083f274538567d851f98478fc83e9Jeff Brown#include <math.h> 278a90e6e3174083f274538567d851f98478fc83e9Jeff Brown#include <limits.h> 288a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 298a90e6e3174083f274538567d851f98478fc83e9Jeff Brown#include <androidfw/VelocityTracker.h> 308a90e6e3174083f274538567d851f98478fc83e9Jeff Brown#include <utils/BitSet.h> 318a90e6e3174083f274538567d851f98478fc83e9Jeff Brown#include <utils/String8.h> 328a90e6e3174083f274538567d851f98478fc83e9Jeff Brown#include <utils/Timers.h> 338a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 349eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown#include <cutils/properties.h> 359eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown 368a90e6e3174083f274538567d851f98478fc83e9Jeff Brownnamespace android { 378a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 3890729403d50488566eb4ae0e09bb1be21979a633Jeff Brown// Nanoseconds per milliseconds. 3990729403d50488566eb4ae0e09bb1be21979a633Jeff Brownstatic const nsecs_t NANOS_PER_MS = 1000000; 4090729403d50488566eb4ae0e09bb1be21979a633Jeff Brown 4190729403d50488566eb4ae0e09bb1be21979a633Jeff Brown// Threshold for determining that a pointer has stopped moving. 4290729403d50488566eb4ae0e09bb1be21979a633Jeff Brown// Some input devices do not send ACTION_MOVE events in the case where a pointer has 4390729403d50488566eb4ae0e09bb1be21979a633Jeff Brown// stopped. We need to detect this case so that we can accurately predict the 4490729403d50488566eb4ae0e09bb1be21979a633Jeff Brown// velocity after the pointer starts moving again. 4590729403d50488566eb4ae0e09bb1be21979a633Jeff Brownstatic const nsecs_t ASSUME_POINTER_STOPPED_TIME = 40 * NANOS_PER_MS; 4690729403d50488566eb4ae0e09bb1be21979a633Jeff Brown 4790729403d50488566eb4ae0e09bb1be21979a633Jeff Brown 4885bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brownstatic float vectorDot(const float* a, const float* b, uint32_t m) { 498a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float r = 0; 508a90e6e3174083f274538567d851f98478fc83e9Jeff Brown while (m--) { 518a90e6e3174083f274538567d851f98478fc83e9Jeff Brown r += *(a++) * *(b++); 528a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 538a90e6e3174083f274538567d851f98478fc83e9Jeff Brown return r; 548a90e6e3174083f274538567d851f98478fc83e9Jeff Brown} 558a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 5685bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brownstatic float vectorNorm(const float* a, uint32_t m) { 578a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float r = 0; 588a90e6e3174083f274538567d851f98478fc83e9Jeff Brown while (m--) { 598a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float t = *(a++); 608a90e6e3174083f274538567d851f98478fc83e9Jeff Brown r += t * t; 618a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 628a90e6e3174083f274538567d851f98478fc83e9Jeff Brown return sqrtf(r); 638a90e6e3174083f274538567d851f98478fc83e9Jeff Brown} 648a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 659eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown#if DEBUG_STRATEGY || DEBUG_VELOCITY 668a90e6e3174083f274538567d851f98478fc83e9Jeff Brownstatic String8 vectorToString(const float* a, uint32_t m) { 678a90e6e3174083f274538567d851f98478fc83e9Jeff Brown String8 str; 688a90e6e3174083f274538567d851f98478fc83e9Jeff Brown str.append("["); 698a90e6e3174083f274538567d851f98478fc83e9Jeff Brown while (m--) { 708a90e6e3174083f274538567d851f98478fc83e9Jeff Brown str.appendFormat(" %f", *(a++)); 718a90e6e3174083f274538567d851f98478fc83e9Jeff Brown if (m) { 728a90e6e3174083f274538567d851f98478fc83e9Jeff Brown str.append(","); 738a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 748a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 758a90e6e3174083f274538567d851f98478fc83e9Jeff Brown str.append(" ]"); 768a90e6e3174083f274538567d851f98478fc83e9Jeff Brown return str; 778a90e6e3174083f274538567d851f98478fc83e9Jeff Brown} 788a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 798a90e6e3174083f274538567d851f98478fc83e9Jeff Brownstatic String8 matrixToString(const float* a, uint32_t m, uint32_t n, bool rowMajor) { 808a90e6e3174083f274538567d851f98478fc83e9Jeff Brown String8 str; 818a90e6e3174083f274538567d851f98478fc83e9Jeff Brown str.append("["); 828a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (size_t i = 0; i < m; i++) { 838a90e6e3174083f274538567d851f98478fc83e9Jeff Brown if (i) { 848a90e6e3174083f274538567d851f98478fc83e9Jeff Brown str.append(","); 858a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 868a90e6e3174083f274538567d851f98478fc83e9Jeff Brown str.append(" ["); 878a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (size_t j = 0; j < n; j++) { 888a90e6e3174083f274538567d851f98478fc83e9Jeff Brown if (j) { 898a90e6e3174083f274538567d851f98478fc83e9Jeff Brown str.append(","); 908a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 918a90e6e3174083f274538567d851f98478fc83e9Jeff Brown str.appendFormat(" %f", a[rowMajor ? i * n + j : j * m + i]); 928a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 938a90e6e3174083f274538567d851f98478fc83e9Jeff Brown str.append(" ]"); 948a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 958a90e6e3174083f274538567d851f98478fc83e9Jeff Brown str.append(" ]"); 968a90e6e3174083f274538567d851f98478fc83e9Jeff Brown return str; 978a90e6e3174083f274538567d851f98478fc83e9Jeff Brown} 988a90e6e3174083f274538567d851f98478fc83e9Jeff Brown#endif 998a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 10085bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown 10185bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown// --- VelocityTracker --- 10285bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown 1039eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown// The default velocity tracker strategy. 1049eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown// Although other strategies are available for testing and comparison purposes, 1059eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown// this is the strategy that applications will actually use. Be very careful 1069eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown// when adjusting the default strategy because it can dramatically affect 1079eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown// (often in a bad way) the user experience. 1089eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brownconst char* VelocityTracker::DEFAULT_STRATEGY = "lsq2"; 1099eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown 1109eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff BrownVelocityTracker::VelocityTracker(const char* strategy) : 1119eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown mLastEventTime(0), mCurrentPointerIdBits(0), mActivePointerId(-1) { 1129eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown char value[PROPERTY_VALUE_MAX]; 1139eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown 1149eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown // Allow the default strategy to be overridden using a system property for debugging. 1159eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown if (!strategy) { 1169eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown int length = property_get("debug.velocitytracker.strategy", value, NULL); 1179eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown if (length > 0) { 1189eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown strategy = value; 1199eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown } else { 1209eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown strategy = DEFAULT_STRATEGY; 1219eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown } 1229eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown } 1239eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown 1249eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown // Configure the strategy. 1259eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown if (!configureStrategy(strategy)) { 1269eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown ALOGD("Unrecognized velocity tracker strategy name '%s'.", strategy); 1279eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown if (!configureStrategy(DEFAULT_STRATEGY)) { 1289eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown LOG_ALWAYS_FATAL("Could not create the default velocity tracker strategy '%s'!", 1299eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown strategy); 1309eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown } 1319eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown } 13285bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown} 13385bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown 13485bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff BrownVelocityTracker::~VelocityTracker() { 13585bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown delete mStrategy; 1368a90e6e3174083f274538567d851f98478fc83e9Jeff Brown} 1378a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 1389eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brownbool VelocityTracker::configureStrategy(const char* strategy) { 1399eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown mStrategy = createStrategy(strategy); 1409eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown return mStrategy != NULL; 1419eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown} 1429eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown 1439eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff BrownVelocityTrackerStrategy* VelocityTracker::createStrategy(const char* strategy) { 1449eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown if (!strcmp("lsq1", strategy)) { 1459eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown // 1st order least squares. Quality: POOR. 1469eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown // Frequently underfits the touch data especially when the finger accelerates 1479eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown // or changes direction. Often underestimates velocity. The direction 1489eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown // is overly influenced by historical touch points. 1499eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown return new LeastSquaresVelocityTrackerStrategy(1); 1509eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown } 1519eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown if (!strcmp("lsq2", strategy)) { 1529eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown // 2nd order least squares. Quality: VERY GOOD. 1539eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown // Pretty much ideal, but can be confused by certain kinds of touch data, 1549eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown // particularly if the panel has a tendency to generate delayed, 1559eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown // duplicate or jittery touch coordinates when the finger is released. 1569eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown return new LeastSquaresVelocityTrackerStrategy(2); 1579eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown } 1589eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown if (!strcmp("lsq3", strategy)) { 1599eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown // 3rd order least squares. Quality: UNUSABLE. 1609eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown // Frequently overfits the touch data yielding wildly divergent estimates 1619eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown // of the velocity when the finger is released. 1629eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown return new LeastSquaresVelocityTrackerStrategy(3); 1639eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown } 16418f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown if (!strcmp("wlsq2-delta", strategy)) { 16518f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // 2nd order weighted least squares, delta weighting. Quality: EXPERIMENTAL 16618f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown return new LeastSquaresVelocityTrackerStrategy(2, 16718f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown LeastSquaresVelocityTrackerStrategy::WEIGHTING_DELTA); 16818f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown } 16918f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown if (!strcmp("wlsq2-central", strategy)) { 17018f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // 2nd order weighted least squares, central weighting. Quality: EXPERIMENTAL 17118f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown return new LeastSquaresVelocityTrackerStrategy(2, 17218f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown LeastSquaresVelocityTrackerStrategy::WEIGHTING_CENTRAL); 17318f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown } 17418f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown if (!strcmp("wlsq2-recent", strategy)) { 17518f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // 2nd order weighted least squares, recent weighting. Quality: EXPERIMENTAL 17618f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown return new LeastSquaresVelocityTrackerStrategy(2, 17718f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown LeastSquaresVelocityTrackerStrategy::WEIGHTING_RECENT); 17818f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown } 17953dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown if (!strcmp("int1", strategy)) { 18053dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown // 1st order integrating filter. Quality: GOOD. 18153dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown // Not as good as 'lsq2' because it cannot estimate acceleration but it is 18253dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown // more tolerant of errors. Like 'lsq1', this strategy tends to underestimate 18353dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown // the velocity of a fling but this strategy tends to respond to changes in 18453dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown // direction more quickly and accurately. 185a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown return new IntegratingVelocityTrackerStrategy(1); 186a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown } 187a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown if (!strcmp("int2", strategy)) { 188a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown // 2nd order integrating filter. Quality: EXPERIMENTAL. 189a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown // For comparison purposes only. Unlike 'int1' this strategy can compensate 190a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown // for acceleration but it typically overestimates the effect. 191a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown return new IntegratingVelocityTrackerStrategy(2); 19253dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown } 19351df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown if (!strcmp("legacy", strategy)) { 19451df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // Legacy velocity tracker algorithm. Quality: POOR. 19551df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // For comparison purposes only. This algorithm is strongly influenced by 19651df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // old data points, consistently underestimates velocity and takes a very long 19751df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // time to adjust to changes in direction. 19851df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown return new LegacyVelocityTrackerStrategy(); 19951df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown } 2009eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown return NULL; 2019eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown} 2029eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown 2038a90e6e3174083f274538567d851f98478fc83e9Jeff Brownvoid VelocityTracker::clear() { 20485bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown mCurrentPointerIdBits.clear(); 2058a90e6e3174083f274538567d851f98478fc83e9Jeff Brown mActivePointerId = -1; 20685bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown 20785bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown mStrategy->clear(); 2088a90e6e3174083f274538567d851f98478fc83e9Jeff Brown} 2098a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 2108a90e6e3174083f274538567d851f98478fc83e9Jeff Brownvoid VelocityTracker::clearPointers(BitSet32 idBits) { 21185bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown BitSet32 remainingIdBits(mCurrentPointerIdBits.value & ~idBits.value); 21285bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown mCurrentPointerIdBits = remainingIdBits; 2138a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 2148a90e6e3174083f274538567d851f98478fc83e9Jeff Brown if (mActivePointerId >= 0 && idBits.hasBit(mActivePointerId)) { 2158a90e6e3174083f274538567d851f98478fc83e9Jeff Brown mActivePointerId = !remainingIdBits.isEmpty() ? remainingIdBits.firstMarkedBit() : -1; 2168a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 21785bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown 21885bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown mStrategy->clearPointers(idBits); 2198a90e6e3174083f274538567d851f98478fc83e9Jeff Brown} 2208a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 2218a90e6e3174083f274538567d851f98478fc83e9Jeff Brownvoid VelocityTracker::addMovement(nsecs_t eventTime, BitSet32 idBits, const Position* positions) { 2228a90e6e3174083f274538567d851f98478fc83e9Jeff Brown while (idBits.count() > MAX_POINTERS) { 2238a90e6e3174083f274538567d851f98478fc83e9Jeff Brown idBits.clearLastMarkedBit(); 2248a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 2258a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 22690729403d50488566eb4ae0e09bb1be21979a633Jeff Brown if ((mCurrentPointerIdBits.value & idBits.value) 22790729403d50488566eb4ae0e09bb1be21979a633Jeff Brown && eventTime >= mLastEventTime + ASSUME_POINTER_STOPPED_TIME) { 22890729403d50488566eb4ae0e09bb1be21979a633Jeff Brown#if DEBUG_VELOCITY 22990729403d50488566eb4ae0e09bb1be21979a633Jeff Brown ALOGD("VelocityTracker: stopped for %0.3f ms, clearing state.", 23090729403d50488566eb4ae0e09bb1be21979a633Jeff Brown (eventTime - mLastEventTime) * 0.000001f); 23190729403d50488566eb4ae0e09bb1be21979a633Jeff Brown#endif 23290729403d50488566eb4ae0e09bb1be21979a633Jeff Brown // We have not received any movements for too long. Assume that all pointers 23390729403d50488566eb4ae0e09bb1be21979a633Jeff Brown // have stopped. 23490729403d50488566eb4ae0e09bb1be21979a633Jeff Brown mStrategy->clear(); 23590729403d50488566eb4ae0e09bb1be21979a633Jeff Brown } 23690729403d50488566eb4ae0e09bb1be21979a633Jeff Brown mLastEventTime = eventTime; 23790729403d50488566eb4ae0e09bb1be21979a633Jeff Brown 23885bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown mCurrentPointerIdBits = idBits; 2398a90e6e3174083f274538567d851f98478fc83e9Jeff Brown if (mActivePointerId < 0 || !idBits.hasBit(mActivePointerId)) { 24085bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown mActivePointerId = idBits.isEmpty() ? -1 : idBits.firstMarkedBit(); 2418a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 2428a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 24385bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown mStrategy->addMovement(eventTime, idBits, positions); 24485bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown 2458a90e6e3174083f274538567d851f98478fc83e9Jeff Brown#if DEBUG_VELOCITY 2468a90e6e3174083f274538567d851f98478fc83e9Jeff Brown ALOGD("VelocityTracker: addMovement eventTime=%lld, idBits=0x%08x, activePointerId=%d", 2478a90e6e3174083f274538567d851f98478fc83e9Jeff Brown eventTime, idBits.value, mActivePointerId); 2488a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (BitSet32 iterBits(idBits); !iterBits.isEmpty(); ) { 2498a90e6e3174083f274538567d851f98478fc83e9Jeff Brown uint32_t id = iterBits.firstMarkedBit(); 2508a90e6e3174083f274538567d851f98478fc83e9Jeff Brown uint32_t index = idBits.getIndexOfBit(id); 2518a90e6e3174083f274538567d851f98478fc83e9Jeff Brown iterBits.clearBit(id); 2528a90e6e3174083f274538567d851f98478fc83e9Jeff Brown Estimator estimator; 25385bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown getEstimator(id, &estimator); 2548a90e6e3174083f274538567d851f98478fc83e9Jeff Brown ALOGD(" %d: position (%0.3f, %0.3f), " 2558a90e6e3174083f274538567d851f98478fc83e9Jeff Brown "estimator (degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f)", 2568a90e6e3174083f274538567d851f98478fc83e9Jeff Brown id, positions[index].x, positions[index].y, 2578a90e6e3174083f274538567d851f98478fc83e9Jeff Brown int(estimator.degree), 258dcab190bd23f632f278af448b0c85b4cadcc6692Jeff Brown vectorToString(estimator.xCoeff, estimator.degree + 1).string(), 259dcab190bd23f632f278af448b0c85b4cadcc6692Jeff Brown vectorToString(estimator.yCoeff, estimator.degree + 1).string(), 2608a90e6e3174083f274538567d851f98478fc83e9Jeff Brown estimator.confidence); 2618a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 2628a90e6e3174083f274538567d851f98478fc83e9Jeff Brown#endif 2638a90e6e3174083f274538567d851f98478fc83e9Jeff Brown} 2648a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 2658a90e6e3174083f274538567d851f98478fc83e9Jeff Brownvoid VelocityTracker::addMovement(const MotionEvent* event) { 2668a90e6e3174083f274538567d851f98478fc83e9Jeff Brown int32_t actionMasked = event->getActionMasked(); 2678a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 2688a90e6e3174083f274538567d851f98478fc83e9Jeff Brown switch (actionMasked) { 2698a90e6e3174083f274538567d851f98478fc83e9Jeff Brown case AMOTION_EVENT_ACTION_DOWN: 2708a90e6e3174083f274538567d851f98478fc83e9Jeff Brown case AMOTION_EVENT_ACTION_HOVER_ENTER: 2718a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // Clear all pointers on down before adding the new movement. 2728a90e6e3174083f274538567d851f98478fc83e9Jeff Brown clear(); 2738a90e6e3174083f274538567d851f98478fc83e9Jeff Brown break; 2748a90e6e3174083f274538567d851f98478fc83e9Jeff Brown case AMOTION_EVENT_ACTION_POINTER_DOWN: { 2758a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // Start a new movement trace for a pointer that just went down. 2768a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // We do this on down instead of on up because the client may want to query the 2778a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // final velocity for a pointer that just went up. 2788a90e6e3174083f274538567d851f98478fc83e9Jeff Brown BitSet32 downIdBits; 2798a90e6e3174083f274538567d851f98478fc83e9Jeff Brown downIdBits.markBit(event->getPointerId(event->getActionIndex())); 2808a90e6e3174083f274538567d851f98478fc83e9Jeff Brown clearPointers(downIdBits); 2818a90e6e3174083f274538567d851f98478fc83e9Jeff Brown break; 2828a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 2838a90e6e3174083f274538567d851f98478fc83e9Jeff Brown case AMOTION_EVENT_ACTION_MOVE: 2848a90e6e3174083f274538567d851f98478fc83e9Jeff Brown case AMOTION_EVENT_ACTION_HOVER_MOVE: 2858a90e6e3174083f274538567d851f98478fc83e9Jeff Brown break; 2868a90e6e3174083f274538567d851f98478fc83e9Jeff Brown default: 2878a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // Ignore all other actions because they do not convey any new information about 2888a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // pointer movement. We also want to preserve the last known velocity of the pointers. 2898a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // Note that ACTION_UP and ACTION_POINTER_UP always report the last known position 2908a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // of the pointers that went up. ACTION_POINTER_UP does include the new position of 2918a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // pointers that remained down but we will also receive an ACTION_MOVE with this 2928a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // information if any of them actually moved. Since we don't know how many pointers 2938a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // will be going up at once it makes sense to just wait for the following ACTION_MOVE 2948a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // before adding the movement. 2958a90e6e3174083f274538567d851f98478fc83e9Jeff Brown return; 2968a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 2978a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 2988a90e6e3174083f274538567d851f98478fc83e9Jeff Brown size_t pointerCount = event->getPointerCount(); 2998a90e6e3174083f274538567d851f98478fc83e9Jeff Brown if (pointerCount > MAX_POINTERS) { 3008a90e6e3174083f274538567d851f98478fc83e9Jeff Brown pointerCount = MAX_POINTERS; 3018a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 3028a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 3038a90e6e3174083f274538567d851f98478fc83e9Jeff Brown BitSet32 idBits; 3048a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (size_t i = 0; i < pointerCount; i++) { 3058a90e6e3174083f274538567d851f98478fc83e9Jeff Brown idBits.markBit(event->getPointerId(i)); 3068a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 3078a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 308dcab190bd23f632f278af448b0c85b4cadcc6692Jeff Brown uint32_t pointerIndex[MAX_POINTERS]; 309dcab190bd23f632f278af448b0c85b4cadcc6692Jeff Brown for (size_t i = 0; i < pointerCount; i++) { 310dcab190bd23f632f278af448b0c85b4cadcc6692Jeff Brown pointerIndex[i] = idBits.getIndexOfBit(event->getPointerId(i)); 311dcab190bd23f632f278af448b0c85b4cadcc6692Jeff Brown } 312dcab190bd23f632f278af448b0c85b4cadcc6692Jeff Brown 3138a90e6e3174083f274538567d851f98478fc83e9Jeff Brown nsecs_t eventTime; 3148a90e6e3174083f274538567d851f98478fc83e9Jeff Brown Position positions[pointerCount]; 3158a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 3168a90e6e3174083f274538567d851f98478fc83e9Jeff Brown size_t historySize = event->getHistorySize(); 3178a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (size_t h = 0; h < historySize; h++) { 3188a90e6e3174083f274538567d851f98478fc83e9Jeff Brown eventTime = event->getHistoricalEventTime(h); 3198a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (size_t i = 0; i < pointerCount; i++) { 320dcab190bd23f632f278af448b0c85b4cadcc6692Jeff Brown uint32_t index = pointerIndex[i]; 321dcab190bd23f632f278af448b0c85b4cadcc6692Jeff Brown positions[index].x = event->getHistoricalX(i, h); 322dcab190bd23f632f278af448b0c85b4cadcc6692Jeff Brown positions[index].y = event->getHistoricalY(i, h); 3238a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 3248a90e6e3174083f274538567d851f98478fc83e9Jeff Brown addMovement(eventTime, idBits, positions); 3258a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 3268a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 3278a90e6e3174083f274538567d851f98478fc83e9Jeff Brown eventTime = event->getEventTime(); 3288a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (size_t i = 0; i < pointerCount; i++) { 329dcab190bd23f632f278af448b0c85b4cadcc6692Jeff Brown uint32_t index = pointerIndex[i]; 330dcab190bd23f632f278af448b0c85b4cadcc6692Jeff Brown positions[index].x = event->getX(i); 331dcab190bd23f632f278af448b0c85b4cadcc6692Jeff Brown positions[index].y = event->getY(i); 3328a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 3338a90e6e3174083f274538567d851f98478fc83e9Jeff Brown addMovement(eventTime, idBits, positions); 3348a90e6e3174083f274538567d851f98478fc83e9Jeff Brown} 3358a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 33685bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brownbool VelocityTracker::getVelocity(uint32_t id, float* outVx, float* outVy) const { 33785bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown Estimator estimator; 33885bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown if (getEstimator(id, &estimator) && estimator.degree >= 1) { 33985bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown *outVx = estimator.xCoeff[1]; 34085bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown *outVy = estimator.yCoeff[1]; 34185bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown return true; 34285bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown } 34385bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown *outVx = 0; 34485bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown *outVy = 0; 34585bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown return false; 34685bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown} 34785bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown 34885bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brownbool VelocityTracker::getEstimator(uint32_t id, Estimator* outEstimator) const { 34985bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown return mStrategy->getEstimator(id, outEstimator); 35085bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown} 35185bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown 35285bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown 35385bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown// --- LeastSquaresVelocityTrackerStrategy --- 35485bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown 35585bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brownconst nsecs_t LeastSquaresVelocityTrackerStrategy::HORIZON; 35685bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brownconst uint32_t LeastSquaresVelocityTrackerStrategy::HISTORY_SIZE; 35785bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown 35818f329e9480fca75210bb7496e5b4bc987b4ad8fJeff BrownLeastSquaresVelocityTrackerStrategy::LeastSquaresVelocityTrackerStrategy( 35918f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown uint32_t degree, Weighting weighting) : 36018f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown mDegree(degree), mWeighting(weighting) { 36185bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown clear(); 36285bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown} 36385bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown 36485bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff BrownLeastSquaresVelocityTrackerStrategy::~LeastSquaresVelocityTrackerStrategy() { 36585bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown} 36685bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown 36785bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brownvoid LeastSquaresVelocityTrackerStrategy::clear() { 36885bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown mIndex = 0; 36985bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown mMovements[0].idBits.clear(); 37085bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown} 37185bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown 37285bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brownvoid LeastSquaresVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { 37385bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value); 37485bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown mMovements[mIndex].idBits = remainingIdBits; 37585bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown} 37685bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown 37785bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brownvoid LeastSquaresVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, 37885bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown const VelocityTracker::Position* positions) { 37985bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown if (++mIndex == HISTORY_SIZE) { 38085bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown mIndex = 0; 38185bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown } 38285bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown 38385bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown Movement& movement = mMovements[mIndex]; 38485bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown movement.eventTime = eventTime; 38585bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown movement.idBits = idBits; 38685bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown uint32_t count = idBits.count(); 38785bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown for (uint32_t i = 0; i < count; i++) { 38885bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown movement.positions[i] = positions[i]; 38985bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown } 39085bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown} 39185bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown 3928a90e6e3174083f274538567d851f98478fc83e9Jeff Brown/** 3938a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * Solves a linear least squares problem to obtain a N degree polynomial that fits 3948a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * the specified input data as nearly as possible. 3958a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * 3968a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * Returns true if a solution is found, false otherwise. 3978a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * 39818f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * The input consists of two vectors of data points X and Y with indices 0..m-1 39918f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * along with a weight vector W of the same size. 40018f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * 4019eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown * The output is a vector B with indices 0..n that describes a polynomial 40218f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * that fits the data, such the sum of W[i] * W[i] * abs(Y[i] - (B[0] + B[1] X[i] 40318f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * + B[2] X[i]^2 ... B[n] X[i]^n)) for all i between 0 and m-1 is minimized. 40418f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * 40518f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * Accordingly, the weight vector W should be initialized by the caller with the 40618f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * reciprocal square root of the variance of the error in each input data point. 40718f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * In other words, an ideal choice for W would be W[i] = 1 / var(Y[i]) = 1 / stddev(Y[i]). 40818f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * The weights express the relative importance of each data point. If the weights are 40918f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * all 1, then the data points are considered to be of equal importance when fitting 41018f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * the polynomial. It is a good idea to choose weights that diminish the importance 41118f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * of data points that may have higher than usual error margins. 41218f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * 41318f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * Errors among data points are assumed to be independent. W is represented here 41418f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * as a vector although in the literature it is typically taken to be a diagonal matrix. 4158a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * 4168a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * That is to say, the function that generated the input data can be approximated 4178a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * by y(x) ~= B[0] + B[1] x + B[2] x^2 + ... + B[n] x^n. 4188a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * 4198a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * The coefficient of determination (R^2) is also returned to describe the goodness 4208a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * of fit of the model for the given data. It is a value between 0 and 1, where 1 4218a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * indicates perfect correspondence. 4228a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * 4238a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * This function first expands the X vector to a m by n matrix A such that 42418f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * A[i][0] = 1, A[i][1] = X[i], A[i][2] = X[i]^2, ..., A[i][n] = X[i]^n, then 42518f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * multiplies it by w[i]./ 4268a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * 4278a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * Then it calculates the QR decomposition of A yielding an m by m orthonormal matrix Q 4288a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * and an m by n upper triangular matrix R. Because R is upper triangular (lower 4298a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * part is all zeroes), we can simplify the decomposition into an m by n matrix 4308a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * Q1 and a n by n matrix R1 such that A = Q1 R1. 4318a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * 43218f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * Finally we solve the system of linear equations given by R1 B = (Qtranspose W Y) 4338a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * to find B. 4348a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * 4358a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * For efficiency, we lay out A and Q column-wise in memory because we frequently 4368a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * operate on the column vectors. Conversely, we lay out R row-wise. 4378a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * 4388a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * http://en.wikipedia.org/wiki/Numerical_methods_for_linear_least_squares 4398a90e6e3174083f274538567d851f98478fc83e9Jeff Brown * http://en.wikipedia.org/wiki/Gram-Schmidt 4408a90e6e3174083f274538567d851f98478fc83e9Jeff Brown */ 44118f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brownstatic bool solveLeastSquares(const float* x, const float* y, 44218f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown const float* w, uint32_t m, uint32_t n, float* outB, float* outDet) { 4439eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown#if DEBUG_STRATEGY 44418f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown ALOGD("solveLeastSquares: m=%d, n=%d, x=%s, y=%s, w=%s", int(m), int(n), 44518f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown vectorToString(x, m).string(), vectorToString(y, m).string(), 44618f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown vectorToString(w, m).string()); 4478a90e6e3174083f274538567d851f98478fc83e9Jeff Brown#endif 4488a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 44918f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // Expand the X vector to a matrix A, pre-multiplied by the weights. 4508a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float a[n][m]; // column-major order 4518a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (uint32_t h = 0; h < m; h++) { 45218f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown a[0][h] = w[h]; 4538a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (uint32_t i = 1; i < n; i++) { 4548a90e6e3174083f274538567d851f98478fc83e9Jeff Brown a[i][h] = a[i - 1][h] * x[h]; 4558a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 4568a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 4579eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown#if DEBUG_STRATEGY 4588a90e6e3174083f274538567d851f98478fc83e9Jeff Brown ALOGD(" - a=%s", matrixToString(&a[0][0], m, n, false /*rowMajor*/).string()); 4598a90e6e3174083f274538567d851f98478fc83e9Jeff Brown#endif 4608a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 4618a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // Apply the Gram-Schmidt process to A to obtain its QR decomposition. 4628a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float q[n][m]; // orthonormal basis, column-major order 4638a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float r[n][n]; // upper triangular matrix, row-major order 4648a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (uint32_t j = 0; j < n; j++) { 4658a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (uint32_t h = 0; h < m; h++) { 4668a90e6e3174083f274538567d851f98478fc83e9Jeff Brown q[j][h] = a[j][h]; 4678a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 4688a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (uint32_t i = 0; i < j; i++) { 4698a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float dot = vectorDot(&q[j][0], &q[i][0], m); 4708a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (uint32_t h = 0; h < m; h++) { 4718a90e6e3174083f274538567d851f98478fc83e9Jeff Brown q[j][h] -= dot * q[i][h]; 4728a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 4738a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 4748a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 4758a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float norm = vectorNorm(&q[j][0], m); 4768a90e6e3174083f274538567d851f98478fc83e9Jeff Brown if (norm < 0.000001f) { 4778a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // vectors are linearly dependent or zero so no solution 4789eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown#if DEBUG_STRATEGY 4798a90e6e3174083f274538567d851f98478fc83e9Jeff Brown ALOGD(" - no solution, norm=%f", norm); 4808a90e6e3174083f274538567d851f98478fc83e9Jeff Brown#endif 4818a90e6e3174083f274538567d851f98478fc83e9Jeff Brown return false; 4828a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 4838a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 4848a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float invNorm = 1.0f / norm; 4858a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (uint32_t h = 0; h < m; h++) { 4868a90e6e3174083f274538567d851f98478fc83e9Jeff Brown q[j][h] *= invNorm; 4878a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 4888a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (uint32_t i = 0; i < n; i++) { 4898a90e6e3174083f274538567d851f98478fc83e9Jeff Brown r[j][i] = i < j ? 0 : vectorDot(&q[j][0], &a[i][0], m); 4908a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 4918a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 4929eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown#if DEBUG_STRATEGY 4938a90e6e3174083f274538567d851f98478fc83e9Jeff Brown ALOGD(" - q=%s", matrixToString(&q[0][0], m, n, false /*rowMajor*/).string()); 4948a90e6e3174083f274538567d851f98478fc83e9Jeff Brown ALOGD(" - r=%s", matrixToString(&r[0][0], n, n, true /*rowMajor*/).string()); 4958a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 4968a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // calculate QR, if we factored A correctly then QR should equal A 4978a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float qr[n][m]; 4988a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (uint32_t h = 0; h < m; h++) { 4998a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (uint32_t i = 0; i < n; i++) { 5008a90e6e3174083f274538567d851f98478fc83e9Jeff Brown qr[i][h] = 0; 5018a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (uint32_t j = 0; j < n; j++) { 5028a90e6e3174083f274538567d851f98478fc83e9Jeff Brown qr[i][h] += q[j][h] * r[j][i]; 5038a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 5048a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 5058a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 5068a90e6e3174083f274538567d851f98478fc83e9Jeff Brown ALOGD(" - qr=%s", matrixToString(&qr[0][0], m, n, false /*rowMajor*/).string()); 5078a90e6e3174083f274538567d851f98478fc83e9Jeff Brown#endif 5088a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 50918f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // Solve R B = Qt W Y to find B. This is easy because R is upper triangular. 5108a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // We just work from bottom-right to top-left calculating B's coefficients. 51118f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown float wy[m]; 51218f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown for (uint32_t h = 0; h < m; h++) { 51318f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown wy[h] = y[h] * w[h]; 51418f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown } 5158a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (uint32_t i = n; i-- != 0; ) { 51618f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown outB[i] = vectorDot(&q[i][0], wy, m); 5178a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (uint32_t j = n - 1; j > i; j--) { 5188a90e6e3174083f274538567d851f98478fc83e9Jeff Brown outB[i] -= r[i][j] * outB[j]; 5198a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 5208a90e6e3174083f274538567d851f98478fc83e9Jeff Brown outB[i] /= r[i][i]; 5218a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 5229eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown#if DEBUG_STRATEGY 5238a90e6e3174083f274538567d851f98478fc83e9Jeff Brown ALOGD(" - b=%s", vectorToString(outB, n).string()); 5248a90e6e3174083f274538567d851f98478fc83e9Jeff Brown#endif 5258a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 5268a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // Calculate the coefficient of determination as 1 - (SSerr / SStot) where 52718f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // SSerr is the residual sum of squares (variance of the error), 52818f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // and SStot is the total sum of squares (variance of the data) where each 52918f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // has been weighted. 5308a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float ymean = 0; 5318a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (uint32_t h = 0; h < m; h++) { 5328a90e6e3174083f274538567d851f98478fc83e9Jeff Brown ymean += y[h]; 5338a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 5348a90e6e3174083f274538567d851f98478fc83e9Jeff Brown ymean /= m; 5358a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 5368a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float sserr = 0; 5378a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float sstot = 0; 5388a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (uint32_t h = 0; h < m; h++) { 5398a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float err = y[h] - outB[0]; 5408a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float term = 1; 5418a90e6e3174083f274538567d851f98478fc83e9Jeff Brown for (uint32_t i = 1; i < n; i++) { 5428a90e6e3174083f274538567d851f98478fc83e9Jeff Brown term *= x[h]; 5438a90e6e3174083f274538567d851f98478fc83e9Jeff Brown err -= term * outB[i]; 5448a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 54518f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown sserr += w[h] * w[h] * err * err; 5468a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float var = y[h] - ymean; 54718f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown sstot += w[h] * w[h] * var * var; 5488a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 5498a90e6e3174083f274538567d851f98478fc83e9Jeff Brown *outDet = sstot > 0.000001f ? 1.0f - (sserr / sstot) : 1; 5509eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown#if DEBUG_STRATEGY 5518a90e6e3174083f274538567d851f98478fc83e9Jeff Brown ALOGD(" - sserr=%f", sserr); 5528a90e6e3174083f274538567d851f98478fc83e9Jeff Brown ALOGD(" - sstot=%f", sstot); 5538a90e6e3174083f274538567d851f98478fc83e9Jeff Brown ALOGD(" - det=%f", *outDet); 5548a90e6e3174083f274538567d851f98478fc83e9Jeff Brown#endif 5558a90e6e3174083f274538567d851f98478fc83e9Jeff Brown return true; 5568a90e6e3174083f274538567d851f98478fc83e9Jeff Brown} 5578a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 55885bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brownbool LeastSquaresVelocityTrackerStrategy::getEstimator(uint32_t id, 55985bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown VelocityTracker::Estimator* outEstimator) const { 5608a90e6e3174083f274538567d851f98478fc83e9Jeff Brown outEstimator->clear(); 5618a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 5628a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // Iterate over movement samples in reverse time order and collect samples. 5638a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float x[HISTORY_SIZE]; 5648a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float y[HISTORY_SIZE]; 56518f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown float w[HISTORY_SIZE]; 5668a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float time[HISTORY_SIZE]; 5678a90e6e3174083f274538567d851f98478fc83e9Jeff Brown uint32_t m = 0; 5688a90e6e3174083f274538567d851f98478fc83e9Jeff Brown uint32_t index = mIndex; 5698a90e6e3174083f274538567d851f98478fc83e9Jeff Brown const Movement& newestMovement = mMovements[mIndex]; 5708a90e6e3174083f274538567d851f98478fc83e9Jeff Brown do { 5718a90e6e3174083f274538567d851f98478fc83e9Jeff Brown const Movement& movement = mMovements[index]; 5728a90e6e3174083f274538567d851f98478fc83e9Jeff Brown if (!movement.idBits.hasBit(id)) { 5738a90e6e3174083f274538567d851f98478fc83e9Jeff Brown break; 5748a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 5758a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 5768a90e6e3174083f274538567d851f98478fc83e9Jeff Brown nsecs_t age = newestMovement.eventTime - movement.eventTime; 57785bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown if (age > HORIZON) { 5788a90e6e3174083f274538567d851f98478fc83e9Jeff Brown break; 5798a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 5808a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 58185bd0d62830a098c1bdc720dfdcf4fe1b18b657cJeff Brown const VelocityTracker::Position& position = movement.getPosition(id); 5828a90e6e3174083f274538567d851f98478fc83e9Jeff Brown x[m] = position.x; 5838a90e6e3174083f274538567d851f98478fc83e9Jeff Brown y[m] = position.y; 58418f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown w[m] = chooseWeight(index); 5858a90e6e3174083f274538567d851f98478fc83e9Jeff Brown time[m] = -age * 0.000000001f; 5868a90e6e3174083f274538567d851f98478fc83e9Jeff Brown index = (index == 0 ? HISTORY_SIZE : index) - 1; 5878a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } while (++m < HISTORY_SIZE); 5888a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 5898a90e6e3174083f274538567d851f98478fc83e9Jeff Brown if (m == 0) { 5908a90e6e3174083f274538567d851f98478fc83e9Jeff Brown return false; // no data 5918a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 5928a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 5938a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // Calculate a least squares polynomial fit. 5949eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown uint32_t degree = mDegree; 5958a90e6e3174083f274538567d851f98478fc83e9Jeff Brown if (degree > m - 1) { 5968a90e6e3174083f274538567d851f98478fc83e9Jeff Brown degree = m - 1; 5978a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 5988a90e6e3174083f274538567d851f98478fc83e9Jeff Brown if (degree >= 1) { 5998a90e6e3174083f274538567d851f98478fc83e9Jeff Brown float xdet, ydet; 6008a90e6e3174083f274538567d851f98478fc83e9Jeff Brown uint32_t n = degree + 1; 60118f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown if (solveLeastSquares(time, x, w, m, n, outEstimator->xCoeff, &xdet) 60218f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown && solveLeastSquares(time, y, w, m, n, outEstimator->yCoeff, &ydet)) { 60390729403d50488566eb4ae0e09bb1be21979a633Jeff Brown outEstimator->time = newestMovement.eventTime; 6048a90e6e3174083f274538567d851f98478fc83e9Jeff Brown outEstimator->degree = degree; 6058a90e6e3174083f274538567d851f98478fc83e9Jeff Brown outEstimator->confidence = xdet * ydet; 6069eb7d86181729c3eb769d71123c4ce9ffc868f08Jeff Brown#if DEBUG_STRATEGY 6078a90e6e3174083f274538567d851f98478fc83e9Jeff Brown ALOGD("estimate: degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f", 6088a90e6e3174083f274538567d851f98478fc83e9Jeff Brown int(outEstimator->degree), 6098a90e6e3174083f274538567d851f98478fc83e9Jeff Brown vectorToString(outEstimator->xCoeff, n).string(), 6108a90e6e3174083f274538567d851f98478fc83e9Jeff Brown vectorToString(outEstimator->yCoeff, n).string(), 6118a90e6e3174083f274538567d851f98478fc83e9Jeff Brown outEstimator->confidence); 6128a90e6e3174083f274538567d851f98478fc83e9Jeff Brown#endif 6138a90e6e3174083f274538567d851f98478fc83e9Jeff Brown return true; 6148a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 6158a90e6e3174083f274538567d851f98478fc83e9Jeff Brown } 6168a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 6178a90e6e3174083f274538567d851f98478fc83e9Jeff Brown // No velocity data available for this pointer, but we do have its current position. 6188a90e6e3174083f274538567d851f98478fc83e9Jeff Brown outEstimator->xCoeff[0] = x[0]; 6198a90e6e3174083f274538567d851f98478fc83e9Jeff Brown outEstimator->yCoeff[0] = y[0]; 62090729403d50488566eb4ae0e09bb1be21979a633Jeff Brown outEstimator->time = newestMovement.eventTime; 6218a90e6e3174083f274538567d851f98478fc83e9Jeff Brown outEstimator->degree = 0; 6228a90e6e3174083f274538567d851f98478fc83e9Jeff Brown outEstimator->confidence = 1; 6238a90e6e3174083f274538567d851f98478fc83e9Jeff Brown return true; 6248a90e6e3174083f274538567d851f98478fc83e9Jeff Brown} 6258a90e6e3174083f274538567d851f98478fc83e9Jeff Brown 62618f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brownfloat LeastSquaresVelocityTrackerStrategy::chooseWeight(uint32_t index) const { 62718f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown switch (mWeighting) { 62818f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown case WEIGHTING_DELTA: { 62918f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // Weight points based on how much time elapsed between them and the next 63018f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // point so that points that "cover" a shorter time span are weighed less. 63118f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // delta 0ms: 0.5 63218f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // delta 10ms: 1.0 63318f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown if (index == mIndex) { 63418f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown return 1.0f; 63518f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown } 63618f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown uint32_t nextIndex = (index + 1) % HISTORY_SIZE; 63718f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown float deltaMillis = (mMovements[nextIndex].eventTime- mMovements[index].eventTime) 63818f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * 0.000001f; 63918f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown if (deltaMillis < 0) { 64018f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown return 0.5f; 64118f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown } 64218f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown if (deltaMillis < 10) { 64318f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown return 0.5f + deltaMillis * 0.05; 64418f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown } 64518f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown return 1.0f; 64618f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown } 64718f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown 64818f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown case WEIGHTING_CENTRAL: { 64918f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // Weight points based on their age, weighing very recent and very old points less. 65018f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // age 0ms: 0.5 65118f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // age 10ms: 1.0 65218f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // age 50ms: 1.0 65318f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // age 60ms: 0.5 65418f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown float ageMillis = (mMovements[mIndex].eventTime - mMovements[index].eventTime) 65518f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * 0.000001f; 65618f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown if (ageMillis < 0) { 65718f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown return 0.5f; 65818f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown } 65918f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown if (ageMillis < 10) { 66018f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown return 0.5f + ageMillis * 0.05; 66118f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown } 66218f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown if (ageMillis < 50) { 66318f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown return 1.0f; 66418f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown } 66518f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown if (ageMillis < 60) { 66618f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown return 0.5f + (60 - ageMillis) * 0.05; 66718f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown } 66818f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown return 0.5f; 66918f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown } 67018f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown 67118f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown case WEIGHTING_RECENT: { 67218f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // Weight points based on their age, weighing older points less. 67318f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // age 0ms: 1.0 67418f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // age 50ms: 1.0 67518f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown // age 100ms: 0.5 67618f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown float ageMillis = (mMovements[mIndex].eventTime - mMovements[index].eventTime) 67718f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown * 0.000001f; 67818f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown if (ageMillis < 50) { 67918f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown return 1.0f; 68018f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown } 68118f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown if (ageMillis < 100) { 68218f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown return 0.5f + (100 - ageMillis) * 0.01f; 68318f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown } 68418f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown return 0.5f; 68518f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown } 68618f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown 68718f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown case WEIGHTING_NONE: 68818f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown default: 68918f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown return 1.0f; 69018f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown } 69118f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown} 69218f329e9480fca75210bb7496e5b4bc987b4ad8fJeff Brown 69353dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown 69453dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown// --- IntegratingVelocityTrackerStrategy --- 69553dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown 696a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff BrownIntegratingVelocityTrackerStrategy::IntegratingVelocityTrackerStrategy(uint32_t degree) : 697a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown mDegree(degree) { 69853dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown} 69953dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown 70053dd12a66884540b87fe428383e2f79d0f5e32baJeff BrownIntegratingVelocityTrackerStrategy::~IntegratingVelocityTrackerStrategy() { 70153dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown} 70253dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown 70353dd12a66884540b87fe428383e2f79d0f5e32baJeff Brownvoid IntegratingVelocityTrackerStrategy::clear() { 70453dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown mPointerIdBits.clear(); 70553dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown} 70653dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown 70753dd12a66884540b87fe428383e2f79d0f5e32baJeff Brownvoid IntegratingVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { 70853dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown mPointerIdBits.value &= ~idBits.value; 70953dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown} 71053dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown 71153dd12a66884540b87fe428383e2f79d0f5e32baJeff Brownvoid IntegratingVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, 71253dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown const VelocityTracker::Position* positions) { 71353dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown uint32_t index = 0; 71453dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown for (BitSet32 iterIdBits(idBits); !iterIdBits.isEmpty();) { 71553dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown uint32_t id = iterIdBits.clearFirstMarkedBit(); 71653dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown State& state = mPointerState[id]; 71753dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown const VelocityTracker::Position& position = positions[index++]; 71853dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown if (mPointerIdBits.hasBit(id)) { 71953dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown updateState(state, eventTime, position.x, position.y); 72053dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown } else { 72153dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown initState(state, eventTime, position.x, position.y); 72253dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown } 72353dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown } 72453dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown 72553dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown mPointerIdBits = idBits; 72653dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown} 72753dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown 72853dd12a66884540b87fe428383e2f79d0f5e32baJeff Brownbool IntegratingVelocityTrackerStrategy::getEstimator(uint32_t id, 72953dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown VelocityTracker::Estimator* outEstimator) const { 73053dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown outEstimator->clear(); 73153dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown 73253dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown if (mPointerIdBits.hasBit(id)) { 73353dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown const State& state = mPointerState[id]; 73453dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown populateEstimator(state, outEstimator); 73553dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown return true; 73653dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown } 73753dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown 73853dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown return false; 73953dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown} 74053dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown 74153dd12a66884540b87fe428383e2f79d0f5e32baJeff Brownvoid IntegratingVelocityTrackerStrategy::initState(State& state, 742a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown nsecs_t eventTime, float xpos, float ypos) const { 74353dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown state.updateTime = eventTime; 744a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown state.degree = 0; 74553dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown 74653dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown state.xpos = xpos; 74753dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown state.xvel = 0; 748a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown state.xaccel = 0; 74953dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown state.ypos = ypos; 75053dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown state.yvel = 0; 751a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown state.yaccel = 0; 75253dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown} 75353dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown 75453dd12a66884540b87fe428383e2f79d0f5e32baJeff Brownvoid IntegratingVelocityTrackerStrategy::updateState(State& state, 755a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown nsecs_t eventTime, float xpos, float ypos) const { 75653dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown const nsecs_t MIN_TIME_DELTA = 2 * NANOS_PER_MS; 75753dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown const float FILTER_TIME_CONSTANT = 0.010f; // 10 milliseconds 75853dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown 75953dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown if (eventTime <= state.updateTime + MIN_TIME_DELTA) { 76053dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown return; 76153dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown } 76253dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown 76353dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown float dt = (eventTime - state.updateTime) * 0.000000001f; 76453dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown state.updateTime = eventTime; 76553dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown 76653dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown float xvel = (xpos - state.xpos) / dt; 76753dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown float yvel = (ypos - state.ypos) / dt; 768a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown if (state.degree == 0) { 76953dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown state.xvel = xvel; 77053dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown state.yvel = yvel; 771a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown state.degree = 1; 77253dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown } else { 77353dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown float alpha = dt / (FILTER_TIME_CONSTANT + dt); 774a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown if (mDegree == 1) { 775a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown state.xvel += (xvel - state.xvel) * alpha; 776a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown state.yvel += (yvel - state.yvel) * alpha; 777a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown } else { 778a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown float xaccel = (xvel - state.xvel) / dt; 779a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown float yaccel = (yvel - state.yvel) / dt; 780a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown if (state.degree == 1) { 781a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown state.xaccel = xaccel; 782a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown state.yaccel = yaccel; 783a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown state.degree = 2; 784a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown } else { 785a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown state.xaccel += (xaccel - state.xaccel) * alpha; 786a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown state.yaccel += (yaccel - state.yaccel) * alpha; 787a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown } 788a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown state.xvel += (state.xaccel * dt) * alpha; 789a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown state.yvel += (state.yaccel * dt) * alpha; 790a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown } 79153dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown } 79253dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown state.xpos = xpos; 79353dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown state.ypos = ypos; 79453dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown} 79553dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown 79653dd12a66884540b87fe428383e2f79d0f5e32baJeff Brownvoid IntegratingVelocityTrackerStrategy::populateEstimator(const State& state, 797a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown VelocityTracker::Estimator* outEstimator) const { 79853dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown outEstimator->time = state.updateTime; 79953dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown outEstimator->confidence = 1.0f; 800a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown outEstimator->degree = state.degree; 80153dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown outEstimator->xCoeff[0] = state.xpos; 80253dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown outEstimator->xCoeff[1] = state.xvel; 803a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown outEstimator->xCoeff[2] = state.xaccel / 2; 80453dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown outEstimator->yCoeff[0] = state.ypos; 80553dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown outEstimator->yCoeff[1] = state.yvel; 806a5b0698231459ac5b54cf8e8952ac5c2b2b2198bJeff Brown outEstimator->yCoeff[2] = state.yaccel / 2; 80753dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown} 80853dd12a66884540b87fe428383e2f79d0f5e32baJeff Brown 80951df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown 81051df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown// --- LegacyVelocityTrackerStrategy --- 81151df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown 81251df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brownconst nsecs_t LegacyVelocityTrackerStrategy::HORIZON; 81351df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brownconst uint32_t LegacyVelocityTrackerStrategy::HISTORY_SIZE; 81451df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brownconst nsecs_t LegacyVelocityTrackerStrategy::MIN_DURATION; 81551df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown 81651df04b93e8e362edd867abd7efaf1659b8b8b82Jeff BrownLegacyVelocityTrackerStrategy::LegacyVelocityTrackerStrategy() { 81751df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown clear(); 81851df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown} 81951df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown 82051df04b93e8e362edd867abd7efaf1659b8b8b82Jeff BrownLegacyVelocityTrackerStrategy::~LegacyVelocityTrackerStrategy() { 82151df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown} 82251df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown 82351df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brownvoid LegacyVelocityTrackerStrategy::clear() { 82451df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown mIndex = 0; 82551df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown mMovements[0].idBits.clear(); 82651df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown} 82751df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown 82851df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brownvoid LegacyVelocityTrackerStrategy::clearPointers(BitSet32 idBits) { 82951df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown BitSet32 remainingIdBits(mMovements[mIndex].idBits.value & ~idBits.value); 83051df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown mMovements[mIndex].idBits = remainingIdBits; 83151df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown} 83251df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown 83351df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brownvoid LegacyVelocityTrackerStrategy::addMovement(nsecs_t eventTime, BitSet32 idBits, 83451df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown const VelocityTracker::Position* positions) { 83551df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown if (++mIndex == HISTORY_SIZE) { 83651df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown mIndex = 0; 83751df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown } 83851df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown 83951df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown Movement& movement = mMovements[mIndex]; 84051df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown movement.eventTime = eventTime; 84151df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown movement.idBits = idBits; 84251df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown uint32_t count = idBits.count(); 84351df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown for (uint32_t i = 0; i < count; i++) { 84451df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown movement.positions[i] = positions[i]; 84551df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown } 84651df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown} 84751df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown 84851df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brownbool LegacyVelocityTrackerStrategy::getEstimator(uint32_t id, 84951df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown VelocityTracker::Estimator* outEstimator) const { 85051df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown outEstimator->clear(); 85151df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown 85251df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown const Movement& newestMovement = mMovements[mIndex]; 85351df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown if (!newestMovement.idBits.hasBit(id)) { 85451df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown return false; // no data 85551df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown } 85651df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown 85751df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // Find the oldest sample that contains the pointer and that is not older than HORIZON. 85851df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown nsecs_t minTime = newestMovement.eventTime - HORIZON; 85951df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown uint32_t oldestIndex = mIndex; 86051df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown uint32_t numTouches = 1; 86151df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown do { 86251df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown uint32_t nextOldestIndex = (oldestIndex == 0 ? HISTORY_SIZE : oldestIndex) - 1; 86351df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown const Movement& nextOldestMovement = mMovements[nextOldestIndex]; 86451df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown if (!nextOldestMovement.idBits.hasBit(id) 86551df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown || nextOldestMovement.eventTime < minTime) { 86651df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown break; 86751df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown } 86851df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown oldestIndex = nextOldestIndex; 86951df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown } while (++numTouches < HISTORY_SIZE); 87051df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown 87151df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // Calculate an exponentially weighted moving average of the velocity estimate 87251df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // at different points in time measured relative to the oldest sample. 87351df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // This is essentially an IIR filter. Newer samples are weighted more heavily 87451df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // than older samples. Samples at equal time points are weighted more or less 87551df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // equally. 87651df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // 87751df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // One tricky problem is that the sample data may be poorly conditioned. 87851df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // Sometimes samples arrive very close together in time which can cause us to 87951df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // overestimate the velocity at that time point. Most samples might be measured 88051df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // 16ms apart but some consecutive samples could be only 0.5sm apart because 88151df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // the hardware or driver reports them irregularly or in bursts. 88251df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown float accumVx = 0; 88351df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown float accumVy = 0; 88451df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown uint32_t index = oldestIndex; 88551df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown uint32_t samplesUsed = 0; 88651df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown const Movement& oldestMovement = mMovements[oldestIndex]; 88751df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown const VelocityTracker::Position& oldestPosition = oldestMovement.getPosition(id); 88851df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown nsecs_t lastDuration = 0; 88951df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown 89051df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown while (numTouches-- > 1) { 89151df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown if (++index == HISTORY_SIZE) { 89251df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown index = 0; 89351df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown } 89451df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown const Movement& movement = mMovements[index]; 89551df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown nsecs_t duration = movement.eventTime - oldestMovement.eventTime; 89651df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown 89751df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // If the duration between samples is small, we may significantly overestimate 89851df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // the velocity. Consequently, we impose a minimum duration constraint on the 89951df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // samples that we include in the calculation. 90051df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown if (duration >= MIN_DURATION) { 90151df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown const VelocityTracker::Position& position = movement.getPosition(id); 90251df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown float scale = 1000000000.0f / duration; // one over time delta in seconds 90351df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown float vx = (position.x - oldestPosition.x) * scale; 90451df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown float vy = (position.y - oldestPosition.y) * scale; 90551df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown accumVx = (accumVx * lastDuration + vx * duration) / (duration + lastDuration); 90651df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown accumVy = (accumVy * lastDuration + vy * duration) / (duration + lastDuration); 90751df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown lastDuration = duration; 90851df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown samplesUsed += 1; 90951df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown } 91051df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown } 91151df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown 91251df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown // Report velocity. 91351df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown const VelocityTracker::Position& newestPosition = newestMovement.getPosition(id); 91451df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown outEstimator->time = newestMovement.eventTime; 91551df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown outEstimator->confidence = 1; 91651df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown outEstimator->xCoeff[0] = newestPosition.x; 91751df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown outEstimator->yCoeff[0] = newestPosition.y; 91851df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown if (samplesUsed) { 91951df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown outEstimator->xCoeff[1] = accumVx; 92051df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown outEstimator->yCoeff[1] = accumVy; 92151df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown outEstimator->degree = 1; 92251df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown } else { 92351df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown outEstimator->degree = 0; 92451df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown } 92551df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown return true; 92651df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown} 92751df04b93e8e362edd867abd7efaf1659b8b8b82Jeff Brown 9288a90e6e3174083f274538567d851f98478fc83e9Jeff Brown} // namespace android 929