public bool Search(MapGraph mapGraph, MapNode startNode, MapNode finishNode, PathfindingInfo pathfindingInfo)
{
PriorityQueue frontier = new PriorityQueue();
Dictionary <MapNode, float> cost_of_node = new Dictionary <MapNode, float>();
frontier.Enqueue(startNode, Heuristic(startNode, finishNode));
mapGraph.AddPreviousNode(startNode, startNode);
cost_of_node.Add(startNode, 0);
while (!frontier.IsEmpty())
{
MapNode currentNode = frontier.Dequeue().Key;
pathfindingInfo.visitedByIterations.Add(currentNode.GetPosition());
if (currentNode.Equals(finishNode))
{
Debug.Log("Path found! " + this.ToString());
return(true);
}
foreach (MapNode next in mapGraph.Neighbours(currentNode))
{
float new_cost = cost_of_node[currentNode] + Heuristic(currentNode, next);
if (!cost_of_node.ContainsKey(next) || new_cost < cost_of_node[next])
{
if (!cost_of_node.ContainsKey(next))
{
cost_of_node.Add(next, new_cost);
}
else
{
cost_of_node[next] = new_cost;
}
float priority = new_cost + Heuristic(next, finishNode);
if (!frontier.Contains(next))
{
frontier.Enqueue(next, priority);
}
mapGraph.AddPreviousNode(next, currentNode);
}
}
pathfindingInfo.FrontierByOrder.Add(frontier.ReturnCurrentNodesAsArray());
pathfindingInfo.iterations++;
}
Debug.Log("Path not found!");
return(false);
}
public Vector2 GetAStarMovement(int instanceID, Vector2 currPos, Vector2 goal_pos)
{
//Make sure every node is cleared
ResetNodesPrevious();
PriorityQueue <MapNode> nodeQueue = new PriorityQueue <MapNode>();
MapNode origin = GetNearestNode(currPos);
MapNode goal = GetNearestNode(goal_pos);
ReplaceClaim(instanceID, origin);
nodeQueue.Enqueue(origin, 0);
MapNode next = origin;
int checks = 0;
while (!nodeQueue.IsEmpty())
{
checks++;
next = nodeQueue.Dequeue();
if (next.Equals(goal))
{
break;
}
if (checks > 200)
{
Debug.Log("FAILED TO FIND AN A* PATH in 200 checks");
return(new Vector2(0, 0));
}
foreach (MapNode node in next.GetNeighbors())
{
if (node.GetPrevious() == null && node.IsActive() && node.GetClaim() == -1)
{
float value_to_assign = next.GetValue() + Vector2.Distance(next.GetPos(), node.GetPos());
node.SetValue(value_to_assign);
node.SetPrevious(next);
value_to_assign += Vector2.Distance(node.GetPos(), goal.GetPos());
nodeQueue.Enqueue(node, value_to_assign);
}
}
}
if (next.Equals(origin))
{
return(new Vector2(0, 0));
}
while (!next.GetPrevious().Equals(origin))
{
next = next.GetPrevious();
}
Vector2 new_movement = next.GetPos() - origin.GetPos();
return(new_movement);
}
public bool Equals(DrawnMapNode other)
{
return(MapNode.Equals(other.MapNode));
}