private static int GetDistance(TANode n1, TANode n2)
{
int dstX = Mathf.Abs(n1.gridX - n2.gridX);
int dstY = Mathf.Abs(n1.gridY - n2.gridY);
if (dstX > dstY)
{
return(14 * dstY + 10 * (dstX - dstY));
}
return(14 * dstX + 10 * (dstY - dstX));
}
// will switch the array
private TANode[] GUpdateArray(TANode[] arr, int newSize, TANode nodeAdded)
{
TANode[] tmp = arr;
arr = new TANode[newSize];
for (int i = 0; i < tmp.Length; i++)
{
arr[i] = tmp[i];
}
arr[newSize - 1] = nodeAdded;
return(arr);
}
// gets closest node to the targets position
public TANode NodeFromWorldPoint(Vector3 position)
{
TANode node = new TANode();
Thread nThread = new Thread(() => { node = GetNearestNode(position); });
nThread.Start();
while (nThread.IsAlive)
{
// wait for it till it stops
}
return(node);
}
private static List <TANode> RetracePath(TANode startNode, TANode endNode)
{
List <TANode> path = new List <TANode>();
TANode currentNode = endNode;
while (currentNode != startNode)
{
path.Add(currentNode);
currentNode = currentNode.parent;
}
path.Reverse();
return(path);
}
// finds the path to the target
private static List <TANode> FindPath(Vector2 seeker, Vector2 target, TANode startNode, TANode endNode)
{
// setting it up the closed and open
List <TANode> open = new List <TANode>();
HashSet <TANode> closed = new HashSet <TANode>();
open.Add(startNode);
while (open.Count > 0)
{
TANode node = open[0];
for (int i = 1; i < open.Count; i++)
{
if (open[i].fCost < node.fCost || open[i].fCost == node.fCost)
{
if (open[i].hCost < node.hCost)
{
node = open[i];
}
}
}
open.Remove(node);
closed.Add(node);
if (node == endNode)
{
return(RetracePath(startNode, endNode));
}
foreach (TANode neighbor in node.connections)
{
if (neighbor.ignore || closed.Contains(neighbor))
{
continue;
}
int newCostToNeighbour = node.gCost + GetDistance(node, neighbor);
if (newCostToNeighbour < neighbor.gCost || !open.Contains(neighbor))
{
neighbor.gCost = newCostToNeighbour;
neighbor.hCost = GetDistance(neighbor, endNode);
neighbor.parent = node;
if (!open.Contains(neighbor))
{
open.Add(neighbor);
}
}
}
}
return(null);
}
// Creates the grid
private TANode[,] CreateGrid(Vector3 position)
{
TANode[,] grid = new TANode[gridSize.x, gridSize.y];
Vector3 worldBottomLeft = position - Vector3.right * gridWorldSize.x / 2 - Vector3.forward * gridWorldSize.y / 2;
for (int x = 0; x < gridSize.x; x++)
{
for (int y = 0; y < gridSize.y; y++)
{
Vector3 worldPoint = worldBottomLeft + Vector3.right * (x * nodeDiameter + nodeRadius) + Vector3.forward * (y * nodeDiameter + nodeRadius);
//bool ignore = !(Physics.CheckSphere(worldPoint, nodeRadius, ignoreLayer));
TANode node = new TANode();
node.position = worldPoint;
//node.ignore = ignore;
grid[x, y] = node;
}
}
return(grid);
}
// use to the the node
private TANode SetConnection(TANode[,] grid, int x, int y)
{
int cx, cy, counter = 0;
// set node to be eqaul to nodes[x,y]
TANode node = grid[x, y];
node.gridX = x;
node.gridY = y;
// setting up node connections variable
if (node.connections == null)
{
node.connections = new TANode[1];
}
// checking x
// checking x above
cx = x + 1;
if (cx >= 0 && cx < grid.GetLength(0))
{
node.connections = GUpdateArray(node.connections, ++counter, grid[cx, y]);
}
// checking x below
cx = x - 1;
if (cx >= 0 && cx < grid.GetLength(0))
{
node.connections = GUpdateArray(node.connections, ++counter, grid[cx, y]);
}
// checking y
cy = y + 1;
if (cy >= 0 && cy < grid.GetLength(1))
{
node.connections = GUpdateArray(node.connections, ++counter, grid[x, cy]);
}
// checking z below
cy = y - 1;
if (cy >= 0 && cy < grid.GetLength(1))
{
node.connections = GUpdateArray(node.connections, ++counter, grid[x, cy]);
}
return(node);
}
// returns a path of the target
private static void GetTargetPath(Vector2 seeker, Vector2 target, TANode startNode, TANode endNode, out List <TANode> path)
{
path = FindPath(seeker, target, startNode, endNode);
}