public OneActorUpdate(Guid id, Godot.Vector3 localPos, Godot.Quat localQuat,
Godot.Vector3 appPos, Godot.Quat appQuat)
{
localTransform = new TransformPatch();
appTransform = new TransformPatch();
localTransform.Position = new Vector3Patch(localPos);
localTransform.Rotation = new QuaternionPatch(localQuat);
appTransform.Position = new Vector3Patch(appPos);
appTransform.Rotation = new QuaternionPatch(appQuat);
actorGuid = id;
}
public Transform(Quat quat, Vector3 origin)
{
basis = new Basis(quat);
this.origin = origin;
}
private void Rotate(Quat quat)
{
this *= new Basis(quat);
}
internal void SetQuantScale(Quat quat, Vector3 scale)
{
SetDiagonal(scale);
Rotate(quat);
}
public void FixedUpdate(Spatial root)
{
// - physics rigid body management
// -set transforms/velocities for key framed bodies
// get all the prediction time infos in this struct
PredictionTimeParameters timeInfo = new PredictionTimeParameters(1f / Engine.IterationsPerSecond);
// start the predictor
_predictor.StartBodyPredicitonForNextFrame();
int index = 0;
MultiSourceCombinedSnapshot snapshot;
_snapshotManager.Step(timeInfo.DT, out snapshot);
_snapshotManager.UpdateDebugDisplay(root); // guarded by ifdef internally
foreach (var rb in _rigidBodies.Values)
{
// if the body is owned then we only set the kinematic flag for the physics
if (rb.Ownership)
{
if (rb.IsKeyframed)
{
if (rb.RigidBody.Mode != Godot.RigidBody.ModeEnum.Kinematic)
{
rb.RigidBody.Mode = Godot.RigidBody.ModeEnum.Kinematic;
}
}
else
{
if (rb.RigidBody.Mode != Godot.RigidBody.ModeEnum.Rigid)
{
rb.RigidBody.Mode = Godot.RigidBody.ModeEnum.Rigid;
}
}
continue;
}
// Find corresponding rigid body info.
// since both are sorted list this should hit without index=0 at the beginning
while (index < snapshot.RigidBodies.Count && rb.Id.CompareTo(snapshot.RigidBodies.Values[index].Id) > 0)
{
index++;
}
if (index < snapshot.RigidBodies.Count && rb.Id == snapshot.RigidBodies.Values[index].Id)
{
// todo: kick-in prediction if we are missing an update for this rigid body
//if (!snapshot.RigidBodies.Values[index].HasUpdate)
//{
// rb.RigidBody.isKinematic = false;
// continue;
//}
RigidBodyTransform transform = snapshot.RigidBodies.Values[index].Transform;
float timeOfSnapshot = snapshot.RigidBodies.Values[index].LocalTime;
// get the key framed stream, and compute implicit velocities
Godot.Vector3 keyFramedPos = root.ToGlobal(transform.Position);
Godot.Quat keyFramedOrientation = root.GlobalTransform.basis.RotationQuat() * transform.Rotation;
Godot.Vector3 JBLinearVelocity =
root.GlobalTransform.basis.GetEuler() * snapshot.RigidBodies.Values[index].LinearVelocity;
Godot.Vector3 JBAngularVelocity =
root.GlobalTransform.basis.GetEuler() * snapshot.RigidBodies.Values[index].AngularVelocity;
// if there is a really new update then also store the implicit velocity
if (rb.lastTimeKeyFramedUpdate < timeOfSnapshot)
{
// we moved the velocity estimation into the jitter buffer
rb.lastValidLinerVelocityOrPos = JBLinearVelocity;
rb.lastValidAngularVelocityorAng = JBAngularVelocity;
#if MRE_PHYSICS_DEBUG
// test the source of large velocities
if (rb.lastValidLinerVelocityOrPos.LengthSquared() > _maxEstimatedLinearVelocity * _maxEstimatedLinearVelocity)
{
// limited debug version
GD.Print(" ACTIVE SPEED LIMIT TRAP RB: " //+ rb.Id.ToString() + " got update lin vel:"
+ rb.lastValidLinerVelocityOrPos + " ang vel:" + rb.lastValidAngularVelocityorAng
+ " time:" + timeOfSnapshot
+ " newR:" + rb.lastTimeKeyFramedUpdate
+ " hasupdate:" + snapshot.RigidBodies.Values[index].HasUpdate);
//+ " DangE:" + eulerAngles + " DangR:" + radianAngles );
}
#endif
// cap the velocities
rb.lastValidLinerVelocityOrPos = ClampLength(
rb.lastValidLinerVelocityOrPos, _maxEstimatedLinearVelocity);
rb.lastValidAngularVelocityorAng = ClampLength(
rb.lastValidAngularVelocityorAng, _maxEstimatedAngularVelocity);
// if body is sleeping then all velocities are zero
if (snapshot.RigidBodies.Values[index].motionType == Patching.Types.MotionType.Sleeping)
{
rb.lastValidLinerVelocityOrPos = new Vector3(0.0F, 0.0F, 0.0F);
rb.lastValidAngularVelocityorAng = new Vector3(0.0F, 0.0F, 0.0F);
}
#if MRE_PHYSICS_DEBUG
if (true)
{
// limited debug version
GD.Print(" Remote body: " + rb.Id.ToString() + " got update lin vel:"
+ rb.lastValidLinerVelocityOrPos + " ang vel:" + rb.lastValidAngularVelocityorAng
+ " time:" + timeOfSnapshot + " newR:" + rb.lastTimeKeyFramedUpdate);
}
else
{
GD.Print(" Remote body: " + rb.Id.ToString() + " got update lin vel:"
+ rb.lastValidLinerVelocityOrPos + " ang vel:" + rb.lastValidAngularVelocityorAng
//+ " DangE:" + eulerAngles + " DangR:" + radianAngles
+ " time:" + timeOfSnapshot + " newp:" + keyFramedPos
+ " newR:" + keyFramedOrientation
+ " oldP:" + rb.RigidBody.GlobalTransform.origin
+ " oldR:" + rb.RigidBody.GlobalTransform.basis.RotationQuat()
+ " OriginalRot:" + transform.Rotation
//+ " keyF:" + rb.RigidBody.isKinematic
+ " KF:" + rb.IsKeyframed);
}
#endif
// cap the velocities
rb.lastValidLinerVelocityOrPos = ClampLength(
rb.lastValidLinerVelocityOrPos, _maxEstimatedLinearVelocity);
rb.lastValidAngularVelocityorAng = ClampLength(
rb.lastValidAngularVelocityorAng, _maxEstimatedAngularVelocity);
// if body is sleeping then all velocities are zero
if (snapshot.RigidBodies.Values[index].motionType == Patching.Types.MotionType.Sleeping)
{
rb.lastValidLinerVelocityOrPos = new Vector3(0.0F, 0.0F, 0.0F);
rb.lastValidAngularVelocityorAng = new Vector3(0.0F, 0.0F, 0.0F);
}
#if MRE_PHYSICS_DEBUG
GD.Print(" Remote body: " + rb.Id.ToString() + " got update lin vel:"
+ rb.lastValidLinerVelocityOrPos + " ang vel:" + rb.lastValidAngularVelocityorAng
//+ " DangE:" + eulerAngles + " DangR:" + radianAngles
+ " time:" + timeOfSnapshot + " newp:" + keyFramedPos
+ " newR:" + keyFramedOrientation
+ " incUpdateDt:" + timeInfo.DT
+ " oldP:" + rb.RigidBody.GlobalTransform.origin
+ " oldR:" + rb.RigidBody.GlobalTransform.basis.RotationQuat()
+ " OriginalRot:" + transform.Rotation
//+ " keyF:" + rb.RigidBody.isKinematic
+ " KF:" + rb.IsKeyframed);
#endif
}
rb.lastTimeKeyFramedUpdate = timeOfSnapshot;
rb.IsKeyframed = (snapshot.RigidBodies.Values[index].motionType == Patching.Types.MotionType.Keyframed);
// code to disable prediction and to use just key framing (and comment out the prediction)
//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);
// call the predictor with this remotely owned body
_predictor.AddAndProcessRemoteBodyForPrediction(rb, transform,
keyFramedPos, keyFramedOrientation, timeOfSnapshot, timeInfo);
}
}
// call the predictor
_predictor.PredictAllRemoteBodiesWithOwnedBodies(ref _rigidBodies, timeInfo);
}
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);
}
