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[kHistorySize];
float y[kHistorySize];
float w[kHistorySize];
float time[kHistorySize];
uint32_t m = 0;
uint32_t index = index_;
const base::TimeDelta horizon = base::TimeDelta::FromMilliseconds(kHorizonMS);
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 PointerXY& position = movement.GetPointerXY(id);
x[m] = position.x;
y[m] = position.y;
w[m] = ChooseWeight(index);
time[m] = -static_cast<float>(age.InSecondsF());
index = (index == 0 ? kHistorySize : index) - 1;
} while (++m < kHistorySize);
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;
}
void ClusterFit::Compress4( void* block )
{
//debug = (run == 1);
//run++;
// declare variables
int const count = m_colours->GetCount();
#if SQUISH_USE_SIMD
Vec4 beststart = VEC4_CONST( 0.0f );
Vec4 bestend = VEC4_CONST( 0.0f );
Vec4 besterror = m_besterror;
Vec4 const twothirds = VEC4_CONST( 2.0f/3.0f );
Vec4 const onethird = VEC4_CONST( 1.0f/3.0f );
Vec4 const zero = VEC4_CONST( 0.0f );
#else
Vec3 beststart( 0.0f );
Vec3 bestend( 0.0f );
float besterror = m_besterror;
float const twothirds = 2.0f/3.0f;
float const onethird = 1.0f/3.0f;
float const zero = 0.0f;
#endif
// check all possible clusters for this total order
u8 indices[16];
u8 bestindices[16];
// first cluster [0,i) is at the start
for( int m = 0; m < count; ++m )
{
indices[m] = 0;
m_alpha[m] = m_weights[m];
m_beta[m] = zero;
}
for( int i = count; i >= 0; --i )
{
// second cluster [i,j) is one third along
for( int m = i; m < count; ++m )
{
indices[m] = 2;
m_alpha[m] = twothirds*m_weights[m];
m_beta[m] = onethird*m_weights[m];
}
for( int j = count; j >= i; --j )
{
// third cluster [j,k) is two thirds along
for( int m = j; m < count; ++m )
{
indices[m] = 3;
m_alpha[m] = onethird*m_weights[m];
m_beta[m] = twothirds*m_weights[m];
}
for( int k = count; k >= j; --k )
{
if (j + k == 0) continue;
// last cluster [k,n) is at the end
if( k < count )
{
indices[k] = 1;
m_alpha[k] = zero;
m_beta[k] = m_weights[k];
}
// solve a least squares problem to place the endpoints
#if SQUISH_USE_SIMD
Vec4 start, end;
Vec4 error = SolveLeastSquares( start, end );
#else
Vec3 start, end;
float error = SolveLeastSquares( start, end );
#endif
// keep the solution if it wins
#if SQUISH_USE_SIMD
if( CompareAnyLessThan( error, besterror ) )
#else
if( error < besterror )
#endif
{
beststart = start;
bestend = end;
for( int m = 0; m < 16; ++m ) // TODO: make this faster?
bestindices[m] = indices[m];
besterror = error;
}
}
}
}
// save the block if necessary
#if SQUISH_USE_SIMD
if( CompareAnyLessThan( besterror, m_besterror ) )
#else
if( besterror < m_besterror )
#endif
{
// remap the indices
u8 unordered[16];
for( int i = 0; i < count; ++i )
unordered[m_order[i]] = bestindices[i];
m_colours->RemapIndices( unordered, bestindices );
// save the block
#if SQUISH_USE_SIMD
WriteColourBlock4( beststart.GetVec3(), bestend.GetVec3(), bestindices, block );
#else
WriteColourBlock4( beststart, bestend, bestindices, block );
#endif
// save the error
m_besterror = besterror;
}
}
void ClusterFit::Compress3( void* block )
{
// declare variables
int const count = m_colours->GetCount();
#if SQUISH_USE_SIMD
Vec4 beststart = VEC4_CONST( 0.0f );
Vec4 bestend = VEC4_CONST( 0.0f );
Vec4 besterror = VEC4_CONST( FLT_MAX );
Vec4 const half = VEC4_CONST( 0.5f );
Vec4 const zero = VEC4_CONST( 0.0f );
#else
Vec3 beststart( 0.0f );
Vec3 bestend( 0.0f );
float besterror = FLT_MAX;
float const half = 0.5f;
float const zero = 0.0f;
#endif
// check all possible clusters for this total order
u8 indices[16];
u8 bestindices[16];
// first cluster [0,i) is at the start
for( int m = 0; m < count; ++m )
{
indices[m] = 0;
m_alpha[m] = m_weights[m];
m_beta[m] = zero;
}
for( int i = count; i >= 0; --i )
{
// second cluster [i,j) is half along
for( int m = i; m < count; ++m )
{
indices[m] = 2;
m_alpha[m] = m_beta[m] = half*m_weights[m];
}
for( int j = count; j > i; --j )
{
// last cluster [j,k) is at the end
if( j < count )
{
indices[j] = 1;
m_alpha[j] = zero;
m_beta[j] = m_weights[j];
}
// solve a least squares problem to place the endpoints
#if SQUISH_USE_SIMD
Vec4 start, end;
Vec4 error = SolveLeastSquares( start, end );
#else
Vec3 start, end;
float error = SolveLeastSquares( start, end );
#endif
// keep the solution if it wins
#if SQUISH_USE_SIMD
if( CompareAnyLessThan( error, besterror ) )
#else
if( error < besterror )
#endif
{
beststart = start;
bestend = end;
for( int m = 0; m < 16; ++m ) // TODO: make this faster?
bestindices[m] = indices[m];
besterror = error;
}
}
}
// save the block if necessary
#if SQUISH_USE_SIMD
if( CompareAnyLessThan( besterror, m_besterror ) )
#else
if( besterror < m_besterror )
#endif
{
// remap the indices
u8 unordered[16];
for( int i = 0; i < count; ++i )
unordered[m_order[i]] = bestindices[i];
m_colours->RemapIndices( unordered, bestindices );
// save the block
#if SQUISH_USE_SIMD
WriteColourBlock3( beststart.GetVec3(), bestend.GetVec3(), bestindices, block );
#else
WriteColourBlock3( beststart, bestend, bestindices, block );
#endif
// save the error
m_besterror = besterror;
}
}
