/// <summary>
/// Inserts in the circulare buffer to keep it ordered
/// by timestamp.
/// </summary>
/// <returns>the index where the data was inserted timestamp.</returns>
private int InsertByTimestamp(TrackingData data)
{
System.Type entryType = data.GetType();
System.Type exitType;
trackingDataBuffer.PushFront(data);
int index = 0;
for (int i = 1; i < trackingDataBuffer.Size; i++)
{
index = i - 1;
if (trackingDataBuffer[i - 1].timestamp < trackingDataBuffer[i].timestamp)
{
// Invert
TrackingData tempData = trackingDataBuffer[i];
trackingDataBuffer[i] = trackingDataBuffer[i - 1];
trackingDataBuffer[i - 1] = tempData;
}
else
{
exitType = trackingDataBuffer[i - 1].GetType();
if (entryType != exitType)
{
Debug.LogWarning("A. Entrype: " + entryType.ToString() + " ExitType: " + exitType.ToString());
}
return(index);
}
}
exitType = trackingDataBuffer[index].GetType();
if (entryType != exitType)
{
Debug.LogWarning("B. Entrype: " + entryType.ToString() + " ExitType: " + exitType.ToString() + " Index: " + index.ToString());
}
return(index);
}
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));
}
