• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1 /*
2  * Copyright (C) 2012 The Android Open Source Project
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  *      http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #define LOG_TAG "VelocityTracker"
18 
19 #include <android-base/logging.h>
20 #include <array>
21 #include <ftl/enum.h>
22 #include <inttypes.h>
23 #include <limits.h>
24 #include <math.h>
25 #include <optional>
26 
27 #include <android-base/stringprintf.h>
28 #include <input/PrintTools.h>
29 #include <input/VelocityTracker.h>
30 #include <utils/BitSet.h>
31 #include <utils/Timers.h>
32 
33 using std::literals::chrono_literals::operator""ms;
34 
35 namespace android {
36 
37 /**
38  * Log debug messages about velocity tracking.
39  * Enable this via "adb shell setprop log.tag.VelocityTrackerVelocity DEBUG" (requires restart)
40  */
41 const bool DEBUG_VELOCITY =
42         __android_log_is_loggable(ANDROID_LOG_DEBUG, LOG_TAG "Velocity", ANDROID_LOG_INFO);
43 
44 /**
45  * Log debug messages about the progress of the algorithm itself.
46  * Enable this via "adb shell setprop log.tag.VelocityTrackerStrategy DEBUG" (requires restart)
47  */
48 const bool DEBUG_STRATEGY =
49         __android_log_is_loggable(ANDROID_LOG_DEBUG, LOG_TAG "Strategy", ANDROID_LOG_INFO);
50 
51 /**
52  * Log debug messages about the 'impulse' strategy.
53  * Enable this via "adb shell setprop log.tag.VelocityTrackerImpulse DEBUG" (requires restart)
54  */
55 const bool DEBUG_IMPULSE =
56         __android_log_is_loggable(ANDROID_LOG_DEBUG, LOG_TAG "Impulse", ANDROID_LOG_INFO);
57 
58 // Nanoseconds per milliseconds.
59 static const nsecs_t NANOS_PER_MS = 1000000;
60 
61 // All axes supported for velocity tracking, mapped to their default strategies.
62 // Although other strategies are available for testing and comparison purposes,
63 // the default strategy is the one that applications will actually use.  Be very careful
64 // when adjusting the default strategy because it can dramatically affect
65 // (often in a bad way) the user experience.
66 static const std::map<int32_t, VelocityTracker::Strategy> DEFAULT_STRATEGY_BY_AXIS =
67         {{AMOTION_EVENT_AXIS_X, VelocityTracker::Strategy::LSQ2},
68          {AMOTION_EVENT_AXIS_Y, VelocityTracker::Strategy::LSQ2},
69          {AMOTION_EVENT_AXIS_SCROLL, VelocityTracker::Strategy::IMPULSE}};
70 
71 // Axes specifying location on a 2D plane (i.e. X and Y).
72 static const std::set<int32_t> PLANAR_AXES = {AMOTION_EVENT_AXIS_X, AMOTION_EVENT_AXIS_Y};
73 
74 // Axes whose motion values are differential values (i.e. deltas).
75 static const std::set<int32_t> DIFFERENTIAL_AXES = {AMOTION_EVENT_AXIS_SCROLL};
76 
77 // Threshold for determining that a pointer has stopped moving.
78 // Some input devices do not send ACTION_MOVE events in the case where a pointer has
79 // stopped.  We need to detect this case so that we can accurately predict the
80 // velocity after the pointer starts moving again.
81 static const std::chrono::duration ASSUME_POINTER_STOPPED_TIME = 40ms;
82 
toString(std::chrono::nanoseconds t)83 static std::string toString(std::chrono::nanoseconds t) {
84     std::stringstream stream;
85     stream.precision(1);
86     stream << std::fixed << std::chrono::duration<float, std::milli>(t).count() << " ms";
87     return stream.str();
88 }
89 
vectorDot(const float * a,const float * b,uint32_t m)90 static float vectorDot(const float* a, const float* b, uint32_t m) {
91     float r = 0;
92     for (size_t i = 0; i < m; i++) {
93         r += *(a++) * *(b++);
94     }
95     return r;
96 }
97 
vectorNorm(const float * a,uint32_t m)98 static float vectorNorm(const float* a, uint32_t m) {
99     float r = 0;
100     for (size_t i = 0; i < m; i++) {
101         float t = *(a++);
102         r += t * t;
103     }
104     return sqrtf(r);
105 }
106 
vectorToString(const float * a,uint32_t m)107 static std::string vectorToString(const float* a, uint32_t m) {
108     std::string str;
109     str += "[";
110     for (size_t i = 0; i < m; i++) {
111         if (i) {
112             str += ",";
113         }
114         str += android::base::StringPrintf(" %f", *(a++));
115     }
116     str += " ]";
117     return str;
118 }
119 
vectorToString(const std::vector<float> & v)120 static std::string vectorToString(const std::vector<float>& v) {
121     return vectorToString(v.data(), v.size());
122 }
123 
matrixToString(const float * a,uint32_t m,uint32_t n,bool rowMajor)124 static std::string matrixToString(const float* a, uint32_t m, uint32_t n, bool rowMajor) {
125     std::string str;
126     str = "[";
127     for (size_t i = 0; i < m; i++) {
128         if (i) {
129             str += ",";
130         }
131         str += " [";
132         for (size_t j = 0; j < n; j++) {
133             if (j) {
134                 str += ",";
135             }
136             str += android::base::StringPrintf(" %f", a[rowMajor ? i * n + j : j * m + i]);
137         }
138         str += " ]";
139     }
140     str += " ]";
141     return str;
142 }
143 
144 
145 // --- VelocityTracker ---
146 
VelocityTracker(const Strategy strategy)147 VelocityTracker::VelocityTracker(const Strategy strategy)
148       : mLastEventTime(0), mCurrentPointerIdBits(0), mOverrideStrategy(strategy) {}
149 
isAxisSupported(int32_t axis)150 bool VelocityTracker::isAxisSupported(int32_t axis) {
151     return DEFAULT_STRATEGY_BY_AXIS.find(axis) != DEFAULT_STRATEGY_BY_AXIS.end();
152 }
153 
configureStrategy(int32_t axis)154 void VelocityTracker::configureStrategy(int32_t axis) {
155     const bool isDifferentialAxis = DIFFERENTIAL_AXES.find(axis) != DIFFERENTIAL_AXES.end();
156     if (isDifferentialAxis || mOverrideStrategy == VelocityTracker::Strategy::DEFAULT) {
157         // Do not allow overrides of strategies for differential axes, for now.
158         mConfiguredStrategies[axis] = createStrategy(DEFAULT_STRATEGY_BY_AXIS.at(axis),
159                                                      /*deltaValues=*/isDifferentialAxis);
160     } else {
161         mConfiguredStrategies[axis] = createStrategy(mOverrideStrategy, /*deltaValues=*/false);
162     }
163 }
164 
createStrategy(VelocityTracker::Strategy strategy,bool deltaValues)165 std::unique_ptr<VelocityTrackerStrategy> VelocityTracker::createStrategy(
166         VelocityTracker::Strategy strategy, bool deltaValues) {
167     switch (strategy) {
168         case VelocityTracker::Strategy::IMPULSE:
169             ALOGI_IF(DEBUG_STRATEGY, "Initializing impulse strategy");
170             return std::make_unique<ImpulseVelocityTrackerStrategy>(deltaValues);
171 
172         case VelocityTracker::Strategy::LSQ1:
173             return std::make_unique<LeastSquaresVelocityTrackerStrategy>(1);
174 
175         case VelocityTracker::Strategy::LSQ2:
176             ALOGI_IF(DEBUG_STRATEGY && !DEBUG_IMPULSE, "Initializing lsq2 strategy");
177             return std::make_unique<LeastSquaresVelocityTrackerStrategy>(2);
178 
179         case VelocityTracker::Strategy::LSQ3:
180             return std::make_unique<LeastSquaresVelocityTrackerStrategy>(3);
181 
182         case VelocityTracker::Strategy::WLSQ2_DELTA:
183             return std::make_unique<
184                     LeastSquaresVelocityTrackerStrategy>(2,
185                                                          LeastSquaresVelocityTrackerStrategy::
186                                                                  Weighting::DELTA);
187         case VelocityTracker::Strategy::WLSQ2_CENTRAL:
188             return std::make_unique<
189                     LeastSquaresVelocityTrackerStrategy>(2,
190                                                          LeastSquaresVelocityTrackerStrategy::
191                                                                  Weighting::CENTRAL);
192         case VelocityTracker::Strategy::WLSQ2_RECENT:
193             return std::make_unique<
194                     LeastSquaresVelocityTrackerStrategy>(2,
195                                                          LeastSquaresVelocityTrackerStrategy::
196                                                                  Weighting::RECENT);
197 
198         case VelocityTracker::Strategy::INT1:
199             return std::make_unique<IntegratingVelocityTrackerStrategy>(1);
200 
201         case VelocityTracker::Strategy::INT2:
202             return std::make_unique<IntegratingVelocityTrackerStrategy>(2);
203 
204         case VelocityTracker::Strategy::LEGACY:
205             return std::make_unique<LegacyVelocityTrackerStrategy>();
206 
207         default:
208             break;
209     }
210     LOG(FATAL) << "Invalid strategy: " << ftl::enum_string(strategy)
211                << ", deltaValues = " << deltaValues;
212 
213     return nullptr;
214 }
215 
clear()216 void VelocityTracker::clear() {
217     mCurrentPointerIdBits.clear();
218     mActivePointerId = std::nullopt;
219     mConfiguredStrategies.clear();
220 }
221 
clearPointer(int32_t pointerId)222 void VelocityTracker::clearPointer(int32_t pointerId) {
223     mCurrentPointerIdBits.clearBit(pointerId);
224 
225     if (mActivePointerId && *mActivePointerId == pointerId) {
226         // The active pointer id is being removed. Mark it invalid and try to find a new one
227         // from the remaining pointers.
228         mActivePointerId = std::nullopt;
229         if (!mCurrentPointerIdBits.isEmpty()) {
230             mActivePointerId = mCurrentPointerIdBits.firstMarkedBit();
231         }
232     }
233 
234     for (const auto& [_, strategy] : mConfiguredStrategies) {
235         strategy->clearPointer(pointerId);
236     }
237 }
238 
addMovement(nsecs_t eventTime,int32_t pointerId,int32_t axis,float position)239 void VelocityTracker::addMovement(nsecs_t eventTime, int32_t pointerId, int32_t axis,
240                                   float position) {
241     if (mCurrentPointerIdBits.hasBit(pointerId) &&
242         std::chrono::nanoseconds(eventTime - mLastEventTime) > ASSUME_POINTER_STOPPED_TIME) {
243         ALOGD_IF(DEBUG_VELOCITY, "VelocityTracker: stopped for %s, clearing state.",
244                  toString(std::chrono::nanoseconds(eventTime - mLastEventTime)).c_str());
245 
246         // We have not received any movements for too long.  Assume that all pointers
247         // have stopped.
248         mConfiguredStrategies.clear();
249     }
250     mLastEventTime = eventTime;
251 
252     mCurrentPointerIdBits.markBit(pointerId);
253     if (!mActivePointerId) {
254         // Let this be the new active pointer if no active pointer is currently set
255         mActivePointerId = pointerId;
256     }
257 
258     if (mConfiguredStrategies.find(axis) == mConfiguredStrategies.end()) {
259         configureStrategy(axis);
260     }
261     mConfiguredStrategies[axis]->addMovement(eventTime, pointerId, position);
262 
263     if (DEBUG_VELOCITY) {
264         ALOGD("VelocityTracker: addMovement eventTime=%" PRId64 ", pointerId=%" PRId32
265               ", activePointerId=%s",
266               eventTime, pointerId, toString(mActivePointerId).c_str());
267 
268         std::optional<Estimator> estimator = getEstimator(axis, pointerId);
269         ALOGD("  %d: axis=%d, position=%0.3f, "
270               "estimator (degree=%d, coeff=%s, confidence=%f)",
271               pointerId, axis, position, int((*estimator).degree),
272               vectorToString((*estimator).coeff.data(), (*estimator).degree + 1).c_str(),
273               (*estimator).confidence);
274     }
275 }
276 
addMovement(const MotionEvent * event)277 void VelocityTracker::addMovement(const MotionEvent* event) {
278     // Stores data about which axes to process based on the incoming motion event.
279     std::set<int32_t> axesToProcess;
280     int32_t actionMasked = event->getActionMasked();
281 
282     switch (actionMasked) {
283         case AMOTION_EVENT_ACTION_DOWN:
284         case AMOTION_EVENT_ACTION_HOVER_ENTER:
285             // Clear all pointers on down before adding the new movement.
286             clear();
287             axesToProcess.insert(PLANAR_AXES.begin(), PLANAR_AXES.end());
288             break;
289         case AMOTION_EVENT_ACTION_POINTER_DOWN: {
290             // Start a new movement trace for a pointer that just went down.
291             // We do this on down instead of on up because the client may want to query the
292             // final velocity for a pointer that just went up.
293             clearPointer(event->getPointerId(event->getActionIndex()));
294             axesToProcess.insert(PLANAR_AXES.begin(), PLANAR_AXES.end());
295             break;
296         }
297         case AMOTION_EVENT_ACTION_MOVE:
298         case AMOTION_EVENT_ACTION_HOVER_MOVE:
299             axesToProcess.insert(PLANAR_AXES.begin(), PLANAR_AXES.end());
300             break;
301         case AMOTION_EVENT_ACTION_POINTER_UP:
302         case AMOTION_EVENT_ACTION_UP: {
303             std::chrono::nanoseconds delaySinceLastEvent(event->getEventTime() - mLastEventTime);
304             if (delaySinceLastEvent > ASSUME_POINTER_STOPPED_TIME) {
305                 ALOGD_IF(DEBUG_VELOCITY,
306                          "VelocityTracker: stopped for %s, clearing state upon pointer liftoff.",
307                          toString(delaySinceLastEvent).c_str());
308                 // We have not received any movements for too long.  Assume that all pointers
309                 // have stopped.
310                 for (int32_t axis : PLANAR_AXES) {
311                     mConfiguredStrategies.erase(axis);
312                 }
313             }
314             // These actions because they do not convey any new information about
315             // pointer movement.  We also want to preserve the last known velocity of the pointers.
316             // Note that ACTION_UP and ACTION_POINTER_UP always report the last known position
317             // of the pointers that went up.  ACTION_POINTER_UP does include the new position of
318             // pointers that remained down but we will also receive an ACTION_MOVE with this
319             // information if any of them actually moved.  Since we don't know how many pointers
320             // will be going up at once it makes sense to just wait for the following ACTION_MOVE
321             // before adding the movement.
322             return;
323         }
324         case AMOTION_EVENT_ACTION_SCROLL:
325             axesToProcess.insert(AMOTION_EVENT_AXIS_SCROLL);
326             break;
327         default:
328             // Ignore all other actions.
329             return;
330     }
331 
332     const size_t historySize = event->getHistorySize();
333     for (size_t h = 0; h <= historySize; h++) {
334         const nsecs_t eventTime = event->getHistoricalEventTime(h);
335         for (size_t i = 0; i < event->getPointerCount(); i++) {
336             if (event->isResampled(i, h)) {
337                 continue; // skip resampled samples
338             }
339             const int32_t pointerId = event->getPointerId(i);
340             for (int32_t axis : axesToProcess) {
341                 const float position = event->getHistoricalAxisValue(axis, i, h);
342                 addMovement(eventTime, pointerId, axis, position);
343             }
344         }
345     }
346 }
347 
getVelocity(int32_t axis,int32_t pointerId) const348 std::optional<float> VelocityTracker::getVelocity(int32_t axis, int32_t pointerId) const {
349     std::optional<Estimator> estimator = getEstimator(axis, pointerId);
350     if (estimator && (*estimator).degree >= 1) {
351         return (*estimator).coeff[1];
352     }
353     return {};
354 }
355 
getComputedVelocity(int32_t units,float maxVelocity)356 VelocityTracker::ComputedVelocity VelocityTracker::getComputedVelocity(int32_t units,
357                                                                        float maxVelocity) {
358     ComputedVelocity computedVelocity;
359     for (const auto& [axis, _] : mConfiguredStrategies) {
360         BitSet32 copyIdBits = BitSet32(mCurrentPointerIdBits);
361         while (!copyIdBits.isEmpty()) {
362             uint32_t id = copyIdBits.clearFirstMarkedBit();
363             std::optional<float> velocity = getVelocity(axis, id);
364             if (velocity) {
365                 float adjustedVelocity =
366                         std::clamp(*velocity * units / 1000, -maxVelocity, maxVelocity);
367                 computedVelocity.addVelocity(axis, id, adjustedVelocity);
368             }
369         }
370     }
371     return computedVelocity;
372 }
373 
getEstimator(int32_t axis,int32_t pointerId) const374 std::optional<VelocityTracker::Estimator> VelocityTracker::getEstimator(int32_t axis,
375                                                                         int32_t pointerId) const {
376     const auto& it = mConfiguredStrategies.find(axis);
377     if (it == mConfiguredStrategies.end()) {
378         return std::nullopt;
379     }
380     return it->second->getEstimator(pointerId);
381 }
382 
383 // --- LeastSquaresVelocityTrackerStrategy ---
384 
LeastSquaresVelocityTrackerStrategy(uint32_t degree,Weighting weighting)385 LeastSquaresVelocityTrackerStrategy::LeastSquaresVelocityTrackerStrategy(uint32_t degree,
386                                                                          Weighting weighting)
387       : mDegree(degree), mWeighting(weighting) {}
388 
~LeastSquaresVelocityTrackerStrategy()389 LeastSquaresVelocityTrackerStrategy::~LeastSquaresVelocityTrackerStrategy() {
390 }
391 
clearPointer(int32_t pointerId)392 void LeastSquaresVelocityTrackerStrategy::clearPointer(int32_t pointerId) {
393     mIndex.erase(pointerId);
394     mMovements.erase(pointerId);
395 }
396 
addMovement(nsecs_t eventTime,int32_t pointerId,float position)397 void LeastSquaresVelocityTrackerStrategy::addMovement(nsecs_t eventTime, int32_t pointerId,
398                                                       float position) {
399     // If data for this pointer already exists, we have a valid entry at the position of
400     // mIndex[pointerId] and mMovements[pointerId]. In that case, we need to advance the index
401     // to the next position in the circular buffer and write the new Movement there. Otherwise,
402     // if this is a first movement for this pointer, we initialize the maps mIndex and mMovements
403     // for this pointer and write to the first position.
404     auto [movementIt, inserted] = mMovements.insert({pointerId, {}});
405     auto [indexIt, _] = mIndex.insert({pointerId, 0});
406     size_t& index = indexIt->second;
407     if (!inserted && movementIt->second[index].eventTime != eventTime) {
408         // When ACTION_POINTER_DOWN happens, we will first receive ACTION_MOVE with the coordinates
409         // of the existing pointers, and then ACTION_POINTER_DOWN with the coordinates that include
410         // the new pointer. If the eventtimes for both events are identical, just update the data
411         // for this time.
412         // We only compare against the last value, as it is likely that addMovement is called
413         // in chronological order as events occur.
414         index++;
415     }
416     if (index == HISTORY_SIZE) {
417         index = 0;
418     }
419 
420     Movement& movement = movementIt->second[index];
421     movement.eventTime = eventTime;
422     movement.position = position;
423 }
424 
425 /**
426  * Solves a linear least squares problem to obtain a N degree polynomial that fits
427  * the specified input data as nearly as possible.
428  *
429  * Returns true if a solution is found, false otherwise.
430  *
431  * The input consists of two vectors of data points X and Y with indices 0..m-1
432  * along with a weight vector W of the same size.
433  *
434  * The output is a vector B with indices 0..n that describes a polynomial
435  * that fits the data, such the sum of W[i] * W[i] * abs(Y[i] - (B[0] + B[1] X[i]
436  * + B[2] X[i]^2 ... B[n] X[i]^n)) for all i between 0 and m-1 is minimized.
437  *
438  * Accordingly, the weight vector W should be initialized by the caller with the
439  * reciprocal square root of the variance of the error in each input data point.
440  * In other words, an ideal choice for W would be W[i] = 1 / var(Y[i]) = 1 / stddev(Y[i]).
441  * The weights express the relative importance of each data point.  If the weights are
442  * all 1, then the data points are considered to be of equal importance when fitting
443  * the polynomial.  It is a good idea to choose weights that diminish the importance
444  * of data points that may have higher than usual error margins.
445  *
446  * Errors among data points are assumed to be independent.  W is represented here
447  * as a vector although in the literature it is typically taken to be a diagonal matrix.
448  *
449  * That is to say, the function that generated the input data can be approximated
450  * by y(x) ~= B[0] + B[1] x + B[2] x^2 + ... + B[n] x^n.
451  *
452  * The coefficient of determination (R^2) is also returned to describe the goodness
453  * of fit of the model for the given data.  It is a value between 0 and 1, where 1
454  * indicates perfect correspondence.
455  *
456  * This function first expands the X vector to a m by n matrix A such that
457  * A[i][0] = 1, A[i][1] = X[i], A[i][2] = X[i]^2, ..., A[i][n] = X[i]^n, then
458  * multiplies it by w[i]./
459  *
460  * Then it calculates the QR decomposition of A yielding an m by m orthonormal matrix Q
461  * and an m by n upper triangular matrix R.  Because R is upper triangular (lower
462  * part is all zeroes), we can simplify the decomposition into an m by n matrix
463  * Q1 and a n by n matrix R1 such that A = Q1 R1.
464  *
465  * Finally we solve the system of linear equations given by R1 B = (Qtranspose W Y)
466  * to find B.
467  *
468  * For efficiency, we lay out A and Q column-wise in memory because we frequently
469  * operate on the column vectors.  Conversely, we lay out R row-wise.
470  *
471  * http://en.wikipedia.org/wiki/Numerical_methods_for_linear_least_squares
472  * http://en.wikipedia.org/wiki/Gram-Schmidt
473  */
solveLeastSquares(const std::vector<float> & x,const std::vector<float> & y,const std::vector<float> & w,uint32_t n,std::array<float,VelocityTracker::Estimator::MAX_DEGREE+1> & outB,float * outDet)474 static bool solveLeastSquares(const std::vector<float>& x, const std::vector<float>& y,
475                               const std::vector<float>& w, uint32_t n,
476                               std::array<float, VelocityTracker::Estimator::MAX_DEGREE + 1>& outB,
477                               float* outDet) {
478     const size_t m = x.size();
479 
480     ALOGD_IF(DEBUG_STRATEGY, "solveLeastSquares: m=%d, n=%d, x=%s, y=%s, w=%s", int(m), int(n),
481              vectorToString(x).c_str(), vectorToString(y).c_str(), vectorToString(w).c_str());
482 
483     LOG_ALWAYS_FATAL_IF(m != y.size() || m != w.size(), "Mismatched vector sizes");
484 
485     // Expand the X vector to a matrix A, pre-multiplied by the weights.
486     float a[n][m]; // column-major order
487     for (uint32_t h = 0; h < m; h++) {
488         a[0][h] = w[h];
489         for (uint32_t i = 1; i < n; i++) {
490             a[i][h] = a[i - 1][h] * x[h];
491         }
492     }
493 
494     ALOGD_IF(DEBUG_STRATEGY, "  - a=%s",
495              matrixToString(&a[0][0], m, n, /*rowMajor=*/false).c_str());
496 
497     // Apply the Gram-Schmidt process to A to obtain its QR decomposition.
498     float q[n][m]; // orthonormal basis, column-major order
499     float r[n][n]; // upper triangular matrix, row-major order
500     for (uint32_t j = 0; j < n; j++) {
501         for (uint32_t h = 0; h < m; h++) {
502             q[j][h] = a[j][h];
503         }
504         for (uint32_t i = 0; i < j; i++) {
505             float dot = vectorDot(&q[j][0], &q[i][0], m);
506             for (uint32_t h = 0; h < m; h++) {
507                 q[j][h] -= dot * q[i][h];
508             }
509         }
510 
511         float norm = vectorNorm(&q[j][0], m);
512         if (norm < 0.000001f) {
513             // vectors are linearly dependent or zero so no solution
514             ALOGD_IF(DEBUG_STRATEGY, "  - no solution, norm=%f", norm);
515             return false;
516         }
517 
518         float invNorm = 1.0f / norm;
519         for (uint32_t h = 0; h < m; h++) {
520             q[j][h] *= invNorm;
521         }
522         for (uint32_t i = 0; i < n; i++) {
523             r[j][i] = i < j ? 0 : vectorDot(&q[j][0], &a[i][0], m);
524         }
525     }
526     if (DEBUG_STRATEGY) {
527         ALOGD("  - q=%s", matrixToString(&q[0][0], m, n, /*rowMajor=*/false).c_str());
528         ALOGD("  - r=%s", matrixToString(&r[0][0], n, n, /*rowMajor=*/true).c_str());
529 
530         // calculate QR, if we factored A correctly then QR should equal A
531         float qr[n][m];
532         for (uint32_t h = 0; h < m; h++) {
533             for (uint32_t i = 0; i < n; i++) {
534                 qr[i][h] = 0;
535                 for (uint32_t j = 0; j < n; j++) {
536                     qr[i][h] += q[j][h] * r[j][i];
537                 }
538             }
539         }
540         ALOGD("  - qr=%s", matrixToString(&qr[0][0], m, n, /*rowMajor=*/false).c_str());
541     }
542 
543     // Solve R B = Qt W Y to find B.  This is easy because R is upper triangular.
544     // We just work from bottom-right to top-left calculating B's coefficients.
545     float wy[m];
546     for (uint32_t h = 0; h < m; h++) {
547         wy[h] = y[h] * w[h];
548     }
549     for (uint32_t i = n; i != 0; ) {
550         i--;
551         outB[i] = vectorDot(&q[i][0], wy, m);
552         for (uint32_t j = n - 1; j > i; j--) {
553             outB[i] -= r[i][j] * outB[j];
554         }
555         outB[i] /= r[i][i];
556     }
557 
558     ALOGD_IF(DEBUG_STRATEGY, "  - b=%s", vectorToString(outB.data(), n).c_str());
559 
560     // Calculate the coefficient of determination as 1 - (SSerr / SStot) where
561     // SSerr is the residual sum of squares (variance of the error),
562     // and SStot is the total sum of squares (variance of the data) where each
563     // has been weighted.
564     float ymean = 0;
565     for (uint32_t h = 0; h < m; h++) {
566         ymean += y[h];
567     }
568     ymean /= m;
569 
570     float sserr = 0;
571     float sstot = 0;
572     for (uint32_t h = 0; h < m; h++) {
573         float err = y[h] - outB[0];
574         float term = 1;
575         for (uint32_t i = 1; i < n; i++) {
576             term *= x[h];
577             err -= term * outB[i];
578         }
579         sserr += w[h] * w[h] * err * err;
580         float var = y[h] - ymean;
581         sstot += w[h] * w[h] * var * var;
582     }
583     *outDet = sstot > 0.000001f ? 1.0f - (sserr / sstot) : 1;
584 
585     ALOGD_IF(DEBUG_STRATEGY, "  - sserr=%f", sserr);
586     ALOGD_IF(DEBUG_STRATEGY, "  - sstot=%f", sstot);
587     ALOGD_IF(DEBUG_STRATEGY, "  - det=%f", *outDet);
588 
589     return true;
590 }
591 
592 /*
593  * Optimized unweighted second-order least squares fit. About 2x speed improvement compared to
594  * the default implementation
595  */
solveUnweightedLeastSquaresDeg2(const std::vector<float> & x,const std::vector<float> & y)596 static std::optional<std::array<float, 3>> solveUnweightedLeastSquaresDeg2(
597         const std::vector<float>& x, const std::vector<float>& y) {
598     const size_t count = x.size();
599     LOG_ALWAYS_FATAL_IF(count != y.size(), "Mismatching array sizes");
600     // Solving y = a*x^2 + b*x + c
601     float sxi = 0, sxiyi = 0, syi = 0, sxi2 = 0, sxi3 = 0, sxi2yi = 0, sxi4 = 0;
602 
603     for (size_t i = 0; i < count; i++) {
604         float xi = x[i];
605         float yi = y[i];
606         float xi2 = xi*xi;
607         float xi3 = xi2*xi;
608         float xi4 = xi3*xi;
609         float xiyi = xi*yi;
610         float xi2yi = xi2*yi;
611 
612         sxi += xi;
613         sxi2 += xi2;
614         sxiyi += xiyi;
615         sxi2yi += xi2yi;
616         syi += yi;
617         sxi3 += xi3;
618         sxi4 += xi4;
619     }
620 
621     float Sxx = sxi2 - sxi*sxi / count;
622     float Sxy = sxiyi - sxi*syi / count;
623     float Sxx2 = sxi3 - sxi*sxi2 / count;
624     float Sx2y = sxi2yi - sxi2*syi / count;
625     float Sx2x2 = sxi4 - sxi2*sxi2 / count;
626 
627     float denominator = Sxx*Sx2x2 - Sxx2*Sxx2;
628     if (denominator == 0) {
629         ALOGW("division by 0 when computing velocity, Sxx=%f, Sx2x2=%f, Sxx2=%f", Sxx, Sx2x2, Sxx2);
630         return std::nullopt;
631     }
632     // Compute a
633     float numerator = Sx2y*Sxx - Sxy*Sxx2;
634     float a = numerator / denominator;
635 
636     // Compute b
637     numerator = Sxy*Sx2x2 - Sx2y*Sxx2;
638     float b = numerator / denominator;
639 
640     // Compute c
641     float c = syi/count - b * sxi/count - a * sxi2/count;
642 
643     return std::make_optional(std::array<float, 3>({c, b, a}));
644 }
645 
getEstimator(int32_t pointerId) const646 std::optional<VelocityTracker::Estimator> LeastSquaresVelocityTrackerStrategy::getEstimator(
647         int32_t pointerId) const {
648     const auto movementIt = mMovements.find(pointerId);
649     if (movementIt == mMovements.end()) {
650         return std::nullopt; // no data
651     }
652     // Iterate over movement samples in reverse time order and collect samples.
653     std::vector<float> positions;
654     std::vector<float> w;
655     std::vector<float> time;
656 
657     uint32_t index = mIndex.at(pointerId);
658     const Movement& newestMovement = movementIt->second[index];
659     do {
660         const Movement& movement = movementIt->second[index];
661 
662         nsecs_t age = newestMovement.eventTime - movement.eventTime;
663         if (age > HORIZON) {
664             break;
665         }
666         if (movement.eventTime == 0 && index != 0) {
667             // All eventTime's are initialized to 0. In this fixed-width circular buffer, it's
668             // possible that not all entries are valid. We use a time=0 as a signal for those
669             // uninitialized values. If we encounter a time of 0 in a position
670             // that's > 0, it means that we hit the block where the data wasn't initialized.
671             // We still don't know whether the value at index=0, with eventTime=0 is valid.
672             // However, that's only possible when the value is by itself. So there's no hard in
673             // processing it anyways, since the velocity for a single point is zero, and this
674             // situation will only be encountered in artificial circumstances (in tests).
675             // In practice, time will never be 0.
676             break;
677         }
678         positions.push_back(movement.position);
679         w.push_back(chooseWeight(pointerId, index));
680         time.push_back(-age * 0.000000001f);
681         index = (index == 0 ? HISTORY_SIZE : index) - 1;
682     } while (positions.size() < HISTORY_SIZE);
683 
684     const size_t m = positions.size();
685     if (m == 0) {
686         return std::nullopt; // no data
687     }
688 
689     // Calculate a least squares polynomial fit.
690     uint32_t degree = mDegree;
691     if (degree > m - 1) {
692         degree = m - 1;
693     }
694 
695     if (degree == 2 && mWeighting == Weighting::NONE) {
696         // Optimize unweighted, quadratic polynomial fit
697         std::optional<std::array<float, 3>> coeff =
698                 solveUnweightedLeastSquaresDeg2(time, positions);
699         if (coeff) {
700             VelocityTracker::Estimator estimator;
701             estimator.time = newestMovement.eventTime;
702             estimator.degree = 2;
703             estimator.confidence = 1;
704             for (size_t i = 0; i <= estimator.degree; i++) {
705                 estimator.coeff[i] = (*coeff)[i];
706             }
707             return estimator;
708         }
709     } else if (degree >= 1) {
710         // General case for an Nth degree polynomial fit
711         float det;
712         uint32_t n = degree + 1;
713         VelocityTracker::Estimator estimator;
714         if (solveLeastSquares(time, positions, w, n, estimator.coeff, &det)) {
715             estimator.time = newestMovement.eventTime;
716             estimator.degree = degree;
717             estimator.confidence = det;
718 
719             ALOGD_IF(DEBUG_STRATEGY, "estimate: degree=%d, coeff=%s, confidence=%f",
720                      int(estimator.degree), vectorToString(estimator.coeff.data(), n).c_str(),
721                      estimator.confidence);
722 
723             return estimator;
724         }
725     }
726 
727     // No velocity data available for this pointer, but we do have its current position.
728     VelocityTracker::Estimator estimator;
729     estimator.coeff[0] = positions[0];
730     estimator.time = newestMovement.eventTime;
731     estimator.degree = 0;
732     estimator.confidence = 1;
733     return estimator;
734 }
735 
chooseWeight(int32_t pointerId,uint32_t index) const736 float LeastSquaresVelocityTrackerStrategy::chooseWeight(int32_t pointerId, uint32_t index) const {
737     const std::array<Movement, HISTORY_SIZE>& movements = mMovements.at(pointerId);
738     switch (mWeighting) {
739         case Weighting::DELTA: {
740             // Weight points based on how much time elapsed between them and the next
741             // point so that points that "cover" a shorter time span are weighed less.
742             //   delta  0ms: 0.5
743             //   delta 10ms: 1.0
744             if (index == mIndex.at(pointerId)) {
745                 return 1.0f;
746             }
747             uint32_t nextIndex = (index + 1) % HISTORY_SIZE;
748             float deltaMillis =
749                     (movements[nextIndex].eventTime - movements[index].eventTime) * 0.000001f;
750             if (deltaMillis < 0) {
751                 return 0.5f;
752             }
753             if (deltaMillis < 10) {
754                 return 0.5f + deltaMillis * 0.05;
755             }
756             return 1.0f;
757         }
758 
759         case Weighting::CENTRAL: {
760             // Weight points based on their age, weighing very recent and very old points less.
761             //   age  0ms: 0.5
762             //   age 10ms: 1.0
763             //   age 50ms: 1.0
764             //   age 60ms: 0.5
765             float ageMillis =
766                     (movements[mIndex.at(pointerId)].eventTime - movements[index].eventTime) *
767                     0.000001f;
768             if (ageMillis < 0) {
769                 return 0.5f;
770             }
771             if (ageMillis < 10) {
772                 return 0.5f + ageMillis * 0.05;
773             }
774             if (ageMillis < 50) {
775                 return 1.0f;
776             }
777             if (ageMillis < 60) {
778                 return 0.5f + (60 - ageMillis) * 0.05;
779             }
780             return 0.5f;
781         }
782 
783         case Weighting::RECENT: {
784             // Weight points based on their age, weighing older points less.
785             //   age   0ms: 1.0
786             //   age  50ms: 1.0
787             //   age 100ms: 0.5
788             float ageMillis =
789                     (movements[mIndex.at(pointerId)].eventTime - movements[index].eventTime) *
790                     0.000001f;
791             if (ageMillis < 50) {
792                 return 1.0f;
793             }
794             if (ageMillis < 100) {
795                 return 0.5f + (100 - ageMillis) * 0.01f;
796             }
797             return 0.5f;
798         }
799 
800         case Weighting::NONE:
801             return 1.0f;
802     }
803 }
804 
805 // --- IntegratingVelocityTrackerStrategy ---
806 
IntegratingVelocityTrackerStrategy(uint32_t degree)807 IntegratingVelocityTrackerStrategy::IntegratingVelocityTrackerStrategy(uint32_t degree) :
808         mDegree(degree) {
809 }
810 
~IntegratingVelocityTrackerStrategy()811 IntegratingVelocityTrackerStrategy::~IntegratingVelocityTrackerStrategy() {
812 }
813 
clearPointer(int32_t pointerId)814 void IntegratingVelocityTrackerStrategy::clearPointer(int32_t pointerId) {
815     mPointerIdBits.clearBit(pointerId);
816 }
817 
addMovement(nsecs_t eventTime,int32_t pointerId,float position)818 void IntegratingVelocityTrackerStrategy::addMovement(nsecs_t eventTime, int32_t pointerId,
819                                                      float position) {
820     State& state = mPointerState[pointerId];
821     if (mPointerIdBits.hasBit(pointerId)) {
822         updateState(state, eventTime, position);
823     } else {
824         initState(state, eventTime, position);
825     }
826 
827     mPointerIdBits.markBit(pointerId);
828 }
829 
getEstimator(int32_t pointerId) const830 std::optional<VelocityTracker::Estimator> IntegratingVelocityTrackerStrategy::getEstimator(
831         int32_t pointerId) const {
832     if (mPointerIdBits.hasBit(pointerId)) {
833         const State& state = mPointerState[pointerId];
834         VelocityTracker::Estimator estimator;
835         populateEstimator(state, &estimator);
836         return estimator;
837     }
838 
839     return std::nullopt;
840 }
841 
initState(State & state,nsecs_t eventTime,float pos) const842 void IntegratingVelocityTrackerStrategy::initState(State& state, nsecs_t eventTime,
843                                                    float pos) const {
844     state.updateTime = eventTime;
845     state.degree = 0;
846 
847     state.pos = pos;
848     state.accel = 0;
849     state.vel = 0;
850 }
851 
updateState(State & state,nsecs_t eventTime,float pos) const852 void IntegratingVelocityTrackerStrategy::updateState(State& state, nsecs_t eventTime,
853                                                      float pos) const {
854     const nsecs_t MIN_TIME_DELTA = 2 * NANOS_PER_MS;
855     const float FILTER_TIME_CONSTANT = 0.010f; // 10 milliseconds
856 
857     if (eventTime <= state.updateTime + MIN_TIME_DELTA) {
858         return;
859     }
860 
861     float dt = (eventTime - state.updateTime) * 0.000000001f;
862     state.updateTime = eventTime;
863 
864     float vel = (pos - state.pos) / dt;
865     if (state.degree == 0) {
866         state.vel = vel;
867         state.degree = 1;
868     } else {
869         float alpha = dt / (FILTER_TIME_CONSTANT + dt);
870         if (mDegree == 1) {
871             state.vel += (vel - state.vel) * alpha;
872         } else {
873             float accel = (vel - state.vel) / dt;
874             if (state.degree == 1) {
875                 state.accel = accel;
876                 state.degree = 2;
877             } else {
878                 state.accel += (accel - state.accel) * alpha;
879             }
880             state.vel += (state.accel * dt) * alpha;
881         }
882     }
883     state.pos = pos;
884 }
885 
populateEstimator(const State & state,VelocityTracker::Estimator * outEstimator) const886 void IntegratingVelocityTrackerStrategy::populateEstimator(const State& state,
887         VelocityTracker::Estimator* outEstimator) const {
888     outEstimator->time = state.updateTime;
889     outEstimator->confidence = 1.0f;
890     outEstimator->degree = state.degree;
891     outEstimator->coeff[0] = state.pos;
892     outEstimator->coeff[1] = state.vel;
893     outEstimator->coeff[2] = state.accel / 2;
894 }
895 
896 
897 // --- LegacyVelocityTrackerStrategy ---
898 
LegacyVelocityTrackerStrategy()899 LegacyVelocityTrackerStrategy::LegacyVelocityTrackerStrategy() {}
900 
~LegacyVelocityTrackerStrategy()901 LegacyVelocityTrackerStrategy::~LegacyVelocityTrackerStrategy() {
902 }
903 
clearPointer(int32_t pointerId)904 void LegacyVelocityTrackerStrategy::clearPointer(int32_t pointerId) {
905     mIndex.erase(pointerId);
906     mMovements.erase(pointerId);
907 }
908 
addMovement(nsecs_t eventTime,int32_t pointerId,float position)909 void LegacyVelocityTrackerStrategy::addMovement(nsecs_t eventTime, int32_t pointerId,
910                                                 float position) {
911     // If data for this pointer already exists, we have a valid entry at the position of
912     // mIndex[pointerId] and mMovements[pointerId]. In that case, we need to advance the index
913     // to the next position in the circular buffer and write the new Movement there. Otherwise,
914     // if this is a first movement for this pointer, we initialize the maps mIndex and mMovements
915     // for this pointer and write to the first position.
916     auto [movementIt, inserted] = mMovements.insert({pointerId, {}});
917     auto [indexIt, _] = mIndex.insert({pointerId, 0});
918     size_t& index = indexIt->second;
919     if (!inserted && movementIt->second[index].eventTime != eventTime) {
920         // When ACTION_POINTER_DOWN happens, we will first receive ACTION_MOVE with the coordinates
921         // of the existing pointers, and then ACTION_POINTER_DOWN with the coordinates that include
922         // the new pointer. If the eventtimes for both events are identical, just update the data
923         // for this time.
924         // We only compare against the last value, as it is likely that addMovement is called
925         // in chronological order as events occur.
926         index++;
927     }
928     if (index == HISTORY_SIZE) {
929         index = 0;
930     }
931 
932     Movement& movement = movementIt->second[index];
933     movement.eventTime = eventTime;
934     movement.position = position;
935 }
936 
getEstimator(int32_t pointerId) const937 std::optional<VelocityTracker::Estimator> LegacyVelocityTrackerStrategy::getEstimator(
938         int32_t pointerId) const {
939     const auto movementIt = mMovements.find(pointerId);
940     if (movementIt == mMovements.end()) {
941         return std::nullopt; // no data
942     }
943     const Movement& newestMovement = movementIt->second[mIndex.at(pointerId)];
944 
945     // Find the oldest sample that contains the pointer and that is not older than HORIZON.
946     nsecs_t minTime = newestMovement.eventTime - HORIZON;
947     uint32_t oldestIndex = mIndex.at(pointerId);
948     uint32_t numTouches = 1;
949     do {
950         uint32_t nextOldestIndex = (oldestIndex == 0 ? HISTORY_SIZE : oldestIndex) - 1;
951         const Movement& nextOldestMovement = mMovements.at(pointerId)[nextOldestIndex];
952         if (nextOldestMovement.eventTime < minTime) {
953             break;
954         }
955         oldestIndex = nextOldestIndex;
956     } while (++numTouches < HISTORY_SIZE);
957 
958     // Calculate an exponentially weighted moving average of the velocity estimate
959     // at different points in time measured relative to the oldest sample.
960     // This is essentially an IIR filter.  Newer samples are weighted more heavily
961     // than older samples.  Samples at equal time points are weighted more or less
962     // equally.
963     //
964     // One tricky problem is that the sample data may be poorly conditioned.
965     // Sometimes samples arrive very close together in time which can cause us to
966     // overestimate the velocity at that time point.  Most samples might be measured
967     // 16ms apart but some consecutive samples could be only 0.5sm apart because
968     // the hardware or driver reports them irregularly or in bursts.
969     float accumV = 0;
970     uint32_t index = oldestIndex;
971     uint32_t samplesUsed = 0;
972     const Movement& oldestMovement = mMovements.at(pointerId)[oldestIndex];
973     float oldestPosition = oldestMovement.position;
974     nsecs_t lastDuration = 0;
975 
976     while (numTouches-- > 1) {
977         if (++index == HISTORY_SIZE) {
978             index = 0;
979         }
980         const Movement& movement = mMovements.at(pointerId)[index];
981         nsecs_t duration = movement.eventTime - oldestMovement.eventTime;
982 
983         // If the duration between samples is small, we may significantly overestimate
984         // the velocity.  Consequently, we impose a minimum duration constraint on the
985         // samples that we include in the calculation.
986         if (duration >= MIN_DURATION) {
987             float position = movement.position;
988             float scale = 1000000000.0f / duration; // one over time delta in seconds
989             float v = (position - oldestPosition) * scale;
990             accumV = (accumV * lastDuration + v * duration) / (duration + lastDuration);
991             lastDuration = duration;
992             samplesUsed += 1;
993         }
994     }
995 
996     // Report velocity.
997     float newestPosition = newestMovement.position;
998     VelocityTracker::Estimator estimator;
999     estimator.time = newestMovement.eventTime;
1000     estimator.confidence = 1;
1001     estimator.coeff[0] = newestPosition;
1002     if (samplesUsed) {
1003         estimator.coeff[1] = accumV;
1004         estimator.degree = 1;
1005     } else {
1006         estimator.degree = 0;
1007     }
1008     return estimator;
1009 }
1010 
1011 // --- ImpulseVelocityTrackerStrategy ---
1012 
ImpulseVelocityTrackerStrategy(bool deltaValues)1013 ImpulseVelocityTrackerStrategy::ImpulseVelocityTrackerStrategy(bool deltaValues)
1014       : mDeltaValues(deltaValues) {}
1015 
~ImpulseVelocityTrackerStrategy()1016 ImpulseVelocityTrackerStrategy::~ImpulseVelocityTrackerStrategy() {
1017 }
1018 
clearPointer(int32_t pointerId)1019 void ImpulseVelocityTrackerStrategy::clearPointer(int32_t pointerId) {
1020     mIndex.erase(pointerId);
1021     mMovements.erase(pointerId);
1022 }
1023 
addMovement(nsecs_t eventTime,int32_t pointerId,float position)1024 void ImpulseVelocityTrackerStrategy::addMovement(nsecs_t eventTime, int32_t pointerId,
1025                                                  float position) {
1026     // If data for this pointer already exists, we have a valid entry at the position of
1027     // mIndex[pointerId] and mMovements[pointerId]. In that case, we need to advance the index
1028     // to the next position in the circular buffer and write the new Movement there. Otherwise,
1029     // if this is a first movement for this pointer, we initialize the maps mIndex and mMovements
1030     // for this pointer and write to the first position.
1031     auto [movementIt, inserted] = mMovements.insert({pointerId, {}});
1032     auto [indexIt, _] = mIndex.insert({pointerId, 0});
1033     size_t& index = indexIt->second;
1034     if (!inserted && movementIt->second[index].eventTime != eventTime) {
1035         // When ACTION_POINTER_DOWN happens, we will first receive ACTION_MOVE with the coordinates
1036         // of the existing pointers, and then ACTION_POINTER_DOWN with the coordinates that include
1037         // the new pointer. If the eventtimes for both events are identical, just update the data
1038         // for this time.
1039         // We only compare against the last value, as it is likely that addMovement is called
1040         // in chronological order as events occur.
1041         index++;
1042     }
1043     if (index == HISTORY_SIZE) {
1044         index = 0;
1045     }
1046 
1047     Movement& movement = movementIt->second[index];
1048     movement.eventTime = eventTime;
1049     movement.position = position;
1050 }
1051 
1052 /**
1053  * Calculate the total impulse provided to the screen and the resulting velocity.
1054  *
1055  * The touchscreen is modeled as a physical object.
1056  * Initial condition is discussed below, but for now suppose that v(t=0) = 0
1057  *
1058  * The kinetic energy of the object at the release is E=0.5*m*v^2
1059  * Then vfinal = sqrt(2E/m). The goal is to calculate E.
1060  *
1061  * The kinetic energy at the release is equal to the total work done on the object by the finger.
1062  * The total work W is the sum of all dW along the path.
1063  *
1064  * dW = F*dx, where dx is the piece of path traveled.
1065  * Force is change of momentum over time, F = dp/dt = m dv/dt.
1066  * Then substituting:
1067  * dW = m (dv/dt) * dx = m * v * dv
1068  *
1069  * Summing along the path, we get:
1070  * W = sum(dW) = sum(m * v * dv) = m * sum(v * dv)
1071  * Since the mass stays constant, the equation for final velocity is:
1072  * vfinal = sqrt(2*sum(v * dv))
1073  *
1074  * Here,
1075  * dv : change of velocity = (v[i+1]-v[i])
1076  * dx : change of distance = (x[i+1]-x[i])
1077  * dt : change of time = (t[i+1]-t[i])
1078  * v : instantaneous velocity = dx/dt
1079  *
1080  * The final formula is:
1081  * vfinal = sqrt(2) * sqrt(sum((v[i]-v[i-1])*|v[i]|)) for all i
1082  * The absolute value is needed to properly account for the sign. If the velocity over a
1083  * particular segment descreases, then this indicates braking, which means that negative
1084  * work was done. So for two positive, but decreasing, velocities, this contribution would be
1085  * negative and will cause a smaller final velocity.
1086  *
1087  * Initial condition
1088  * There are two ways to deal with initial condition:
1089  * 1) Assume that v(0) = 0, which would mean that the screen is initially at rest.
1090  * This is not entirely accurate. We are only taking the past X ms of touch data, where X is
1091  * currently equal to 100. However, a touch event that created a fling probably lasted for longer
1092  * than that, which would mean that the user has already been interacting with the touchscreen
1093  * and it has probably already been moving.
1094  * 2) Assume that the touchscreen has already been moving at a certain velocity, calculate this
1095  * initial velocity and the equivalent energy, and start with this initial energy.
1096  * Consider an example where we have the following data, consisting of 3 points:
1097  *                 time: t0, t1, t2
1098  *                 x   : x0, x1, x2
1099  *                 v   : 0 , v1, v2
1100  * Here is what will happen in each of these scenarios:
1101  * 1) By directly applying the formula above with the v(0) = 0 boundary condition, we will get
1102  * vfinal = sqrt(2*(|v1|*(v1-v0) + |v2|*(v2-v1))). This can be simplified since v0=0
1103  * vfinal = sqrt(2*(|v1|*v1 + |v2|*(v2-v1))) = sqrt(2*(v1^2 + |v2|*(v2 - v1)))
1104  * since velocity is a real number
1105  * 2) If we treat the screen as already moving, then it must already have an energy (per mass)
1106  * equal to 1/2*v1^2. Then the initial energy should be 1/2*v1*2, and only the second segment
1107  * will contribute to the total kinetic energy (since we can effectively consider that v0=v1).
1108  * This will give the following expression for the final velocity:
1109  * vfinal = sqrt(2*(1/2*v1^2 + |v2|*(v2-v1)))
1110  * This analysis can be generalized to an arbitrary number of samples.
1111  *
1112  *
1113  * Comparing the two equations above, we see that the only mathematical difference
1114  * is the factor of 1/2 in front of the first velocity term.
1115  * This boundary condition would allow for the "proper" calculation of the case when all of the
1116  * samples are equally spaced in time and distance, which should suggest a constant velocity.
1117  *
1118  * Note that approach 2) is sensitive to the proper ordering of the data in time, since
1119  * the boundary condition must be applied to the oldest sample to be accurate.
1120  */
kineticEnergyToVelocity(float work)1121 static float kineticEnergyToVelocity(float work) {
1122     static constexpr float sqrt2 = 1.41421356237;
1123     return (work < 0 ? -1.0 : 1.0) * sqrtf(fabsf(work)) * sqrt2;
1124 }
1125 
calculateImpulseVelocity(const nsecs_t * t,const float * x,size_t count,bool deltaValues)1126 static float calculateImpulseVelocity(const nsecs_t* t, const float* x, size_t count,
1127                                       bool deltaValues) {
1128     // The input should be in reversed time order (most recent sample at index i=0)
1129     // t[i] is in nanoseconds, but due to FP arithmetic, convert to seconds inside this function
1130     static constexpr float SECONDS_PER_NANO = 1E-9;
1131 
1132     if (count < 2) {
1133         return 0; // if 0 or 1 points, velocity is zero
1134     }
1135     if (t[1] > t[0]) { // Algorithm will still work, but not perfectly
1136         ALOGE("Samples provided to calculateImpulseVelocity in the wrong order");
1137     }
1138 
1139     // If the data values are delta values, we do not have to calculate deltas here.
1140     // We can use the delta values directly, along with the calculated time deltas.
1141     // Since the data value input is in reversed time order:
1142     //      [a] for non-delta inputs, instantenous velocity = (x[i] - x[i-1])/(t[i] - t[i-1])
1143     //      [b] for delta inputs, instantenous velocity = -x[i-1]/(t[i] - t[i - 1])
1144     // e.g., let the non-delta values are: V = [2, 3, 7], the equivalent deltas are D = [2, 1, 4].
1145     // Since the input is in reversed time order, the input values for this function would be
1146     // V'=[7, 3, 2] and D'=[4, 1, 2] for the non-delta and delta values, respectively.
1147     //
1148     // The equivalent of {(V'[2] - V'[1]) = 2 - 3 = -1} would be {-D'[1] = -1}
1149     // Similarly, the equivalent of {(V'[1] - V'[0]) = 3 - 7 = -4} would be {-D'[0] = -4}
1150 
1151     if (count == 2) { // if 2 points, basic linear calculation
1152         if (t[1] == t[0]) {
1153             ALOGE("Events have identical time stamps t=%" PRId64 ", setting velocity = 0", t[0]);
1154             return 0;
1155         }
1156         const float deltaX = deltaValues ? -x[0] : x[1] - x[0];
1157         return deltaX / (SECONDS_PER_NANO * (t[1] - t[0]));
1158     }
1159     // Guaranteed to have at least 3 points here
1160     float work = 0;
1161     for (size_t i = count - 1; i > 0 ; i--) { // start with the oldest sample and go forward in time
1162         if (t[i] == t[i-1]) {
1163             ALOGE("Events have identical time stamps t=%" PRId64 ", skipping sample", t[i]);
1164             continue;
1165         }
1166         float vprev = kineticEnergyToVelocity(work); // v[i-1]
1167         const float deltaX = deltaValues ? -x[i-1] : x[i] - x[i-1];
1168         float vcurr = deltaX / (SECONDS_PER_NANO * (t[i] - t[i-1])); // v[i]
1169         work += (vcurr - vprev) * fabsf(vcurr);
1170         if (i == count - 1) {
1171             work *= 0.5; // initial condition, case 2) above
1172         }
1173     }
1174     return kineticEnergyToVelocity(work);
1175 }
1176 
getEstimator(int32_t pointerId) const1177 std::optional<VelocityTracker::Estimator> ImpulseVelocityTrackerStrategy::getEstimator(
1178         int32_t pointerId) const {
1179     const auto movementIt = mMovements.find(pointerId);
1180     if (movementIt == mMovements.end()) {
1181         return std::nullopt; // no data
1182     }
1183 
1184     // Iterate over movement samples in reverse time order and collect samples.
1185     float positions[HISTORY_SIZE];
1186     nsecs_t time[HISTORY_SIZE];
1187     size_t m = 0; // number of points that will be used for fitting
1188     size_t index = mIndex.at(pointerId);
1189     const Movement& newestMovement = movementIt->second[index];
1190     do {
1191         const Movement& movement = movementIt->second[index];
1192 
1193         nsecs_t age = newestMovement.eventTime - movement.eventTime;
1194         if (age > HORIZON) {
1195             break;
1196         }
1197         if (movement.eventTime == 0 && index != 0) {
1198             // All eventTime's are initialized to 0. If we encounter a time of 0 in a position
1199             // that's >0, it means that we hit the block where the data wasn't initialized.
1200             // It's also possible that the sample at 0 would be invalid, but there's no harm in
1201             // processing it, since it would be just a single point, and will only be encountered
1202             // in artificial circumstances (in tests).
1203             break;
1204         }
1205 
1206         positions[m] = movement.position;
1207         time[m] = movement.eventTime;
1208         index = (index == 0 ? HISTORY_SIZE : index) - 1;
1209     } while (++m < HISTORY_SIZE);
1210 
1211     if (m == 0) {
1212         return std::nullopt; // no data
1213     }
1214     VelocityTracker::Estimator estimator;
1215     estimator.coeff[0] = 0;
1216     estimator.coeff[1] = calculateImpulseVelocity(time, positions, m, mDeltaValues);
1217     estimator.coeff[2] = 0;
1218 
1219     estimator.time = newestMovement.eventTime;
1220     estimator.degree = 2; // similar results to 2nd degree fit
1221     estimator.confidence = 1;
1222 
1223     ALOGD_IF(DEBUG_STRATEGY, "velocity: %.1f", estimator.coeff[1]);
1224 
1225     if (DEBUG_IMPULSE) {
1226         // TODO(b/134179997): delete this block once the switch to 'impulse' is complete.
1227         // Calculate the lsq2 velocity for the same inputs to allow runtime comparisons.
1228         // X axis chosen arbitrarily for velocity comparisons.
1229         VelocityTracker lsq2(VelocityTracker::Strategy::LSQ2);
1230         for (ssize_t i = m - 1; i >= 0; i--) {
1231             lsq2.addMovement(time[i], pointerId, AMOTION_EVENT_AXIS_X, positions[i]);
1232         }
1233         std::optional<float> v = lsq2.getVelocity(AMOTION_EVENT_AXIS_X, pointerId);
1234         if (v) {
1235             ALOGD("lsq2 velocity: %.1f", *v);
1236         } else {
1237             ALOGD("lsq2 velocity: could not compute velocity");
1238         }
1239     }
1240     return estimator;
1241 }
1242 
1243 } // namespace android
1244