public void InterpolateTo(Position position, bool keepHeading)
{
if (PhysicsObj == null)
{
return;
}
var dest = PositionQueue.Count > 0 && PositionQueue.Last().Type == InterpolationNodeType.PositionType ?
PositionQueue.Last().Position : PhysicsObj.Position;
var dist = dest.Distance(position);
if (PhysicsObj.GetAutonomyBlipDistance() >= dist)
{
if (PhysicsObj.Position.Distance(position) > 0.05f)
{
while (PositionQueue.Count > 0)
{
var lastNode = PositionQueue.Last();
if (lastNode.Type != InterpolationNodeType.PositionType || lastNode.Position.Distance(position) >= 0.05f)
{
break;
}
PositionQueue.DequeueLast();
}
while (PositionQueue.Count >= 20)
{
PositionQueue.Dequeue();
}
var interpolationNode = new InterpolationNode(InterpolationNodeType.PositionType, position);
if (keepHeading)
{
interpolationNode.Position.Frame.set_heading(PhysicsObj.get_heading());
}
PositionQueue.Enqueue(interpolationNode);
}
else
{
if (!keepHeading)
{
PhysicsObj.set_heading(position.Frame.get_heading(), true);
}
StopInterpolating();
}
}
else
{
var interpolationNode = new InterpolationNode(InterpolationNodeType.PositionType, position);
if (keepHeading)
{
interpolationNode.Position.Frame.set_heading(PhysicsObj.get_heading());
}
PositionQueue.Enqueue(interpolationNode);
NodeFailCounter = 4;
}
}