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(); } } }