IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
Star_Node startNode = grid.NodeFromWorldPoint(startPos);
Star_Node targetNode = grid.NodeFromWorldPoint(targetPos);
if (startNode.walkable && targetNode.walkable)
{
Heap <Star_Node> openSet = new Heap <Star_Node>(grid.MaxSize);
HashSet <Star_Node> closedSet = new HashSet <Star_Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Star_Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
pathSuccess = true;
break;
}
foreach (Star_Node neighbour in grid.GetNeighbours(currentNode))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, targetNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
}
yield return(null);
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
}
requestManager.FinishedProcessingPath(waypoints, pathSuccess);
}
int GetDistance(Star_Node nodeA, Star_Node nodeB)
{
int dstX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
int dstY = Mathf.Abs(nodeA.gridY - nodeB.gridY);
if (dstX > dstY)
{
return(14 * dstY + 10 * (dstX - dstY));
}
return(14 * dstX + 10 * (dstY - dstX));
}
Vector3[] RetracePath(Star_Node startNode, Star_Node endNode)
{
List <Star_Node> path = new List <Star_Node>();
Star_Node currentNode = endNode;
while (currentNode != startNode)
{
path.Add(currentNode);
currentNode = currentNode.parent;
}
path.Add(startNode);
Vector3[] waypoints = SimplifyPath(path);
Array.Reverse(waypoints);
return(waypoints);
}
void CreateGrid()
{
grid = new Star_Node[gridSizeX, gridSizeY];
//Vector3 worldBottomLeft = transform.position - Vector3.right * gridWorldSize.x / 2 - Vector3.up * gridWorldSize.y / 2;
Vector3 worldBottomLeft = new Vector3(lx, ly, -1f);
for (int x = 0; x < gridSizeX; x++)
{
for (int y = 0; y < gridSizeY; y++)
{
Vector3 worldPoint = worldBottomLeft + Vector3.right * (x * nodeDiameter + nodeRadius) + Vector3.up * (y * nodeDiameter + nodeRadius);
bool walkable = !(Physics2D.OverlapCircle(worldPoint, nodeRadius, unwalkableMask));
grid[x, y] = new Star_Node(walkable, worldPoint, x, y);
}
}
}
public List <Star_Node> GetNeighbours(Star_Node node)
{
List <Star_Node> neighbors = new List <Star_Node>();
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
if (x == 0 && y == 0)
{
continue;
}
int checkX = node.gridX + x;
int checkY = node.gridY + y;
if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY)
{
neighbors.Add(grid[checkX, checkY]);
}
}
}
return(neighbors);
}