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