public void PushTransform(CarTimestamp timestamp, Matrix4 transform)
{
lock (lockobj) {
TransformEntry te = new TransformEntry(timestamp, transform);
rollingQueue.Add(te);
// check if this is the most recent timestamp
if (!double.IsNaN(recentTimestamp.ts) && timestamp < recentTimestamp)
{
// do a reverse bubble-sort
int i = rollingQueue.Count - 2;
// find the insertion point
while (i >= 0 && rollingQueue[i].timestamp > timestamp)
{
// shift the entry up
rollingQueue[i + 1] = rollingQueue[i];
i--;
}
// i+1 contains the empty slot to insert at
rollingQueue[i + 1] = te;
}
else
{
// update the recent timestamp
recentTimestamp = timestamp;
}
if (!rollingQueue.VerifySort(delegate(TransformEntry l, TransformEntry r) { return(l.timestamp.CompareTo(r.timestamp)); }))
{
Trace.TraceError("relative transform sort is donzoed, flushing queue");
Reset();
}
}
}
public void PushAbsolutePose(AbsolutePose pose)
{
lock (lockobj) {
queue.Add(pose);
if (!queue.VerifySort(delegate(AbsolutePose l, AbsolutePose r) { return(l.timestamp.CompareTo(r.timestamp)); }))
{
OperationalTrace.WriteError("absolute sort is donzoed, flushing queue");
queue.Clear();
}
}
}
void TrackedDistance_DataValueAdded(object sender, SourceDataValueAddedEventArgs <double> e)
{
// check if we received something before
if (double.IsNaN(lastTime.ts))
{
// we haven't received anything, initialize starting time
lastTime = e.Time;
}
else if (e.Time < lastTime)
{
//OperationalTrace.WriteWarning("resetting tracked distanace: event time {0}, last time {1}", e.Time, lastTime);
// timestamp rollover/reset
// clear everything out
lock (lockobj) {
queue.Clear();
lastTime = e.Time;
lastDist = 0;
}
}
else
{
// calculate dt
double dt = e.Time.ts - lastTime.ts;
// calculate delta distance
double dx = Math.Abs(e.Value) * dt;
// get the lock
lock (lockobj) {
lastDist += dx;
lastTime = e.Time;
//OperationalTrace.WriteVerbose("adding dist {0}, time {1}", lastDist, lastTime);
queue.Add(new TrackedDistanceItem(lastTime, lastDist));
}
}
}
