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