public RigidBodyState(Guid id, float time, RigidBodyTransform transform, bool hasUpdate, MotionType mType)
{
Id = id;
LocalTime = time;
Transform = transform;
HasUpdate = hasUpdate;
motionType = mType;
}
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 RigidBodyState(Guid id, float time, RigidBodyTransform transform,
Vector3 linearVelocity, Vector3 angularVelocity,
bool hasUpdate, MotionType mType)
{
Id = id;
LocalTime = time;
Transform = transform;
LinearVelocity = linearVelocity;
AngularVelocity = angularVelocity;
HasUpdate = hasUpdate;
motionType = mType;
}
public void SetVelocitiesFromTransformsDiff(float DT, RigidBodyTransform current,
RigidBodyTransform prev)
{
if (Math.Abs(DT) > 0.0005f)
{
float invUpdateDT = 1.0f / (DT); // DT has to be different from zero
UnityEngine.Vector3 eulerAngles = (UnityEngine.Quaternion.Inverse(prev.Rotation) * current.Rotation).eulerAngles;
UnityEngine.Vector3 radianAngles = UtilMethods.TransformEulerAnglesToRadians(eulerAngles);
//if (radianAngles.sqrMagnitude < 0.02F)
//{
// Debug.Log("NULL DT=" + DT + " prevRot=" + prev.Rotation.ToString() + " next=" + current.Rotation.ToString());
//}
AngularVelocity = radianAngles * invUpdateDT;
LinearVelocity = (current.Position - prev.Position) * invUpdateDT;
}
}
public TransformInfo(Guid rigidBodyId, RigidBodyTransform transform, MotionType motionType)
{
RigidBodyId = rigidBodyId;
Transform = transform;
MotionType = motionType;
}
public void Lerp(RigidBodyTransform t0, RigidBodyTransform t1, float f)
{
Position = Vector3.Lerp(t0.Position, t1.Position, f);
Rotation = Quaternion.Lerp(t0.Rotation, t1.Rotation, f);
}
public void AddAndProcessRemoteBodyForPrediction(RigidBodyPhysicsBridgeInfo rb,
RigidBodyTransform transform, UnityEngine.Vector3 keyFramedPos,
UnityEngine.Quaternion keyFramedOrientation, float timeOfSnapshot,
PredictionTimeParameters timeInfo)
{
var collisionInfo = new CollisionSwitchInfo();
collisionInfo.startPosition = rb.RigidBody.transform.position;
collisionInfo.startOrientation = rb.RigidBody.transform.rotation;
collisionInfo.rigidBodyId = rb.Id;
collisionInfo.isKeyframed = rb.IsKeyframed;
// test is this remote body is in the monitor stream or if this is grabbed & key framed then this should not be dynamic
if (_monitorCollisionInfo.ContainsKey(rb.Id) && !rb.IsKeyframed)
{
// dynamic
rb.RigidBody.isKinematic = false;
collisionInfo.monitorInfo = _monitorCollisionInfo[rb.Id];
collisionInfo.monitorInfo.timeFromStartCollision += timeInfo.DT;
collisionInfo.linearVelocity = rb.RigidBody.velocity;
collisionInfo.angularVelocity = rb.RigidBody.angularVelocity;
#if MRE_PHYSICS_DEBUG
Debug.Log(" Remote body: " + rb.Id.ToString() + " is dynamic since:"
+ collisionInfo.monitorInfo.timeFromStartCollision + " relative distance:" + collisionInfo.monitorInfo.relativeDistance
+ " interpolation:" + collisionInfo.monitorInfo.keyframedInterpolationRatio);
#endif
// if time passes by then make a transformation between key framed and dynamic
// but only change the positions not the velocity
if (collisionInfo.monitorInfo.keyframedInterpolationRatio > 0.05f)
{
// interpolate between key framed and dynamic transforms
float t = collisionInfo.monitorInfo.keyframedInterpolationRatio;
UnityEngine.Vector3 interpolatedPos;
UnityEngine.Quaternion interpolatedQuad;
interpolatedPos = t * keyFramedPos + (1.0f - t) * rb.RigidBody.transform.position;
interpolatedQuad = UnityEngine.Quaternion.Slerp(keyFramedOrientation, rb.RigidBody.transform.rotation, t);
#if MRE_PHYSICS_DEBUG
Debug.Log(" Interpolate body " + rb.Id.ToString() + " t=" + t
+ " time=" + UnityEngine.Time.time
+ " pos KF:" + keyFramedPos
+ " dyn:" + rb.RigidBody.transform.position
+ " interp pos:" + interpolatedPos
+ " rb vel:" + rb.RigidBody.velocity
+ " KF vel:" + rb.lastValidLinerVelocity);
#endif
// apply these changes only if they are significant in order to not to bother the physics engine
// for settled objects
UnityEngine.Vector3 posdiff = rb.RigidBody.transform.position - interpolatedPos;
if (posdiff.magnitude > interpolationPosEpsilon)
{
rb.RigidBody.transform.position = interpolatedPos;
}
float angleDiff = Math.Abs(
UnityEngine.Quaternion.Angle(rb.RigidBody.transform.rotation, interpolatedQuad));
if (angleDiff > interpolationAngularEps)
{
rb.RigidBody.transform.rotation = interpolatedQuad;
}
// apply velocity damping if we are in the interpolation phase
if (collisionInfo.monitorInfo.keyframedInterpolationRatio >= velocityDampingInterpolationValueStart)
{
rb.RigidBody.velocity *= velocityDampingForInterpolation;
rb.RigidBody.angularVelocity *= velocityDampingForInterpolation;
}
}
}
else
{
// 100% key framing
rb.RigidBody.isKinematic = true;
rb.RigidBody.transform.position = keyFramedPos;
rb.RigidBody.transform.rotation = keyFramedOrientation;
rb.RigidBody.velocity.Set(0.0f, 0.0f, 0.0f);
rb.RigidBody.angularVelocity.Set(0.0f, 0.0f, 0.0f);
collisionInfo.linearVelocity = rb.lastValidLinerVelocity;
collisionInfo.angularVelocity = rb.lastValidAngularVelocity;
collisionInfo.monitorInfo = new CollisionMonitorInfo();
#if MRE_PHYSICS_DEBUG
if (rb.IsKeyframed)
{
Debug.Log(" Remote body: " + rb.Id.ToString() + " is key framed:"
+ " linvel:" + collisionInfo.linearVelocity
+ " angvel:" + collisionInfo.angularVelocity);
}
#endif
}
// <todo> add more filtering here to cancel out unnecessary items,
// but for small number of bodies should be OK
_switchCollisionInfos.Add(collisionInfo);
}
public TransformInfo(Guid id, RigidBodyTransform transform, MotionType mType)
{
Id = id;
Transform = transform;
motionType = mType;
}
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);
}
}
}
public void AddAndProcessRemoteBodyForPrediction(RigidBodyPhysicsBridgeInfo rb,
RigidBodyTransform transform, Godot.Vector3 keyFramedPos,
Godot.Quat keyFramedOrientation, float timeOfSnapshot,
PredictionTimeParameters timeInfo)
{
var collisionInfo = new CollisionSwitchInfo();
collisionInfo.startPosition = rb.RigidBody.GlobalTransform.origin;
collisionInfo.startOrientation = rb.RigidBody.GlobalTransform.basis.RotationQuat();
collisionInfo.rigidBodyId = rb.Id;
collisionInfo.isKeyframed = rb.IsKeyframed;
// test is this remote body is in the monitor stream or if this is grabbed & key framed then this should not be dynamic
if (_monitorCollisionInfo.ContainsKey(rb.Id) && !rb.IsKeyframed)
{
// dynamic
if (rb.RigidBody.Mode != Godot.RigidBody.ModeEnum.Rigid)
{
rb.RigidBody.Mode = Godot.RigidBody.ModeEnum.Rigid;
}
collisionInfo.monitorInfo = _monitorCollisionInfo[rb.Id];
collisionInfo.monitorInfo.timeFromStartCollision += timeInfo.DT;
collisionInfo.linearVelocity = rb.RigidBody.LinearVelocity;
collisionInfo.angularVelocity = rb.RigidBody.AngularVelocity;
#if MRE_PHYSICS_DEBUG
GD.Print(" Remote body: " + rb.Id.ToString() + " is dynamic since:"
+ collisionInfo.monitorInfo.timeFromStartCollision + " relative distance:" + collisionInfo.monitorInfo.relativeDistance
+ " interpolation:" + collisionInfo.monitorInfo.keyframedInterpolationRatio);
#endif
// if time passes by then make a transformation between key framed and dynamic
// but only change the positions not the velocity
if (collisionInfo.monitorInfo.keyframedInterpolationRatio > 0.05f)
{
// interpolate between key framed and dynamic transforms
float t = collisionInfo.monitorInfo.keyframedInterpolationRatio;
Godot.Vector3 interpolatedPos;
Godot.Quat interpolatedQuad;
interpolatedPos = t * keyFramedPos + (1.0f - t) * rb.RigidBody.GlobalTransform.origin;
interpolatedQuad = keyFramedOrientation.Slerp(rb.RigidBody.GlobalTransform.basis.RotationQuat(), t);
#if MRE_PHYSICS_DEBUG
GD.Print(" Interpolate body " + rb.Id.ToString() + " t=" + t
+ " time=" + OS.GetTicksMsec() * 0.001
+ " pos KF:" + keyFramedPos
+ " dyn:" + rb.RigidBody.Transform.origin
+ " interp pos:" + interpolatedPos
+ " rb vel:" + rb.RigidBody.LinearVelocity
+ " KF vel:" + rb.lastValidLinerVelocityOrPos);
#endif
// apply these changes only if they are significant in order to not to bother the physics engine
// for settled objects
Godot.Vector3 posdiff = rb.RigidBody.GlobalTransform.origin - interpolatedPos;
Vector3 rigidBodyOrigin = rb.RigidBody.GlobalTransform.origin;
Basis rigidBodyBasis = rb.RigidBody.GlobalTransform.basis;
if (posdiff.Length() > interpolationPosEpsilon)
{
rigidBodyOrigin = interpolatedPos;
}
float angleDiff = Math.Abs(Mathf.Rad2Deg(rigidBodyBasis.GetEuler().AngleTo(interpolatedQuad.GetEuler())));
if (angleDiff > interpolationAngularEps)
{
rigidBodyBasis = new Basis(interpolatedQuad);
}
rb.RigidBody.GlobalTransform = new Transform(rigidBodyBasis, rigidBodyOrigin);
// apply velocity damping if we are in the interpolation phase
if (collisionInfo.monitorInfo.keyframedInterpolationRatio >= velocityDampingInterpolationValueStart)
{
rb.RigidBody.LinearVelocity *= velocityDampingForInterpolation;
rb.RigidBody.AngularVelocity *= velocityDampingForInterpolation;
}
}
}
else
{
// 100% key framing
if (rb.RigidBody.Mode != Godot.RigidBody.ModeEnum.Kinematic)
{
rb.RigidBody.Mode = Godot.RigidBody.ModeEnum.Kinematic;
}
rb.RigidBody.GlobalTransform = new Transform(new Basis(keyFramedOrientation), keyFramedPos);
rb.RigidBody.LinearVelocity = new Vector3(0.0f, 0.0f, 0.0f);
rb.RigidBody.AngularVelocity = new Vector3(0.0f, 0.0f, 0.0f);
collisionInfo.linearVelocity = rb.lastValidLinerVelocityOrPos;
collisionInfo.angularVelocity = rb.lastValidAngularVelocityorAng;
collisionInfo.monitorInfo = new CollisionMonitorInfo();
#if MRE_PHYSICS_DEBUG
if (rb.IsKeyframed)
{
GD.Print(" Remote body: " + rb.Id.ToString() + " is key framed:"
+ " linvel:" + collisionInfo.linearVelocity
+ " angvel:" + collisionInfo.angularVelocity);
}
#endif
}
// <todo> add more filtering here to cancel out unnecessary items,
// but for small number of bodies should be OK
_switchCollisionInfos.Add(collisionInfo);
}
public void Lerp(RigidBodyTransform t0, RigidBodyTransform t1, float f)
{
Position = t0.Position.LinearInterpolate(t1.Position, f);
// FIXEM: There is no Quat.Lerp.
Rotation = t0.Rotation.Slerp(t1.Rotation, f);
}
