/* find the most plausible velocity. That is, the most distant
* (in time) tracker which isn't too old, the movement vector was
* in the same octant, and where the velocity is within an
* acceptable range to the inital velocity.
*
* @return The tracker's velocity or 0 if the above conditions are unmet
*/
static double
QueryTrackers(DeviceVelocityPtr vel, int cur_t)
{
int offset, dir = UNDEFINED, used_offset = -1, age_ms;
/* initial velocity: a low-offset, valid velocity */
double initial_velocity = 0, result = 0, velocity_diff;
double velocity_factor = vel->corr_mul * vel->const_acceleration; /* premultiply */
/* loop from current to older data */
for (offset = 1; offset < vel->num_tracker; offset++) {
MotionTracker *tracker = TRACKER(vel, offset);
double tracker_velocity;
age_ms = cur_t - tracker->time;
/* bail out if data is too old and protect from overrun */
if (age_ms >= vel->reset_time || age_ms < 0) {
DebugAccelF("query: tracker too old (reset after %d, age is %d)\n",
vel->reset_time, age_ms);
break;
}
/*
* this heuristic avoids using the linear-motion velocity formula
* in CalcTracker() on motion that isn't exactly linear. So to get
* even more precision we could subdivide as a final step, so possible
* non-linearities are accounted for.
*/
dir &= tracker->dir;
if (dir == 0) { /* we've changed octant of movement (e.g. NE → NW) */
DebugAccelF("query: no longer linear\n");
/* instead of breaking it we might also inspect the partition after,
* but actual improvement with this is probably rare. */
break;
}
tracker_velocity = CalcTracker(tracker, cur_t) * velocity_factor;
if ((initial_velocity == 0 || offset <= vel->initial_range) &&
tracker_velocity != 0) {
/* set initial velocity and result */
result = initial_velocity = tracker_velocity;
used_offset = offset;
}
else if (initial_velocity != 0 && tracker_velocity != 0) {
velocity_diff = fabs(initial_velocity - tracker_velocity);
if (velocity_diff > vel->max_diff &&
velocity_diff / (initial_velocity + tracker_velocity) >=
vel->max_rel_diff) {
/* we're not in range, quit - it won't get better. */
DebugAccelF("query: tracker too different:"
" old %2.2f initial %2.2f diff: %2.2f\n",
tracker_velocity, initial_velocity, velocity_diff);
break;
}
/* we're in range with the initial velocity,
* so this result is likely better
* (it contains more information). */
result = tracker_velocity;
used_offset = offset;
}
}
if (offset == vel->num_tracker) {
DebugAccelF("query: last tracker in effect\n");
used_offset = vel->num_tracker - 1;
}
if (used_offset >= 0) {
#ifdef PTRACCEL_DEBUGGING
MotionTracker *tracker = TRACKER(vel, used_offset);
DebugAccelF("result: offset %i [dx: %f dy: %f diff: %i]\n",
used_offset, tracker->dx, tracker->dy,
cur_t - tracker->time);
#endif
}
return result;
}
/* find the most plausible velocity. That is, the most distant
* (in time) tracker which isn't too old, beyond a linear partition,
* or simply too much off initial velocity.
*
* May return 0.
*/
static float
QueryTrackers(DeviceVelocityPtr vel, int cur_t){
int n, offset, dir = 255, i = -1, age_ms;
/* initial velocity: a low-offset, valid velocity */
float iveloc = 0, res = 0, tmp, vdiff;
float vfac = vel->corr_mul * vel->const_acceleration; /* premultiply */
/* loop from current to older data */
for(offset = 1; offset < vel->num_tracker; offset++){
n = TRACKER_INDEX(vel, offset);
age_ms = cur_t - vel->tracker[n].time;
/* bail out if data is too old and protect from overrun */
if (age_ms >= vel->reset_time || age_ms < 0) {
DebugAccelF("(dix prtacc) query: tracker too old\n");
break;
}
/*
* this heuristic avoids using the linear-motion velocity formula
* in CalcTracker() on motion that isn't exactly linear. So to get
* even more precision we could subdivide as a final step, so possible
* non-linearities are accounted for.
*/
dir &= vel->tracker[n].dir;
if(dir == 0){
DebugAccelF("(dix prtacc) query: no longer linear\n");
/* instead of breaking it we might also inspect the partition after,
* but actual improvement with this is probably rare. */
break;
}
tmp = CalcTracker(vel, offset, cur_t) * vfac;
if ((iveloc == 0 || offset <= vel->initial_range) && tmp != 0) {
/* set initial velocity and result */
res = iveloc = tmp;
i = offset;
} else if (iveloc != 0 && tmp != 0) {
vdiff = fabs(iveloc - tmp);
if (vdiff <= vel->max_diff ||
vdiff/(iveloc + tmp) < vel->max_rel_diff) {
/* we're in range with the initial velocity,
* so this result is likely better
* (it contains more information). */
res = tmp;
i = offset;
}else{
/* we're not in range, quit - it won't get better. */
DebugAccelF("(dix prtacc) query: tracker too different:"
" old %2.2f initial %2.2f diff: %2.2f\n",
tmp, iveloc, vdiff);
break;
}
}
}
if(offset == vel->num_tracker){
DebugAccelF("(dix prtacc) query: last tracker in effect\n");
i = vel->num_tracker-1;
}
if(i>=0){
n = TRACKER_INDEX(vel, i);
DebugAccelF("(dix prtacc) result: offset %i [dx: %i dy: %i diff: %i]\n",
i,
vel->tracker[n].dx,
vel->tracker[n].dy,
cur_t - vel->tracker[n].time);
}
return res;
}
