Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
using System;
using System.Collections.Generic;
using UnityEngine;
[Serializable]
public class PiecewiseLinearIntepolation {
/// <summary>
/// How long to wait before starting interpolation
/// </summary>
public static float interpolationDelay = 0.07f;
public static float extrapolationMax = 0.025f;
/// <summary>
/// Each 20 samples, get a mean.
/// </summary>
private const int connectionSamplesMax = 20;
/// <summary>
/// All interpolation entries currently
/// </summary>
private Queue<InterpEntry> interpolationQueue = new Queue<InterpEntry>();
/// <summary>
/// This is the current time relative to the current interpolation piece.
/// </summary>
private float currentTime = -1f;
/// <summary>
/// The latest position stored
/// </summary>
private Vector3 latestPosition;
/// <summary>
/// The latest rotation stored
/// </summary>
private Quaternion latestRotation;
private float latestStamp;
private float oldestReceivedTime = 0f;
/// <summary>
/// Enqueues a new interpolation entry.
/// This is where the client receives data from the server.
/// </summary>
/// <param name="timeReceived"></param>
/// <param name="pos"></param>
/// <param name="velocity"></param>
public void Enqueue(float timestamp, Vector3 pos, Quaternion rot) {
interpolationQueue.Enqueue(new InterpEntry(timestamp, Time.time, pos, rot));
latestPosition = pos;
latestRotation = rot;
latestStamp = timestamp;
lastReceived = Time.time;
}
/// <summary>
/// Interpolates between all positions in the queue.
/// </summary>
/// <param name="dt"></param>
/// <returns></returns>
public void Interp(float dt, Transform target) {
//Store oldest
if (interpolationQueue.Count > 0) {
oldestReceivedTime = interpolationQueue.Peek().timeReceived;
}
//Delay the interpolation here, return oldest pos.
if (Time.time - oldestReceivedTime < interpolationDelay) /* + connectionMean)*/ {
//Reset current time
currentTime = -1f;
return;
}
//If we have elements, we need to start the interpolation
if (interpolationQueue.Count > 0) {
//This is E(n-1)
InterpEntry prev = null;
//If we don't have a time set, set the time to the oldest interp entry
//The oldest entry is the first in the queue.
if (currentTime < 0f) {
prev = interpolationQueue.Peek();
currentTime = prev.timestamp - interpolationDelay;
} else {
//If we had time, we need to move forward by adding the delta time
//From the last frame.
currentTime += dt;
}
/*
* Here we know where we are.
* The time is:
*
* T0 T0+dt <- currentTime
* |-------------|========
*
* where T0 is the oldest entry in the queue.
* Now we need to find the current two entries
* that we need to interpolate to.
* This is done by finding the first entry where
* T0+dt is smaller the the timestamp.
*
* Entry(n-1) T0+dt Entry(n)
* |-----------------=|=--------|-----------
*
* Then we just need to interpolate pos to from E(n-1) to E(n).
*/
//Boundaries More than last
if (currentTime > latestStamp) {
//Set to latest position
target.position = Vector3.LerpUnclamped(target.position, latestPosition, currentTime / latestStamp);
target.rotation = Quaternion.LerpUnclamped(target.rotation, latestRotation, currentTime / latestStamp);
//Dump queue and clear time
currentTime = -1;
interpolationQueue.Clear();
return;
}
//Current position is towards the first element of the queue
//This means, since the first frame, the interpolation is still towards the first entry
if (prev != null && currentTime < prev.timestamp) {
target.position = Vector3.Lerp(target.position, prev.position, currentTime / prev.timestamp);
target.rotation = Quaternion.Lerp(target.rotation, prev.rotation, currentTime / prev.timestamp);
return;
}
//This is E(n)
InterpEntry next = interpolationQueue.Dequeue();
//Find the first entry where the current time doesn't pass it.
while (currentTime > next.timestamp && interpolationQueue.Count > 0) {
//Debug.Log("Current time > next.timeStamp");
prev = next;
next = interpolationQueue.Dequeue();
}
//Current position is towards the first element of the queue
//This means, since the first frame, the interpolation is still towards the first entry
//This is probably impossible
if (prev == null) {
target.position = Vector3.Lerp(target.position, next.position, currentTime / next.timestamp);
target.rotation = Quaternion.Lerp(target.rotation, next.rotation, currentTime / next.timestamp);
return;
}
//Calculate the time between points
float timeBetween = next.timestamp - prev.timestamp;
//Calculate T0+dt since prev instead of since begining
float timeSincePrev = currentTime - prev.timestamp;
//If there is no time, return next
//This is probably impossible
if (timeBetween == 0) {
target.position = Vector3.LerpUnclamped(target.position, next.position, currentTime / next.timestamp);
target.rotation = Quaternion.LerpUnclamped(target.rotation, next.rotation, currentTime / next.timestamp);
currentTime = -1;
return;
}
//Return the interpolation of T0+dt in this piece
target.position = Vector3.LerpUnclamped(prev.position, next.position, (timeSincePrev / timeBetween));
target.rotation = Quaternion.Lerp(prev.rotation, next.rotation, (timeSincePrev / timeBetween));
} else {
//We reach here each time we are within the oldest stamp delay but we cleared the list
//Extrapolation
if (currentTime - latestStamp < extrapolationMax && latestStamp > 0) {
currentTime += dt;
target.position = Vector3.LerpUnclamped(target.position, latestPosition, currentTime / latestStamp);
} else {
//Reset current time
currentTime = -1f;
}
}
}
private class InterpEntry {
public float timestamp;
public float timeReceived;
public Vector3 position;
public Quaternion rotation;
public InterpEntry(float timestamp, float timeReceived, Vector3 pos, Quaternion rot) {
this.timestamp = timestamp;
this.timeReceived = timeReceived;
position = pos;
rotation = rot;
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment