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