IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
List <Node> path = new List <Node>();
bool pathSuccess = false;
Node startNode = grid.GetNodeFromWorldPoint(startPos);
Node targetNode = grid.GetNodeFromWorldPoint(targetPos);
if (startNode.walkable && targetNode.walkable)
{
Heap <Node> openSet = new Heap <Node>(grid.MaxSize);
HashSet <Node> closeSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
closeSet.Add(currentNode);
if (currentNode == targetNode)
{
path = RetracePath(startNode, targetNode);
pathSuccess = true;
break;
}
List <Node> neighbourNodes = grid.GetNeighbourNodes(currentNode);
foreach (var neighbour in neighbourNodes)
{
if (neighbour.walkable == false || closeSet.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);
}
}
}
}
}
yield return(null);
if (pathSuccess)
{
Vector3[] waypoints = GenerateWayPoints(path);
requestManager.FinishedProcessPath(waypoints, pathSuccess);
}
requestManager.FinishedProcessPath(new Vector3[0], pathSuccess);
}
// finds a path from the start to the end
IEnumerator FindPath(Vector3 start, Vector3 end)
{
Stopwatch sw = new Stopwatch(); // stopwatch for efficiency diagnostics
sw.Start();
Vector3[] waypoints = new Vector3[0]; // the waypoints on the path - will be properly set up later
bool pathSuccess = false; // we have not successfully found a path yet
// get the nodes on the grid based on the world positions
TileNode startNode = grid.NodeFromWorldPosition(start);
TileNode targetNode = grid.NodeFromWorldPosition(end);
// only pathfind if both the start and end are walkable
if (startNode.walkable && targetNode.walkable)
{
//List<TileNode> openSet = new List<TileNode>(); // what nodes are available to check
Heap <TileNode> openSet = new Heap <TileNode>(grid.MaxSize); // use a Binary Heap instead of List so that we only compare to parent rather than iterate everything
HashSet <TileNode> closedSet = new HashSet <TileNode>(); // what nodes have been checked
openSet.Add(startNode); // add the start node
while (openSet.Count > 0)
{
TileNode currentNode = openSet.PopFirst(); // openSet[0]
// very slow because it has to iterate through everything
//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); // add the current node to the closed set so we can't check it again
// if we find the node we want, then retrace the path and exit the loop because that's our path
if (currentNode == targetNode)
{
sw.Stop();
print("Path found: " + sw.ElapsedMilliseconds + "ms");
pathSuccess = true;
break;
}
// check each neighbor
foreach (TileNode neighbor in grid.GetNeighbors(currentNode))
{
// if the neighbor is not walkable (value of 1) or it's in the closed set, don't check it
if (!neighbor.walkable || closedSet.Contains(neighbor))
{
continue;
}
// get path from the current node to the neighbor. Also add weight value here
int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor) + neighbor.movementPenalty; // add the current node's gCost to find out how far it is from the start in total
// if the new path to the neighbor is shorter or the the neighbor isn't in the open set yet
if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
{
// set up the neighbor
neighbor.gCost = newMovementCostToNeighbor;
neighbor.hCost = GetDistance(neighbor, targetNode); // heuristic distance to the target
neighbor.parent = currentNode; // set the neighbor's parent to the currentNode (this allows us to traverse the path later)
// add the neighbor if it's not in the open set
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
else // if it is in the open set, update it's values so we get the correct path
{
openSet.UpdateItem(neighbor);
}
}
}
}
}
yield return(null); // wait a frame then try again
// if we found a path, set the waypoints to the retrace
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
pathSuccess = waypoints.Length > 0; // in case we move the target
}
requestManager.FinishedProcessPath(waypoints, pathSuccess); // give the requestmanager the data
}
IEnumerator PathFinding(Vector3 StartPos, Vector3 TargetPos)
{
Stopwatch sw = new Stopwatch();
sw.Start();
Vector3[] waypoints = new Vector3[0];
bool PathSuccess = false;
grid = GetComponent <Grid>();
Node StartNode = grid.GetNodePositionFromWorld(StartPos);
Node TargetNode = grid.GetNodePositionFromWorld(TargetPos);
if (StartNode.walkable && TargetNode.walkable)
{
//List<Node> openList = new List<Node> ();
Heap <Node> openList = new Heap <Node> (grid.MaxSize);
HashSet <Node> closeList = new HashSet <Node> ();
openList.Add(StartNode);
while (openList.Count > 0)
{
//use loop to find the minium fCost instead of heap
/*Node currentNode = openList [0];
*
* for (int i = 1; i < openList.Count; i++) {
* if (openList [i].fCost < currentNode.fCost || (openList [i].fCost == currentNode.fCost && openList [i].hCost < currentNode.hCost)) {
* currentNode = openList [i];
* }
*
* }*/
//Node currentNode = openList [0];
//openList.Remove (currentNode);
Node currentNode = openList.RemoveFirst();
closeList.Add(currentNode);
if (currentNode == TargetNode)
{
if (ShowPathFindingTime)
{
sw.Stop();
print("Path Found: " + sw.ElapsedMilliseconds + " ms");
}
PathSuccess = true;
break;
}
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
if (!neighbour.walkable || closeList.Contains(neighbour))
{
continue;
}
int MoveCostToNeighbour = currentNode.gCost + GetMoveCost(currentNode, neighbour) + neighbour.movePenality;
if (MoveCostToNeighbour < neighbour.gCost || !openList.Contains(neighbour))
{
neighbour.gCost = MoveCostToNeighbour;
neighbour.hCost = GetManHattanDiatance(neighbour, TargetNode);
//neighbour.hCost = GetMoveCost (neighbour, TargetNode);
neighbour.parent = currentNode;
if (!openList.Contains(neighbour))
{
openList.Add(neighbour);
}
else
{
openList.Update(neighbour);
}
}
}
}
yield return(null);
if (PathSuccess)
{
waypoints = getPath(StartNode, TargetNode);
closeList.Clear();
}
requestManger.FinishedProcessPath(waypoints, PathSuccess);
}
else
{
print("Start Node or End Node is unwalkable.");
}
}
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
Vector3[] waypoint = new Vector3[0];
bool pathSuccess = false;
Node startNode = grid.NodeFromWorldPoint(startPos);
Node targetNode = grid.NodeFromWorldPoint(targetPos);
if (startNode.walkable && targetNode.walkable)
{
List <Node> openSet = new List <Node>();
HashSet <Node> closeSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet[0];
for (int i = 0; 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);
closeSet.Add(currentNode);
if (currentNode == targetNode)
{
pathSuccess = true;
break;
}
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
if (!neighbour.walkable || closeSet.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);
}
}
}
}
}
yield return(null);
if (pathSuccess)
{
waypoint = RetracePath(startNode, targetNode);
requestManager.FinishedProcessPath(waypoint, pathSuccess);
}
}
