public void CircularBufferTestMixedUnderwrite()
{
var buffer = new CircularBuffer <long>(5);
buffer.PushFront(0);
buffer.PushFront(1);
buffer.PushBack(-1);
Assert.AreEqual(1, buffer.Front());
Assert.AreEqual(-1, buffer.Back());
buffer.PushFront(2);
Assert.AreEqual(2, buffer.Front());
Assert.AreEqual(-1, buffer.Back());
buffer.PopBack();
Assert.AreEqual(0, buffer.Back());
Assert.AreEqual(2, buffer.Front());
buffer.PushBack(-2);
Assert.AreEqual(2, buffer.Front());
Assert.AreEqual(-2, buffer.Back());
Assert.AreEqual(-2, buffer[0]);
Assert.AreEqual(0, buffer[1]);
Assert.AreEqual(1, buffer[2]);
Assert.AreEqual(2, buffer[3]);
Assert.ExceptionExpected(typeof(IndexOutOfRangeException), () => { var test = buffer[4]; });
Assert.AreEqual(buffer.Count, 4);
Assert.AreEqual(buffer.Capacity, 5);
}
public void CircularBufferTestMixedOverwrite()
{
var buffer = new CircularBuffer <long>(3);
buffer.PushFront(0);
buffer.PushFront(1);
buffer.PushBack(-1);
Assert.AreEqual(1, buffer.Front());
Assert.AreEqual(-1, buffer.Back());
buffer.PushFront(2);
Assert.AreEqual(2, buffer.Front());
Assert.AreEqual(0, buffer.Back());
buffer.PushBack(-2);
Assert.AreEqual(1, buffer.Front());
buffer.PushFront(3);
Assert.AreEqual(0, buffer.Back());
buffer.PushBack(-3);
Assert.AreEqual(-3, buffer[0]);
Assert.AreEqual(0, buffer[1]);
Assert.AreEqual(1, buffer[2]);
Assert.AreEqual(1, buffer.Front());
Assert.AreEqual(-3, buffer.Back());
Assert.AreEqual(buffer.Count, 3);
Assert.AreEqual(buffer.Capacity, 3);
}
public void CircularBufferTestFrontUnderwrite()
{
var buffer = new CircularBuffer <long>(5);
for (int i = 0; i < 4; i++)
{
buffer.PushFront(i);
}
buffer.PopFront();
buffer.PushFront(4);
Assert.AreEqual(4, buffer.Front());
Assert.AreEqual(0, buffer.Back());
buffer.PopFront();
buffer.PushFront(5);
Assert.AreEqual(0, buffer[0]);
Assert.AreEqual(1, buffer[1]);
Assert.AreEqual(2, buffer[2]);
Assert.AreEqual(5, buffer[3]);
Assert.ExceptionExpected(typeof(IndexOutOfRangeException), () => { var test = buffer[4]; });
Assert.AreEqual(5, buffer.Front());
Assert.AreEqual(0, buffer.Back());
Assert.AreEqual(buffer.Count, 4);
Assert.AreEqual(buffer.Capacity, 5);
}
/// <summary>
/// Gets element at given index from top of history stack, and adds all skipped elements to reverse stack
/// </summary>
/// <remarks>If skipElements is bigger, then amount of elements in history, returns null, clears history and pushes all to reverse stack</remarks>
/// <param name="skipElements">Amount of elements to skip from history stack</param>
/// <returns>>Found element or null</returns>
public IHistoryAction TravelBack(int skipElements)
{
if (skipElements <= 0)
{
throw new ArgumentOutOfRangeException(nameof(skipElements), "argument cannot be smaller or equal to 0");
}
if (_historyActions.Count - skipElements <= 0)
{
foreach (var historyAction in _historyActions)
{
_reverseActions.PushFront(historyAction);
}
var result = _historyActions.Back();
_historyActions.Clear();
return(result);
}
// iterate all but one elements to skip. Last element is handled exclusivly
for (int i = 0; i < skipElements - 1; i++)
{
PopHistory();
}
return(PopHistory());
}
public void CircularBuffer_BackOfBufferOverflowByOne_CorrectItem()
{
var buffer = new CircularBuffer <int>(5, new[] { 0, 1, 2, 3, 4 });
buffer.PushBack(42);
Assert.That(buffer.ToArray(), Is.EqualTo(new[] { 1, 2, 3, 4, 42 }));
Assert.That(buffer.Back(), Is.EqualTo(42));
}
public void CircularBuffer_BackOfBufferOverflowByOne_CorrectItem()
{
var buffer = new CircularBuffer <int>(5, new[] { 0, 1, 2, 3, 4 });
buffer.PushBack(42);
buffer.ToArray().Should().Equal(1, 2, 3, 4, 42);
buffer.Back().Should().Be(42);
}
public void CircularBuffer_Back_EmptyBufferThrowsException()
{
var buffer = new CircularBuffer <int>(5);
Assert.That(() => buffer.Back(),
Throws.Exception.TypeOf <InvalidOperationException>().
With.Property("Message").Contains("empty buffer"));
}
public void CircularBuffer_Back_CorrectItem( )
{
var buffer = new CircularBuffer <int> (5,
new [] { 0, 1, 2, 3, 4 });
buffer.Back( )
.Should( )
.Be(4);
}
public void CircularBuffer_Back_EmptyBufferThrowsException( )
{
var buffer = new CircularBuffer <int> (5);
Action action = () => buffer.Back( );
action.Should( )
.Throw <InvalidOperationException> ( )
.WithMessage("Cannot access an empty buffer.");
}
public void CircularBuffer_WithDifferentSizeAndCapacity_BackReturnsLastArrayPosition()
{
// test to confirm this issue does not happen anymore:
// https://github.com/joaoportela/CircullarBuffer-CSharp/issues/2
var buffer = new CircularBuffer <int>(5, new[] { 0, 1, 2, 3, 4 });
buffer.PopFront(); // (make size and capacity different)
Assert.That(buffer.Back(), Is.EqualTo(4));
}
public void CircularBufferTestExceptions()
{
Assert.ExceptionExpected(typeof(ArgumentOutOfRangeException), () => { new CircularBuffer <long>(0); });
Assert.ExceptionExpected(typeof(ArgumentOutOfRangeException), () => { new CircularBuffer <long>(-5); });
var buffer = new CircularBuffer <long>(1);
Assert.ExceptionExpected(typeof(IndexOutOfRangeException), () => { buffer.Front(); });
Assert.ExceptionExpected(typeof(IndexOutOfRangeException), () => { buffer.Back(); });
Assert.ExceptionExpected(typeof(ArgumentOutOfRangeException), () => { var test = buffer[-1]; });
Assert.ExceptionExpected(typeof(IndexOutOfRangeException), () => { var test = buffer[1]; });
Assert.ExceptionExpected(typeof(ArgumentOutOfRangeException), () => { buffer[-1] = 0; });
Assert.ExceptionExpected(typeof(IndexOutOfRangeException), () => { buffer[1] = 0; });
Assert.ExceptionExpected(typeof(IndexOutOfRangeException), () => { buffer.PopBack(); });
Assert.ExceptionExpected(typeof(IndexOutOfRangeException), () => { buffer.PopFront(); });
}
public void CircularBufferTestConstructors()
{
var array = new long[5];
for (int i = 0; i < 5; i++)
{
array[i] = i + 10;
}
var buffer = new CircularBuffer <long>(15, array, 5);
Assert.AreEqual(10, buffer.Back());
Assert.AreEqual(14, buffer.Front());
Assert.AreEqual(5, buffer.Count);
Assert.AreEqual(15, buffer.Capacity);
Assert.AreEqual(false, buffer.IsEmpty);
}
public void CircularBuffer_BackOfBufferOverflowByOne_CorrectItem( )
{
var buffer = new CircularBuffer <int> (5,
new [] { 0, 1, 2, 3, 4 });
buffer.PushBack(42);
using var scope = new AssertionScope( );
buffer.ToArray( )
.Should( )
.BeEquivalentTo(new [] { 1, 2, 3, 4, 42 });
buffer.Back( )
.Should( )
.Be(42);
}
private static long HashDecks(CircularBuffer <byte> player1, CircularBuffer <byte> player2)
{
// not convinced this is good enough but it gets the right answer so....
var hash = 0L;
hash |= player1.Front() << 24;
hash |= player1.Back() << 16;
hash |= player1.Count;
hash <<= 32;
hash |= player2.Front() << 24;
hash |= player2.Back() << 16;
hash |= player2.Count;
return(hash);
}
public void CircularBufferTestBackOverwrite()
{
var buffer = new CircularBuffer <long>(3);
for (int i = 0; i < 5; i++)
{
buffer.PushBack(i);
}
Assert.AreEqual(4, buffer[0]);
Assert.AreEqual(3, buffer[1]);
Assert.AreEqual(2, buffer[2]);
Assert.AreEqual(2, buffer.Front());
Assert.AreEqual(4, buffer.Back());
Assert.AreEqual(buffer.Count, 3);
Assert.AreEqual(buffer.Capacity, 3);
}
private static void checkBuffer(CircularBuffer buffer, int capacity, int count, object startValue, object endValue)
{
var array = buffer.ToArray();
Assert.AreEqual(buffer.Capacity, capacity, "capacity must be " + capacity);
Assert.AreEqual(buffer.Count, count, "Count must be " + count);
if (count == 0)
{
Assert.IsTrue(buffer.Empty, "Empty must be true");
}
else
{
Assert.IsFalse(buffer.Empty, "Empty must be false");
}
if (count == capacity)
{
Assert.IsTrue(buffer.Full, "Full must be true");
}
else
{
Assert.IsFalse(buffer.Full, "Full must be false");
}
Assert.AreEqual(buffer.Front(), startValue, "Front must be " + startValue);
Assert.AreEqual(buffer.Back(), endValue, "Back must be " + endValue);
int value = startValue != null? (Int32)startValue: 0;
foreach (var i in buffer)
{
Assert.AreEqual(i, value, "buffer element must be " + value);
value++;
}
if (buffer.Empty)
{
Assert.IsNull(array, "ToArray must be null");
}
else
{
Assert.AreEqual(array.Length, count, "array length must be " + count);
for (int i = 0; i < array.Length; i++)
{
Assert.AreEqual(array[i], buffer[i], "array item must be equal as buffer item, index: " + i);
}
}
}
public void CircularBufferTestFrontAndBack()
{
var rand = new Random();
var buffer = new CircularBuffer <long>(3);
for (int i = 0; i < 25; i++)
{
var cur = rand.Next();
buffer.PushFront(cur);
Assert.AreEqual(cur, buffer.Front());
buffer.PopFront();
}
for (int i = 0; i < 25; i++)
{
var cur = rand.Next();
buffer.PushBack(cur);
Assert.AreEqual(cur, buffer.Back());
buffer.PopBack();
}
}
public virtual Vector3 GetPosition(double timestamp)
{
if (dataInQueue)
{
lock (dataQueue){
for (int i = 0; i < dataQueue.Size; i++)
{
TrackingData data = dataQueue.Back();
if (data.GetType() == typeof(TrackingDataIMU))
{
AddAccelerationMeasurement((TrackingDataIMU)data);
}
else if (data.GetType() == typeof(TrackingDataPosition))
{
AddPositionMeasurement((TrackingDataPosition)data);
}
dataQueue.PopBack();
}
dataInQueue = false;
}
}
int lastPositionIndex = GetLastPositionIndex();
if (lastPositionIndex == -1)
{
// Debug.LogWarning("Couldn't find any last position");
return(lastCalculatedPosition);
}
float delaySinceLastGetPosition = (float)(timestamp - lastCalculatedPositionTimestamp);
float delaySinceLastPositionMeasurement = lastPositionIndex == -1 ? maxSpeedViabilityDelay : (float)(timestamp - trackingDataBuffer[lastPositionIndex].timestamp);
float delaySinceLastMeasurement = (float)(timestamp - (trackingDataBuffer.Size > 0 ? trackingDataBuffer[0].timestamp : 0.0d));
if (delaySinceLastMeasurement > maxPredictionDelaySinceLastMeasurement || trackingDataBuffer.Size < 1)
{
return(lastCalculatedPosition);
}
int lastAccelerationIndex = GetLastAccelerationIndex();
Vector3 lastAcceleration = lastAccelerationIndex == -1 ? Vector3.zero : ((TrackingDataIMU)trackingDataBuffer[lastAccelerationIndex]).acceleration;
float delaySinceLastAccelerationMeasurement = lastAccelerationIndex == -1 ? maxAccelerationViabilityDelay : (float)(timestamp - trackingDataBuffer[lastAccelerationIndex].timestamp);
List <PositionOffset> deleteList = new List <PositionOffset>();
Vector3 currentOffset = Vector3.zero;
foreach (PositionOffset off in offsets)
{
currentOffset += off.GetOffset(timestamp);
if (off.CorrectionOver(timestamp))
{
deleteList.Add(off);
}
}
foreach (PositionOffset off in deleteList)
{
offsets.Remove(off);
}
if (currentOffset.magnitude > lerpDiscardDistance || (trackingDataBuffer[lastPositionIndex].position - lastCalculatedPosition).magnitude > lerpDiscardDistance)
{
// if (blink)
// {
offsets.Clear();
if (Blink != null)
{
Blink();
}
// }
}
// ACC
if (trackingDataBuffer[0].GetType() == typeof(TrackingDataIMU))
{
// Position calculation with Slerp in case we have been predicting with ACC for too long
Vector3 newOffset = Vector3.Slerp(trackingDataBuffer[0].speed, Vector3.zero, delaySinceLastPositionMeasurement / maxSpeedViabilityDelay) * delaySinceLastGetPosition + 0.5f * Vector3.Slerp(lastAcceleration, Vector3.zero, delaySinceLastAccelerationMeasurement / maxAccelerationViabilityDelay) * delaySinceLastGetPosition * delaySinceLastGetPosition;
lastCalculatedPosition = Vector3.Slerp(lastCalculatedPosition + newOffset, lastCalculatedPosition, delaySinceLastPositionMeasurement / accelerationOnlyTrackingDelay);
lastCalculatedPositionTimestamp = timestamp;
}
// POS
else
{
// Position calculation with Slerp in case we have been predicting with ACC for too long
Vector3 newOffset = Vector3.Slerp(trackingDataBuffer[0].speed, Vector3.zero, delaySinceLastPositionMeasurement / maxSpeedViabilityDelay) * delaySinceLastGetPosition + 0.5f * Vector3.Slerp(lastAcceleration, Vector3.zero, delaySinceLastAccelerationMeasurement / maxAccelerationViabilityDelay) * delaySinceLastGetPosition * delaySinceLastGetPosition;
lastCalculatedPosition = Vector3.Slerp(lastCalculatedPosition + newOffset, lastCalculatedPosition, delaySinceLastPositionMeasurement / accelerationOnlyTrackingDelay);
lastCalculatedPositionTimestamp = timestamp;
}
/* TRY TO FIX LONG OFFSET JUMPS, if line 110 isn't enought
* if ((trackingDataBuffer[lastPositionIndex].position - lastCalculatedPosition).magnitude > discardDistance)
* {
* ResetFilter();
* if (blink)
* {
* if (Blink != null)
* Blink();
* }
* }*/
// return lastCalculatedPosition + currentOffset;
return(oneEuro.Filter(lastCalculatedPosition + currentOffset, (float)timestamp));
}
public void CircularBuffer_Back_EmptyBufferThrowsException()
{
var buffer = new CircularBuffer <int>(5);
Assert.Throws <InvalidOperationException>(() => buffer.Back());
}
public void CircularBuffer_Back_CorrectItem()
{
var buffer = new CircularBuffer <int>(5, new[] { 0, 1, 2, 3, 4 });
Assert.That(buffer.Back(), Is.EqualTo(4));
}
// Update is called at 50 FPS
void FixedUpdate()
{
if (!m_levelBuffer.IsEmpty)
{
m_lastLevel = m_levelBuffer.Back();
}
m_levelBuffer.PushBack(m_level.normalizedLevel);
m_BPMBuffer.PushBack(BPM);
var arr = m_levelBuffer.ToArray();
Array.Copy(arr, m_currentLevels, arr.Length);
Array.Copy(arr, m_fftreals, arr.Length);
Array.Copy(arr, m_fftcomplex, arr.Length);
for (int s = 0; s < MAX_SHIFT; s++)
{
float sad = 0;
float numSamples = 0;
for (int i = 0; i < arr.Length; i++)
{
if (i + s < arr.Length)
{
sad += Mathf.Abs(arr[i] - arr[i + s]);
numSamples++;
}
}
if (numSamples > 0)
{
m_currentWeights[s] = sad / numSamples;
}
}
float alpha = m_correlationLerp;
for (int i = 0; i < m_weights.Length; i++)
{
m_weights[i] = alpha * m_weights[i] + (1.0f - alpha) * m_currentWeights[i];
}
for (int i = 0; i < currentPeaks.Length; i++)
{
currentPeaks[i] = 0;
currentPeakWeights[i] = 0;
}
GetPeaks(m_weights, 15);
GetHarmonicFromPeaks(ref currentPeaks, ref harmonicPeaks);
GetMaxBPM();
float thisFreq = BPM / 60f;
if (Input.GetKeyDown(KeyCode.Space))
{
phaseOffset = Time.time;
}
frameNum++;
}
