public static NavGrid ClosestNavGrid(Vector3 position) { NavGrid[] navgrids = GameObject.FindObjectsOfType <NavGrid>(); float lowestDistance = float.MaxValue; NavGrid closestGrid = null; foreach (NavGrid navgrid in navgrids) { float distance = Vector3.Distance(navgrid.transform.position, position); if (distance < lowestDistance) { lowestDistance = distance; closestGrid = navgrid; } } return(closestGrid); }
//The method used by the pathfinding thread, calls back with the finished path when ready if no other threads are started first private void PathFindingThread(uint activeThreadID, LocationData locationData, Vector2Int targetNode, Action <Path> CALLBACK) { NavGrid navgrid = locationData.navgrid; bool foundPath = false; // A* PATHFINDING IMPLEMENTATION List <PathingNode> openNodes = new List <PathingNode>(); HashSet <PathingNode> closedNodes = new HashSet <PathingNode>(); PathingNode selectedNode = null; openNodes.Add(new PathingNode(locationData.coordinates, null, 0, CalculateHCost(locationData.coordinates, targetNode))); while (openNodes.Count > 0) { selectedNode = SelectLowestCostNode(openNodes); openNodes.Remove(selectedNode); closedNodes.Add(selectedNode); if (selectedNode.coordinate == targetNode) { foundPath = true; break; } //Check each surrounding node for (int i = 0; i < 4; i++) { //Setup direction NavGrid.Direction direction = NavGrid.Direction.NORTH; Vector2Int offset = new Vector2Int(); switch (i) { case 0: direction = NavGrid.Direction.NORTH; offset = new Vector2Int(0, 1); break; case 1: direction = NavGrid.Direction.EAST; offset = new Vector2Int(1, 0); break; case 2: direction = NavGrid.Direction.SOUTH; offset = new Vector2Int(0, -1); break; case 3: direction = NavGrid.Direction.WEST; offset = new Vector2Int(-1, 0); break; } //Is there is no wall if (!navgrid.HasWall(direction, selectedNode.coordinate)) { Vector2Int newCoordinate = selectedNode.coordinate + offset; PathingNode newPathNode = new PathingNode(newCoordinate, selectedNode); //Make sure it isn't in the closed list and Check for pathability if (!closedNodes.Contains(newPathNode) && navgrid.IsPathable(newCoordinate)) { //Compute score newPathNode.gCost = selectedNode.gCost; newPathNode.hCost = CalculateHCost(newCoordinate, targetNode); //Add to open list if its not already in it if (!openNodes.Contains(newPathNode)) { openNodes.Add(newPathNode); } //If it is, modify it if the new version has a lower gcost else { int index = openNodes.IndexOf(newPathNode); if (newPathNode.gCost < openNodes[index].gCost) { openNodes.Remove(newPathNode); openNodes.Add(newPathNode); } } } } } } if (!foundPath) { return; } //Construct and send the path Path path = ConstructPath(selectedNode); if (this.activeThreadID == activeThreadID) { CALLBACK(path); } }