IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = grid.NodeFromWorldPoint(startPos);
Node targetNode = grid.NodeFromWorldPoint(targetPos);
if (startNode.walkable && targetNode.walkable)
{
Heap <Node> openSet = new Heap <Node>(grid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
pathSuccess = true;
break;
}
foreach (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);
}
void FindPath(Vector3 startPos, Vector3 targetPos)
{
Node startNode = nGrid.NodeFromWorldPoint(startPos);
Node targetNode = nGrid.NodeFromWorldPoint(targetPos);
//List<Node> openSet = new List<Node>();
Heap <Node> openSet = new Heap <Node>(nGrid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
//Node currentNode = openSet[0];
//for(int i = 1; i < openSet.Count; i++)
//{
// if(openSet[i].FCost < currentNode.FCost || openSet[i].FCost == currentNode.FCost && openSet[i].hCost < currentNode.hCost)
// {
// currentNode = openSet[i];
// }
//}
//openSet.Remove(currentNode);
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
RetracePath(startNode, targetNode);
return;
}
foreach (Node neighbour in nGrid.GetNeighbours(currentNode))
{
if (!neighbour.isWalkable || 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);
}
}
}
}
}
//A* pathfinding executed in this function
public void FindPath(Vector3 startPos, Vector3 targetPos)
{
path.Clear();
Node startNode = grid.NodeFromWorldPoint(startPos);
Node targetNode = grid.NodeFromWorldPoint(targetPos);
Heap <Node> openSet = new Heap <Node>(grid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
RetracePath(startNode, targetNode);
return;
}
//check values of neighboring nodes
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
//if value is lowest, make it the next node
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);
}
}
}
}
}
/// <summary>
/// Finds the shortest path with help of nodes, and according to the A-star algorithm
/// </summary>
/// <param name="request"></param>
/// <param name="callback"></param>
public void FindPath(PathRequest request, Action <PathResult> callback)
{
NodeGrid nodeGrid = NodeGrid.Instance;
Vector3[] waypoints = new Vector3[0];
bool pathFound = false;
Node startNode = nodeGrid.NodeFromWorldPoint(request.pathStart);
Node targetNode = nodeGrid.NodeFromWorldPoint(request.pathEnd);
if (startNode == null || targetNode == null)
{
callback(new PathResult(new Vector3[0], false, request.callback));
}
else if (startNode.walkable && targetNode.walkable)
{
Heap <Node> openSet = new Heap <Node>(nodeGrid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node activeNode = openSet.RemoveFirst();
closedSet.Add(activeNode);
if (activeNode == targetNode)
{
pathFound = true;
break;
}
foreach (Node neighbour in nodeGrid.GetNeighbours(activeNode))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
int newCostToNeighbour = activeNode.gCost + GetDistance(activeNode, neighbour);
if (newCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, targetNode);
neighbour.parent = activeNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
if (pathFound)
{
waypoints = RetracePath(startNode, targetNode);
pathFound = waypoints.Length > 0;
}
callback(new PathResult(waypoints, pathFound, request.callback));
}
}
//! Returns List<Node>
public List <Node> FindPath(Node startNode, Node targetNode, ref NodeGrid grid, bool isPathSimplified = false, bool isPathFromStartToTarget = true, bool hasDiagonalMovement = true)
{
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
sw.Start();
bool pathSuccess = false;
//Node startNode = grid.NodeFromWorldPosition(startPos);
//Node targetNode = grid.NodeFromWorldPosition(targetPos);
if (startNode.isWalkable && targetNode.isWalkable)
{
//Heap<Node> openSet = new Heap<Node>(grid.MaxHeapSize);
Heap <Node> openSet = new Heap <Node>(grid.MaxHeapSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.Remove();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
sw.Stop();
pathSuccess = true;
Debug.Log("Path found: " + sw.ElapsedMilliseconds + " ms");
break;
}
foreach (var neighbourNode in grid.GetNeighbours(currentNode, hasDiagonalMovement))
{
if (!neighbourNode.isWalkable || closedSet.Contains(neighbourNode))
{
continue;
}
//count distance for each neighbour at grid generation => create set/array/list of neighbours?
int movementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbourNode) + neighbourNode.movementPenalty;
bool isInOpenSet = openSet.Contains(neighbourNode);
if (movementCostToNeighbour < neighbourNode.gCost || !isInOpenSet)
{
neighbourNode.gCost = movementCostToNeighbour;
neighbourNode.hCost = GetDistance(neighbourNode, targetNode);
neighbourNode.parent = currentNode;
if (!isInOpenSet)
{
openSet.Add(neighbourNode);
}
else
{
openSet.UpdateItem(neighbourNode);
}
}
}
}
}
if (pathSuccess)
{
List <Node> path = RetracePath(startNode, targetNode);
if (isPathSimplified)
{
SimplifyPathReturnAsNodes(ref path);
pathSuccess = path.Count > 0;
}
if (isPathFromStartToTarget)
{
ReversePath(ref path);
}
if (pathSuccess)
{
return(path);
}
}
return(null);
}
public void FindPath(PathRequest request, Action <PathResult> callback)
{
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = grid.NodeFromWorldPoint(request.pathStart);
Node targetNode = grid.NodeFromWorldPoint(request.pathEnd);
startNode.parent = startNode;
if (targetNode.walkable)
{
openSet = new Heap <Node>();
closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
pathSuccess = true;
break;
}
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
if (request.randomize)
{
neighbour.movementPenalty = UnityEngine.Random.Range(0, 50); //To make AI go randomly
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;
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);
}
}
}
}
}
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
pathSuccess = waypoints.Length > 0;
}
callback(new PathResult(waypoints, pathSuccess, request.callback));
}
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
Stopwatch sw = new Stopwatch();
sw.Start();
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
// First, we'll need to know which node we will be starting from, and which node we will be going to
Node startNode = grid.NodeFromWorldPoint(startPos);
Node targetNode = grid.NodeFromWorldPoint(targetPos);
// Check if the start and target nodes are walkable (since all would be useless if either are in places that are supposed to
// be inaccessible)
if (startNode.walkable && targetNode.walkable)
{
// We need an open set and closed set of nodes to tell us which nodes have been considered and which will need
// to be considered.
// The Open Set contains nodes that are next to the nodes that have been considered, which are within the Closed Set.
// When the node has been thoroughly considered, we will move this node into the closed set.
// When first initialized, the closed set will be empty, and the open set will only have the starting node within to be
// considered.
Heap <Node> openSet = new Heap <Node>(grid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
// While there are nodes to be considered, we keep the first node in mind while we scan through the list of nodes after it.
// If we find a node that has a lower overall cost than the current node, we will shift the focus to this new node instead.
// If that node has the same overall cost as the focused node, we look at the herustic cost of both nodes, and focus on the
// one with the lower hCost instead. Once done, we need to shift the previously focused node to the Closed Set, and remove
// it from the Open Set.
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
// If we have reached the target node, we can leave the loop
if (currentNode == targetNode)
{
sw.Stop();
print("Path found in: " + sw.ElapsedMilliseconds + "ms");
pathSuccess = true;
break;
}
// Check each neighbouring node
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
// Skip the checking for the focused node if it is unwalkable, or if it has already been considered before.
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
// Calculate the new distance from the start node to the neighbouring node of the current node. Only add the neighbour node to the
// open set for consideration if the new distance from the neighbouring node to the start node is smaller than before, or if it has not
// yet been considered before. This ignores nodes that have already been considered thanks to the if statement above.
int newMoveCost2Neighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.weight;
if (newMoveCost2Neighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
// Recalculate costs of neighbour node based on current node. We'll also parent the node to this one, so we can use the parent list to
// backtrace our path to the start node.
neighbour.gCost = newMoveCost2Neighbour;
neighbour.hCost = GetDistance(neighbour, targetNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
// If loop is completed without finding a path, we can conclude that the target cannot be reached.
if (!pathSuccess)
{
UnityEngine.Debug.Log("No path found. Target cannot be reached");
}
}
yield return(null);
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
}
requestManager.FinishedProcessingPath(waypoints, pathSuccess);
}
public List <Node> FindPath(Vector2Int start, Vector2Int end)
{
List <Node> closedList = new List <Node>();
List <Node> openList = new List <Node>();
Node startNode = grid.GetNode(start);
Node endNode = grid.GetNode(end);
openList.Add(startNode);
InitializeNodes();
startNode.Gcost = 0;
startNode.Hcost = CalculateDistance(startNode, endNode);
while (openList.Count > 0)
{
Node currentNode = GetLowestFcostNode(openList);
if (currentNode == endNode)
{
return(CalculatePath(endNode));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (Node n in grid.GetNeighbours(currentNode, 1))
{
if (closedList.Contains(n))
{
continue;
}
int tempGcost = currentNode.Gcost + CalculateDistance(currentNode, n);
if (tempGcost < n.Fcost)
{
n.previousNode = currentNode;
n.Gcost = tempGcost;
n.Hcost = CalculateDistance(n, endNode);
n.CalculateFcost();
if (!openList.Contains(n))
{
openList.Add(n);
}
}
}
}
//No more nodes
Debug.LogError($"Could not find path cList:{closedList.Count}");
string pathString = "Closed list: ";
foreach (Node n in closedList)
{
pathString += $"\n {n.position}";
}
Debug.Log(pathString);
return(null);
}
