/// <summary> /// Interpolates this transform to the other `transform` by `weight`. /// </summary> /// <param name="transform">The other transform.</param> /// <param name="weight">A value on the range of 0.0 to 1.0, representing the amount of interpolation.</param> /// <returns>The interpolated transform.</returns> public Transform InterpolateWith(Transform transform, real_t weight) { /* not sure if very "efficient" but good enough? */ Vector3 sourceScale = basis.Scale; Quat sourceRotation = basis.RotationQuat(); Vector3 sourceLocation = origin; Vector3 destinationScale = transform.basis.Scale; Quat destinationRotation = transform.basis.RotationQuat(); Vector3 destinationLocation = transform.origin; var interpolated = new Transform(); interpolated.basis.SetQuatScale(sourceRotation.Slerp(destinationRotation, weight).Normalized(), sourceScale.Lerp(destinationScale, weight)); interpolated.origin = sourceLocation.Lerp(destinationLocation, weight); return(interpolated); }
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); }