void UpdateOrientation(double delta)
{
if (receivedOrientTime < BufferTime || orientBuffer.Count < 2)
{
setQ = false;
return;
}
setQ = true;
var dataA = orientBuffer[0];
var dataB = orientBuffer[1];
var lerpTime = SeqDiff(dataB.Tick, dataA.Tick) / 1000.0;
if (lerpTime == 0)
{
currentQuat = dataB.Orient;
}
else
{
var t = quatTimer / lerpTime;
currentQuat = Quaternion.Slerp(dataA.Orient, dataB.Orient, (float)t);
}
quatTimer += delta;
if (quatTimer > lerpTime)
{
receivedOrientTime -= (int)(lerpTime * 1000);
orientBuffer.Dequeue();
quatTimer -= lerpTime;
}
if (float.IsNaN(currentQuat.X))
{
throw new Exception("NaN orientation");
}
}
void UpdatePosition(double delta)
{
if (receivedPosTime < BufferTime || posBuffer.Count < 2)
{
setV = false;
return;
}
setV = true;
var dataA = posBuffer[0];
var dataB = posBuffer[1];
var lerpTime = SeqDiff(dataB.Tick, dataA.Tick) / 1000.0;
if (lerpTime == 0)
{
currentPos = dataB.Pos;
}
else
{
var t = posTimer / lerpTime;
currentPos = Vector3.Lerp(dataA.Pos, dataB.Pos, (float)t);
posTimer += delta;
}
if (posTimer > lerpTime)
{
receivedPosTime -= (int)(lerpTime * 1000);
posBuffer.Dequeue();
posTimer -= lerpTime;
}
if (float.IsNaN(currentPos.X))
{
throw new Exception("NaN position");
}
}
void UpdateOrientation(TimeSpan delta)
{
if (receivedOrientTime < BUFFER_MS || orientBuffer.Count < 2)
{
setQ = false;
return;
}
setQ = true;
var dataA = orientBuffer[0];
var dataB = orientBuffer[1];
var lerpTime = TimeSpan.FromMilliseconds(SeqDiff(dataB.Tick, dataA.Tick));
var t = quatTimer / lerpTime;
currentQuat = Quaternion.Slerp(dataA.Orient, dataB.Orient, (float)t);
quatTimer += delta;
if (quatTimer > lerpTime)
{
receivedOrientTime -= (int)lerpTime.TotalMilliseconds;
orientBuffer.Dequeue();
quatTimer -= lerpTime;
}
}
void UpdatePosition(TimeSpan delta)
{
if (receivedPosTime < BUFFER_MS || posBuffer.Count < 2)
{
setV = false;
return;
}
setV = true;
var dataA = posBuffer[0];
var dataB = posBuffer[1];
var lerpTime = TimeSpan.FromMilliseconds(SeqDiff(dataB.Tick, dataA.Tick));
var t = posTimer / lerpTime;
currentPos = Vector3.Lerp(dataA.Pos, dataB.Pos, (float)t);
posTimer += delta;
if (posTimer > lerpTime)
{
receivedPosTime -= lerpTime.TotalMilliseconds;
posBuffer.Dequeue();
posTimer -= lerpTime;
}
}
