public bool FindPath(Vector2Int gridstart, Vector2Int gridend, ref List <Node> pathref)
{
// create open set
List <Node> open_set = new List <Node>();
Node start_node = grid_.GetNode(gridstart.x, gridstart.y);
Node end_node = grid_.GetNode(gridend.x, gridend.y);
open_set.Add(start_node);
start_node.inOpen_ = true;
nodes_to_reset_.Add(start_node);
while (open_set.Count > 0)
{
Node current_node = open_set[GetLowestFNode(open_set)];
// erase current node from open set
open_set.Remove(current_node);
// remove current node from open set
current_node.inOpen_ = false;
// add current node to close set by flag
current_node.inClosed_ = true;
// if current node equals to end node, we have reached destination
if (current_node == end_node)
{
pathref = RetracePath(start_node, end_node);
return(true);
}
// else continue search
foreach (Node n in grid_.GetNeighbours(current_node))
{
// if not traversable or in closed set, skip
if (!n.isWalkable_ || n.inClosed_)
{
continue;
}
// if new g cost < g cost (need updating), or if not in open set, update/calculate f cost, add to open set
// calculate new g cost of neighbour relative to current node
int new_g_cost = current_node.gCost_ + GetDistanceBetweenNodes(current_node, n);
if (new_g_cost < n.gCost_ || !n.inOpen_)
{
n.gCost_ = new_g_cost;
n.hCost_ = GetDistanceBetweenNodes(n, end_node);
n.parent_ = current_node;
if (!n.inOpen_)
{
open_set.Add(n);
n.inOpen_ = true;
}
nodes_to_reset_.Add(n);
}
}
}
return(false);
}
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
Vector3[] wayppoints = new Vector3[0];
bool pathSuccess = false;
PathfindingGrid.Node startNode = grid.NodeFromWorldPoint(startPos);
PathfindingGrid.Node targetNode = grid.NodeFromWorldPoint(targetPos);
if (startNode.walkable && targetNode.walkable)
{
Heap <PathfindingGrid.Node> openSet = new Heap <PathfindingGrid.Node>(grid.MaxSize());
HashSet <PathfindingGrid.Node> closedSet = new HashSet <PathfindingGrid.Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
PathfindingGrid.Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (targetNode == currentNode)
{
pathSuccess = true;
break;
}
foreach (PathfindingGrid.Node neighbour in grid.GetNeighbours(currentNode))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + Distance(currentNode, neighbour);
if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = Distance(neighbour, targetNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
}
}
}
}
yield return(null);
if (pathSuccess)
{
wayppoints = RetracePath(startNode, targetNode);
}
requestManager.FinishedProcessingPath(wayppoints, pathSuccess);
}
private void CalculatePath(Vector2 start, Vector2 end)
{
List <Node> OpenNodes = new List <Node>();
List <Node> ClosedNodes = new List <Node>();
// adding the starting node
OpenNodes.Add(grid.nodeGrid[(int)start.x, (int)start.y]);
Node startNode = grid.nodeGrid[(int)start.x, (int)start.y];
startNode.GCost = 0;
startNode.HCost = GetDistance(start, end);
Node targetNode = grid.nodeGrid[(int)end.x, (int)end.y];
while (OpenNodes.Count > 0)
{
Node currentNode = GetLowestFCostNode(OpenNodes);
OpenNodes.Remove(currentNode);
ClosedNodes.Add(currentNode);
if (currentNode == targetNode)
{
RetracePath(startNode, targetNode);
return;
}
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
if (!neighbour.Walkable || ClosedNodes.Contains(neighbour))
{
continue;
}
float newCostToNeighbour = currentNode.GCost + GetDistance(currentNode.Position, neighbour.Position);
if (newCostToNeighbour < neighbour.GCost || !OpenNodes.Contains(neighbour))
{
neighbour.GCost = newCostToNeighbour;
neighbour.HCost = GetDistance(neighbour.Position, targetNode.Position);
neighbour.ParentNode = currentNode;
if (!OpenNodes.Contains(neighbour))
{
OpenNodes.Add(neighbour);
}
}
}
}
}
IEnumerator FindThePath(Vector3 startPos, Vector3 targetPos)
{
//For measuring pathfinding speed
Stopwatch stopWatch = new Stopwatch();
stopWatch.Start();
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
//Creating nodes from transforms by NodeFromWorldPoint method
Node startNode = grid.NodeFromWorldPoint(startPos);
Node endNode = grid.NodeFromWorldPoint(targetPos);
if (startNode.walkable && endNode.walkable)
{
//Creating open and closed lists (changed for Heap class)
Heap <Node> openSet = new Heap <Node>(grid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
//Adding the first element to open list
openSet.Add(startNode);
//Main loop of pathfinding
while (openSet.Count > 0)
{
//Heap class placement
Node currentNode = openSet.RemoveFirst();
//Moving nodes from open to closed list
closedSet.Add(currentNode);
//Pathfinding success
if (currentNode == endNode)
{
stopWatch.Stop();
print("Found in miliseconds: " + stopWatch.ElapsedMilliseconds);
pathSuccess = true;
break;
}
//Neighbour operating
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
//Ignoring not walkable and closed nodes
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
//Setting gCost, hCost, parent of neighbour and moving it to an open list
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;
if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, endNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
}
yield return(null);
if (pathSuccess)
{
waypoints = RetracePath(startNode, endNode);
}
requestManager.FinishedProcessingPath(waypoints, pathSuccess);
}
/// <summary>
/// Plot a path for the PathRequest received
/// </summary>
public void FindPath(PathRequest request, Action <PathResult> callback)
{
if (request == null)
{
return;
}
var pathSuccess = false;
// Get the start and end nodes
var startNode = Grid.NodeFromWorldPoint(request.PathStart);
var targetNode = Grid.NodeFromWorldPoint(request.PathEnd);
startNode.Parent = startNode;
if (startNode.Walkable && targetNode.Walkable)
{
var frontier = new Heap <Node>(Grid.MaxSize); // open set
var visited = new HashSet <Node>(); // closed set
frontier.Add(startNode);
while (frontier.Count > 0) // While there are nodes in the frontier
{
var currentNode = frontier.RemoveFirst(); // get the cheapest node
// if the current node is the target node, then we arrived
if (currentNode == targetNode)
{
pathSuccess = true;
break;
}
// add the current node to the visited set
visited.Add(currentNode);
foreach (var neighbor in Grid.GetNeighbours(currentNode))
{
// skip if this neighbor is not walkable or if it was already visited
if (neighbor.Walkable == false || visited.Contains(neighbor))
{
continue;
}
// calculate the G cost for this neighbor. The G cost is the Real Cost
var newCostToNeighbor = currentNode.GCost + GetDistance(currentNode, neighbor) +
neighbor.MovementPenalty;
// if the new cost is lower than the previous GCost or if the neighbor is not in the frontier (first time visited), we then update the cost
if (newCostToNeighbor < neighbor.GCost || frontier.Contains(neighbor) == false)
{
neighbor.GCost = newCostToNeighbor;
neighbor.HCost = GetDistance(neighbor, targetNode); // distance in straight line from this neighbor to the target node
neighbor.Parent = currentNode; // set the current node as the node we used to arrive to this neighbor
// if not in the frontier, add it to the frontier
if (frontier.Contains(neighbor) == false)
{
frontier.Add(neighbor);
}
else
{
frontier.UpdateItem(neighbor);
}
}
}
}
}
var wayPoints = new Node[0];
if (pathSuccess)
{
wayPoints = RetracePath(startNode, targetNode);
}
callback(new PathResult(wayPoints, pathSuccess, request.Callback));
}
