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