private Node findPath(int initialPosition, Vector3 target, bool gatherTowers) { Node solutionPath = null; Vector3 currentDirection = target - _swarm._positions [initialPosition]; float distance = currentDirection.magnitude; int width = World.getInstance().getWorldWidth(); Node root = new Node(); root._i = initialPosition / width; root._j = initialPosition % width; root._prev = null; FastPriorityQueue <Node> queue = new FastPriorityQueue <Node> (_swarm.getMaxPossiblePositions()); queue.Enqueue(root, 0); bool solution = false; bool[] evaluated = new bool[width * width]; evaluated [initialPosition] = true; float minDistanceToTarget = Constants.VERTEX_SPACING + (Constants.VERTEX_SPACING * 0.5f); while (!solution && queue.Count > 0) { Node next = queue.Dequeue(); int currentIndex = next._i * width + next._j; Vector3 currentPosition = _swarm._positions [currentIndex]; float currentDistanceToTarget = (target - currentPosition).magnitude; if (currentDistanceToTarget <= minDistanceToTarget) { solutionPath = next; solution = true; break; } int istart = Mathf.Max(next._i - 1, 0); int jstart = Mathf.Max(next._j - 1, 0); int iend = Mathf.Min(next._i + 2, width); int jend = Mathf.Min(next._j + 2, width); for (int i = istart; i < iend; i++) { for (int j = jstart; j < jend; j++) { int tempIndex = i * width + j; if (evaluated [tempIndex]) { continue; } evaluated [tempIndex] = true; if (_grid [tempIndex] == WALKABLE_CELL) { Vector3 possiblePos = _swarm._positions [tempIndex]; Vector3 direction = target - possiblePos; int nodeIStart = Mathf.Max(i - 1, 0); int nodeJStart = Mathf.Max(j - 1, 0); int nodeIEnd = Mathf.Min(i + 2, width); int nodeJEnd = Mathf.Min(j + 2, width); int badNeighbours = 0; for (int k = nodeIStart; k < nodeIEnd; k++) { for (int z = nodeJStart; z < nodeJEnd; z++) { byte neighbourValue = _grid [k * width + z]; if (neighbourValue == OBSTACLE_CELL) { badNeighbours++; } else if (gatherTowers && neighbourValue != WALKABLE_CELL && !_towerPositions.ContainsKey(neighbourValue)) { _towerPositions.Add(neighbourValue, k * width + z); } } } Node newNode = new Node(); newNode._i = i; newNode._j = j; newNode._prev = next; queue.Enqueue(newNode, direction.magnitude + (badNeighbours * 10)); } } } } return(solutionPath); }