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; } }