public void CalculatePaths(object userData) { while (run) { if (navQueue.Count > 0) { Node2D nextAgent = navQueue.Keys.First(); NavQueueItem queueItem = navQueue[nextAgent]; Vector2[] points; if (LastAgent != null && IsInstanceValid(LastAgent) && LastTarget.Round() == queueItem.Target.Round() && LastAgent.GlobalPosition.DistanceTo(nextAgent.GlobalPosition) < 256) { points = LastPath; } else { GD.Print("Calculating."); points = GetSimplePath(nextAgent.GlobalPosition, queueItem.Target); } var newPath = new Godot.Collections.Array <Vector2>(); foreach (Vector2 point in points) { newPath.Add(point); } navQueue.Remove(nextAgent); LastAgent = nextAgent; LastTarget = queueItem.Target; LastPath = points; queueItem.Callback(newPath); } } pathThread.CallDeferred("WaitToFinish"); }