Merge "Velocity Tracker II: The Revenge of Velocity Tracker Bug: 5265529" (abb37794) · Commits · e / os / platform_frameworks_native

include/ui/Input.h

+40 −9

Original line number	Diff line number	Diff line
		@@ -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;

libs/ui/Input.cpp

+280 −45

Original line number	Diff line number	Diff line
		@@ -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,47 +865,228 @@ void VelocityTracker::addMovement(const MotionEvent* event) {
		addMovement(eventTime, idBits, positions);
		}

		/**
		* 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];
		}
		}

		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

		// 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;
		}

		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;
		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 accumulate velocity.
		// 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 {
		index = (index == 0 ? HISTORY_SIZE : index) - 1;
		const Movement& movement = mMovements[index];
		if (!movement.idBits.hasBit(id)) {
		break;
		}

		nsecs_t age = newestMovement.eventTime - movement.eventTime;
		if (age > MAX_AGE) {
		if (age > horizon) {
		break;
		}

		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;
		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 data available for this pointer.
		*outVx = 0;
		*outVy = 0;
		return false;
		// 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;
		}