public void PredictAllRemoteBodiesWithOwnedBodies( ref SortedList <Guid, RigidBodyPhysicsBridgeInfo> allRigidBodiesOfThePhysicsBridge, PredictionTimeParameters timeInfo) { // clear here all the monitoring since we will re add them _monitorCollisionInfo.Clear(); #if MRE_PHYSICS_DEBUG Debug.Log(" ===================== "); #endif // test collisions of each owned body with each not owned body foreach (var rb in allRigidBodiesOfThePhysicsBridge.Values) { if (rb.Ownership) { // test here all the remote-owned collisions and those should be turned to dynamic again. foreach (var remoteBodyInfo in _switchCollisionInfos) { var remoteBody = allRigidBodiesOfThePhysicsBridge[remoteBodyInfo.rigidBodyId].RigidBody; // if this is key framed then also on the remote side it will stay key framed if (remoteBodyInfo.isKeyframed) { continue; } var comDist = (remoteBody.transform.position - rb.RigidBody.transform.position).magnitude; var remoteHitPoint = remoteBody.ClosestPointOnBounds(rb.RigidBody.transform.position); var ownedHitPoint = rb.RigidBody.ClosestPointOnBounds(remoteBody.transform.position); var remoteRelativeHitP = (remoteHitPoint - remoteBody.transform.position); var ownedRelativeHitP = (ownedHitPoint - rb.RigidBody.transform.position); var radiousRemote = radiusExpansionFactor * remoteRelativeHitP.magnitude; var radiusOwnedBody = radiusExpansionFactor * ownedRelativeHitP.magnitude; var totalDistance = radiousRemote + radiusOwnedBody + 0.0001f; // avoid division by zero // project the linear velocity of the body var projectedOwnedBodyPos = rb.RigidBody.transform.position + rb.RigidBody.velocity * timeInfo.DT; var projectedComDist = (remoteBody.transform.position - projectedOwnedBodyPos).magnitude; var collisionMonitorInfo = remoteBodyInfo.monitorInfo; float lastApproxDistance = Math.Min(comDist, projectedComDist); collisionMonitorInfo.relativeDistance = lastApproxDistance / totalDistance; bool addToMonitor = false; bool isWithinCollisionRange = (projectedComDist < totalDistance || comDist < totalDistance); float timeSinceCollisionStart = collisionMonitorInfo.timeFromStartCollision; bool isAlreadyInMonitor = _monitorCollisionInfo.ContainsKey(remoteBodyInfo.rigidBodyId); #if MRE_PHYSICS_DEBUG //if (remoteBodyInfo.isKeyframed) { Debug.Log("prprojectedComDistoj: " + projectedComDist + " comDist:" + comDist + " totalDistance:" + totalDistance + " remote body pos:" + remoteBodyInfo.startPosition.ToString() + "input lin vel:" + remoteBodyInfo.linearVelocity + " radiousRemote:" + radiousRemote + " radiusOwnedBody:" + radiusOwnedBody + " relative dist:" + collisionMonitorInfo.relativeDistance + " timeSinceCollisionStart:" + timeSinceCollisionStart + " isAlreadyInMonitor:" + isAlreadyInMonitor); } #endif // unconditionally add to the monitor stream if this is a reasonable collision and we are only at the beginning if (collisionMonitorInfo.timeFromStartCollision > timeInfo.halfDT && timeSinceCollisionStart <= startInterpolatingBack && collisionMonitorInfo.relativeDistance < inCollisionRangeRelativeDistanceFactor) { #if MRE_PHYSICS_DEBUG //if (remoteBodyInfo.isKeyframed) { Debug.Log(" unconditionally add to collision stream time:" + timeSinceCollisionStart + " relative dist:" + collisionMonitorInfo.relativeDistance); } #endif addToMonitor = true; } // switch over smoothly to the key framing if (!addToMonitor && timeSinceCollisionStart > 5 * timeInfo.DT && // some basic check for lower limit //(!isAlreadyInMonitor || collisionMonitorInfo.relativeDistance > 1.2f) && // if this is already added then ignore large values timeSinceCollisionStart <= endInterpolatingBack) { addToMonitor = true; float oldVal = collisionMonitorInfo.keyframedInterpolationRatio; // we might enter here before startInterpolatingBack timeSinceCollisionStart = Math.Max(timeSinceCollisionStart, startInterpolatingBack + timeInfo.DT); // progress the interpolation collisionMonitorInfo.keyframedInterpolationRatio = invInterpolationTimeWindows * (timeSinceCollisionStart - startInterpolatingBack); #if MRE_PHYSICS_DEBUG Debug.Log(" doing interpolation new:" + collisionMonitorInfo.keyframedInterpolationRatio + " old:" + oldVal + " relative distance:" + collisionMonitorInfo.relativeDistance); #endif // stop interpolation if (collisionMonitorInfo.relativeDistance < collisionRelDistLimit && collisionMonitorInfo.keyframedInterpolationRatio > limitCollisionInterpolation) { #if MRE_PHYSICS_DEBUG Debug.Log(" Stop interpolation time with DT:" + DT + " Ratio:" + collisionMonitorInfo.keyframedInterpolationRatio + " relDist:" + collisionMonitorInfo.relativeDistance + " t=" + collisionMonitorInfo.timeFromStartCollision); #endif // --- this is a different way to drop the percentage for key framing --- //collisionMonitorInfo.timeFromStartCollision -= // Math.Min( 4.0f, ( (1.0f/limitCollisionInterpolation) * collisionMonitorInfo.keyframedInterpolationRatio) )* DT; // this version just drops the percentage back to the limit collisionMonitorInfo.timeFromStartCollision = startInterpolatingBack + limitCollisionInterpolation * (endInterpolatingBack - startInterpolatingBack); collisionMonitorInfo.keyframedInterpolationRatio = limitCollisionInterpolation; } } // we add to the collision stream either when it is within range or if (isWithinCollisionRange || addToMonitor) { // add to the monitor stream if (isAlreadyInMonitor) { // if there is existent collision already with this remote body, then build the minimum var existingMonitorInfo = _monitorCollisionInfo[remoteBodyInfo.rigidBodyId]; #if MRE_PHYSICS_DEBUG Debug.Log(" START merge collision info: " + remoteBodyInfo.rigidBodyId.ToString() + " r:" + existingMonitorInfo.keyframedInterpolationRatio + " d:" + existingMonitorInfo.relativeDistance); #endif existingMonitorInfo.timeFromStartCollision = Math.Min(existingMonitorInfo.timeFromStartCollision, collisionMonitorInfo.timeFromStartCollision); existingMonitorInfo.relativeDistance = Math.Min(existingMonitorInfo.relativeDistance, collisionMonitorInfo.relativeDistance); existingMonitorInfo.keyframedInterpolationRatio = Math.Min(existingMonitorInfo.keyframedInterpolationRatio, collisionMonitorInfo.keyframedInterpolationRatio); collisionMonitorInfo = existingMonitorInfo; #if MRE_PHYSICS_DEBUG // <todo> check why is this working sometimes weired. _monitorCollisionInfo[remoteBodyInfo.rigidBodyId] = collisionMonitorInfo; existingMonitorInfo = _monitorCollisionInfo[remoteBodyInfo.rigidBodyId]; Debug.Log(" merge collision info: " + remoteBodyInfo.rigidBodyId.ToString() + " r:" + existingMonitorInfo.keyframedInterpolationRatio + " d:" + existingMonitorInfo.relativeDistance); #endif } else { _monitorCollisionInfo.Add(remoteBodyInfo.rigidBodyId, collisionMonitorInfo); #if MRE_PHYSICS_DEBUG //if (remoteBodyInfo.isKeyframed) { var existingMonitorInfo = _monitorCollisionInfo[remoteBodyInfo.rigidBodyId]; Debug.Log(" Add collision info: " + remoteBodyInfo.rigidBodyId.ToString() + " r:" + existingMonitorInfo.keyframedInterpolationRatio + " d:" + existingMonitorInfo.relativeDistance); } #endif } // this is a new collision if (collisionMonitorInfo.timeFromStartCollision < timeInfo.halfDT) { // switch back to dynamic remoteBody.isKinematic = false; remoteBody.transform.position = remoteBodyInfo.startPosition; remoteBody.transform.rotation = remoteBodyInfo.startOrientation; remoteBody.velocity = remoteBodyInfo.linearVelocity; remoteBody.angularVelocity = remoteBodyInfo.angularVelocity; #if MRE_PHYSICS_DEBUG //if (remoteBodyInfo.isKeyframed) { Debug.Log(" remote body velocity SWITCH to collision: " + remoteBody.velocity.ToString() + " start position:" + remoteBody.transform.position.ToString() + " linVel:" + remoteBody.velocity + " angVel:" + remoteBody.angularVelocity); } #endif } else { #if MRE_PHYSICS_DEBUG // this is a previous collision so do nothing just leave dynamic Debug.Log(" remote body velocity stay in collision: " + remoteBody.velocity.ToString() + " start position:" + remoteBody.transform.position.ToString() + " relative dist:" + collisionMonitorInfo.relativeDistance + " Ratio:" + collisionMonitorInfo.keyframedInterpolationRatio); #endif } } } } } // end for each } // end of PredictAllRemoteBodiesWithOwnedBodies
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 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); }