diff options
| -rw-r--r-- | core/java/android/view/VelocityTracker.java | 92 | ||||
| -rw-r--r-- | core/java/com/android/internal/widget/PointerLocationView.java | 24 | ||||
| -rw-r--r-- | core/jni/android_view_VelocityTracker.cpp | 62 | ||||
| -rw-r--r-- | include/ui/Input.h | 49 | ||||
| -rw-r--r-- | libs/ui/Input.cpp | 325 |
5 files changed, 496 insertions, 56 deletions
diff --git a/core/java/android/view/VelocityTracker.java b/core/java/android/view/VelocityTracker.java index 5a91d31..dfb2c32 100644 --- a/core/java/android/view/VelocityTracker.java +++ b/core/java/android/view/VelocityTracker.java @@ -59,6 +59,8 @@ public final class VelocityTracker implements Poolable<VelocityTracker> { private static native void nativeComputeCurrentVelocity(int ptr, int units, float maxVelocity); private static native float nativeGetXVelocity(int ptr, int id); private static native float nativeGetYVelocity(int ptr, int id); + private static native boolean nativeGetEstimator(int ptr, int id, + int degree, int horizonMillis, Estimator outEstimator); /** * Retrieve a new VelocityTracker object to watch the velocity of a @@ -215,4 +217,94 @@ public final class VelocityTracker implements Poolable<VelocityTracker> { public float getYVelocity(int id) { return nativeGetYVelocity(mPtr, id); } + + /** + * Get an estimator for the movements of a pointer using past movements of the + * pointer to predict future movements. + * + * It is not necessary to call {@link #computeCurrentVelocity(int)} before calling + * this method. + * + * @param id Which pointer's velocity to return. + * @param degree The desired polynomial degree. The actual estimator may have + * a lower degree than what is requested here. If -1, uses the default degree. + * @param horizonMillis The maximum age of the oldest sample to consider, in milliseconds. + * If -1, uses the default horizon. + * @param outEstimator The estimator to populate. + * @return True if an estimator was obtained, false if there is no information + * available about the pointer. + * + * @hide For internal use only. Not a final API. + */ + public boolean getEstimator(int id, int degree, int horizonMillis, Estimator outEstimator) { + if (outEstimator == null) { + throw new IllegalArgumentException("outEstimator must not be null"); + } + return nativeGetEstimator(mPtr, id, degree, horizonMillis, outEstimator); + } + + /** + * An estimator for the movements of a pointer based on a polynomial model. + * + * The last recorded position of the pointer is at time zero seconds. + * Past estimated positions are at negative times and future estimated positions + * are at positive times. + * + * First coefficient is position (in pixels), second is velocity (in pixels per second), + * third is acceleration (in pixels per second squared). + * + * @hide For internal use only. Not a final API. + */ + public static final class Estimator { + // Must match VelocityTracker::Estimator::MAX_DEGREE + private static final int MAX_DEGREE = 2; + + /** + * Polynomial coefficients describing motion in X. + */ + public final float[] xCoeff = new float[MAX_DEGREE + 1]; + + /** + * Polynomial coefficients describing motion in Y. + */ + public final float[] yCoeff = new float[MAX_DEGREE + 1]; + + /** + * Polynomial degree, or zero if only position information is available. + */ + public int degree; + + /** + * Confidence (coefficient of determination), between 0 (no fit) and 1 (perfect fit). + */ + public float confidence; + + /** + * Gets an estimate of the X position of the pointer at the specified time point. + * @param time The time point in seconds, 0 is the last recorded time. + * @return The estimated X coordinate. + */ + public float estimateX(float time) { + return estimate(time, xCoeff); + } + + /** + * Gets an estimate of the Y position of the pointer at the specified time point. + * @param time The time point in seconds, 0 is the last recorded time. + * @return The estimated Y coordinate. + */ + public float estimateY(float time) { + return estimate(time, yCoeff); + } + + private float estimate(float time, float[] c) { + float a = 0; + float scale = 1; + for (int i = 0; i <= degree; i++) { + a += c[i] * scale; + scale *= time; + } + return a; + } + } } diff --git a/core/java/com/android/internal/widget/PointerLocationView.java b/core/java/com/android/internal/widget/PointerLocationView.java index 158291b..9a0ce3a 100644 --- a/core/java/com/android/internal/widget/PointerLocationView.java +++ b/core/java/com/android/internal/widget/PointerLocationView.java @@ -51,7 +51,10 @@ public class PointerLocationView extends View { // Most recent velocity. private float mXVelocity; private float mYVelocity; - + + // Position estimator. + private VelocityTracker.Estimator mEstimator = new VelocityTracker.Estimator(); + public void clearTrace() { mTraceCount = 0; } @@ -75,6 +78,10 @@ public class PointerLocationView extends View { } } + private final int ESTIMATE_PAST_POINTS = 4; + private final int ESTIMATE_FUTURE_POINTS = 2; + private final float ESTIMATE_INTERVAL = 0.02f; + private final ViewConfiguration mVC; private final Paint mTextPaint; private final Paint mTextBackgroundPaint; @@ -278,8 +285,20 @@ public class PointerLocationView extends View { haveLast = true; } - // Draw velocity vector. if (drawn) { + // Draw movement estimate curve. + mPaint.setARGB(128, 128, 0, 128); + float lx = ps.mEstimator.estimateX(-ESTIMATE_PAST_POINTS * ESTIMATE_INTERVAL); + float ly = ps.mEstimator.estimateY(-ESTIMATE_PAST_POINTS * ESTIMATE_INTERVAL); + for (int i = -ESTIMATE_PAST_POINTS + 1; i <= ESTIMATE_FUTURE_POINTS; i++) { + float x = ps.mEstimator.estimateX(i * ESTIMATE_INTERVAL); + float y = ps.mEstimator.estimateY(i * ESTIMATE_INTERVAL); + canvas.drawLine(lx, ly, x, y, mPaint); + lx = x; + ly = y; + } + + // Draw velocity vector. mPaint.setARGB(255, 255, 64, 128); float xVel = ps.mXVelocity * (1000 / 60); float yVel = ps.mYVelocity * (1000 / 60); @@ -517,6 +536,7 @@ public class PointerLocationView extends View { ps.addTrace(coords.x, coords.y); ps.mXVelocity = mVelocity.getXVelocity(id); ps.mYVelocity = mVelocity.getYVelocity(id); + mVelocity.getEstimator(id, -1, -1, ps.mEstimator); ps.mToolType = event.getToolType(i); } } diff --git a/core/jni/android_view_VelocityTracker.cpp b/core/jni/android_view_VelocityTracker.cpp index 01a4c09..516e421 100644 --- a/core/jni/android_view_VelocityTracker.cpp +++ b/core/jni/android_view_VelocityTracker.cpp @@ -29,6 +29,14 @@ namespace android { // Special constant to request the velocity of the active pointer. static const int ACTIVE_POINTER_ID = -1; +static struct { + jfieldID xCoeff; + jfieldID yCoeff; + jfieldID degree; + jfieldID confidence; +} gEstimatorClassInfo; + + // --- VelocityTrackerState --- class VelocityTrackerState { @@ -39,6 +47,8 @@ public: void addMovement(const MotionEvent* event); void computeCurrentVelocity(int32_t units, float maxVelocity); void getVelocity(int32_t id, float* outVx, float* outVy); + bool getEstimator(int32_t id, uint32_t degree, nsecs_t horizon, + VelocityTracker::Estimator* outEstimator); private: struct Velocity { @@ -118,6 +128,11 @@ void VelocityTrackerState::getVelocity(int32_t id, float* outVx, float* outVy) { } } +bool VelocityTrackerState::getEstimator(int32_t id, uint32_t degree, nsecs_t horizon, + VelocityTracker::Estimator* outEstimator) { + return mVelocityTracker.getEstimator(id, degree, horizon, outEstimator); +} + // --- JNI Methods --- @@ -169,6 +184,30 @@ static jfloat android_view_VelocityTracker_nativeGetYVelocity(JNIEnv* env, jclas return vy; } +static jboolean android_view_VelocityTracker_nativeGetEstimator(JNIEnv* env, jclass clazz, + jint ptr, jint id, jint degree, jint horizonMillis, jobject outEstimatorObj) { + VelocityTrackerState* state = reinterpret_cast<VelocityTrackerState*>(ptr); + VelocityTracker::Estimator estimator; + bool result = state->getEstimator(id, + degree < 0 ? VelocityTracker::DEFAULT_DEGREE : uint32_t(degree), + horizonMillis < 0 ? VelocityTracker::DEFAULT_HORIZON : + nsecs_t(horizonMillis) * 1000000L, + &estimator); + + jfloatArray xCoeffObj = jfloatArray(env->GetObjectField(outEstimatorObj, + gEstimatorClassInfo.xCoeff)); + jfloatArray yCoeffObj = jfloatArray(env->GetObjectField(outEstimatorObj, + gEstimatorClassInfo.yCoeff)); + + env->SetFloatArrayRegion(xCoeffObj, 0, VelocityTracker::Estimator::MAX_DEGREE + 1, + estimator.xCoeff); + env->SetFloatArrayRegion(yCoeffObj, 0, VelocityTracker::Estimator::MAX_DEGREE + 1, + estimator.yCoeff); + env->SetIntField(outEstimatorObj, gEstimatorClassInfo.degree, estimator.degree); + env->SetFloatField(outEstimatorObj, gEstimatorClassInfo.confidence, estimator.confidence); + return result; +} + // --- JNI Registration --- @@ -195,12 +234,35 @@ static JNINativeMethod gVelocityTrackerMethods[] = { { "nativeGetYVelocity", "(II)F", (void*)android_view_VelocityTracker_nativeGetYVelocity }, + { "nativeGetEstimator", + "(IIIILandroid/view/VelocityTracker$Estimator;)Z", + (void*)android_view_VelocityTracker_nativeGetEstimator }, }; +#define FIND_CLASS(var, className) \ + var = env->FindClass(className); \ + LOG_FATAL_IF(! var, "Unable to find class " className); + +#define GET_FIELD_ID(var, clazz, fieldName, fieldDescriptor) \ + var = env->GetFieldID(clazz, fieldName, fieldDescriptor); \ + LOG_FATAL_IF(! var, "Unable to find field " fieldName); + int register_android_view_VelocityTracker(JNIEnv* env) { int res = jniRegisterNativeMethods(env, "android/view/VelocityTracker", gVelocityTrackerMethods, NELEM(gVelocityTrackerMethods)); LOG_FATAL_IF(res < 0, "Unable to register native methods."); + + jclass clazz; + FIND_CLASS(clazz, "android/view/VelocityTracker$Estimator"); + + GET_FIELD_ID(gEstimatorClassInfo.xCoeff, clazz, + "xCoeff", "[F"); + GET_FIELD_ID(gEstimatorClassInfo.yCoeff, clazz, + "yCoeff", "[F"); + GET_FIELD_ID(gEstimatorClassInfo.degree, clazz, + "degree", "I"); + GET_FIELD_ID(gEstimatorClassInfo.confidence, clazz, + "confidence", "F"); return 0; } diff --git a/include/ui/Input.h b/include/ui/Input.h index af899ef..438a1a0 100644 --- a/include/ui/Input.h +++ b/include/ui/Input.h @@ -620,10 +620,41 @@ private: */ class VelocityTracker { public: + // Default polynomial degree. (used by getVelocity) + static const uint32_t DEFAULT_DEGREE = 2; + + // Default sample horizon. (used by getVelocity) + // We don't use too much history by default since we want to react to quick + // changes in direction. + static const nsecs_t DEFAULT_HORIZON = 100 * 1000000; // 100 ms + struct Position { float x, y; }; + struct Estimator { + static const size_t MAX_DEGREE = 2; + + // Polynomial coefficients describing motion in X and Y. + float xCoeff[MAX_DEGREE + 1], yCoeff[MAX_DEGREE + 1]; + + // Polynomial degree (number of coefficients), or zero if no information is + // available. + uint32_t degree; + + // Confidence (coefficient of determination), between 0 (no fit) and 1 (perfect fit). + float confidence; + + inline void clear() { + degree = 0; + confidence = 0; + for (size_t i = 0; i <= MAX_DEGREE; i++) { + xCoeff[i] = 0; + yCoeff[i] = 0; + } + } + }; + VelocityTracker(); // Resets the velocity tracker state. @@ -645,10 +676,16 @@ public: void addMovement(const MotionEvent* event); // Gets the velocity of the specified pointer id in position units per second. - // Returns false and sets the velocity components to zero if there is no movement - // information for the pointer. + // Returns false and sets the velocity components to zero if there is + // insufficient movement information for the pointer. bool getVelocity(uint32_t id, float* outVx, float* outVy) const; + // Gets a quadratic estimator for the movements of the specified pointer id. + // Returns false and clears the estimator if there is no information available + // about the pointer. + bool getEstimator(uint32_t id, uint32_t degree, nsecs_t horizon, + Estimator* outEstimator) const; + // Gets the active pointer id, or -1 if none. inline int32_t getActivePointerId() const { return mActivePointerId; } @@ -657,13 +694,7 @@ public: private: // Number of samples to keep. - static const uint32_t HISTORY_SIZE = 10; - - // Oldest sample to consider when calculating the velocity. - static const nsecs_t MAX_AGE = 100 * 1000000; // 100 ms - - // The minimum duration between samples when estimating velocity. - static const nsecs_t MIN_DURATION = 5 * 1000000; // 5 ms + static const uint32_t HISTORY_SIZE = 20; struct Movement { nsecs_t eventTime; diff --git a/libs/ui/Input.cpp b/libs/ui/Input.cpp index 0d25823..a5ba57d 100644 --- a/libs/ui/Input.cpp +++ b/libs/ui/Input.cpp @@ -13,6 +13,9 @@ // Log debug messages about velocity tracking. #define DEBUG_VELOCITY 0 +// Log debug messages about least squares fitting. +#define DEBUG_LEAST_SQUARES 0 + // Log debug messages about acceleration. #define DEBUG_ACCELERATION 0 @@ -682,9 +685,61 @@ bool MotionEvent::isTouchEvent(int32_t source, int32_t action) { // --- VelocityTracker --- +const uint32_t VelocityTracker::DEFAULT_DEGREE; +const nsecs_t VelocityTracker::DEFAULT_HORIZON; const uint32_t VelocityTracker::HISTORY_SIZE; -const nsecs_t VelocityTracker::MAX_AGE; -const nsecs_t VelocityTracker::MIN_DURATION; + +static inline float vectorDot(const float* a, const float* b, uint32_t m) { + float r = 0; + while (m--) { + r += *(a++) * *(b++); + } + return r; +} + +static inline float vectorNorm(const float* a, uint32_t m) { + float r = 0; + while (m--) { + float t = *(a++); + r += t * t; + } + return sqrtf(r); +} + +#if DEBUG_LEAST_SQUARES || DEBUG_VELOCITY +static String8 vectorToString(const float* a, uint32_t m) { + String8 str; + str.append("["); + while (m--) { + str.appendFormat(" %f", *(a++)); + if (m) { + str.append(","); + } + } + str.append(" ]"); + return str; +} + +static String8 matrixToString(const float* a, uint32_t m, uint32_t n, bool rowMajor) { + String8 str; + str.append("["); + for (size_t i = 0; i < m; i++) { + if (i) { + str.append(","); + } + str.append(" ["); + for (size_t j = 0; j < n; j++) { + if (j) { + str.append(","); + } + str.appendFormat(" %f", a[rowMajor ? i * n + j : j * m + i]); + } + str.append(" ]"); + } + str.append(" ]"); + return str; +} +#endif VelocityTracker::VelocityTracker() { clear(); @@ -733,16 +788,15 @@ void VelocityTracker::addMovement(nsecs_t eventTime, BitSet32 idBits, const Posi uint32_t id = iterBits.firstMarkedBit(); uint32_t index = idBits.getIndexOfBit(id); iterBits.clearBit(id); - float vx, vy; - bool available = getVelocity(id, &vx, &vy); - if (available) { - LOGD(" %d: position (%0.3f, %0.3f), vx=%0.3f, vy=%0.3f, speed=%0.3f", - id, positions[index].x, positions[index].y, vx, vy, sqrtf(vx * vx + vy * vy)); - } else { - LOG_ASSERT(vx == 0 && vy == 0); - LOGD(" %d: position (%0.3f, %0.3f), velocity not available", - id, positions[index].x, positions[index].y); - } + Estimator estimator; + getEstimator(id, DEFAULT_DEGREE, DEFAULT_HORIZON, &estimator); + LOGD(" %d: position (%0.3f, %0.3f), " + "estimator (degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f)", + id, positions[index].x, positions[index].y, + int(estimator.degree), + vectorToString(estimator.xCoeff, estimator.degree).string(), + vectorToString(estimator.yCoeff, estimator.degree).string(), + estimator.confidence); } #endif } @@ -811,49 +865,230 @@ void VelocityTracker::addMovement(const MotionEvent* event) { addMovement(eventTime, idBits, positions); } -bool VelocityTracker::getVelocity(uint32_t id, float* outVx, float* outVy) const { - const Movement& newestMovement = mMovements[mIndex]; - if (newestMovement.idBits.hasBit(id)) { - const Position& newestPosition = newestMovement.getPosition(id); - float accumVx = 0; - float accumVy = 0; - float duration = 0; - - // Iterate over movement samples in reverse time order and accumulate velocity. - uint32_t index = mIndex; - do { - index = (index == 0 ? HISTORY_SIZE : index) - 1; - const Movement& movement = mMovements[index]; - if (!movement.idBits.hasBit(id)) { - break; +/** + * Solves a linear least squares problem to obtain a N degree polynomial that fits + * the specified input data as nearly as possible. + * + * Returns true if a solution is found, false otherwise. + * + * The input consists of two vectors of data points X and Y with indices 0..m-1. + * The output is a vector B with indices 0..n-1 that describes a polynomial + * that fits the data, such the sum of abs(Y[i] - (B[0] + B[1] X[i] + B[2] X[i]^2 ... B[n] X[i]^n)) + * for all i between 0 and m-1 is minimized. + * + * That is to say, the function that generated the input data can be approximated + * by y(x) ~= B[0] + B[1] x + B[2] x^2 + ... + B[n] x^n. + * + * The coefficient of determination (R^2) is also returned to describe the goodness + * of fit of the model for the given data. It is a value between 0 and 1, where 1 + * indicates perfect correspondence. + * + * This function first expands the X vector to a m by n matrix A such that + * A[i][0] = 1, A[i][1] = X[i], A[i][2] = X[i]^2, ..., A[i][n] = X[i]^n. + * + * Then it calculates the QR decomposition of A yielding an m by m orthonormal matrix Q + * and an m by n upper triangular matrix R. Because R is upper triangular (lower + * part is all zeroes), we can simplify the decomposition into an m by n matrix + * Q1 and a n by n matrix R1 such that A = Q1 R1. + * + * Finally we solve the system of linear equations given by R1 B = (Qtranspose Y) + * to find B. + * + * For efficiency, we lay out A and Q column-wise in memory because we frequently + * operate on the column vectors. Conversely, we lay out R row-wise. + * + * http://en.wikipedia.org/wiki/Numerical_methods_for_linear_least_squares + * http://en.wikipedia.org/wiki/Gram-Schmidt + */ +static bool solveLeastSquares(const float* x, const float* y, uint32_t m, uint32_t n, + float* outB, float* outDet) { +#if DEBUG_LEAST_SQUARES + LOGD("solveLeastSquares: m=%d, n=%d, x=%s, y=%s", int(m), int(n), + vectorToString(x, m).string(), vectorToString(y, m).string()); +#endif + + // Expand the X vector to a matrix A. + float a[n][m]; // column-major order + for (uint32_t h = 0; h < m; h++) { + a[0][h] = 1; + for (uint32_t i = 1; i < n; i++) { + a[i][h] = a[i - 1][h] * x[h]; + } + } +#if DEBUG_LEAST_SQUARES + LOGD(" - a=%s", matrixToString(&a[0][0], m, n, false /*rowMajor*/).string()); +#endif + + // Apply the Gram-Schmidt process to A to obtain its QR decomposition. + float q[n][m]; // orthonormal basis, column-major order + float r[n][n]; // upper triangular matrix, row-major order + for (uint32_t j = 0; j < n; j++) { + for (uint32_t h = 0; h < m; h++) { + q[j][h] = a[j][h]; + } + for (uint32_t i = 0; i < j; i++) { + float dot = vectorDot(&q[j][0], &q[i][0], m); + for (uint32_t h = 0; h < m; h++) { + q[j][h] -= dot * q[i][h]; } + } - nsecs_t age = newestMovement.eventTime - movement.eventTime; - if (age > MAX_AGE) { - break; + float norm = vectorNorm(&q[j][0], m); + if (norm < 0.000001f) { + // vectors are linearly dependent or zero so no solution +#if DEBUG_LEAST_SQUARES + LOGD(" - no solution, norm=%f", norm); +#endif + return false; + } + + float invNorm = 1.0f / norm; + for (uint32_t h = 0; h < m; h++) { + q[j][h] *= invNorm; + } + for (uint32_t i = 0; i < n; i++) { + r[j][i] = i < j ? 0 : vectorDot(&q[j][0], &a[i][0], m); + } + } +#if DEBUG_LEAST_SQUARES + LOGD(" - q=%s", matrixToString(&q[0][0], m, n, false /*rowMajor*/).string()); + LOGD(" - r=%s", matrixToString(&r[0][0], n, n, true /*rowMajor*/).string()); + + // calculate QR, if we factored A correctly then QR should equal A + float qr[n][m]; + for (uint32_t h = 0; h < m; h++) { + for (uint32_t i = 0; i < n; i++) { + qr[i][h] = 0; + for (uint32_t j = 0; j < n; j++) { + qr[i][h] += q[j][h] * r[j][i]; } + } + } + LOGD(" - qr=%s", matrixToString(&qr[0][0], m, n, false /*rowMajor*/).string()); +#endif - const Position& position = movement.getPosition(id); - accumVx += newestPosition.x - position.x; - accumVy += newestPosition.y - position.y; - duration += age; - } while (index != mIndex); - - // Make sure we used at least one sample. - if (duration >= MIN_DURATION) { - float scale = 1000000000.0f / duration; // one over time delta in seconds - *outVx = accumVx * scale; - *outVy = accumVy * scale; - return true; + // Solve R B = Qt Y to find B. This is easy because R is upper triangular. + // We just work from bottom-right to top-left calculating B's coefficients. + for (uint32_t i = n; i-- != 0; ) { + outB[i] = vectorDot(&q[i][0], y, m); + for (uint32_t j = n - 1; j > i; j--) { + outB[i] -= r[i][j] * outB[j]; + } + outB[i] /= r[i][i]; + } +#if DEBUG_LEAST_SQUARES + LOGD(" - b=%s", vectorToString(outB, n).string()); +#endif + + // Calculate the coefficient of determination as 1 - (SSerr / SStot) where + // SSerr is the residual sum of squares (squared variance of the error), + // and SStot is the total sum of squares (squared variance of the data). + float ymean = 0; + for (uint32_t h = 0; h < m; h++) { + ymean += y[h]; + } + ymean /= m; + + float sserr = 0; + float sstot = 0; + for (uint32_t h = 0; h < m; h++) { + float err = y[h] - outB[0]; + float term = 1; + for (uint32_t i = 1; i < n; i++) { + term *= x[h]; + err -= term * outB[i]; } + sserr += err * err; + float var = y[h] - ymean; + sstot += var * var; } + *outDet = sstot > 0.000001f ? 1.0f - (sserr / sstot) : 1; +#if DEBUG_LEAST_SQUARES + LOGD(" - sserr=%f", sserr); + LOGD(" - sstot=%f", sstot); + LOGD(" - det=%f", *outDet); +#endif + return true; +} - // No data available for this pointer. - *outVx = 0; - *outVy = 0; +bool VelocityTracker::getVelocity(uint32_t id, float* outVx, float* outVy) const { + Estimator estimator; + if (getEstimator(id, DEFAULT_DEGREE, DEFAULT_HORIZON, &estimator)) { + if (estimator.degree >= 1) { + *outVx = estimator.xCoeff[1]; + *outVy = estimator.yCoeff[1]; + return true; + } + } return false; } +bool VelocityTracker::getEstimator(uint32_t id, uint32_t degree, nsecs_t horizon, + Estimator* outEstimator) const { + outEstimator->clear(); + + // Iterate over movement samples in reverse time order and collect samples. + float x[HISTORY_SIZE]; + float y[HISTORY_SIZE]; + float time[HISTORY_SIZE]; + uint32_t m = 0; + uint32_t index = mIndex; + const Movement& newestMovement = mMovements[mIndex]; + do { + const Movement& movement = mMovements[index]; + if (!movement.idBits.hasBit(id)) { + break; + } + + nsecs_t age = newestMovement.eventTime - movement.eventTime; + if (age > horizon) { + break; + } + + const Position& position = movement.getPosition(id); + x[m] = position.x; + y[m] = position.y; + time[m] = -age * 0.000000001f; + index = (index == 0 ? HISTORY_SIZE : index) - 1; + } while (++m < HISTORY_SIZE); + + if (m == 0) { + return false; // no data + } + + // Calculate a least squares polynomial fit. + if (degree > Estimator::MAX_DEGREE) { + degree = Estimator::MAX_DEGREE; + } + if (degree > m - 1) { + degree = m - 1; + } + if (degree >= 1) { + float xdet, ydet; + uint32_t n = degree + 1; + if (solveLeastSquares(time, x, m, n, outEstimator->xCoeff, &xdet) + && solveLeastSquares(time, y, m, n, outEstimator->yCoeff, &ydet)) { + outEstimator->degree = degree; + outEstimator->confidence = xdet * ydet; +#if DEBUG_LEAST_SQUARES + LOGD("estimate: degree=%d, xCoeff=%s, yCoeff=%s, confidence=%f", + int(outEstimator->degree), + vectorToString(outEstimator->xCoeff, n).string(), + vectorToString(outEstimator->yCoeff, n).string(), + outEstimator->confidence); +#endif + return true; + } + } + + // No velocity data available for this pointer, but we do have its current position. + outEstimator->xCoeff[0] = x[0]; + outEstimator->yCoeff[0] = y[0]; + outEstimator->degree = 0; + outEstimator->confidence = 1; + return true; +} + // --- VelocityControl --- |