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