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());
}
}
