public void Update(ref RigidBodyData entry)
{
while (_nextIndex < _next.Transforms.Count &&
_next.Transforms[_nextIndex].RigidBodyId.CompareTo(entry.RigidBodyId) < 0)
{
_nextIndex++;
}
while (_prevIndex < _prev.Transforms.Count &&
_prev.Transforms[_prevIndex].RigidBodyId.CompareTo(entry.RigidBodyId) < 0)
{
_prevIndex++;
}
if (_nextIndex >= _next.Transforms.Count ||
_next.Transforms[_nextIndex].RigidBodyId != entry.RigidBodyId)
{
// velocity is zero
entry.SetZeroVelocities();
if (entry.MotionType == MotionType.Sleeping)
{
entry.Time = _timestamp;
entry.Updated = true;
}
else
{
entry.Updated = false;
}
return;
}
if (_prevIndex < _prev.Transforms.Count &&
_prev.Transforms[_prevIndex].RigidBodyId == _next.Transforms[_nextIndex].RigidBodyId)
{
RigidBodyTransform t = new RigidBodyTransform();
{
t.Lerp(_prev.Transforms[_prevIndex].Transform, _next.Transforms[_nextIndex].Transform, _frac);
}
entry.Transform = t;
entry.SetZeroVelocities();
// estimate velocity
float DT = _next.Time - _prev.Time;
var prevT = _prev.Transforms[_prevIndex].Transform;
var currentT = _next.Transforms[_nextIndex].Transform;
entry.SetVelocitiesFromTransformsDiff(DT, currentT, prevT);
}
else
{
entry.Transform = _next.Transforms[_nextIndex].Transform;
entry.SetZeroVelocities();
}
entry.Time = _timestamp;
entry.MotionType = _next.Transforms[_nextIndex].MotionType;
entry.Updated = true;
}
public void step(float timestep)
{
if (_mode == Mode.Init)
{
// todo: do we need this warm up?
if (SnapshotBuffer.Snapshots.Count >= 4)
{
_mode = Mode.Play;
CurrentSnapshot = SnapshotBuffer.Snapshots.First().Value;
CurrentLocalTime = CurrentSnapshot.Time;
}
}
else if (_mode != Mode.Init)
{
// todo: consider exposing as a parameter or calculating based on local and remote timesteps.
const float bufferTimeBiasTimeUnit = 1.0f / 60;
float targetBufferTime = _stats.mean() - _stats.deviation();
float biasedTargetBufferTime = targetBufferTime / bufferTimeBiasTimeUnit;
biasedTargetBufferTime = biasedTargetBufferTime > 0 ?
((int)biasedTargetBufferTime) * bufferTimeBiasTimeUnit : ((int)biasedTargetBufferTime - 1) * bufferTimeBiasTimeUnit;
float nextTimestamp = CurrentLocalTime + timestep;
float bufferedTime = SnapshotBuffer.Snapshots.Last().Value.Time - nextTimestamp;
_stats.addSample(bufferedTime);
// check if time shift is required
float timeShift;
if (Math.Abs(biasedTargetBufferTime) > 0.001)
{
// todo: limit slow-down, don't allow time to move to the past
timeShift = biasedTargetBufferTime > 0 ?
_speedUpCoef * biasedTargetBufferTime : _speedDownCoef * biasedTargetBufferTime;
nextTimestamp += timeShift;
_stats.shiftTime(-timeShift);
}
else
{
timeShift = 0;
}
#if DEBUG_JITTER_BUFFER
_debugStats.add(bufferedTime, _stats.mean(), targetBufferTime, biasedTargetBufferTime, timeShift, nextTimestamp);
#endif
if (nextTimestamp <= SnapshotBuffer.Snapshots.Last().Value.Time)
{
// Snapshot can be interpolated from the buffers
HasUpdate = true;
Snapshot prev, next;
SnapshotBuffer.step(nextTimestamp, out prev, out next);
if (Math.Abs(next.Time - nextTimestamp) < 0.001)
{
// if offset is less than a millisecond, just use 'next' snapshot time
CurrentLocalTime = next.Time;
CurrentSnapshot = next;
}
else if (prev != null)
{
if (Math.Abs(prev.Time - nextTimestamp) < 0.001)
{
// if offset is less than a millisecond, just use 'prev' snapshot time
CurrentLocalTime = prev.Time;
CurrentSnapshot = prev;
}
else
{
float frac = (nextTimestamp - prev.Time) / (next.Time - prev.Time);
List <Snapshot.TransformInfo> transforms = new List <Snapshot.TransformInfo>(next.Transforms.Count);
int prevIndex = 0;
int nextIndex = 0;
for (; nextIndex < next.Transforms.Count; nextIndex++)
{
// find corresponding transform in prev snapshot
while (prevIndex < prev.Transforms.Count && prev.Transforms[prevIndex].Id.CompareTo(next.Transforms[nextIndex].Id) < 0)
{
prevIndex++;
}
if (prevIndex < prev.Transforms.Count &&
prev.Transforms[prevIndex].Id == next.Transforms[nextIndex].Id)
{
RigidBodyTransform t = new RigidBodyTransform();
{
t.Lerp(prev.Transforms[prevIndex].Transform, next.Transforms[nextIndex].Transform, frac);
}
transforms.Add(new Snapshot.TransformInfo(next.Transforms[nextIndex].Id, t,
next.Transforms[nextIndex].motionType));
}
else
{
transforms.Add(new Snapshot.TransformInfo(next.Transforms[nextIndex].Id,
next.Transforms[nextIndex].Transform, next.Transforms[nextIndex].motionType));
}
}
// interpolated snapshot
CurrentLocalTime = nextTimestamp;
CurrentSnapshot = new Snapshot(nextTimestamp, transforms);
}
}
else
{
// if prev snapshot is missing, just use current transforms with past timestamp
CurrentLocalTime = nextTimestamp;
CurrentSnapshot = next;
}
}
else
{
// Snapshot can not be interpolated from the buffers
HasUpdate = false;
CurrentLocalTime = nextTimestamp;
CurrentSnapshot = new Snapshot(CurrentLocalTime, CurrentSnapshot.Transforms);
}
}
}
