public TunablePlan(EstimatedUrgentWorkflow workflow) : base(workflow) { roots = workflow.Tasks.OrderBy(t => t.DepthLevel).Select(t => t.Id).ToArray(); leaves = roots.Reverse().ToArray(); baseValues = Estimations.ToDictionary(e => e.First().Id, e => e.Count()); optionsIdxs = baseValues.ToDictionary(p => p.Key, p => 0); maxIndex = baseValues.Values.Max(); }
/// <summary> /// Method extrapolating the position and rotation every fixed update based on rigidbody state from latest snapshots /// </summary> protected IEnumerator ExtrapolateSnapshots() { var previousAppliedVelocity = Vector3.zero; while (IsInitialized) { if (newestSnapshot.Timestamp == DateTime.MinValue) { yield return(new WaitForEndOfFrame()); continue; } var timeAfterNewestSnapshot = (float)(DateTime.UtcNow - newestSnapshot.Timestamp).TotalMilliseconds / 1000.0f; //Apply extrapolation if there are at least two snapshots and extrapolate no longer than 100ms if (previousSnapshot.Timestamp == DateTime.MinValue || timeAfterNewestSnapshot > ExtrapolationLimit) { CachedRigidbody.position = newestSnapshot.LocalPosition + transform.parent.position; CachedRigidbody.rotation = newestSnapshot.Rotation; yield return(new WaitForEndOfFrame()); continue; } //Convert time to seconds var timeBetweenSnapshots = (float)(newestSnapshot.Timestamp - previousSnapshot.Timestamp).TotalMilliseconds / 1000.0f; var t = (timeAfterNewestSnapshot + timeBetweenSnapshots) / timeBetweenSnapshots; switch (SimulationType) { case MockingSimulationType.ExtrapolateVelocities: // Limit the extrapolation of the rotation var angularVelocity = Estimations.SphericalInterpolation(previousSnapshot.AngularVelocity, newestSnapshot.AngularVelocity, t); CachedRigidbody.rotation = newestSnapshot.Rotation * Quaternion.Euler( angularVelocity * (Mathf.Rad2Deg * timeAfterNewestSnapshot)); var velocity = Estimations.LinearInterpolation(previousSnapshot.Velocity, newestSnapshot.Velocity, t); //Apply velocity only if it has the same direction as the previous one if (Vector3.Dot(velocity, previousAppliedVelocity) > 0) { var extrapolatedPosition = newestSnapshot.LocalPosition + velocity * timeAfterNewestSnapshot; CachedRigidbody.position = extrapolatedPosition + transform.parent.position; } else { CachedRigidbody.position = newestSnapshot.LocalPosition + transform.parent.position; } previousAppliedVelocity = velocity; break; } yield return(new WaitForEndOfFrame()); } }