private UniformGridNode GetHighestPriorityNode(List <UniformGridNode> openList)
{
UniformGridNode highestPriorityNode = openList[0];
foreach (UniformGridNode node in openList)
{
if (node.g + node.e_distance < highestPriorityNode.g + highestPriorityNode.e_distance)
{
highestPriorityNode = node;
}
}
return(highestPriorityNode);
}
private bool NodeListContains(List <UniformGridNode> nodeList, UniformGridNode node)
{
bool contains = false;
foreach (UniformGridNode n in nodeList)
{
if (n.x == node.x && n.y == node.y && n.z == node.z)
{
contains = true;
break;
}
}
return(contains);
}
//private void Update()
//{
// for (ushort x = 0; x < maxGridSizeX; x++)
// {
// for (ushort y = 0; y < maxGridSizeY; y++)
// {
// for (ushort z = 0; z < maxGridSizeZ; z++)
// {
// // gridNodes[x, y, z].DebugNodeOverlap();
// }
// }
// }
//}
private void InitGrid()
{
gridNodes = new UniformGridNode[maxGridSizeX, maxGridSizeY, maxGridSizeZ];
for (ushort x = 0; x < maxGridSizeX; x++)
{
for (ushort y = 0; y < maxGridSizeY; y++)
{
for (ushort z = 0; z < maxGridSizeZ; z++)
{
gridNodes[x, y, z] = new UniformGridNode(x, y, z, Mathf.Infinity, cellSize / 4f, Mathf.Infinity, new GameObject());
}
}
}
}
private List <UniformGridNode> GetNeighboursNonDiagonal(UniformGridNode node)
{
List <UniformGridNode> neighbours = new List <UniformGridNode>();
// TOP
if (node.y < gridNodes.GetLength(1) - 1)
{
neighbours.Add(gridNodes[node.x, node.y + 1, node.z]);
}
// BOTTOM
if (node.y > 0)
{
neighbours.Add(gridNodes[node.x, node.y - 1, node.z]);
}
// LEFT
if (node.x > 0)
{
neighbours.Add(gridNodes[node.x - 1, node.y, node.z]);
}
// RIGHT
if (node.x < gridNodes.GetLength(0) - 1)
{
neighbours.Add(gridNodes[node.x + 1, node.y, node.z]);
}
// FORWARD
if (node.z < gridNodes.GetLength(2) - 1)
{
neighbours.Add(gridNodes[node.x, node.y, node.z + 1]);
}
// BACK
if (node.z > 0)
{
neighbours.Add(gridNodes[node.x, node.y, node.z - 1]);
}
neighbours.RemoveAll(n => !n.isTraversable);
return(neighbours);
}
public IEnumerator FindPathRoutine(Vector3 startPos, Vector3 endPos)
{
path.Clear();
ushort startX = (ushort)Mathf.Floor(startPos.x / cellSize);
ushort startY = (ushort)Mathf.Floor(startPos.y / cellSize);
ushort startZ = (ushort)Mathf.Floor(startPos.z / cellSize);
ushort endX = (ushort)Mathf.Floor(endPos.x / cellSize);
ushort endY = (ushort)Mathf.Floor(endPos.y / cellSize);
ushort endZ = (ushort)Mathf.Floor(endPos.z / cellSize);
List <UniformGridNode> openList = new List <UniformGridNode>();
List <UniformGridNode> closedList = new List <UniformGridNode>();
for (ushort x = 0; x < maxGridSizeX; x++)
{
for (ushort y = 0; y < maxGridSizeY; y++)
{
for (ushort z = 0; z < maxGridSizeZ; z++)
{
if (gridNodes[x, y, z].isTraversable)
{
gridNodes[x, y, z].ChangeLRColor(Color.grey);
gridNodes[x, y, z].g = Mathf.Infinity;
gridNodes[x, y, z].parents.Clear();
gridNodes[x, y, z].CalculateDistanceToEnd(endX, endY, endZ);
}
}
}
}
gridNodes[startX, startY, startZ].ChangeLRColor(Color.yellow);
gridNodes[endX, endY, endZ].ChangeLRColor(Color.cyan);
gridNodes[startX, startY, startZ].g = 0f;
openList.Add(gridNodes[startX, startY, startZ]);
UniformGridNode endNode = new UniformGridNode();
while (openList.Count > 0)
{
yield return(new WaitForSeconds(0.0000001f));
UniformGridNode currentNode = GetHighestPriorityNode(openList);
openList.Remove(currentNode);
closedList.Add(currentNode);
currentNode.ChangeLRColor(Color.red);
if (currentNode.x == endX && currentNode.y == endY && currentNode.z == endZ)
{
endNode = currentNode;
break;
}
//Debug.Log("Current is: " + currentNode.toString());
//Debug.LogFormat("Current g ({0}) and e_dist ({1})", currentNode.g, currentNode.e_distance);
foreach (UniformGridNode neighbour in GetNeighboursNonDiagonal(currentNode))
{
//Debug.Log("Neighbour: " + neighbour.toString());
//Debug.Log("Neighbour g: " + neighbour.g);
//Debug.Log("Neighbour e: " + neighbour.e_distance);
if (NodeListContains(openList, neighbour))
{
//Debug.Log("Open contains");
if (currentNode.g + neighbour.travelCost < neighbour.g)
{
//Debug.Log("Updated open");
neighbour.g = currentNode.g + neighbour.travelCost;
neighbour.parents.Clear();
neighbour.parents.Add(currentNode);
}
}
else if (NodeListContains(closedList, neighbour))
{
//Debug.Log("Closed contains");
if (currentNode.g + neighbour.travelCost < neighbour.g)
{
//Debug.LogFormat("Closed g ({0}) and e_dist ({1})", neighbour.g, neighbour.e_distance);
// Debug.Log("Updated closed");
neighbour.g = currentNode.g + neighbour.travelCost;
neighbour.parents.Clear();
neighbour.parents.Add(currentNode);
closedList.Remove(neighbour);
openList.Add(neighbour);
}
}
else
{
// Debug.Log("Unexplored");
neighbour.g = currentNode.g + neighbour.travelCost;
neighbour.parents.Clear();
neighbour.parents.Add(currentNode);
openList.Add(neighbour);
}
}
}
//Debug.Log("We got the path!");
// Debug.Log("here:::: " + gridNodes[0,0,0].parents.Count);
int breakCounter = 500;
while (endNode.parents.Count > 0 && breakCounter > 0)
{
yield return(new WaitForSeconds(0.000001f));
endNode.ChangeLRColor(Color.cyan);
path.Push(new Vector3(endNode.x * cellSize, endNode.y * cellSize, endNode.z * cellSize));
// Debug.Log("(" + endNode.parents[0].x + "," + endNode.parents[0].y + "," + endNode.parents[0].z + ")");
// Debug.Log("g val: " + endNode.g);
// Debug.Log("e_dist: " + endNode.e_distance);
endNode = endNode.parents[0];
breakCounter--;
}
}