// Copyright 2014 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.
#include "ui/events/gesture_detection/velocity_tracker.h"
#include <cmath>
#include "base/logging.h"
#include "ui/events/gesture_detection/motion_event.h"
using base::TimeDelta;
using base::TimeTicks;
namespace ui {
// Implements a particular velocity tracker algorithm.
class VelocityTrackerStrategy {
public:
virtual ~VelocityTrackerStrategy() {}
virtual void Clear() = 0;
virtual void ClearPointers(BitSet32 id_bits) = 0;
virtual void AddMovement(const base::TimeTicks& event_time,
BitSet32 id_bits,
const Position* positions) = 0;
virtual bool GetEstimator(uint32_t id, Estimator* out_estimator) const = 0;
protected:
VelocityTrackerStrategy() {}
};
namespace {
COMPILE_ASSERT(MotionEvent::MAX_POINTER_ID < 32, max_pointer_id_too_large);
// Threshold for determining that a pointer has stopped moving.
// Some input devices do not send ACTION_MOVE events in the case where a pointer
// has stopped. We need to detect this case so that we can accurately predict
// the velocity after the pointer starts moving again.
const int kAssumePointerStoppedTimeMs = 40;
struct Position {
float x, y;
};
struct Estimator {
enum { MAX_DEGREE = 4 };
// Estimator time base.
TimeTicks time;
// 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() {
time = TimeTicks();
degree = 0;
confidence = 0;
for (size_t i = 0; i <= MAX_DEGREE; i++) {
xcoeff[i] = 0;
ycoeff[i] = 0;
}
}
};
float VectorDot(const float* a, const float* b, uint32_t m) {
float r = 0;
while (m--) {
r += *(a++) * *(b++);
}
return r;
}
float VectorNorm(const float* a, uint32_t m) {
float r = 0;
while (m--) {
float t = *(a++);
r += t * t;
}
return sqrtf(r);
}
// Velocity tracker algorithm based on least-squares linear regression.
class LeastSquaresVelocityTrackerStrategy : public VelocityTrackerStrategy {
public:
enum Weighting {
// No weights applied. All data points are equally reliable.
WEIGHTING_NONE,
// Weight by time delta. Data points clustered together are weighted less.
WEIGHTING_DELTA,
// Weight such that points within a certain horizon are weighed more than
// those outside of that horizon.
WEIGHTING_CENTRAL,
// Weight such that points older than a certain amount are weighed less.
WEIGHTING_RECENT,
};
// Number of samples to keep.
enum { HISTORY_SIZE = 20 };
// Degree must be no greater than Estimator::MAX_DEGREE.
LeastSquaresVelocityTrackerStrategy(uint32_t degree,
Weighting weighting = WEIGHTING_NONE);
virtual ~LeastSquaresVelocityTrackerStrategy();
virtual void Clear() OVERRIDE;
virtual void ClearPointers(BitSet32 id_bits) OVERRIDE;
virtual void AddMovement(const TimeTicks& event_time,
BitSet32 id_bits,
const Position* positions) OVERRIDE;
virtual bool GetEstimator(uint32_t id,
Estimator* out_estimator) const OVERRIDE;
private:
// Sample horizon.
// We don't use too much history by default since we want to react to quick
// changes in direction.
enum { HORIZON_MS = 100 };
struct Movement {
TimeTicks event_time;
BitSet32 id_bits;
Position positions[VelocityTracker::MAX_POINTERS];
inline const Position& GetPosition(uint32_t id) const {
return positions[id_bits.get_index_of_bit(id)];
}
};
float ChooseWeight(uint32_t index) const;
const uint32_t degree_;
const Weighting weighting_;
uint32_t index_;
Movement movements_[HISTORY_SIZE];
};
// Velocity tracker algorithm that uses an IIR filter.
class IntegratingVelocityTrackerStrategy : public VelocityTrackerStrategy {
public:
// Degree must be 1 or 2.
explicit IntegratingVelocityTrackerStrategy(uint32_t degree);
virtual ~IntegratingVelocityTrackerStrategy();
virtual void Clear() OVERRIDE;
virtual void ClearPointers(BitSet32 id_bits) OVERRIDE;
virtual void AddMovement(const TimeTicks& event_time,
BitSet32 id_bits,
const Position* positions) OVERRIDE;
virtual bool GetEstimator(uint32_t id,
Estimator* out_estimator) const OVERRIDE;
private:
// Current state estimate for a particular pointer.
struct State {
TimeTicks update_time;
uint32_t degree;
float xpos, xvel, xaccel;
float ypos, yvel, yaccel;
};
const uint32_t degree_;
BitSet32 pointer_id_bits_;
State mPointerState[MotionEvent::MAX_POINTER_ID + 1];
void InitState(State& state,
const TimeTicks& event_time,
float xpos,
float ypos) const;
void UpdateState(State& state,
const TimeTicks& event_time,
float xpos,
float ypos) const;
void PopulateEstimator(const State& state, Estimator* out_estimator) const;
};
VelocityTrackerStrategy* CreateStrategy(VelocityTracker::Strategy strategy) {
switch (strategy) {
case VelocityTracker::LSQ1:
return new LeastSquaresVelocityTrackerStrategy(1);
case VelocityTracker::LSQ2:
return new LeastSquaresVelocityTrackerStrategy(2);
case VelocityTracker::LSQ3:
return new LeastSquaresVelocityTrackerStrategy(3);
case VelocityTracker::WLSQ2_DELTA:
return new LeastSquaresVelocityTrackerStrategy(
2, LeastSquaresVelocityTrackerStrategy::WEIGHTING_DELTA);
case VelocityTracker::WLSQ2_CENTRAL:
return new LeastSquaresVelocityTrackerStrategy(
2, LeastSquaresVelocityTrackerStrategy::WEIGHTING_CENTRAL);
case VelocityTracker::WLSQ2_RECENT:
return new LeastSquaresVelocityTrackerStrategy(
2, LeastSquaresVelocityTrackerStrategy::WEIGHTING_RECENT);
case VelocityTracker::INT1:
return new IntegratingVelocityTrackerStrategy(1);
case VelocityTracker::INT2:
return new IntegratingVelocityTrackerStrategy(2);
}
NOTREACHED() << "Unrecognized velocity tracker strategy: " << strategy;
return CreateStrategy(VelocityTracker::STRATEGY_DEFAULT);
}
} // namespace
// --- VelocityTracker ---
VelocityTracker::VelocityTracker()
: current_pointer_id_bits_(0),
active_pointer_id_(-1),
strategy_(CreateStrategy(STRATEGY_DEFAULT)) {}
VelocityTracker::VelocityTracker(Strategy strategy)
: current_pointer_id_bits_(0),
active_pointer_id_(-1),
strategy_(CreateStrategy(strategy)) {}
VelocityTracker::~VelocityTracker() {}
void VelocityTracker::Clear() {
current_pointer_id_bits_.clear();
active_pointer_id_ = -1;
strategy_->Clear();
}
void VelocityTracker::ClearPointers(BitSet32 id_bits) {
BitSet32 remaining_id_bits(current_pointer_id_bits_.value & ~id_bits.value);
current_pointer_id_bits_ = remaining_id_bits;
if (active_pointer_id_ >= 0 && id_bits.has_bit(active_pointer_id_)) {
active_pointer_id_ = !remaining_id_bits.is_empty()
? remaining_id_bits.first_marked_bit()
: -1;
}
strategy_->ClearPointers(id_bits);
}
void VelocityTracker::AddMovement(const TimeTicks& event_time,
BitSet32 id_bits,
const Position* positions) {
while (id_bits.count() > MAX_POINTERS)
id_bits.clear_last_marked_bit();
if ((current_pointer_id_bits_.value & id_bits.value) &&
event_time >= (last_event_time_ + base::TimeDelta::FromMilliseconds(
kAssumePointerStoppedTimeMs))) {
// We have not received any movements for too long. Assume that all
// pointers
// have stopped.
strategy_->Clear();
}
last_event_time_ = event_time;
current_pointer_id_bits_ = id_bits;
if (active_pointer_id_ < 0 || !id_bits.has_bit(active_pointer_id_))
active_pointer_id_ = id_bits.is_empty() ? -1 : id_bits.first_marked_bit();
strategy_->AddMovement(event_time, id_bits, positions);
}
void VelocityTracker::AddMovement(const MotionEvent& event) {
int32_t actionMasked = event.GetAction();
switch (actionMasked) {
case MotionEvent::ACTION_DOWN:
// case MotionEvent::HOVER_ENTER:
// Clear all pointers on down before adding the new movement.
Clear();
break;
case MotionEvent::ACTION_POINTER_DOWN: {
// Start a new movement trace for a pointer that just went down.
// We do this on down instead of on up because the client may want to
// query the final velocity for a pointer that just went up.
BitSet32 downIdBits;
downIdBits.mark_bit(event.GetPointerId(event.GetActionIndex()));
ClearPointers(downIdBits);
break;
}
case MotionEvent::ACTION_MOVE:
// case MotionEvent::ACTION_HOVER_MOVE:
break;
default:
// Ignore all other actions because they do not convey any new information
// about pointer movement. We also want to preserve the last known
// velocity of the pointers.
// Note that ACTION_UP and ACTION_POINTER_UP always report the last known
// position of the pointers that went up. ACTION_POINTER_UP does include
// the new position of pointers that remained down but we will also
// receive an ACTION_MOVE with this information if any of them actually
// moved. Since we don't know how many pointers will be going up at once
// it makes sense to just wait for the following ACTION_MOVE before adding
// the movement.
return;
}
size_t pointer_count = event.GetPointerCount();
if (pointer_count > MAX_POINTERS) {
pointer_count = MAX_POINTERS;
}
BitSet32 id_bits;
for (size_t i = 0; i < pointer_count; i++) {
id_bits.mark_bit(event.GetPointerId(i));
}
uint32_t pointer_index[MAX_POINTERS];
for (size_t i = 0; i < pointer_count; i++) {
pointer_index[i] = id_bits.get_index_of_bit(event.GetPointerId(i));
}
Position positions[MAX_POINTERS];
size_t historySize = event.GetHistorySize();
for (size_t h = 0; h < historySize; h++) {
for (size_t i = 0; i < pointer_count; i++) {
uint32_t index = pointer_index[i];
positions[index].x = event.GetHistoricalX(i, h);
positions[index].y = event.GetHistoricalY(i, h);
}
AddMovement(event.GetHistoricalEventTime(h), id_bits, positions);
}
for (size_t i = 0; i < pointer_count; i++) {
uint32_t index = pointer_index[i];
positions[index].x = event.GetX(i);
positions[index].y = event.GetY(i);
}
AddMovement(event.GetEventTime(), id_bits, positions);
}
bool VelocityTracker::GetVelocity(uint32_t id,
float* out_vx,
float* out_vy) const {
Estimator estimator;
if (GetEstimator(id, &estimator) && estimator.degree >= 1) {
*out_vx = estimator.xcoeff[1];
*out_vy = estimator.ycoeff[1];
return true;
}
*out_vx = 0;
*out_vy = 0;
return false;
}
void LeastSquaresVelocityTrackerStrategy::AddMovement(
const TimeTicks& event_time,
BitSet32 id_bits,
const Position* positions) {
if (++index_ == HISTORY_SIZE) {
index_ = 0;
}
Movement& movement = movements_[index_];
movement.event_time = event_time;
movement.id_bits = id_bits;
uint32_t count = id_bits.count();
for (uint32_t i = 0; i < count; i++) {
movement.positions[i] = positions[i];
}
}
bool VelocityTracker::GetEstimator(uint32_t id,
Estimator* out_estimator) const {
return strategy_->GetEstimator(id, out_estimator);
}
// --- LeastSquaresVelocityTrackerStrategy ---
LeastSquaresVelocityTrackerStrategy::LeastSquaresVelocityTrackerStrategy(
uint32_t degree,
Weighting weighting)
: degree_(degree), weighting_(weighting) {
DCHECK_LT(degree_, static_cast<uint32_t>(Estimator::MAX_DEGREE));
Clear();
}
LeastSquaresVelocityTrackerStrategy::~LeastSquaresVelocityTrackerStrategy() {}
void LeastSquaresVelocityTrackerStrategy::Clear() {
index_ = 0;
movements_[0].id_bits.clear();
}
/**
* 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
* along with a weight vector W of the same size.
*
* The output is a vector B with indices 0..n that describes a polynomial
* that fits the data, such the sum of W[i] * W[i] * 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.
*
* Accordingly, the weight vector W should be initialized by the caller with the
* reciprocal square root of the variance of the error in each input data point.
* In other words, an ideal choice for W would be W[i] = 1 / var(Y[i]) = 1 /
* stddev(Y[i]).
* The weights express the relative importance of each data point. If the
* weights are* all 1, then the data points are considered to be of equal
* importance when fitting the polynomial. It is a good idea to choose weights
* that diminish the importance of data points that may have higher than usual
* error margins.
*
* Errors among data points are assumed to be independent. W is represented
* here as a vector although in the literature it is typically taken to be a
* diagonal matrix.
*
* 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
* multiplies it by w[i]./
*
* 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 W 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,
const float* w,
uint32_t m,
uint32_t n,
float* out_b,
float* out_det) {
// MSVC does not support variable-length arrays (used by the original Android
// implementation of this function).
#if defined(COMPILER_MSVC)
enum {
M_ARRAY_LENGTH = LeastSquaresVelocityTrackerStrategy::HISTORY_SIZE,
N_ARRAY_LENGTH = Estimator::MAX_DEGREE
};
DCHECK_LE(m, static_cast<uint32_t>(M_ARRAY_LENGTH));
DCHECK_LE(n, static_cast<uint32_t>(N_ARRAY_LENGTH));
#else
const uint32_t M_ARRAY_LENGTH = m;
const uint32_t N_ARRAY_LENGTH = n;
#endif
// Expand the X vector to a matrix A, pre-multiplied by the weights.
float a[N_ARRAY_LENGTH][M_ARRAY_LENGTH]; // column-major order
for (uint32_t h = 0; h < m; h++) {
a[0][h] = w[h];
for (uint32_t i = 1; i < n; i++) {
a[i][h] = a[i - 1][h] * x[h];
}
}
// Apply the Gram-Schmidt process to A to obtain its QR decomposition.
// Orthonormal basis, column-major order.
float q[N_ARRAY_LENGTH][M_ARRAY_LENGTH];
// Upper triangular matrix, row-major order.
float r[N_ARRAY_LENGTH][N_ARRAY_LENGTH];
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
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);
}
}
// Solve R B = Qt W 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.
float wy[M_ARRAY_LENGTH];
for (uint32_t h = 0; h < m; h++) {
wy[h] = y[h] * w[h];
}
for (uint32_t i = n; i-- != 0;) {
out_b[i] = VectorDot(&q[i][0], wy, m);
for (uint32_t j = n - 1; j > i; j--) {
out_b[i] -= r[i][j] * out_b[j];
}
out_b[i] /= r[i][i];
}
// Calculate the coefficient of determination as 1 - (SSerr / SStot) where
// SSerr is the residual sum of squares (variance of the error),
// and SStot is the total sum of squares (variance of the data) where each
// has been weighted.
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] - out_b[0];
float term = 1;
for (uint32_t i = 1; i < n; i++) {
term *= x[h];
err -= term * out_b[i];
}
sserr += w[h] * w[h] * err * err;
float var = y[h] - ymean;
sstot += w[h] * w[h] * var * var;
}
*out_det = sstot > 0.000001f ? 1.0f - (sserr / sstot) : 1;
return true;
}
void LeastSquaresVelocityTrackerStrategy::ClearPointers(BitSet32 id_bits) {
BitSet32 remaining_id_bits(movements_[index_].id_bits.value & ~id_bits.value);
movements_[index_].id_bits = remaining_id_bits;
}
bool LeastSquaresVelocityTrackerStrategy::GetEstimator(
uint32_t id,
Estimator* out_estimator) const {
out_estimator->Clear();
// Iterate over movement samples in reverse time order and collect samples.
float x[HISTORY_SIZE];
float y[HISTORY_SIZE];
float w[HISTORY_SIZE];
float time[HISTORY_SIZE];
uint32_t m = 0;
uint32_t index = index_;
const base::TimeDelta horizon = base::TimeDelta::FromMilliseconds(HORIZON_MS);
const Movement& newest_movement = movements_[index_];
do {
const Movement& movement = movements_[index];
if (!movement.id_bits.has_bit(id))
break;
TimeDelta age = newest_movement.event_time - movement.event_time;
if (age > horizon)
break;
const Position& position = movement.GetPosition(id);
x[m] = position.x;
y[m] = position.y;
w[m] = ChooseWeight(index);
time[m] = -age.InSecondsF();
index = (index == 0 ? HISTORY_SIZE : index) - 1;
} while (++m < HISTORY_SIZE);
if (m == 0)
return false; // no data
// Calculate a least squares polynomial fit.
uint32_t degree = degree_;
if (degree > m - 1)
degree = m - 1;
if (degree >= 1) {
float xdet, ydet;
uint32_t n = degree + 1;
if (SolveLeastSquares(time, x, w, m, n, out_estimator->xcoeff, &xdet) &&
SolveLeastSquares(time, y, w, m, n, out_estimator->ycoeff, &ydet)) {
out_estimator->time = newest_movement.event_time;
out_estimator->degree = degree;
out_estimator->confidence = xdet * ydet;
return true;
}
}
// No velocity data available for this pointer, but we do have its current
// position.
out_estimator->xcoeff[0] = x[0];
out_estimator->ycoeff[0] = y[0];
out_estimator->time = newest_movement.event_time;
out_estimator->degree = 0;
out_estimator->confidence = 1;
return true;
}
float LeastSquaresVelocityTrackerStrategy::ChooseWeight(uint32_t index) const {
switch (weighting_) {
case WEIGHTING_DELTA: {
// Weight points based on how much time elapsed between them and the next
// point so that points that "cover" a shorter time span are weighed less.
// delta 0ms: 0.5
// delta 10ms: 1.0
if (index == index_) {
return 1.0f;
}
uint32_t next_index = (index + 1) % HISTORY_SIZE;
float delta_millis =
static_cast<float>((movements_[next_index].event_time -
movements_[index].event_time).InMillisecondsF());
if (delta_millis < 0)
return 0.5f;
if (delta_millis < 10)
return 0.5f + delta_millis * 0.05;
return 1.0f;
}
case WEIGHTING_CENTRAL: {
// Weight points based on their age, weighing very recent and very old
// points less.
// age 0ms: 0.5
// age 10ms: 1.0
// age 50ms: 1.0
// age 60ms: 0.5
float age_millis =
static_cast<float>((movements_[index_].event_time -
movements_[index].event_time).InMillisecondsF());
if (age_millis < 0)
return 0.5f;
if (age_millis < 10)
return 0.5f + age_millis * 0.05;
if (age_millis < 50)
return 1.0f;
if (age_millis < 60)
return 0.5f + (60 - age_millis) * 0.05;
return 0.5f;
}
case WEIGHTING_RECENT: {
// Weight points based on their age, weighing older points less.
// age 0ms: 1.0
// age 50ms: 1.0
// age 100ms: 0.5
float age_millis =
static_cast<float>((movements_[index_].event_time -
movements_[index].event_time).InMillisecondsF());
if (age_millis < 50) {
return 1.0f;
}
if (age_millis < 100) {
return 0.5f + (100 - age_millis) * 0.01f;
}
return 0.5f;
}
case WEIGHTING_NONE:
default:
return 1.0f;
}
}
// --- IntegratingVelocityTrackerStrategy ---
IntegratingVelocityTrackerStrategy::IntegratingVelocityTrackerStrategy(
uint32_t degree)
: degree_(degree) {
DCHECK_LT(degree_, static_cast<uint32_t>(Estimator::MAX_DEGREE));
}
IntegratingVelocityTrackerStrategy::~IntegratingVelocityTrackerStrategy() {}
void IntegratingVelocityTrackerStrategy::Clear() { pointer_id_bits_.clear(); }
void IntegratingVelocityTrackerStrategy::ClearPointers(BitSet32 id_bits) {
pointer_id_bits_.value &= ~id_bits.value;
}
void IntegratingVelocityTrackerStrategy::AddMovement(
const TimeTicks& event_time,
BitSet32 id_bits,
const Position* positions) {
uint32_t index = 0;
for (BitSet32 iter_id_bits(id_bits); !iter_id_bits.is_empty();) {
uint32_t id = iter_id_bits.clear_first_marked_bit();
State& state = mPointerState[id];
const Position& position = positions[index++];
if (pointer_id_bits_.has_bit(id))
UpdateState(state, event_time, position.x, position.y);
else
InitState(state, event_time, position.x, position.y);
}
pointer_id_bits_ = id_bits;
}
bool IntegratingVelocityTrackerStrategy::GetEstimator(
uint32_t id,
Estimator* out_estimator) const {
out_estimator->Clear();
if (pointer_id_bits_.has_bit(id)) {
const State& state = mPointerState[id];
PopulateEstimator(state, out_estimator);
return true;
}
return false;
}
void IntegratingVelocityTrackerStrategy::InitState(State& state,
const TimeTicks& event_time,
float xpos,
float ypos) const {
state.update_time = event_time;
state.degree = 0;
state.xpos = xpos;
state.xvel = 0;
state.xaccel = 0;
state.ypos = ypos;
state.yvel = 0;
state.yaccel = 0;
}
void IntegratingVelocityTrackerStrategy::UpdateState(
State& state,
const TimeTicks& event_time,
float xpos,
float ypos) const {
const base::TimeDelta MIN_TIME_DELTA = TimeDelta::FromMicroseconds(2);
const float FILTER_TIME_CONSTANT = 0.010f; // 10 milliseconds
if (event_time <= state.update_time + MIN_TIME_DELTA)
return;
float dt = static_cast<float>((event_time - state.update_time).InSecondsF());
state.update_time = event_time;
float xvel = (xpos - state.xpos) / dt;
float yvel = (ypos - state.ypos) / dt;
if (state.degree == 0) {
state.xvel = xvel;
state.yvel = yvel;
state.degree = 1;
} else {
float alpha = dt / (FILTER_TIME_CONSTANT + dt);
if (degree_ == 1) {
state.xvel += (xvel - state.xvel) * alpha;
state.yvel += (yvel - state.yvel) * alpha;
} else {
float xaccel = (xvel - state.xvel) / dt;
float yaccel = (yvel - state.yvel) / dt;
if (state.degree == 1) {
state.xaccel = xaccel;
state.yaccel = yaccel;
state.degree = 2;
} else {
state.xaccel += (xaccel - state.xaccel) * alpha;
state.yaccel += (yaccel - state.yaccel) * alpha;
}
state.xvel += (state.xaccel * dt) * alpha;
state.yvel += (state.yaccel * dt) * alpha;
}
}
state.xpos = xpos;
state.ypos = ypos;
}
void IntegratingVelocityTrackerStrategy::PopulateEstimator(
const State& state,
Estimator* out_estimator) const {
out_estimator->time = state.update_time;
out_estimator->confidence = 1.0f;
out_estimator->degree = state.degree;
out_estimator->xcoeff[0] = state.xpos;
out_estimator->xcoeff[1] = state.xvel;
out_estimator->xcoeff[2] = state.xaccel / 2;
out_estimator->ycoeff[0] = state.ypos;
out_estimator->ycoeff[1] = state.yvel;
out_estimator->ycoeff[2] = state.yaccel / 2;
}
} // namespace ui