public void FindPath(Grid _grid)
{
Node start = _grid.StartNode;
Node end = _grid.EndNode;
open = new Heap<Node>(_grid.GridMaxSize);
close = new HashSet<Node>();
open.Add(start);
while (open.Count > 0)
{
Node current = open.GetFirst();
if (current.GridBlock.Equals(end.GridBlock))
return;
foreach(Node p in _grid.GetNeighbours(current))
{
if (p.GridBlock.Type != GridBlock.BlockType.Obstacle || close.Contains(p))
continue;
int gCost = current.gCost + GetDistance(current, p);
if(gCost < current.gCost || !open.Contains(p))
{
p.gCost = gCost;
p.hCost = GetDistance(current, p);
p.Parent = current;
if (!open.Contains(p))
open.Add(p);
}
}
}
}
List<Point> FindRoadPath(Road a, Road b, RoadType T)
{
AstarObject[,] Set = new AstarObject[14, 9];
for (int x = 0; x < 14; x++)
{
for (int y = 0; y < 9; y++)
{
Set[x, y] = new AstarObject(x, y, this);
}
}
Heap<AstarObject> OpenSet = new Heap<AstarObject>(14 * 9);
HashSet<AstarObject> ClosedSet = new HashSet<AstarObject>();
AstarObject Start = Set[a.X, a.Y];
AstarObject End = Set[b.X, b.Y];
OpenSet.Add(Start);
while (OpenSet.Count > 0)
{
AstarObject CurrentLocation = OpenSet.RemoveFirst();
ClosedSet.Add(CurrentLocation);
if (CurrentLocation == End)
{
return RetracePath(Start, End);
//Retracepath and stuff.
}
List<AstarObject> Neighbours = GetNeighbours(CurrentLocation, ref Set, NeighbourhoodType.Neumann, MapsizeXR, MapsizeYR);
foreach (AstarObject neighbour in Neighbours)
{
if (neighbour.RType != T || ClosedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = CurrentLocation.gCost + GetDistance(CurrentLocation, neighbour);
if (newMovementCostToNeighbour < neighbour.gCost || !OpenSet.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, End);
neighbour.parent = CurrentLocation;
if (!OpenSet.Contains(neighbour))
{
OpenSet.Add(neighbour);
}
else
{
OpenSet.UpdateItem(neighbour);
}
}
}
}
return new List<Point>();
}
/* 1 Implement a class PriorityQueue<T> based
* on the data structure "binary heap".
* */
static void Main(string[] args)
{
var heap = new Heap<int>();
heap.Add(1);
heap.Add(2);
heap.Add(3);
Debug.Assert(heap.SameContents(new[] { 1, 2, 3 }));
Console.WriteLine(string.Join(",", heap));
Debug.Assert(heap.ChopHead() == 3);
Debug.Assert(heap.ChopHead() == 2);
Debug.Assert(heap.ChopHead() == 1);
Debug.Assert(heap.IsEmpty);
// higher string means lower priority
var pqueue = new PriorityQueue<string, string>((s1, s2) => -s1.CompareTo(s2));
pqueue.Enqueue("18:00", "Buy food");
pqueue.Enqueue("06:00", "Walk dog");
pqueue.Enqueue("21:00", "Do homework");
pqueue.Enqueue("09:00", "Go to work");
pqueue.Enqueue("21:00", "Drink beer");
Debug.Assert(pqueue.Count == 5);
Debug.Assert(pqueue.Dequeue() == "Walk dog");
Debug.Assert(pqueue.Dequeue() == "Go to work");
Debug.Assert(pqueue.Dequeue() == "Buy food");
Debug.Assert(new[] { "Do homework", "Drink beer" }.Contains(pqueue.Dequeue()));
Debug.Assert(new[] { "Do homework", "Drink beer" }.Contains(pqueue.Dequeue()));
}
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
Stopwatch sw = new Stopwatch();
sw.Start();
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)
{
sw.Stop();
print ("Path found: " + sw.ElapsedMilliseconds + " ms");
pathSuccess = true;
break;
}
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
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);
}
}
}
}
yield return null;
if (pathSuccess) {
waypoints = RetracePath(startNode,targetNode);
}
requestManager.FinishedProcessingPath(waypoints,pathSuccess);
}
void FindPath(Vector3 startPos, Vector3 targetPos) { // performs A* search on the grid to find a path
startPos = new Vector3(startPos.x + 8.0f, 0, startPos.z - 2.0f); // offsets
targetPos = new Vector3(targetPos.x + 8.0f, 0, targetPos.z - 2.0f); // offsets
Node startNode = grid.GetNodeFromWorldPoint(startPos);
Node targetNode = grid.GetNodeFromWorldPoint(targetPos);
if (targetNode.walkable == false) return; // don't try to path find if we're on an unwalkable area
Heap<Node> openSet = new Heap<Node>(grid.MaxSize);
HashSet<Node> closedSet = new HashSet<Node>();
openSet.Add(startNode);
while(openSet.Count > 0) { // we still have nodes
Node currentNode = openSet.pop();
closedSet.Add(currentNode);
if(currentNode == targetNode) { // we've found exit
RetracePath(startNode, targetNode);
path = backTrackPath(startNode, targetNode);
return;
}
foreach(Node n in grid.GetNeighbours(currentNode)) {
if (!n.walkable || closedSet.Contains(n)) continue;
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, n);
if(newMovementCostToNeighbour < n.gCost || !openSet.contains(n)) {
n.gCost = newMovementCostToNeighbour;
n.hCost = GetDistance(n, targetNode);
n.parent = currentNode;
if (!openSet.contains(n)) openSet.Add(n); // add our neighbour into open set
else openSet.UpdateItem(n);
}
}
}
}
public void Simple()
{
var heap = new Heap<int>(HeapType.Minimum)
{
5
};
Assert.AreEqual(heap.Count, 1);
Assert.IsFalse(heap.IsEmpty);
Assert.AreEqual(heap.Root, 5);
heap.Add(2);
Assert.AreEqual(heap.Count, 2);
Assert.IsFalse(heap.IsEmpty);
Assert.AreEqual(heap.Root, 2);
heap.Add(3);
Assert.AreEqual(heap.Count, 3);
Assert.IsFalse(heap.IsEmpty);
Assert.AreEqual(heap.Root, 2);
Assert.AreEqual(heap.RemoveRoot(), 2);
heap.Add(1);
Assert.AreEqual(heap.Count, 3);
Assert.IsFalse(heap.IsEmpty);
Assert.AreEqual(heap.Root, 1);
}
public void aStar(Vector3 startPos, Vector3 endPos)
{
Stopwatch watch = new Stopwatch();
watch.Start();
//List<Node_K> openSet = new List<Node_K>();
Heap<Node_K> openSet = new Heap<Node_K>(AIarea.GetMaxSize);
HashSet<Node_K> closedSet = new HashSet<Node_K>();
Node_K sourceNode = AIarea.getNodeAtPos(startPos);
Node_K targetNode = AIarea.getNodeAtPos(endPos);
openSet.Add(sourceNode);
while(openSet.Count > 0)
{
Node_K currentNode = openSet.RemoveFirst();
//Node_K 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)
{
reversePath(sourceNode, targetNode);
watch.Stop();
time = watch.ElapsedMilliseconds.ToString();
print("Astar " + watch.ElapsedMilliseconds + "ms");
return;
}
List<Node_K> neighbors = new List<Node_K>();
neighbors = AIarea.neighbors(currentNode);
for (int i = 0; i < neighbors.Count; i++)
{
if (neighbors[i].walkable == false || closedSet.Contains(neighbors[i]))
continue;
int cost = currentNode.gCost + moveCost(currentNode, neighbors[i]);
if (!openSet.Contains(neighbors[i]) || (cost < neighbors[i].gCost))
{
neighbors[i].gCost = cost;
neighbors[i].hCost = moveCost(neighbors[i], targetNode);
neighbors[i].parent = currentNode;
if (!openSet.Contains(neighbors[i]))
openSet.Add(neighbors[i]);
else
openSet.UpdateItem(neighbors[i]);
}
}
}
}
//Generate path from start to destination using A*
public Path findPath(Vector3 startPos, Vector3 targetPos, int steps) {
//Convert 3D Positions to GridNodes
GridNode startNode = grid.getNode(startPos);
GridNode targetNode = grid.getNode(targetPos);
//Exit if nodes are out of bounds
if (startNode == null || targetNode == null)
{
return null;
}
//Instantiate Open and Closed set DataStructures for A* Algorithm
HashSet<GridNode> closedSet = new HashSet<GridNode>();
Heap<GridNode> openSet = new Heap<GridNode>(grid.MaxSize);
openSet.Add(startNode); //Add start node to the open set
int count = 0;
//Iterate through as long as openset is not empty
while(openSet.Count > 0 && count++ < steps) {
//Store first node in collection
GridNode currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
//Check if arrived at target node
if(currentNode == targetNode) {
//Retrace and get coordinates
List<GridNode> points = retracePath(startNode, targetNode);
//Convert to a path
return getPath(points);
}
//Iterate through all neighbor nodes
foreach(GridNode neighbor in grid.GetNeighbors(currentNode)) {
//Skip node if not walkable or already used
if (!neighbor.isWalkable() || closedSet.Contains(neighbor))
{
continue;
}
//Calculate the move cost to the neighbor nodes
int moveCost = currentNode.gCost + GridNode.getDistance(currentNode, neighbor);
//Calculate neighbor's costs
if(moveCost < neighbor.gCost || !openSet.Contains(neighbor)) {
//Set Costs
neighbor.gCost = moveCost;
neighbor.hCost = GridNode.getDistance(neighbor, targetNode);
neighbor.parent = currentNode;
//If the open set doesn't already contain the neighboring node, add it
if (!openSet.Contains(neighbor))
openSet.Add(neighbor);
}
}
}
//Unable to find a path
return null;
}
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = grid.NodeFromWorlPoint (startPos);
Node targetNode = grid.NodeFromWorlPoint (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);
} else {
requestManager.FinishedProcessingPath(waypoints, pathSuccess);
yield break;
}
}
//A* implementation
public static IEnumerator CalculatePath(PathNode startNode, PathNode endNode, PathNode[] allNodes, Action<Vector2[], bool> callback){
#if DEBUG
Stopwatch sw = new Stopwatch();
sw.Start();
#endif
var openList = new Heap<PathNode>(allNodes.Length);
var closedList = new HashSet<PathNode>();
var success = false;
openList.Add(startNode);
while (openList.Count > 0){
var currentNode = openList.RemoveFirst();
closedList.Add(currentNode);
if (currentNode == endNode){
#if DEBUG
sw.Stop();
//UnityEngine.Debug.Log("Path Found: " + sw.ElapsedMilliseconds + " ms.");
#endif
success = true;
break;
}
foreach (var neighbour in currentNode.links.Select( index => allNodes[index] )){
if (closedList.Contains(neighbour))
continue;
var costToNeighbour = currentNode.gCost + GetDistance( currentNode, neighbour );
if (costToNeighbour < neighbour.gCost || !openList.Contains(neighbour) ){
neighbour.gCost = costToNeighbour;
neighbour.hCost = GetDistance(neighbour, endNode);
neighbour.parent = currentNode;
if (!openList.Contains(neighbour)){
openList.Add(neighbour);
openList.UpdateItem(neighbour);
}
}
}
}
yield return null;
if (success){
callback( RetracePath(startNode, endNode), true );
} else {
callback( new Vector2[0], false );
}
}
public void AddExample()
{
var heap = new Heap<string>(HeapType.Minimum);
heap.Add("cat");
heap.Add("dog");
heap.Add("canary");
// There should be 3 items in the heap.
Assert.AreEqual(3, heap.Count);
}
Vector2[] FindPath(Vector2 from, Vector2 to)
{
Stopwatch sw = new Stopwatch();
sw.Start();
Vector2[] waypoints = new Vector2[0];
bool pathSuccess = false;
Node startNode = grid.NodeFromWorldPoint(from);
Node targetNode = grid.NodeFromWorldPoint(to);
startNode.parent = startNode;
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) {
sw.Stop();
print ("Path found: " + sw.ElapsedMilliseconds + " ms");
pathSuccess = true;
break;
}
foreach (Node neighbour in grid.GetNeighbours(currentNode)) {
if (!neighbour.walkable || closedSet.Contains(neighbour)) {
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour)+TurningCost(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);
}
}
}
}
if (pathSuccess) {
waypoints = RetracePath(startNode,targetNode);
}
return waypoints;
}
void FindPath(Vector2 startPos, Vector2 targetPos)
{
Stopwatch sw = new Stopwatch();
sw.Start();
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){
sw.Stop();
print("Path found" + sw.ElapsedMilliseconds + "ms");
RetracePath(startNode,targetNode);
return;
}
//check if neighbour is walkable
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);
}
}
}
}
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
//Create and start stopwatch to log method completion time
Stopwatch sw = new Stopwatch();
sw.Start();
//Initialize waypoints array
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
//Determine the start and target node based on the Gameobjects Vector 3 positions
Node startNode = grid.NodeFromWorldPoint(startPos);
Node targetNode = grid.NodeFromWorldPoint(targetPos);
if(startNode.walkable && targetNode.walkable) {
Heap<Node> openSet = new Heap<Node>(grid.MaxSize); //Holds all the nodes in the grid
HashSet<Node> closedSet = new HashSet<Node>(); //Used to store nodes that are
openSet.Add(startNode);
while (openSet.Count > 0) {
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode) {
sw.Stop();
print("Path Found: " + sw.ElapsedMilliseconds + " ms");
pathSuccess = true;
break;
}
foreach(Node neighbour in grid.GetNeighbours(currentNode)) {
if(!neighbour.walkable || closedSet.Contains(neighbour)) {
continue;
}
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);
}
}
}
}
yield return null;
if (pathSuccess) {
waypoints = RetracePath(startNode, targetNode);
}
requestManager.FinishedProcessingPath(waypoints, pathSuccess);
}
void CreatePath(Node startNode, Node targetNode)
{
bool pathSuccess = false;
int traffic;
int trafficCap;
openSet = new Heap<Node>(grid.RoadSet.Count);
closedSet = new HashSet<Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
// remove the smallest fCost from Heap
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
pathSuccess = true;
break;
}
foreach (Node neighbour in grid.PathGetRoadNeighbours(currentNode))
{
if (closedSet.Contains(neighbour))
continue;
lock (currentNode)
traffic = currentNode.trafficTimer.TrafficIntensity;
trafficCap = currentNode.trafficTimer.InitialTrafficIntensity;
int penalty = 17 - 17 * traffic / (trafficCap + 17/trafficCap);
int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbour) + penalty;
if (newMovementCostToNeighbor < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbor;
neighbour.hCost = GetDistance(neighbour, targetNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
openSet.Add(neighbour);
else
openSet.UpdateItem(neighbour);
}
}
}
requestManager.FinishedProcessingPath(ReverseParents(startNode, targetNode), pathSuccess);
}
public bool FindPathImmediate(GridNode start, GridNode end, List<List<GridNode>> grid, out List<GridNode> path)
{
path = new List<GridNode>();
bool success = false;
if (start.walkable && end.walkable)
{
Heap<GridNode> openSet = new Heap<GridNode>(grid.Count * grid[0].Count);
HashSet<GridNode> closedSet = new HashSet<GridNode>();
openSet.Add(start);
while (openSet.Count > 0)
{
GridNode currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
//path found
if (currentNode == end)
{
success = true;
break;
}
foreach (GridNode neighbour in currentNode.GetNeighbours())
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
continue;
int newNeighbourMoveCost = currentNode.gCost + GetDistanceManhatten(currentNode, neighbour);
if (newNeighbourMoveCost < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newNeighbourMoveCost;
neighbour.hCost = GetDistanceManhatten(neighbour, end);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
openSet.Add(neighbour);
else
openSet.UpdateItem(neighbour);
}
}
}
}
if (success)
{
path = RetracePath(start, end);
}
return success;
}
IEnumerator FindPath(Vector3 startPosition, Vector3 targetPosition)//here we get 2 positions from the game and check on wich node they are, we do that with NodeFromWorldPoint wich we have in the grid script
{
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = grid.NodeFromWorldPoint(startPosition);//this is going to be the startng point f the path
Node targetNode = grid.NodeFromWorldPoint(targetPosition);//this wil be the base or player etc, just the target
if (startNode.walkable && targetNode.walkable) {
Heap<Node> openSet = new Heap<Node>(grid.MaxSize);//list of nodes that havent been checked yet
HashSet<Node> closedSet = new HashSet<Node>();//a list for the nodes that have been checked
openSet.Add(startNode);//the verry first node that wil be checked, this is the starting position
while (openSet.Count > 0)//as long as there are nodes to be checked, go on :)
{
Node currentNode = openSet.RemoveFirst();//first to be checked a
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
pathSuccess = true;
break;
}
foreach (Node neighbour in grid.GetNeighbours(currentNode)) {
if (!neighbour.walkable || closedSet.Contains(neighbour)) {
continue;
}
int newCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
if (newCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) {
neighbour.gCost = newCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, targetNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
openSet.Add(neighbour);
}
}
}
}
yield return null;
if (pathSuccess) {
waypoints = RetracePath(startNode,targetNode);
}
requestManager.FinishedProcessingPath(waypoints,pathSuccess);
}
IEnumerator CalculatePath(Vector3 startPosition, Vector3 targetPosition, LongPathGrid longPathGrid)
{
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
GridNode startNode = longPathGrid.GetGridNodeFromWorldPoint(startPosition);
GridNode targetNode = longPathGrid.GetGridNodeFromWorldPoint(targetPosition);
if (targetNode.walkable) {
Heap<GridNode> openSet = new Heap<GridNode>(longPathGrid.MaxSize);
HashSet<GridNode> closedSet = new HashSet<GridNode>();
openSet.Add(startNode);
while(openSet.Count > 0) {
GridNode currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode) {
pathSuccess = true;
break;
}
foreach (GridNode neighbour in longPathGrid.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);
navGridManager.FinishProcessingLongPath(waypoints,pathSuccess);
}
void FindPath(Vector3 startPos, Vector3 targetPos) {
Stopwatch sw = new Stopwatch ();
sw.Start ();
Node startNode = grid.NodeFromWorldPoint (startPos);
Node targetNode = grid.NodeFromWorldPoint (targetPos);
//List<Node> openSet = new List<Node>();
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();
// 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) {
// retrace steps to get the path using parent
sw.Stop();
print("Path found in " + sw.ElapsedMilliseconds + " ms");
RetracePath(startNode,targetNode);
return;
}
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);
}
}
}
}
public List<Portal> FindPath()
{
Stopwatch sw = new Stopwatch();
sw.Start();
bool success = false;
CreateStartNode(startPos);
CreateEndNode(targetPos);
if (startPortal == null || endPortal == null)
return null;
Heap<Portal> frontier = new Heap<Portal>(portals.Count+2);
HashSet<Portal> visited = new HashSet<Portal>();
frontier.Add(startPortal);
while(frontier.size() > 0){
Portal current = frontier.RemoveFirst();
visited.Add(current);
if (current == endPortal){
sw.Stop();
success = true;
print("Path found in: "+ sw.ElapsedMilliseconds + " ms");
break;
}
foreach(KeyValuePair<Portal, int> nb in current.GetNeighbours()){
if (visited.Contains(nb.Key)){
continue;
}
int newDistance = current.gCost + nb.Value;
if (newDistance < nb.Key.gCost || !frontier.Contains(nb.Key)){
nb.Key.gCost = newDistance;
nb.Key.hCost = CalcDistance(nb.Key);
nb.Key.SetParent(current);
if (!frontier.Contains(nb.Key)){
frontier.Add(nb.Key);
}
}
}
}
if (success){
return RetracePath();
}
else return null;
}
public List<Node> FindPath(Node start, Node destination)
{
// an open set to store nodes with the potentional to lead towards the goal
openSet = new Heap<Node>(game.nodeGrid.nodesInGridWidth * game.nodeGrid.nodesInGridHeight);
// a closed set to keep track of nodes already expanded from
closedSet = new HashSet<Node>();
openSet.Add(start);
// as long as the openset contains nodes we keep searching a path
while (openSet.Count > 0)
{
// get the node with the lowest F value
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
// if we find the goal, retrace our path and send it to the requester
if (currentNode == destination)
return RetracePath(start, destination);
// expand from the current node, get neighbours in all 8 directions
foreach (Node neighbour in game.nodeGrid.GetAllNeighbours(currentNode))
{
if (neighbour.blocked || closedSet.Contains(neighbour))
continue;
// the new cost is the current Node G value + the distance to the neighbour (either 10 or 14)
int newGCost = currentNode.G + GetDistance(currentNode, neighbour);
if (newGCost < neighbour.G || !openSet.Contains(neighbour))
{
neighbour.G = newGCost;
neighbour.H = GetDistance(neighbour, destination);
neighbour.parent = currentNode;
// the openSet maintains itself in such away that the node with the lowest F value is always the first element.
if (!openSet.Contains(neighbour))
openSet.Add(neighbour);
else
openSet.UpdateItem(neighbour);
}
}
}
// we went through every node we could reach and havent found the goal. it is unreachable
return null;
}
//Calculates the path from a start to finish using the A* Pathfinding algorithm
IEnumerator CalculatePath(Vector3 startPosition, Vector3 endPosition)
{
Vector3[] waypoints = new Vector3[0];
bool success = false;
Node startNode = grid.NodeFromPoint(startPosition);//Creates a node from the point
Node endNode = grid.NodeFromPoint(endPosition);
if (endNode.Walkable ) { //See if the end node is a walkable object or it is a resource
Heap<Node> open = new Heap<Node> (grid.MaxGridSize);
HashSet<Node> closed = new HashSet<Node> ();
open.Add (startNode);
while (open.Count > 0) {
Node current = open.Pop ();//Removes first node in heap and re-sorts it
closed.Add (current);//Adds the node to the closed list
if (current == endNode) {
success = true;
break;
}
foreach (Node neighbour in grid.GetNeighbours(current)) {//Traverses through the neighbours of the nodes
if (!neighbour.Walkable || closed.Contains (neighbour)) {
continue;
}
int costToNeighbour = current.G + GetDist (current, neighbour);
if (costToNeighbour < neighbour.G || !open.Contains (neighbour)) {//If the cost to neighbour < neighbouring g value
neighbour.G = costToNeighbour;
neighbour.H = GetDist (neighbour, endNode);
neighbour.Parent = current;
if (!open.Contains (neighbour)) {
open.Add (neighbour); //Adds the neighbour to the heap
}
else{
open.UpdateItem(neighbour); // Resort the heap to include the neighbour
}
}
}
}
}
yield return null;//Makes wait for one frame
if (success) // If successfully calculates path
{
waypoints = ReversePath(startNode, endNode); //reverse path since path is stored from end to beginning
}
request.FinishedProcessing(waypoints, success);
}
void FindPath(Vector3 startPos, Vector3 targetPos)
{
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;
}
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);
}
}
}
}
public void ComparerDelegateWithIntialSize()
{
var heap = new Heap<SimpleClass>(HeapType.Maximum, 10, (x, y) => x.TestProperty.CompareTo(y.TestProperty))
{
new SimpleClass("5")
};
Assert.AreEqual(heap.Count, 1);
Assert.AreEqual(heap.Root.TestProperty, "5");
heap.Add(new SimpleClass("2"));
Assert.AreEqual(heap.Count, 2);
Assert.AreEqual(heap.Root.TestProperty, "5");
heap.Add(new SimpleClass("3"));
Assert.AreEqual(heap.Count, 3);
Assert.AreEqual(heap.Root.TestProperty, "5");
Assert.AreEqual(heap.RemoveRoot().TestProperty, "5");
heap.Add(new SimpleClass("1"));
Assert.AreEqual(heap.Count, 3);
Assert.AreEqual(heap.Root.TestProperty, "3");
}
public void BuildTest()
{
// Build a heap
Heap<int, int> heap = new Heap<int, int>((i) => i);
int size = 255;
Random rnd = new Random();
for (int index = 0; index < size; index++)
{
heap.Add(rnd.Next(0,255));
}
// validate
Assert.IsFalse(heap.IsEmpty);
Assert.AreEqual<int>(8, GetMaxDepth(heap.root));
Assert.AreEqual<int>(8, GetMinDepth(heap.root));
Assert.IsTrue(IsHeap(heap.root));
}
public void Stress()
{
var heap = new Heap<int>(HeapType.Minimum);
const int maximum = 5000;
for (var i = maximum; i > 0; i--)
{
heap.Add(i);
Assert.AreEqual(heap.Root, i);
}
for (var i = 1; i <= maximum; i++)
{
Assert.AreEqual(heap.RemoveRoot(), i);
}
}
public void Simple()
{
var heap = new Heap<int>(HeapType.Minimum);
for (var i = 20; i > 0; i--)
{
heap.Add(i);
Assert.AreEqual(heap.Root, i);
}
Assert.IsFalse(heap.IsEmpty);
Assert.AreEqual(heap.Count, 20);
heap.Clear();
Assert.AreEqual(heap.Count, 0);
Assert.IsTrue(heap.IsEmpty);
}
public void TestHeap()
{
Heap<int> heap = new Heap<int>();
heap.Add(6);
heap.Add(2);
heap.Add(7);
heap.Add(1);
heap.Add(4);
heap.Add(5);
heap.Add(3);
Assert.AreEqual(1, heap.ExtractDominating());
Assert.AreEqual(2, heap.ExtractDominating());
Assert.AreEqual(3, heap.ExtractDominating());
Assert.AreEqual(4, heap.ExtractDominating());
Assert.AreEqual(5, heap.ExtractDominating());
Assert.AreEqual(6, heap.ExtractDominating());
Assert.AreEqual(7, heap.ExtractDominating());
}
/// <summary>
/// Finds the connections.
/// </summary>
/// <param name="start">Start.</param>
public void FindConnections(Node start)
{
Tile startTile = TileFromPosition (start.center);
Dictionary<Tile, Node> targets = new Dictionary<Tile, Node> ();
foreach (KeyValuePair<Vector3, Node> kv in nodes) {
if (kv.Value != start)
targets.Add (TileFromPosition (kv.Key), kv.Value);
}
if (startTile.cost == 255)
return;
Heap<Tile> frontier = new Heap<Tile> (sizeX * sizeY);
HashSet<Tile> visited = new HashSet<Tile> ();
frontier.Add (startTile);
while (frontier.size () > 0) {
Tile current = frontier.RemoveFirst ();
visited.Add (current);
Node intranode = null;
targets.TryGetValue (current, out intranode);
if (intranode != null && !intranode.ConnectedTo (start)) {
Node.Connect (start, intranode, current.distance);
}
foreach (Tile neighbour in GetNeighbours(current)) {
if (neighbour == null || neighbour.cost == 255 || visited.Contains (neighbour)) {
continue;
}
neighbour.distance = current.distance + 1;
if (!frontier.Contains (neighbour))
frontier.Add (neighbour);
}
}
}
#pragma warning restore 649
public void FindPath(PathRequest request, Action <PathResult> callback)
{
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
PathNode startNode = _grid.NodeFromWorldPoint(request.pathStart);
PathNode targetNode = _grid.NodeFromWorldPoint(request.pathEnd);
if (startNode.Walkable && targetNode.Walkable)
{
Heap <PathNode> openSet = new Heap <PathNode>(_grid.MaxSize);
HashSet <PathNode> closedSet = new HashSet <PathNode>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
PathNode currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
/*sw.Stop();
* UnityEngine.Debug.Log("Path found in " + sw.Elapsed);*/
pathSuccess = true;
break;
}
foreach (PathNode neighbour in _grid.GetNeighbours(currentNode))
{
if (!neighbour.Walkable || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour =
currentNode.GCost +
Mathf.RoundToInt(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));
}
}
/// <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));
}
}
internal static Path FindPath(NodeHandle startNode, NodeHandle targetNode, ConcurrentSet <NodeHandle> closedSet, uint maxMs, CancellationToken cancelToken, uint clearance, Action <NodeHandle> progress, bool debug = false)
{
var s = new Stopwatch();
findPathTimer.Start();
try {
Vector3 targetNodePos = Position(targetNode);
if (startNode == 0)
{
return(Fail($"[{targetNode}] FindPath failed: startNode is zero."));
}
if (targetNode == 0)
{
return(Fail($"[{targetNode}] FindPath failed: targetNode is zero."));
}
closedSetTimer.Start();
closedSet.Remove(startNode);
if (closedSet.Contains(targetNode))
{
return(Fail($"[{targetNode}] FindPath failed: targetNode is blocked"));
}
closedSetTimer.Stop();
// TODO: it would be best if we could combine fScore and openSet
// fScore should be a heap that re-heaps when a value updates
// isOpen(node) becomes fScore.Contains(node)
// var fScore = new Dictionary<NodeHandle, float>();
fScoreTimer.Start();
var fScore = new Heap <NodeHandle>();
fScore.Add(startNode, Estimate(startNode, targetNode));
fScoreTimer.Stop();
var cameFrom = new Dictionary <NodeHandle, NodeHandle>();
var gScore = new Dictionary <NodeHandle, float>();
gScore.TryAdd(startNode, 0);
float GScore(NodeHandle n) => gScore.ContainsKey(n) ? gScore[n] : float.MaxValue;
s.Start();
fScoreTimer.Start();
while (fScore.TryPop(out NodeHandle best))
{
fScoreTimer.Stop();
if (cancelToken.IsCancellationRequested)
{
return(Fail($"[{targetNode}] Cancelled."));
}
if (s.ElapsedMilliseconds > maxMs)
{
return(Fail($"[{targetNode}] Searching for too long, ({closedSet.Count} nodes in {s.ElapsedMilliseconds}ms."));
}
// close this node we are just about to visit
closedSetTimer.Start();
closedSet.Add(best);
closedSetTimer.Stop();
// update the progress callback
progress(best);
Vector3 curPos = Position(best);
float dist = (curPos - targetNodePos).LengthSquared();
// Log($"dist = {dist:F2}");
if (dist <= .5f)
{
var ret = new Path(UnrollPath(cameFrom, best, debug));
Log($"[{targetNode}] Found a path of {ret.Count()} steps ({closedSet.Count} searched in {s.ElapsedMilliseconds}ms)");
return(ret);
}
foreach (NodeHandle e in Edges(best))
{
closedSetTimer.Start();
bool closed = closedSet.Contains(e);
closedSetTimer.Stop();
if (!closed && Clearance(e) >= clearance)
{
gScoreTimer.Start();
Vector3 ePos = Position(e);
float scoreOfNewPath = GScore(best) + (curPos - ePos).Length();
float scoreOfOldPath = GScore(e);
gScoreTimer.Stop();
if (scoreOfNewPath < scoreOfOldPath)
{
cameFrom[e] = best;
gScore[e] = scoreOfNewPath;
fScoreTimer.Start();
fScore.AddOrUpdate(e,
gScore[e] // best path to e so far
+ Estimate(ePos, targetNodePos) // plus standard A* estimate
+ Abs(ePos.Z - curPos.Z) // plus a penalty for going vertical
+ ((15 - Clearance(e)) * .3f) // plus a penalty for low clearance
);
fScoreTimer.Stop();
}
}
}
fScoreTimer.Start();
}
return(Fail($"[{targetNode}] Searched all reachable nodes ({closedSet.Count} nodes in {s.ElapsedMilliseconds}ms)."));
} finally {
findPathTimer.Stop();
fScoreTimer.Stop();
}
}
/// <summary>
/// Finds the path.
/// </summary>
private IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
// Stopwatch sw = new Stopwatch ();
// sw.Start ();
Vector2[] wayPoints = new Vector2[0];
bool pathSuccess = false;
// Get our Nodes based on our Vector3 positions.
Node startNode = aStarGrid.NodeFromWorldPoint(startPos);
Node targetNode = aStarGrid.NodeFromWorldPoint(targetPos);
// IF our target node is walkable.
if (targetNode.GetWalkable())
{
// Create a Heap of Nodes with the max size being our 2D grid for A Star movement.
// All start off being openNodes.
Heap <Node> openSet = new Heap <Node>(aStarGrid.MaxSize);
// Create an empty HashSet of Nodes for our Nodes that are closed.
HashSet <Node> closedSet = new HashSet <Node>();
// Add our Starting Node to begin our pathing.
openSet.Add(startNode);
// WHILE we have Nodes in our Heap.
while (openSet.Count > 0)
{
// Pop the first Node off and that is our current Node.
Node currentNode = openSet.RemoveFirst();
// We know about this Node now so we move it to our closedSet.
closedSet.Add(currentNode);
// IF the Node we are on is the targetNode (our destination).
if (currentNode == targetNode)
{
// sw.Stop ();
// print ("Path found : " + sw.ElapsedMilliseconds + " ms");
// Our pathing has been successful
pathSuccess = true;
// Break the loop.
break;
}
// Loop to each neighbour in our A Star Pathfinding Grid.
foreach (Node neighbour in aStarGrid.GetNeighbours(currentNode))
{
// IF the Node is NOT Walkable OR the Node is in our closedSet.
if (!neighbour.GetWalkable() || closedSet.Contains(neighbour))
{
// We go to the next next.
continue;
}
// Get our movement cost if we were to move to this neighbour.
int newMovementCostToNeighbour = currentNode.GetGCost() + GetDistance(currentNode, neighbour) + neighbour.GetMovementPenalty();
// IF the new movement cost to move to our neighbour is less than our previous GCost OR
// IF the openSet does NOT contain our neighbour meaning we have not visited it yet
if (newMovementCostToNeighbour < neighbour.GetGCost() || !openSet.Contains(neighbour))
{
// Set our GCost
neighbour.SetGCost(newMovementCostToNeighbour);
// Set our HCost
neighbour.SetHCost(GetDistance(neighbour, targetNode));
// Set the parent of this node.
neighbour.SetParent(currentNode);
// IF this neighbour Node happened to be a node that is NOT in our openSet,
// ELSE this neighbour Node that IS in our openSet.
if (!openSet.Contains(neighbour))
{
// We add this neighbour node to our openSet.
openSet.Add(neighbour);
}
else
{
// It is in our openSet so lets update the Node.
openSet.UpdateItem(neighbour);
}
}
}
}
}
// IF we happened to find a path to our targetNode.
if (pathSuccess)
{
// Retrace our path and set those points as our wayPoints.
wayPoints = RetracePath(startNode, targetNode);
}
// Finalize the path processing.
requestManager.FinishedProcessingPath(wayPoints, pathSuccess);
yield return(null);
}
IEnumerator TrouverPath2(Vector3 positionDépart, Vector3 positionCible)
{
Stopwatch stopwatch = new Stopwatch();
stopwatch.Start();
Vector3[] wayPoints = new Vector3[0];
bool pathSuccess = false;
Node nodeDépart = Grille.NodePositionMonde(positionDépart);
Node nodeCible = Grille.NodePositionMonde(positionCible);
if (nodeCible.EstSurfacePourMarcher) //nodeDépart.EstSurfacePourMarcher && nodeCible.EstSurfacePourMarcher)
{
//List<Node> openSet = new List<Node>(Grille.TailleMax);
Heap<Node> openSet = new Heap<Node>(Grille.TailleMax);
HashSet<Node> closedSet = new HashSet<Node>();
openSet.Add(nodeDépart);
while (openSet.Count > 0)
{
//Node nodeActuel = openSet[0];
//for (int i = 1; i < openSet.Count; ++i)
//{
// if (openSet [i].FCost < nodeActuel.FCost || openSet [i].FCost == nodeActuel.FCost && openSet [i].HCost < nodeActuel.HCost)
// {
// nodeActuel = openSet [i];
// }
//}
//openSet.Remove (nodeActuel);
Node nodeActuel = openSet.EnleverPremier();
closedSet.Add(nodeActuel);
if (nodeActuel == nodeCible)
{
stopwatch.Stop();
Debug.Print("Path trouvé: " + stopwatch.ElapsedMilliseconds + " ms");
//RetracerPath(nodeDépart, nodeCible);
//return;
pathSuccess = true;
break;
}
foreach (Node voisin in Grille.GetVoisins(nodeActuel))
{
if (!voisin.EstSurfacePourMarcher || closedSet.Contains(voisin))
{
continue;
}
int CoutNouveauMouvementVoisin = nodeActuel.GCost + GetDistance(nodeActuel, voisin);
if (CoutNouveauMouvementVoisin < voisin.GCost || !openSet.Contains(voisin))
{
voisin.GCost = CoutNouveauMouvementVoisin;
voisin.HCost = GetDistance(voisin, nodeCible);
voisin.Parent = nodeActuel;
if (!openSet.Contains(voisin))
{
openSet.Add(voisin);
}
else
{
openSet.UpdateObjet(voisin);
}
}
}
}
}
yield return null; // // // non
if (pathSuccess) // // // non
{ // // // non
wayPoints = RetracerPath2(nodeDépart, nodeCible); // // // non
} // // // non
RequêtePathManager.FinishingProcessingPath(wayPoints, pathSuccess); // // // non
}
/// <summary>
/// Sparsify vector A (keep at most <paramref name="top"/>+<paramref name="bottom"/> values)
/// and optionally rescale values to the [-1, 1] range.
/// <param name="a">Vector to be sparsified and normalized.</param>
/// <param name="top">How many top (positive) elements to preserve after sparsification.</param>
/// <param name="bottom">How many bottom (negative) elements to preserve after sparsification.</param>
/// <param name="normalize">Whether to normalize results to [-1,1] range.</param>
/// </summary>
public static void SparsifyNormalize(ref VBuffer <Float> a, int top, int bottom, bool normalize)
{
Contracts.CheckParam(top >= 0, nameof(top), "Top count needs to be non-negative");
Contracts.CheckParam(bottom >= 0, nameof(bottom), "Bottom count needs to be non-negative");
Float absMax = 0;
// In the top heap, we pop the smallest values, so that the 'top' largest remain.
var topHeap = new Heap <KeyValuePair <int, Float> >((left, right) => right.Value < left.Value, top + 1);
var bottomHeap = new Heap <KeyValuePair <int, Float> >((left, right) => right.Value > left.Value, bottom + 1);
bool isDense = a.IsDense;
for (int i = 0; i < a.Count; i++)
{
int idx = isDense ? i : a.Indices[i];
var value = a.Values[i];
if (value < 0 && bottom > 0)
{
if (bottomHeap.Count == bottom && value > bottomHeap.Top.Value)
{
continue;
}
bottomHeap.Add(new KeyValuePair <int, float>(idx, value));
if (bottomHeap.Count > bottom)
{
bottomHeap.Pop();
Contracts.Assert(bottomHeap.Count == bottom);
}
}
if (value > 0 && top > 0)
{
if (topHeap.Count == top && value < topHeap.Top.Value)
{
continue;
}
topHeap.Add(new KeyValuePair <int, float>(idx, value));
if (topHeap.Count > top)
{
topHeap.Pop();
Contracts.Assert(topHeap.Count == top);
}
}
}
var newCount = topHeap.Count + bottomHeap.Count;
var indices = a.Indices;
Utils.EnsureSize(ref indices, newCount);
Contracts.Assert(Utils.Size(a.Values) >= newCount);
int count = 0;
while (topHeap.Count > 0)
{
var pair = topHeap.Pop();
indices[count] = pair.Key;
a.Values[count++] = pair.Value;
}
while (bottomHeap.Count > 0)
{
var pair = bottomHeap.Pop();
indices[count] = pair.Key;
a.Values[count++] = pair.Value;
}
Contracts.Assert(count == newCount);
if (normalize)
{
for (var i = 0; i < newCount; i++)
{
var value = a.Values[i];
var absValue = Math.Abs(value);
if (absValue > absMax)
{
absMax = absValue;
}
}
if (absMax != 0)
{
var ratio = 1 / absMax;
for (var i = 0; i < newCount; i++)
{
a.Values[i] = ratio * a.Values[i];
}
}
}
if (indices != null)
{
Array.Sort(indices, a.Values, 0, newCount);
}
a = new VBuffer <float>(a.Length, newCount, a.Values, indices);
}
//replaced durch Courutine
////Updates with the movement of the gameObject attached to (seeker = player), (Target = opposite player)
//void Update()
//{
// //Only change if we ask it to
// if (Input.GetButtonDown("Jump"))
// FindPath(seeker.position, target.position);
//}
//ones it found a path it needs to call the finishprocessingpath of the pathrequest manager script
public void FindPath(PathRequest request, Action <PathResult> callback)
{
//To see performance gain
Stopwatch sw = new Stopwatch();
sw.Start();
Vector3[] waypoints = new Vector3[0];
//check if we find a path
bool pathSuccess = false;
//Convert Worldpos in Nodes, already done in grid method
Node startNode = grid.NodeFromWorldPoint(request.pathStart);
Node targetNode = grid.NodeFromWorldPoint(request.pathEnd);
//Openset of nodes to be evaluated
//List<Node> openSet = new List<Node>();
//if they are not walkable its impossible to find a path
if (startNode.walkable && targetNode.walkable)
{
//Optimising with Heap
Heap <Node> openSet = new Heap <Node>(grid.MaxSize);
//Closedset of nodes to be already evaluated
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
//search for node with lowes f_cost is the most expensive part of algorithm
while (openSet.Count > 0)
{
//find Node with the lowest f_cost
Node currentNode = openSet.RemoveFirst();
////now we found the node with the lowest fCost in the openset
////remove it from openSet and add to closeSet
//openSet.Remove(currentNode);
closedSet.Add(currentNode);
//we found ouer path
if (currentNode == targetNode)
{
sw.Stop();
print("Path found: " + sw.ElapsedMilliseconds + " ms");
//found path
pathSuccess = true;
break;
}
//if not found the past, loop through each neighbouring node of current node
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
//check if the neighbour in the not walkable or in the close list, than skip
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
//Check if the new path to the neighbar is shorter than the old one, or if the neighbour is not in the node list
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;
if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
//set the fXost of the neighbour
//we calculate the gCost and hCost
neighbour.gCost = newMovementCostToNeighbour;
//distance from the node to the end node
neighbour.hCost = GetDistance(neighbour, targetNode);
//set the parent of the neighbour to the current node
neighbour.parent = currentNode;
//check if the neighbour is not in the open set, if not add to open set
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
}
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
//fix problem when target moves only a little bit
pathSuccess = waypoints.Length > 0;
}
callback(new PathResult(waypoints, pathSuccess, request.callback));
}
private IEnumerator FindThePath(Vector3 startPos, float waterLevel)
{
if (grid.nodes == null || grid.nodes.Length == 0)
{
ScanGrid();
}
Path p = new Path();
bool pathSuccess = false;
Node startNode = grid.NearWalkable(startPos);
Node targetNode = grid.NearWaterable(startPos, waterLevel);
if (startNode == null || targetNode == null)
{
OnProccesingDone(p, pathSuccess);
yield break;
}
Node last = null;
if (startNode.Walkable)
{
Heap <Node> open = new Heap <Node>(grid.maxSize);
HashSet <Node> closed = new HashSet <Node>();
open.Add(startNode);
while (open.Count > 0)
{
Node currentNode = open.RemoveFirst();
closed.Add(currentNode);
if (open.Count == 1)
{
last = currentNode;
pathSuccess = true;
break;
}
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
if (neighbour == null || !neighbour.Walkable || closed.Contains(neighbour) || neighbour.height > currentNode.height || currentNode.height <= waterLevel)
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + grid.GetDistance(currentNode, neighbour);
if ((newMovementCostToNeighbour < neighbour.gCost || !open.Contains(neighbour)))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = grid.GetDistance(neighbour, targetNode);
neighbour.parent = currentNode;
if (!open.Contains(neighbour))
{
open.Add(neighbour);
}
}
}
}
}
if (pathSuccess)
{
p = grid.RetracePath(startNode, last);
}
OnProccesingDone(p, true);
}
public bool FindPath(Vector2Int start, Vector2Int end)
{
Reset();
//如果直接可以通过就不做寻路
if (CheckWalkAble(start, end))
{
findResult.Add(end);
findResult.Add(start);
return(true);
}
if (start == end)
{
return(true);;
}
PathNode endNode = GetNode(end.x, end.y);
PathNode startNode = GetNode(start.x, start.y);
openList.Add(startNode);
while (openList.Count > 0)
{
var currentNode = openList.RemoveFirst();
closeSet.Add(currentNode.Index);
if (endNode == currentNode)
{
break;
}
Vector2Int currPos = new Vector2Int(Grid.GetX(currentNode.Index), Grid.GetY(currentNode.Index));
for (int i = 0; i < SurroundOffSet.Length; ++i)
{
var offset = SurroundOffSet[i];
Vector2Int pos = offset + currPos;
if (!Grid.Get(pos.x, pos.y))
{
continue;
}
var neighbour = GetNode(pos.x, pos.y);
if (closeSet.Contains(neighbour.Index))
{
continue;
}
int cost = SurroundCost[i];
int G = currentNode.G + cost;
if (openList.Contains(neighbour))
{
if (G < neighbour.G)
{
neighbour.Parent = currentNode;
neighbour.G = G;
}
}
else
{
neighbour.Parent = currentNode;
neighbour.G = G;
neighbour.H = CalcH(pos, end);
openList.Add(neighbour);
}
}
}
if (endNode.Parent != null)
{
findResult.Add(end);
var prePos = Grid.ToPos(endNode.Parent.Index);
findResult.Add(prePos);
Vector2Int normal = prePos - end;
var preNode = endNode.Parent.Parent;
//平滑路径处理
while (preNode != null)
{
Vector2Int curPos = Grid.ToPos(preNode.Index);
Vector2Int newNormal = curPos - prePos;
if (newNormal == normal)
{
findResult[findResult.Count - 1] = curPos;
}
else
{
findResult.Add(curPos);
normal = newNormal;
}
prePos = curPos;
preNode = preNode.Parent;
}
findResult.Reverse();
return(true);
}
return(false);
}
public Vector3 generatePath(Vector3 _position, Vector3 _target, ref List <Vector3> _path)
{
int count = 0;
if (collisionMap.CollisionTable == null)
{
return(Vector3.zero);
}
// if _target not walkable, re-compute to closer walkable node
// if _start not walkable something bad happened, break and debug
// create start node
int startX = (int)_position.x;
int startY = (int)-_position.y;
Node startNode = new Node(!collisionMap.CheckCollision(startX, startY), startX, startY);
if (!startNode.walkable)
{
Debug.Log("Unwalkable start node.");
return(Vector3.zero);
}
// create target node
int targetX = (int)_target.x;
int targetY = (int)-_target.y;
Node targetNode = new Node(!collisionMap.CheckCollision(targetX, targetY), targetX, targetY);
// if target not walkable, consider moving target to closest walkable node
Heap <Node> openSet = new Heap <Node>(collisionMap.MapWidth * collisionMap.MapHeight * 10);
List <Node> closedSet = new List <Node>();
openSet.Add(startNode);
// main loop
while (openSet.Count > 0)
{
count++;
if (count >= breakCount)
{
return(Vector3.zero);
}
// get node in openSet with lowest fCost
Node current = openSet.RemoveFirst();
closedSet.Add(current);
// check if we found target Node
if (current.Equals(targetNode))
{
Debug.Log("Path found: " + new Vector3(current.x, current.y, 0.0f));
_path = ReconstructPath(current);
return(new Vector3(current.x, current.y, 0.0f));
}
for (int i = 0; i < dirMap.Length; i++)
{
// create new Nodes based on dir and check for collision
int neighX = current.x + dirMap[i].x;
int neighY = current.y + dirMap[i].y;
Node neighbour = new Node(!collisionMap.CheckCollision(neighX, neighY), neighX, neighY);
if (!neighbour.walkable)
{
continue;
}
if (closedSet.Contains(neighbour))
{
continue;
}
int tempGScore = current.gCost + 1;
if (tempGScore < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = tempGScore;
neighbour.hCost = GetDistance(neighbour, targetNode);
neighbour.parent = current;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
// failed
Debug.Log("No path found");
return(Vector3.zero);
}
public void FindPath(PathRequest a_request, Action <PathResult> a_callback)
{
Vector3[] wayPoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = grid.NodeFromWorldPoint(a_request.pathStart);
Node targetNode = grid.NodeFromWorldPoint(a_request.pathEnd);
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.RemoveFirstItem();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
// Found the path
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);
}
}
}
}
}
if (pathSuccess)
{
wayPoints = RetracePath(startNode, targetNode);
pathSuccess = wayPoints.Length > 0;
}
a_callback(new PathResult(wayPoints, pathSuccess, a_request.callBack));
}
//for Ai
public Node[] AIFindPath(Vector3 startPos, Vector3 targetPos, bool canFly, int unitPlayerID)
{
bool pathSuccess = false;
HeuristicFunction heuristicFunction = new HeuristicFunction(GetDistance);
Node startNode = gridRef.NodeFromWorldPoint(startPos);
Node targetNode = gridRef.NodeFromWorldPoint(targetPos);
Unit currentUnit = startNode.GetUnit();
if (startNode.canWalkHere && targetNode.canWalkHere)
{
Heap <Node> openSet = new Heap <Node>(gridRef.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 gridRef.GetNeighbours(currentNode))
{
bool checkHostile = false;
if (neighbour.GetUnit() != null)
{
if (neighbour.GetUnit().GetUnitPlayerID() != unitPlayerID)
{
checkHostile = true;
}
}
if ((!canFly && !neighbour.canWalkHere) || closedSet.Contains(neighbour) || checkHostile)
{
continue; //Skips unwalkable nodes when unit cannot fly, or if any node in closed set or if the unit in the node is hostile
//considers unwalkable nodes if unit can fly, and ignores any node if in closed set
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = heuristicFunction(neighbour, targetNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
}
}
}
}
if (pathSuccess)
{
Node[] waypoints = RetracePath(startNode, targetNode);
currentUnit.SetMovementSpeedLeft(currentUnit.GetMovementSpeedLeft() - (waypoints.Length - 1));
return(waypoints);
}
return(new[] { startNode });
}
public void FindPath(PathRequest request, Action <PathResult> callback)
{
Stopwatch sw = new Stopwatch();
sw.Start();
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = _tilemapReader.NodeFromWorldPoint(request.pathStart);
Node targetNode = _tilemapReader.NodeFromWorldPoint(request.pathEnd);
startNode.parent = startNode;
if (startNode.walkable && targetNode.walkable)
{
Heap <Node> openSet = new Heap <Node>(_tilemapReader.MaxTileSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
sw.Stop();
//print ("Path found: " + sw.ElapsedMilliseconds + " ms");
pathSuccess = true;
break;
}
foreach (Node neighbour in _tilemapReader.GetNeighbours(currentNode))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
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 endPos)
{
//heap 구조 시간
Stopwatch sw = new Stopwatch();
sw.Start();
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
//시작 노드와 도착 노드 를 그리드 좌표로
Node startNode = grid.NodeFromWorldPoint(startPos);
Node endNode = grid.NodeFromWorldPoint(endPos);
if (startNode.walkable && endNode.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);
//최근 노드와 도착노드가 같다면 끝낸다(while문 탈출)
if (currentNode == endNode)
{
sw.Stop();
//print("Path found : " + sw.ElapsedMilliseconds + "ms");
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, endNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
}
//이부분은 아직 도착노드에 하지 않은 상태이므로 다시 while 문 실행
}
}
}
yield return(null);
if (pathSuccess)
{
waypoints = RetracePath(startNode, endNode);
}
requestManager.FinishedProcessingPath(waypoints, pathSuccess);
}
/// <summary>
/// Finds the shortest path from the start node to the goal node
/// </summary>
/// <param name="AStartNode">Start node</param>
/// <param name="AGoalNode">Goal node</param>
public void FindPath(AStarNode AStartNode, AStarNode AGoalNode)
{
FStartNode = AStartNode;
FGoalNode = AGoalNode;
FOpenList.Add(FStartNode);
int i = 0;
while (FOpenList.Count > 0 && i < 2000)
{
// Get the node with the lowest TotalCost
AStarNode NodeCurrent = (AStarNode)FOpenList.Pop();
// If the node is the goal copy the path to the solution array
if (NodeCurrent.IsGoal())
{
while (NodeCurrent != null)
{
FSolution.Insert(0, NodeCurrent);
NodeCurrent = NodeCurrent.Parent;
}
break;
}
// Get successors to the current node
NodeCurrent.GetSuccessors(FSuccessors);
foreach (AStarNode NodeSuccessor in FSuccessors)
{
// Test if the currect successor node is on the open list, if it is and
// the TotalCost is higher, we will throw away the current successor.
AStarNode NodeOpen = null;
if (FOpenList.Contains(NodeSuccessor))
{
NodeOpen = (AStarNode)FOpenList[FOpenList.IndexOf(NodeSuccessor)];
}
if ((NodeOpen != null) && (NodeSuccessor.TotalCost > NodeOpen.TotalCost))
{
continue;
}
// Test if the currect successor node is on the closed list, if it is and
// the TotalCost is higher, we will throw away the current successor.
AStarNode NodeClosed = null;
if (FClosedList.Contains(NodeSuccessor))
{
NodeClosed = (AStarNode)FClosedList[FClosedList.IndexOf(NodeSuccessor)];
}
if ((NodeClosed != null) && (NodeSuccessor.TotalCost > NodeClosed.TotalCost))
{
continue;
}
// Remove the old successor from the open list
FOpenList.Remove(NodeOpen);
// Remove the old successor from the closed list
FClosedList.Remove(NodeClosed);
// Add the current successor to the open list
FOpenList.Push(NodeSuccessor);
}
// Add the current node to the closed list
FClosedList.Add(NodeCurrent);
i++;
}
if (i == 2000)
{
m_pathPossible = false;
}
}
void FindPath(Vector3 startPosition, Vector3 targetPosition)
{
Stopwatch sw = new Stopwatch();
sw.Start();
Node startNode = grid.getNodeFromWorldPoint(startPosition);
Node targetNode = grid.getNodeFromWorldPoint(targetPosition);
//List<Node> openSet = new List<Node>();
Heap <Node> openSet = new Heap <Node>(grid.MaxHeapSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
//Node currentNode = openSet[0];
//// find node with lowest f-cost; costly part of the algorithm.
//for(int x = 1; x < openSet.Count; x++)
//{
// if(openSet[x].fCost < currentNode.fCost || openSet[x].fCost == currentNode.fCost)
// {
// if(openSet[x].hCost < currentNode.hCost)
// {
// currentNode = openSet[x];
// }
// }
//}
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
sw.Stop();
RetracePath(startNode, targetNode);
print("Found path in: " + sw.ElapsedMilliseconds + " ms");
return;
}
foreach (Node neighbor in grid.GetNeighbors(currentNode))
{
if (!neighbor.walkable || closedSet.Contains(neighbor))
{
continue;
}
int newCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbor);
if (newCostToNeighbour < neighbor.gCost || !openSet.Contains(neighbor))
{
neighbor.gCost = newCostToNeighbour;
neighbor.hCost = GetDistance(neighbor, targetNode);
neighbor.parent = currentNode;
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
}
}
}
}
private IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
//Debug.Log($"START: {startPos}, END: {targetPos}");
// Stopwatch to see the performance gain through heap optimization
Stopwatch sw = new Stopwatch();
// Start stopwatch
sw.Start();
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = graph.NodeFromWorldPoint(startPos);
Node targetNode = graph.NodeFromWorldPoint(targetPos);
// If the start or end nodes are not walkable do not bother finding a path
if (startNode.walkable && targetNode.walkable)
{
// Implementing the Heap Optimization into the former lists.
Heap <Node> openSet = new Heap <Node>(graph.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
// This is equal to the first node in the open set
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
// Stop stopwatch
sw.Stop();
print("Path found: " + sw.ElapsedMilliseconds + " ms");
pathSuccess = true;
break;
}
foreach (Node neighbour in graph.GetNeighbours(currentNode))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
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);
}
}
}
}
}
// wait one frame before returning
yield return(null);
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
}
requestManager.FinishedProcessingPath(waypoints, pathSuccess);
}
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
Stopwatch sw = new Stopwatch();
sw.Start();
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)
{
sw.Stop();
print("Path found: " + sw.ElapsedMilliseconds + " ms");
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);
}
/* This function finds a path between 2 nodes
*
* @param startPos - the starting node
* @param targetPos - the target node
*
*/
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
Stopwatch sw = new Stopwatch();
sw.Start();
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();
/* This part of the algorithm is costly when using a list.
* Comparing the fCost, or hCost as a backup, to get the least cost node before proceeding with the algorithm
* By using a heap instead, finding a path takes less than one fourth of the time.
*
* Node currentNode = openSet[0];
* for (int i = 1; i< openSet.Count; i++) // Most costly part of algorithm
* {
* 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);
// We have reached the targetNode (found the path)
if (currentNode == targetNode)
{
sw.Stop();
print("Path found: " + sw.ElapsedMilliseconds + "ms");
pathSuccess = true;
RetracePath(startNode, targetNode);
break; // Coroutine requires this instead of return to exit pathfinding loop
}
foreach (Node neighbor in grid.GetNeighbors(currentNode))
{
if (!neighbor.walkable || closedSet.Contains(neighbor))
{
continue;
}
int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor) + neighbor.movementPenalty; //movementPenalty for weighted cost
if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
{
neighbor.gCost = newMovementCostToNeighbor;
neighbor.hCost = GetDistance(neighbor, targetNode);
neighbor.parent = currentNode;
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
else
{
openSet.UpdateItem(neighbor);
}
}
}
}
}
// wait for one frame before returning
yield return(null);
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
}
requestManager.FinishedProcessingPath(waypoints, pathSuccess);
}
/// <summary>
/// Calculates Path from Start Node to Target Node and Returns List of Nodes within Path.
/// </summary>
/// <param name="a_startNode"></param>
/// <param name="a_targetNode"></param>
/// <returns></returns>
private List <Node> PathFind(Node a_startNode, Node a_targetNode)
{
// Algorithm Begins
List <Node> returnPath = new List <Node>();
// 2 Lists Required
// Heap
Heap <Node> openSet = new Heap <Node>(nodeGrid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
// Add Start Node to Open Set
openSet.Add(a_startNode);
// Run as Long as we have unvisited nodes
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode.Equals(a_targetNode))
{
// Destination Found, Retrace Path Here
returnPath = RetracePath(a_startNode, currentNode);
break;
}
// Still Havent Reached Target
foreach (Node neighbour in NodeGrid.Instance.GetNeighbours(currentNode))
{
// Check if Neighbour is walkable or hasn't already been checked
if (closedSet.Contains(neighbour))
{
continue;
}
// Create new movement cost
float MovementCostToNeighbour = currentNode.GCost + GetDistance(currentNode, neighbour);
if (MovementCostToNeighbour < neighbour.GCost || !openSet.Contains(neighbour))
{
// Calculate new costs
neighbour.GCost = MovementCostToNeighbour;
neighbour.HCost = GetDistance(neighbour, a_targetNode);
// Assign Previous Node
neighbour.PreviousNode = currentNode;
// Add neighbour to open set
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
// Hash Set Update Item
openSet.UpdateItem(neighbour);
}
}
}
}
return(returnPath);
}
public void FindPath(PathRequest request, Action <PathResult> callback)
{
//Stopwatch sw = new Stopwatch();
//sw.Start();
Vector3[] waypoints = new Vector3[0];
bool pathFound = false;
PathNode startNode = grid.NodeFromWorldPoint(request.pathStart);
PathNode endNode = grid.NodeFromWorldPoint(request.pathEnd);
if (endNode.clear != PathNode.Passability.Obstacle)
{
Heap <PathNode> openSet = new Heap <PathNode>(grid.MaxSize);
HashSet <PathNode> closedSet = new HashSet <PathNode>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
PathNode currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
// Found path
if (currentNode == endNode)
{
//sw.Stop();
pathFound = true;
break; // Exit out of while
}
// Check neighbors
foreach (PathNode neighbor in grid.FindNeighbors(currentNode))
{
// Obstacle or already in our set
if (neighbor.clear == PathNode.Passability.Obstacle || closedSet.Contains(neighbor))
{
continue;
}
// Lower cost found or an unitialized node
int newMoveCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor);
if (newMoveCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
{
// Initialize movement costs
neighbor.gCost = newMoveCostToNeighbor;
neighbor.hCost = GetDistance(neighbor, endNode);
neighbor.parent = currentNode;
// Add to open set
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
// Updates automatically
}
else // Must manually update because costs changed
{
openSet.UpdateItem(neighbor);
}
}
}
} // while nodes in openset
} // endpoints clear
if (pathFound)
{
waypoints = RetracePath(startNode, endNode, request.pathStart, request.pathEnd);
pathFound = waypoints.Length > 0; // Waypoints could be empty
}
callback(new PathResult(waypoints, pathFound, request.callback));
} // FindPath()
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;
}
List <Node> nodeTest = grid.getNeighbours(currentNode);
foreach (Node neighbour in grid.getNeighbours(currentNode))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
if (neighbour.movementPenalty > 0)
{
Debug.Log("Tough Road");
}
int newMovementCost = currentNode.gCost + getDistance(currentNode, neighbour) + neighbour.movementPenalty;
if (newMovementCost < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newMovementCost;
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);
}
public static Graph <T> KruskalsAlgorithm <T>(Graph <T> weightedGraph)
{
Guard.ArgumentNotNull(weightedGraph, "weightedGraph");
// There is going be Vertices - 1 edges when we finish
var edgeCount = weightedGraph.Vertices.Count - 1;
var vertexToParent = new Dictionary <Vertex <T>, Vertex <T> >();
var oldToNew = new Dictionary <Vertex <T>, Vertex <T> >();
var edgeQueue =
new Heap <Association <double, Edge <T> > >(
HeapType.Minimum,
new AssociationKeyComparer <double, Edge <T> >());
// Now build the return graph, always return non directed.
var returnGraph = new Graph <T>(false);
// As we mew the new vertices for the new graph from the old
// one, we also map the old ones to the new ones, and set up
// our dictionary used to track forests of vertices.
foreach (var vertex in weightedGraph.Vertices)
{
var vertex2 = new Vertex <T>(vertex.Data);
oldToNew.Add(vertex, vertex2);
returnGraph.AddVertex(vertex2);
vertexToParent.Add(vertex, null);
}
// We need to move the edges into a priority queue
// and use the weight in the association to sort them.
foreach (var e in weightedGraph.Edges)
{
edgeQueue.Add(new Association <double, Edge <T> >(e.Weight, e));
}
// We know when we are done, when we hit the number of edges
// or when there is no more edges.
while ((edgeQueue.Count > 0) && (edgeCount > 0))
{
// Pull off the least weight edge that hasn't been added or discarded yet.
var association = edgeQueue.RemoveRoot();
// Save the value of the association to the proper type, an edge
var edge = association.Value;
// Here is the start of search to make find the heads
// of the forest, because if they are the same heads,
// there is a cycle.
var fromVertexHead = edge.FromVertex;
var toVertexHead = edge.ToVertex;
// Find the head vertex of the forest the fromVertex is in
while (vertexToParent[fromVertexHead] != null)
{
fromVertexHead = vertexToParent[fromVertexHead];
}
// Find the head vertex of the forest the toVertex is in
while (vertexToParent[toVertexHead] != null)
{
toVertexHead = vertexToParent[toVertexHead];
}
// Check to see if the heads are the same
// if are equal, it is a cycle, and we cannot
// include the edge in the new graph.
if (fromVertexHead != toVertexHead)
{
// Join the FromVertex forest to the ToVertex
vertexToParent[fromVertexHead] = edge.ToVertex;
// We have one less edge we need to find
edgeCount--;
// Add the edge to the new new graph, map the old vertices to the new ones
returnGraph.AddEdge(oldToNew[edge.FromVertex], oldToNew[edge.ToVertex], edge.Weight);
}
}
// All done :)
return(returnGraph);
}
Vector2[] FindPath(Vector2 from, Vector2 to)
{
location = gameObject.transform.position;
Stopwatch sw = new Stopwatch();
sw.Start();
Vector2[] waypoints = new Vector2[0];
bool pathSuccess = false;
Node startNode = grid.NodeFromWorldPoint(from - location);
Node targetNode = grid.NodeFromWorldPoint(to - location);
startNode.parent = startNode;
if (!startNode.walkable)
{
startNode = grid.ClosestWalkableNode(startNode);
}
if (!targetNode.walkable)
{
targetNode = grid.ClosestWalkableNode(targetNode);
}
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)
{
sw.Stop();
// print("Path found: " + sw.ElapsedMilliseconds + " ms");
pathSuccess = true;
break;
}
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + TurningCost(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);
}
}
}
}
}
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
}
return(waypoints);
}
public static Graph <T> DijkstrasAlgorithm <T>(Graph <T> weightedGraph, Vertex <T> fromVertex)
{
#region Parameter Checks
Guard.ArgumentNotNull(weightedGraph, "weightedGraph");
Guard.ArgumentNotNull(fromVertex, "fromVertex");
if (!weightedGraph.ContainsVertex(fromVertex))
{
throw new ArgumentException(Graph <T> .couldNotBeFoundInTheGraph, "fromVertex");
}
#endregion
var heap =
new Heap <Association <double, Vertex <T> > >(
HeapType.Minimum,
new AssociationKeyComparer <double, Vertex <T> >());
var vertexStatus = new Dictionary <Vertex <T>, VertexInfo <T> >();
// Initialise the vertex distances to the maximum possible.
foreach (var vertex in weightedGraph.Vertices)
{
vertexStatus.Add(vertex, new VertexInfo <T>(double.MaxValue, null, false));
}
vertexStatus[fromVertex].Distance = 0;
// Add the source vertex to the heap - we'll be branching out from it.
heap.Add(new Association <double, Vertex <T> >(0, fromVertex));
while (heap.Count > 0)
{
var item = heap.RemoveRoot();
var vertexInfo = vertexStatus[item.Value];
if (!vertexInfo.IsFinalised)
{
var edges = item.Value.EmanatingEdges;
vertexStatus[item.Value].IsFinalised = true;
// Enumerate through all the edges emanating from this node
for (var i = 0; i < edges.Count; i++)
{
var edge = edges[i];
var partnerVertex = edge.GetPartnerVertex(item.Value);
// Calculate the new distance to this distance
var distance = vertexInfo.Distance + edge.Weight;
var newVertexInfo = vertexStatus[partnerVertex];
// Found a better path, update the vertex status and add the
// vertex to the heap for further analysis
if (distance < newVertexInfo.Distance)
{
newVertexInfo.EdgeFollowed = edge;
newVertexInfo.Distance = distance;
heap.Add(new Association <double, Vertex <T> >(distance, partnerVertex));
}
}
}
}
return(BuildGraphDijkstra(weightedGraph, fromVertex, vertexStatus));
}
private IEnumerator FindPath(Vector3 _startPosition, Vector3 _targetPosition)
{
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
sw.Start();
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = AStarGrid.g.GetNodeFromWorldPosition(_startPosition);
Node targetNode = AStarGrid.g.GetNodeFromWorldPosition(_targetPosition);
//Debug for draw gizmos
AStarGrid.g.targetNode = targetNode;
//if the nodes are not obstacles
if (startNode.walkable && targetNode.walkable)
{
//Nodes to evaluate
Heap <Node> openList = new Heap <Node>((int)(AStarGrid.g.gridSize.x * AStarGrid.g.gridSize.y));
//Already evaluated nodes
List <Node> closedList = new List <Node>();
startNode.gCost = 0;
openList.Add(startNode);
int safetyCheck = 0;
//Loop through open list
while (openList.Count > 0)
{
//Debug.Log("Looping through the open list...");
safetyCheck++;
//Can only search in a 20 by 20 space so max 400
if (safetyCheck > 400)
{
break;
}
//Remove current node from the open list because it is being evaluated
Node currentNode = openList.RemoveFirstItem();
// -- Very slow, optimised using the heap -- //
/*Node lowestFCostNode = null;
* foreach(Node n in openList)
* {
* if ((lowestFCostNode != null && lowestFCostNode.fCost > n.fCost)
|| (n.fCost == lowestFCostNode.fCost && n.hCost < lowestFCostNode.hCost))
|| lowestFCostNode = n;
||}
||Node currentNode = lowestFCostNode;*/
//Add the current node to the closed (evaluated) list
closedList.Add(currentNode);
//Path found
if (currentNode == targetNode)
{
sw.Stop();
pathSuccess = true;
Statistics.instance.SavePathfindingTime(sw.ElapsedMilliseconds);
//Debug.Log("Path found in " + sw.ElapsedMilliseconds + " ms");
break;
}
foreach (Node neighbour in AStarGrid.g.GetNodeNeighbours(currentNode))
{
//if the neighbour is an obstacle or has already been evaluated
//or is in a building
if (!neighbour.walkable || closedList.Contains(neighbour))
{
continue;
}
int movementCost = currentNode.gCost + GetDistanceBetweenNodes(currentNode, neighbour);
if (movementCost < neighbour.gCost || !openList.Contains(neighbour))
{
//set f cost of neighbour (g cost + h cost)
neighbour.gCost = movementCost;
neighbour.hCost = GetDistanceBetweenNodes(neighbour, targetNode);
//set parent of neighbour to current
neighbour.parentNode = currentNode;
if (!openList.Contains(neighbour))
{
openList.Add(neighbour);
}
else
{
openList.UpdateItem(neighbour);
}
}
}
//yield return null;
}
}
if (pathSuccess)
{
waypoints = DefinePath(startNode, targetNode);
}
//else
//{
// Debug.LogWarning("Path not found!");
//}
prm.FinishedProcessingPath(waypoints, pathSuccess);
yield return(null);
}
//! 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);
}
List <Node> FindPath(Vector3 startPos, Node targetNode)
{
//Stopwatch sw = new Stopwatch ();
//sw.Start ();
Node startNode = grid.NodeFromWorldPoint(startPos);
//Node targetNode = grid.NodeFromWorldPoint (targetPos); // OBSELETE
RaycastHit2D hit = Physics2D.Linecast(startNode.worldPosition, targetNode.worldPosition, 8); // Base case -- saves performance?
//Debug.DrawLine(startNode.worldPosition,targetNode.worldPosition,Color.red);
if (hit == null)
{
List <Node> ShortList = new List <Node> ();
ShortList.Add(startNode);
ShortList.Add(targetNode);
return(ShortList);
}
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)
{
//sw.Stop ();
//print ("Path Found " + sw.ElapsedMilliseconds + "ms");
return(RetracePath(startNode, targetNode));
}
foreach (Node neighbor in grid.GetNeighbors(currentNode))
{
if (!neighbor.walkable || closedSet.Contains(neighbor))
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbor);
if (newMovementCostToNeighbour < neighbor.gCost || !openSet.Contains(neighbor))
{
neighbor.gCost = newMovementCostToNeighbour;
neighbor.hCost = GetDistance(neighbor, targetNode);
neighbor.parent = currentNode;
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
}
}
}
return(null);
}
private Vector3[] FindPath(Vector3 start, Vector3 end)
{
Node startNode = grid.pathGridData.NodeFromWorldPoint(start);
Node endNode = grid.pathGridData.NodeFromWorldPoint(end);
Node recentWalk = startNode;
int minDistance = int.MaxValue;
Heap <Node> openSet = new Heap <Node>(grid.pathGridData.MaxSize);
HashSet <Node> closeSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
closeSet.Add(currentNode);
if (currentNode == endNode)
{
// 找到了
break;
}
// 获取相邻节点
List <Node> neighbours = grid.pathGridData.GetNeighbours(currentNode);
foreach (Node n in neighbours)
{
if (!n.walkable || closeSet.Contains(n))
{
continue;
}
int newCostToNeighbours = currentNode.gCost + GetDistence(currentNode, endNode);
if (newCostToNeighbours < n.gCost || !openSet.Contains(n))
{
// 更新 gCost
n.gCost = newCostToNeighbours;
int hCost = GetDistence(n, endNode);
n.hCost = hCost;
n.parent = currentNode;
if (!openSet.Contains(n))
{
openSet.Add(n);
// 更新最近的节点
if (hCost < minDistance)
{
minDistance = hCost;
recentWalk = n;
}
}
else
{
openSet.UpdateItem(n);
}
}
}
}
// 获取路径
if (recentWalk == startNode)
{
//在起始点没动
return(null);
}
else
{
return(RetarcePath(startNode, recentWalk));
}
}
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
Vector3[] wayPoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = grid.GetNodeFromWorld(startPos);
Node targetNode = grid.GetNodeFromWorld(targetPos);
//Debug.Log(startNode.walkable);
if (true) //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();
//for (int i = 1; i < openSet.Count; i++) {
// if (currentNode.fCost > openSet[i].fCost || (currentNode.fCost == openSet[i].fCost &&
// currentNode.hCost > openSet[i].hCost))
// {
// currentNode = openSet[i];
// }
//}
//openSet.Remove(currentNode);
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
//Debug.Log("find");
pathSuccess = true;
break;
}
foreach (Node neighbor in grid.GetNeighbors(currentNode))
{
if (!neighbor.walkable || closedSet.Contains(neighbor))
{
continue;
}
int newCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor) + neighbor.movementPenalty;
if (newCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
{
neighbor.gCost = newCostToNeighbor;
neighbor.hCost = GetDistance(neighbor, targetNode);
neighbor.parent = currentNode;
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
else
{
openSet.UpdateItem(neighbor);
}
}
}
}
}
yield return(null);
if (pathSuccess)
{
wayPoints = RetracePath(startNode, targetNode);
}
pathRequestManager.FinishProcessingPath(wayPoints, pathSuccess);
}
/// <summary>
/// Finds the shortest paths to all other vertices from the specified source vertex.
/// </summary>
/// <param name="weightedGraph"> The weighted graph. </param>
/// <param name="fromVertex"> The source vertex. </param>
/// <returns> A graph representing the shortest paths from the source node to all other nodes in the graph. </returns>
public static Graph <T> FindShortestPaths(Graph <T> weightedGraph, Vertex <T> fromVertex)
{
#region Parameter Checks
if (weightedGraph == null)
{
throw new ArgumentNullException("weightedGraph");
}
if (fromVertex == null)
{
throw new ArgumentNullException("fromVertex");
}
if (!weightedGraph.ContainsVertex(fromVertex))
{
throw new ArgumentException(Properties.Resources.VertexCouldNotBeFound);
}
#endregion
var heap =
new Heap <Association <double, Vertex <T> > >(
HeapType.MinHeap,
new AssociationKeyComparer <double, Vertex <T> >());
var vertexStatus = new Dictionary <Vertex <T>, VertexInfo <T> >();
// Initialise the vertex distances to
using (var verticeEnumerator = weightedGraph.Vertices) while (verticeEnumerator.MoveNext())
{
vertexStatus.Add(verticeEnumerator.Current, new VertexInfo <T>(double.MaxValue, null, false));
}
vertexStatus[fromVertex].Distance = 0;
// Add the source vertex to the heap - we'll be branching out from it.
heap.Add(new Association <double, Vertex <T> >(0, fromVertex));
while (heap.Count > 0)
{
var item = heap.RemoveRoot();
var vertexInfo = vertexStatus[item.Value];
if (!vertexInfo.IsFinalised)
{
var edges = item.Value.EmanatingEdgeList;
vertexStatus[item.Value].IsFinalised = true;
// Enumerate through all the edges emanating from this node
foreach (var edge in edges)
{
var partnerVertex = edge.GetPartnerVertex(item.Value);
// Calculate the new distance to this distance
var distance = vertexInfo.Distance + edge.Weight;
var newVertexInfo = vertexStatus[partnerVertex];
// Found a better path, update the vertex status and add the
// vertex to the heap for further analysis
if (distance < newVertexInfo.Distance)
{
newVertexInfo.EdgeFollowed = edge;
newVertexInfo.Distance = distance;
heap.Add(new Association <double, Vertex <T> >(distance, partnerVertex));
}
}
}
}
// Now build the new graph
var newGraph = new Graph <T>(weightedGraph.IsDirected);
var enumerator = vertexStatus.GetEnumerator();
// This dictionary is used for mapping between the old vertices and the new vertices put into the graph
var vertexMap = new Dictionary <Vertex <T>, Vertex <T> >(vertexStatus.Count);
var newVertices = new Vertex <T> [vertexStatus.Count];
while (enumerator.MoveNext())
{
var newVertex = new Vertex <T>(
enumerator.Current.Key.Data,
enumerator.Current.Value.Distance
);
vertexMap.Add(enumerator.Current.Key, newVertex);
newGraph.AddVertex(newVertex);
}
enumerator = vertexStatus.GetEnumerator();
while (enumerator.MoveNext())
{
var info = enumerator.Current.Value;
// Check if an edge has been included to this vertex
if ((info.EdgeFollowed != null) && (enumerator.Current.Key != fromVertex))
{
newGraph.AddEdge(
vertexMap[info.EdgeFollowed.GetPartnerVertex(enumerator.Current.Key)],
vertexMap[enumerator.Current.Key],
info.EdgeFollowed.Weight);
}
}
return(newGraph);
}