StateHistory.cs

StateHistory.cs

using System;
using System.Collections.Concurrent;
using UnityEngine;

public class StateHistory<T> where T : IState<T> {
	protected T[] buffer;
	public int Head { get; protected set; }
	public int Tail {
		get {
			if (Count < buffer.Length) {
				return 0;
			}

			int idx = Head - Count;
			if (idx < 0) {
				idx += Count + 1;
			}

			return idx % buffer.Length;
		}
	}

	public T this [int index] {
		get {
			if (index < 0) {
				index += buffer.Length;
			}

			return buffer[index % buffer.Length];
		}
	}

	public int Count { get; protected set; }

	public StateHistory (int capacity) {
		Head = -1;
		buffer = new T[capacity];
	}

	public void Add (T item) {
		Head = (Head + 1) % buffer.Length;
		buffer[Head] = item;
		if (Count < buffer.Length) {
			Count++;
		} else {
			Count = buffer.Length;
		}
	}

	public int FindClosestIndex (float time) {
		int closestIndex = -1;
		float minDelta = float.MaxValue;

		for (int i = Head; i >= -Tail; i--) {
			float delta = Math.Abs(this[i].Time - time);
			if (delta < minDelta) {
				minDelta = delta;
				closestIndex = i;
			}
		}

		if (closestIndex < 0) {
			closestIndex += buffer.Length;
		}
		return closestIndex % buffer.Length;
	}

	public T FindClosest (float time) {
		return buffer[FindClosestIndex(time)];
	}

	public T Interpolate (float time) {
		int closestIndex = FindClosestIndex(time);
		T closest = this[closestIndex];

		if (closestIndex == Tail && time < closest.Time) {
			return closest;
		} else if (closestIndex == Head && time > closest.Time) {
			return closest;
		}

		if (closest.Time >= time) {
			var other = this[closestIndex - 1];
			float t = Mathf.InverseLerp(other.Time, closest.Time, time);
			return other.Interpolate(closest, t);
		} else {
			var other = this[closestIndex + 1];
			float t = Mathf.InverseLerp(closest.Time, other.Time, time);
			return closest.Interpolate(other, t);
		}
	}
}