public AStarSearch(WeightedGraph<Position> graph, Position start, Position goal)
{
var frontier = new SimplePriorityQueue<Position>();
frontier.Enqueue(start, 0);
cameFrom[start] = start;
costSoFar[start] = 0;
while (frontier.Count > 0)
{
var current = frontier.Dequeue();
if(current.Equals(goal))
{
break;
}
foreach (var next in graph.Neighbors(current))
{
int newCost = costSoFar[current] + graph.Cost(current, next);
if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next])
{
costSoFar[next] = newCost;
int priority = newCost + Heuristic(next, goal);
frontier.Enqueue(next, priority);
cameFrom[next] = current;
}
}
}
}
/// <summary>
/// Generar el árbol SPF de la topología en un proyecto, tomando el parámetro como punto de origen
/// </summary>
/// <param name="idRouterOrigen"></param>
/// <param name="idProyecto"></param>
/// <returns></returns>
public static SimplePriorityQueue<NodoDijkstra> GenerarRutas(NodoDijkstra idRouterOrigen, int idProyecto)
{
idRouterOrigen.nMinDistancia = 0.0;
SimplePriorityQueue<NodoDijkstra> routerQueue = new SimplePriorityQueue<NodoDijkstra>();
routerQueue.Enqueue(idRouterOrigen, 1);
while (routerQueue.Count > 0)
{
NodoDijkstra currentRouter = routerQueue.Dequeue();
//Visita cada enlace adyacente al router u
foreach (var enlace in currentRouter.listaEnlaces)
{
NodoDijkstra vecino = new NodoDijkstra(enlace.idRouterB, idProyecto);
double nPesoBandwidth = enlace.nBandwidth;
double nDistanciaTotal = currentRouter.nMinDistancia + nPesoBandwidth;
if (nDistanciaTotal < vecino.nMinDistancia)
{
routerQueue.Remove(vecino);
vecino.nMinDistancia = nDistanciaTotal;
vecino.idRouterPrevio = currentRouter;
routerQueue.Enqueue(vecino, 1);
}
}
}
return routerQueue;
}
public static void RunExample()
{
//First, we create the priority queue.
SimplePriorityQueue<string> priorityQueue = new SimplePriorityQueue<string>();
//Now, let's add them all to the queue (in some arbitrary order)!
priorityQueue.Enqueue("4 - Joseph", 4);
priorityQueue.Enqueue("2 - Tyler", 0); //Note: Priority = 0 right now!
priorityQueue.Enqueue("1 - Jason", 1);
priorityQueue.Enqueue("4 - Ryan", 4);
priorityQueue.Enqueue("3 - Valerie", 3);
//Change one of the string's priority to 2. Since this string is already in the priority queue, we call UpdatePriority() to do this
priorityQueue.UpdatePriority("2 - Tyler", 2);
//Finally, we'll dequeue all the strings and print them out
while(priorityQueue.Count != 0)
{
string nextUser = priorityQueue.Dequeue();
Console.WriteLine(nextUser);
}
//Output:
//1 - Jason
//2 - Tyler
//3 - Valerie
//4 - Joseph
//4 - Ryan
//Notice that when two strings with the same priority were enqueued, they were dequeued in the same order that they were enqueued.
}
public Queue<Vector2> GetPath(Node start, Node goal)
{
List<Node> closedSet = new List<Node>(); // The set of nodes already evaluated
SimplePriorityQueue<Node> openSet = new SimplePriorityQueue<Node>(); // The set of tentative nodes to be evaluated
openSet.Enqueue(start, 0);
Dictionary<Node, Node> came_from = new Dictionary<Node, Node>();
Dictionary<Node, float> g_score = new Dictionary<Node, float>();
Dictionary<Node, float> f_score = new Dictionary<Node, float>();
foreach (Node node in graph.nodes.Values)
{
g_score[node] = Mathf.Infinity;
f_score[node] = Mathf.Infinity;
}
g_score[start] = 0f;
f_score[start] = heuristic_cost_estimate(start, goal);
while(openSet.Count > 0)
{
Node current = openSet.Dequeue();
if(current == goal )
{
return reconstruct_path(came_from, current);
}
closedSet.Add(current);
foreach(Node neighbour in current.edges.Keys)
{
if (closedSet.Contains(neighbour))
continue;
float tentative_g_score = g_score[current] + dist_between(current, neighbour); // length of this path.
if (openSet.Contains(neighbour) && tentative_g_score >= g_score[neighbour])
{
continue;
}
came_from[neighbour] = current;
g_score[neighbour] = tentative_g_score;
f_score[neighbour] = g_score[neighbour] + heuristic_cost_estimate(neighbour, goal);
openSet.Enqueue(neighbour, f_score[neighbour]);
}
}
// Failed to find a path.
return null;
}
private int Dijkstry(int from, int to)
{
int[] distance = new int[count];
int[] prev = new int[count];
SimplePriorityQueue <int> queue = new SimplePriorityQueue <int>();
for (int i = 0; i < count; i++)
{
distance[i] = int.MaxValue;
queue.Enqueue(i, float.MaxValue);
}
distance[from] = 0;
queue.UpdatePriority(from, 0);
while (queue.Count > 0)
{
int v = queue.Dequeue();
foreach (var e in matrix[v])
{
if (distance[e.To] > distance[v] + e.Weight)
{
distance[e.To] = distance[v] + e.Weight;
prev[e.To] = v;
queue.UpdatePriority(e.To, distance[e.To]);
}
}
if (v == target)
{
break;
}
}
return(distance[to]);
}
private void AddToQueue(MethodData methodData)
{
lock (workingSet)
{
if (!workingSet.Contains(methodData))
{
workingSet.Add(methodData);
Interlocked.Increment(ref totalMethods);
}
}
lock (queue)
{
if (queue.Contains(methodData))
{
//Debug.WriteLine($"Already in Queue: {method}");
return; // already queued
}
//Debug.WriteLine($"Queued: {method}");
var priority = GetCompilePriorityLevel(methodData);
queue.Enqueue(methodData, priority);
Interlocked.Increment(ref totalQueued);
}
}
private void ProcessNeighbours(Id <TNode> nodeId, AStarNode <TNode> node)
{
var connections = _map.GetConnections(nodeId);
foreach (var connection in connections)
{
var gCost = node.G + connection.Cost;
var neighbour = connection.Target;
if (_nodeLookup.NodeIsVisited(neighbour))
{
var targetAStarNode = _nodeLookup.GetNodeValue(neighbour);
// If we already processed the neighbour in the past or we already found in the past
// a better path to reach this node that the current one, just skip it, else create
// and replace a new PathNode
if (targetAStarNode.Status == CellStatus.Closed || gCost >= targetAStarNode.G)
{
continue;
}
targetAStarNode = new AStarNode <TNode>(nodeId, gCost, targetAStarNode.H, CellStatus.Open);
_openQueue.UpdatePriority(neighbour, targetAStarNode.F);
_nodeLookup.SetNodeValue(neighbour, targetAStarNode);
}
else
{
var newHeuristic = _calculateHeuristic(neighbour);
var newAStarNode = new AStarNode <TNode>(nodeId, gCost, newHeuristic, CellStatus.Open);
_openQueue.Enqueue(neighbour, newAStarNode.F);
_nodeLookup.SetNodeValue(neighbour, newAStarNode);
}
}
}
static void VisitCell(HexCell cell, HexCell from, float priority,
ref SimplePriorityQueue <HexCell> frontier,
ref Dictionary <HexCell, HexCell> visited)
{
frontier.Enqueue(cell, priority);
visited[cell] = from;
}
// Determine what to with the given cell in terms of adding it to the open list
private void ProcessCell(GridCell currentSquare, GridCell cell, SimplePriorityQueue <GridCell> openList, List <GridCell> closedList)
{
if (cell != null)
{
if (cell.walkable && !closedList.Contains(cell))
{
if (!openList.Contains(cell))
{
cell.g = currentSquare.g + 1;
cell.f = cell.g + cell.CalculateH(destRow, destCol);
openList.Enqueue(cell, cell.f);
}
else
{
// If this path would be shorter
if (cell.g > (currentSquare.g + 1))
{
cell.g = currentSquare.g + 1;
cell.f = cell.g + cell.CalculateH(destRow, destCol);
openList.UpdatePriority(cell, cell.f);
}
}
}
}
}
private int FindPath()
{
openList = new SimplePriorityQueue <GridCell>();
closedList = new List <GridCell>();
// Add the starting cell to the open list
GridCell startingCell = maze[startRow, startCol];
openList.Enqueue(startingCell, startingCell.f);
while (openList.Count != 0)
{
GridCell currentSquare = openList.Dequeue();
closedList.Add(currentSquare);
if (currentSquare.row == destRow && currentSquare.col == destCol)
{
return(currentSquare.g);
}
// Check the four possible neighbour squares
GridCell top = (currentSquare.row != 0) ? maze[currentSquare.row - 1, currentSquare.col] : null;
GridCell bottom = (currentSquare.row != totalRows - 1) ? maze[currentSquare.row + 1, currentSquare.col] : null;
GridCell left = (currentSquare.col != 0) ? maze[currentSquare.row, currentSquare.col - 1] : null;
GridCell right = (currentSquare.col != totalCols - 1) ? maze[currentSquare.row, currentSquare.col + 1] : null;
// Process each of the potential neighbour cells
ProcessCell(currentSquare, top, openList, closedList);
ProcessCell(currentSquare, bottom, openList, closedList);
ProcessCell(currentSquare, left, openList, closedList);
ProcessCell(currentSquare, right, openList, closedList);
}
return(-1);
}
public static void IDAS(State initialState, int size)
{
SimplePriorityQueue <State> priorityQueue = new SimplePriorityQueue <State>();
priorityQueue.Enqueue(initialState, initialState.Weight);
while (priorityQueue.Count != 0)
{
priorityQueue.TryDequeue(out State currentState);
int h = currentState.CalculateHeuristic();
if (h == 0)
{
currentState.PrintState();
Console.WriteLine($"\nDistance from the goal state -> {currentState.Distance}");
foreach (var movement in currentState.PathDirections)
{
Console.WriteLine(movement.ToString());
}
Console.WriteLine($"Steps count -> {currentState.PathDirections.Count}");
break;
}
else
{
GetChildStates(currentState, priorityQueue);
}
}
}
public double Prim(long pointCount, long[][] points)
{
double path = 0;
double[] cost = new double[pointCount];
SimplePriorityQueue <long, double> priorityQ = new SimplePriorityQueue <long, double>();
for (int I = 0; I < pointCount; I++)
{
cost[I] = int.MaxValue;
priorityQ.Enqueue(I, int.MaxValue);
}
cost[0] = 0;
priorityQ.UpdatePriority(0, 0);
while (priorityQ.Count != 0)
{
long v = priorityQ.Dequeue();
path += cost[v];
for (int i = 0; i < pointCount; i++)
{
if (i == v || !priorityQ.Contains(i))
{
continue;
}
double dist = RealDist(points[v][0], points[v][1], points[i][0], points[i][1]);
if (cost[i] > dist)
{
priorityQ.UpdatePriority(i, dist);
cost[i] = dist;
}
}
}
return(path);
}
//So slow, bfs
public T GetClosestTile(Vector2 pos, T nullValue)
{
Vector2 currentIndex = GetTileIndex(pos);
Vector2[] dirs = NevPointMap.Dirs;
SimplePriorityQueue <Vector2> searchQueue = new SimplePriorityQueue <Vector2>();
HashSet <Vector2> searchedSet = new HashSet <Vector2>();
Action <Vector2> insertIndex = (index) => {
searchQueue.Enqueue(index, (GetTileIndexCenter(index) - originPos).magnitude);
searchedSet.Add(index);
};
insertIndex(currentIndex);
while (searchQueue.Count != 0)
{
Vector2 index = searchQueue.Dequeue();
T tile = GetTileByIndex(index);
if (!tile.Equals(nullValue))
{
return(tile);
}
foreach (var dir in dirs)
{
Vector2 tar = index + dir;
if (!searchedSet.Contains(tar))
{
insertIndex(tar);
}
}
}
return(nullValue);
}
public void Start(float timeout, bool periodic, Action?handlerFunc)
{
_elapsedTime = 0f;
_timeout = timeout < MinimumTimeout ? MinimumTimeout : timeout;
_periodic = periodic;
_handlerFunc = handlerFunc;
if (_queue.Contains(this))
{
_queue.UpdatePriority(this, _time + timeout);
}
else
{
_queue.Enqueue(this, _time + timeout);
}
}
private void UpdateChunkQueues()
{
for (int x = -LoadDistance; x <= LoadDistance; x++)
{
for (int z = -LoadDistance; z <= LoadDistance; z++)
{
for (int y = 0; y < Height; y++)
{
ChunkLocation l = new ChunkLocation(centerX + x, y, centerZ + z);
if (GetDistanceSquared(l) <= LoadDistance * LoadDistance && !currentlyLoading.Contains(l) && !chunkMap.ContainsKey(l))
{
if (chunkLoadQueue.Contains(l))
{
chunkLoadQueue.TryUpdatePriority(l, GetDistanceSquared(l, true));
}
else
{
chunkLoadQueue.Enqueue(l, GetDistanceSquared(l, true));
}
}
}
}
}
foreach (Chunk chunk in chunkMap.Values.Where(c => GetDistanceSquared(c.Location) > UnloadDistance * UnloadDistance))
{
chunkUnloadStack.Push(chunk);
}
}
public bool AStarSearchEx()
{
sw = Stopwatch.StartNew();
while (openSet.Count > 0)
{
var current = openSet.Dequeue();
Debug.WriteLine("dequeu" + current.ToString());
if (current.Equals(goal))
{
sw.Stop();
return(true);
}
foreach (var next in graph.Neighbors(current))
{
float newG = g[current]
+ graph.Cost(current, next);
if (!g.ContainsKey(next) ||
newG < g[next])
{
g[next] = newG;
float h = Heuristic(next, goal);
gh[next] = new float[] { newG, h }; //I want to store g and h.
float f = newG + h;
openSet.Enqueue(next, f);
cameFrom[next] = current;
}
}
}
sw.Stop();
return(false);
}
public long Dijkstra(long startNode, long endNode)
{
SimplePriorityQueue <long, long> H = new SimplePriorityQueue <long, long>();
long[] dist = new long[V];
for (int i = 0; i < V; i++)
{
dist[i] = long.MaxValue;
}
dist[startNode] = 0;
for (int i = 0; i < V; i++)
{
H.Enqueue(i, dist[i]);
}
while (H.Count > 0)
{
var u = H.Dequeue();
foreach (var edge in adj[u])
{
if (dist[u] != long.MaxValue)
{
if (dist[edge.Item1] > dist[u] + edge.Item2)
{
dist[edge.Item1] = dist[u] + edge.Item2;
H.UpdatePriority(edge.Item1, dist[edge.Item1]);
}
}
}
}
if (dist[endNode] != long.MaxValue)
{
return(dist[endNode]);
}
return(-1);
}
public void RegisterEntity(ITimeNode node)
{
if (!turnQueue.Contains(node))
{
turnQueue.Enqueue(node, node.Tick);
}
}
public static bool Search(GraphNode source, GraphNode destination, ref List <GraphNode> path, int maxSteps)
{
bool found = false;
path = new List <GraphNode>();
//A* algorithm
SimplePriorityQueue <GraphNode> nodes = new SimplePriorityQueue <GraphNode>();
source.Cost = 0;
source.Heuristic = Vector3.Distance(source.transform.position, destination.transform.position);
nodes.Enqueue(source, source.Cost + source.Heuristic);
int steps = 0;
while (!found && nodes.Count > 0 && steps++ < maxSteps)
{
GraphNode node = nodes.Dequeue();
if (node == destination)
{
found = true;
continue;
}
foreach (GraphNode.Edge edge in node.Edges)
{
float cost = node.Cost + Vector3.Distance(edge.nodeA.transform.position, edge.nodeB.transform.position);
if (cost < edge.nodeB.Cost)
{
edge.nodeB.Cost = cost;
edge.nodeB.Parent = node;
edge.nodeB.Heuristic = Vector3.Distance(edge.nodeB.transform.position, destination.transform.position);
nodes.EnqueueWithoutDuplicates(edge.nodeB, edge.nodeB.Cost + edge.nodeB.Heuristic);
}
}
}
if (found)
{
GraphNode node = destination;
while (node != null)
{
path.Add(node);
node = node.Parent;
}
path.Reverse();
}
else
{
while (nodes.Count > 0)
{
path.Add(nodes.Dequeue());
}
}
return(found);
}
public void Search(IGraph graph)
{
Path.RemoveRange(_posIndex, Path.Count - _posIndex);
var cameFrom = new Dictionary <AbstractTile, AbstractTile>();
var gScore = new Dictionary <AbstractTile, double>();
var closed = new HashSet <AbstractTile>();
var open = new SimplePriorityQueue <AbstractTile, double>();
gScore[Position] = 0;
open.Enqueue(Position, Helpers.Metrics.Octile(Position, Target)); // TODO add agitation noise (see Silver)
AbstractTile current;
while (open.Count > 0)
{
current = open.Dequeue();
closed.Add(current);
if (current == Target)
{
Path.AddRange(ReconstructPath(cameFrom, current));
return;
}
foreach (var neighbor in graph.AdjacentVertices(current))
{
if (closed.Contains(neighbor))
{
continue;
}
var tentative_gScore = gScore.GetValueOrDefault(current, double.PositiveInfinity) + Helpers.Metrics.Octile(current, neighbor); // TODO add agitation noise (see Silver)
if (tentative_gScore < gScore.GetValueOrDefault(neighbor, double.PositiveInfinity))
{
cameFrom[neighbor] = current;
gScore[neighbor] = tentative_gScore;
var fScore = tentative_gScore + Helpers.Metrics.Octile(neighbor, Target);
if (!open.TryUpdatePriority(neighbor, fScore))
{
open.Enqueue(neighbor, fScore);
}
}
}
}
}
public static HashSet <Point> FindUnwrappedCellsWithin(Point start, Map map, int maxDistance, bool unwrappedBlocks)
{
var dist = new Dictionary <Point, int>();
var prev = new Dictionary <Point, Point>();
var Q = new SimplePriorityQueue <Point, int>();
var found = new HashSet <Point>();
dist[start] = 0;
Q.Enqueue(start, 0);
while (Q.Count > 0)
{
var u = Q.Dequeue();
if (map.CellAt(u) == Map.CellType.Empty)
{
found.Add(u);
}
var alt = dist[u] + 1;
if (alt > maxDistance)
{
continue;
}
foreach (var v in map.Neighbors(u, unwrappedBlocks))
{
if (!dist.ContainsKey(v) || alt < dist[v])
{
dist[v] = alt;
prev[v] = u;
if (Q.Contains(v))
{
Q.UpdatePriority(v, alt);
}
else
{
Q.Enqueue(v, alt);
}
}
}
}
;
return(found);
}
public static Stack <TileNode> Astar(TileGraph tg, TileNode start, TileNode end)
{
frontier = new SimplePriorityQueue <TileNode, float>();
frontier.Enqueue(start, 0f);
//node_dict = new Dictionary<TileNode, NodeInfo>();
node_dict = DijkstraInitialDictLoad(start, tg);
List <TileNode> Expanded = new List <TileNode>();
TileNode v;
TileNode other;
float cost_so_far;
float edge_weight;
float dist_to_node;
float manhattanDistance;
while (frontier.Count > 0)
{
v = frontier.Dequeue();
cost_so_far = node_dict[v].dist;
Expanded.Add(v);
List <Edge> experiment = tg.getAdjacentEdges(v) as List <Edge>;
foreach (Edge adj_edge in tg.getAdjacentEdges(v))
{
other = adj_edge.getNeighbor(v);
edge_weight = adj_edge.weight;
dist_to_node = node_dict[other].dist;
manhattanDistance = Mathf.Abs(end.transform.position.z - other.transform.position.z) + Mathf.Abs(end.transform.position.x - other.transform.position.x);
if (cost_so_far + edge_weight < dist_to_node)
{
node_dict[other].dist = cost_so_far + edge_weight;
node_dict[other].heuristic = node_dict[other].dist + manhattanDistance;
node_dict[other].parent = v;
}
if (!Expanded.Any(node => node.isEqual(other)) && !frontier.Any(node => node.isEqual(other)))
{
frontier.Enqueue(other, node_dict[other].heuristic);
}
}
}
Path = NodeDictToPath(node_dict, start, end);
return(Path);
}
//HashSet<Edge> Edges = new HashSet<Edge>();
public ArtistsConnectionsForm()
{
InitializeComponent();
var config = SpotifyClientConfig.CreateDefault().WithAuthenticator(new ClientCredentialsAuthenticator("e6bf2e305d98443190c472ee318fd511", "96bad35ecf9c41f581a761eb3a85348b"));
Spotify = new SpotifyClient(config);
ArtistsToCheck.Enqueue("4JxdBEK730fH7tgcyG3vuv", 0);
int n = 0;
while (ArtistsToCheck.Count > 0 && ArtistsToCheck.GetPriority(ArtistsToCheck.First) <= 2)
{
string first = ArtistsToCheck.First;
float priority = ArtistsToCheck.GetPriority(first);
ArtistsToCheck.Dequeue();
Task <List <string> > artistsTask = FindAllConnectedArtists(first, priority);
artistsTask.Wait();
n++;
}
List <string> edges = new List <string>();
foreach (Edge edge in ArtistsGraph.Edges)
{
string sourceName;
string targetName;
ArtistIdToName.TryGetValue(edge.Source, out sourceName);
ArtistIdToName.TryGetValue(edge.Source, out targetName);
edges.Add(sourceName + " - " + edge.LabelText + " - " + targetName);
}
ListBox1.DataSource = edges;
while (ArtistsToCheck.Count > 0)
{
string name;
ArtistIdToName.TryGetValue(ArtistsToCheck.First, out name);
float priority = ArtistsToCheck.GetPriority(ArtistsToCheck.First);
ArtistsToCheck.Dequeue();
ListBox3.Items.Add(name + " - " + priority);
}
foreach (string id in CheckedArtists)
{
string name;
ArtistIdToName.TryGetValue(id, out name);
ListBox2.Items.Add(name);
}
label1.Text = ListBox1.Items.Count.ToString();
label2.Text = ListBox2.Items.Count.ToString();
label3.Text = ListBox3.Items.Count.ToString();
}
public static int FindShortestPathLength(Board board, int player)
{
SimplePriorityQueue <QueueNode> open = new SimplePriorityQueue <QueueNode>();
bool[,] visited = new bool[Board.BOARD_SIZE, Board.BOARD_SIZE];
int GOAL;
if (player > 1) //4 players
{
GOAL = board.playerStatus[player].goalY;
}
else
{
GOAL = board.playerStatus[player].goalX;
}
QueueNode root = new QueueNode(board.playerStatus[player].x, board.playerStatus[player].y, 0);
open.Enqueue(root, F(root));
while (open.Count != 0)
{
QueueNode current = open.Dequeue();
if ((player > 1 && current.Pos.Y == GOAL) ||
(player <= 1 && current.Pos.X == GOAL))
{
return(current.PathCost);
}
int x = current.Pos.X;
int y = current.Pos.Y;
visited[x, y] = true;
foreach (Position pos in new List <Position> {
new Position(x + 1, y), new Position(x - 1, y), new Position(x, y + 1), new Position(x, y - 1)
})
{
QueueNode temp = new QueueNode(pos, current.PathCost + 1);
if ((pos.X >= 0 && pos.X < Board.BOARD_SIZE) &&
(pos.Y >= 0 && pos.Y < Board.BOARD_SIZE) &&
visited[pos.X, pos.Y] == false &&
!open.Contains(temp) &&
board.IsPawnMoveLegalSimplified(x, y, pos.X, pos.Y))
{
open.Enqueue(temp, F(temp));
}
}
}
return(-1);
}
public List <NetworkLane> CalculateRoute(string startID, string endID)
{
var q = new SimplePriorityQueue <NetworkLaneConnection> ();
var distances = new Dictionary <string, double> ();
var previous = new Dictionary <string, NetworkLaneConnection> ();
NetworkLaneConnection start;
if (!connectivityGraph.TryGetValue(startID, out start))
{
Debug.Log("Start node was not found!");
return(null);
}
distances [startID] = 0;
q.Enqueue(start, distances [startID]);
while (q.Count > 0)
{
var current = q.Dequeue();
if (current.id == endID)
{
return(BacktrackRoute(current, previous));
}
foreach (string id in current.adjacentLanes)
{
double newDist = distances [current.id] + current.Weight(id);
if (!distances.ContainsKey(id) || newDist < distances [id])
{
previous [id] = current;
if (!distances.ContainsKey(id))
{
q.Enqueue(connectivityGraph [id], newDist);
}
else
{
q.TryUpdatePriority(connectivityGraph [id], newDist);
}
distances [id] = newDist;
}
}
}
return(null);
}
public SimplePriorityQueue <Generator> FindGeneratorToAccept(ItemOrder io)
{
int num = io.amount;
string item = io.GetItemName();
GameObject[] objs = GameObject.FindGameObjectsWithTag("Generator");
SimplePriorityQueue <Generator> queue = new SimplePriorityQueue <Generator>();
if (objs.Length == 0)
{
return(queue);
}
foreach (GameObject go in objs)
{
Generator gen = go.GetComponent <Generator>();
//if null, continue
if (gen == null)
{
continue;
}
if (!gen.Operational)
{
continue;
}
int index = gen.IngredientIndex(item);
//only add to list if it needs this ingredient
if (index == -1)
{
continue;
}
//only add to list if it has an entrance
List <Node> entrancesHere = GetAdjRoadTiles();
List <Node> entrancesThere = gen.GetAdjRoadTiles();
if (entrancesHere.Count == 0 || entrancesThere.Count == 0)
{
continue;
}
//only add to list if it has none of this item
if (gen.IngredientNeeded(index) <= 0)
{
continue;
}
float distance = entrancesHere[0].DistanceTo(entrancesThere[0]);
queue.Enqueue(gen, distance);
}
return(queue);
}
/*
* A node gets added to the priority queue, and the 'contains' list for the priority
* queue
*/
private void addNode(State <T> node)
{
openList.Enqueue(node, Convert.ToSingle(node.cost));
if (!openListContains.ContainsKey(node))
{
openListContains.Add(node, node.cost);
}
}
public void StartSearch()
{
reachedGoal = false;
isReplaying = false;
isSearching = true;
simRenderer.enabled = isGhostVisualized;
simPlayer.transform.position = playerMovement.transform.position;
priorityQueue.Enqueue(new Node(GetSimPlayerState(new Action(ActionType.WalkR, 0)), null, GetHeuristic(), 0),
GetHeuristic());
startTime = Time.unscaledTime;
mainPlayerRB.constraints = RigidbodyConstraints2D.FreezeRotation;
simPlayerRB.constraints = RigidbodyConstraints2D.FreezeRotation;
}
public static List <Node> Search(GridGraph graph, Node start, Node goal)
{
Dictionary <Node, Node> came_from = new Dictionary <Node, Node>();
Dictionary <Node, float> cost_so_far = new Dictionary <Node, float>();
List <Node> path = new List <Node>();
SimplePriorityQueue <Node> frontier = new SimplePriorityQueue <Node>();
frontier.Enqueue(start, 0);
came_from.Add(start, start);
cost_so_far.Add(start, 0);
Node current = new Node(0, 0);
while (frontier.Count > 0)
{
current = frontier.Dequeue();
if (current == goal)
{
break; // Early exit
}
foreach (Node next in graph.Neighbours(current))
{
float new_cost = cost_so_far[current] + graph.Cost(next);
if (!cost_so_far.ContainsKey(next) || new_cost < cost_so_far[next])
{
cost_so_far[next] = new_cost;
came_from[next] = current;
float priority = new_cost + Heuristic(next, goal);
frontier.Enqueue(next, priority);
next.Priority = new_cost;
}
}
}
while (current != start)
{
path.Add(current);
current = came_from[current];
}
path.Reverse();
return(path);
}
void Enqueue(Vector v, Vector father)
{
Tuple <Vector, double, double> t = new Tuple <Vector, double, double>
(father, values[father].Item2 + 1, Heuristic(v));
queue.Enqueue(v, (float)(t.Item2 + t.Item3));
values[v] = t;
}
private NevPoint[] aStarSearch(NevPoint from, NevPoint to)
{
SimplePriorityQueue <NevPoint> searchQueue = new SimplePriorityQueue <NevPoint>();
Dictionary <NevPoint, NevPoint> last = new Dictionary <NevPoint, NevPoint>();
Dictionary <NevPoint, float> dis = new Dictionary <NevPoint, float>();
Action <NevPoint, NevPoint, float, float> insertPoint = (point, lastPoint, _dis, priority) => {
searchQueue.Enqueue(point, priority);
last.Add(point, lastPoint);
dis.Add(point, _dis);
};
insertPoint(from, from, 0, 0);
while (searchQueue.Count != 0)
{
NevPoint point = searchQueue.Dequeue();
if (point == to)
{
break;
}
foreach (var dir in Dirs)
{
if (point.GetAroundFlag(dir))
{
NevPoint tar = GetClosestNevPoint(point.pos + dir * step);
float nowDis = dis[point] + step;
float priority = nowDis + (to.pos - tar.pos).magnitude;
bool contain = last.ContainsKey(tar);
if (!contain)
{
insertPoint(tar, point, nowDis, priority);
}
else if (searchQueue.Contains(tar) && searchQueue.GetPriority(tar) > priority)
{
searchQueue.UpdatePriority(tar, priority);
last[tar] = point;
dis[tar] = nowDis;
}
}
}
}
Action <Action <NevPoint> > doeachPathNode = (a) => {
var node = to;
while (last[node] != from)
{
node = last[node];
a(node);
}
};
int pathSize = 0;
doeachPathNode((p) => pathSize++);
NevPoint[] result = new NevPoint[pathSize]; //Not contain from and to
doeachPathNode((p) => {
pathSize--;
result[pathSize] = p;
});
return(result);
}
public List <Action> GetShortestPath(State fromState, State toState)
{
SimplePriorityQueue <SearchNode <State, Action>, float> frontier = new SimplePriorityQueue <SearchNode <State, Action>, float>();
HashSet <State> exploredSet = new HashSet <State>();
Dictionary <State, SearchNode <State, Action> > frontierMap = new Dictionary <State, SearchNode <State, Action> >();
SearchNode <State, Action> startNode = new SearchNode <State, Action>(null, 0, 0, fromState, default(Action));
frontier.Enqueue(startNode, 0);
frontierMap.Add(fromState, startNode);
while (true)
{
if (frontier.Count == 0)
{
return(null);
}
SearchNode <State, Action> node = frontier.Dequeue();
if (node.state.Equals(toState))
{
return(BuildSolution(node));
}
exploredSet.Add(node.state);
// expand node and add to frontier
foreach (Action action in info.Expand(node.state))
{
State child = info.ApplyAction(node.state, action);
SearchNode <State, Action> frontierNode = null;
bool isNodeInFrontier = frontierMap.TryGetValue(child, out frontierNode);
if (!exploredSet.Contains(child) && !isNodeInFrontier)
{
SearchNode <State, Action> searchNode = CreateSearchNode(node, action, child, toState);
frontier.Enqueue(searchNode, searchNode.f);
exploredSet.Add(child);
}
else if (isNodeInFrontier)
{
SearchNode <State, Action> searchNode = CreateSearchNode(node, action, child, toState);
if (frontierNode.f > searchNode.f)
{
//frontier.Replace(frontierNode, frontierNode.f, searchNode.f);
frontier.UpdatePriority(frontierNode, searchNode.f);
}
}
}
}
}
public AstarResult Astar(int start, int end)
{
if (start >= this.NodesCount() || end >= this.NodesCount())
{
throw new ArgumentException("Nodes not in graph");
}
var gScore = new Dictionary <int, (double distance, int prev)>
{
{ start, (0, -1) }
};
HashSet <int> closedset = new HashSet <int>();
var openset = new SimplePriorityQueue <int, double>();
openset.Enqueue(start, 0 + this.HeuristicDistance(this.Nodes[start], this.Nodes[end]));
while (openset.Count > 0)
{
int u = openset.Dequeue();
closedset.Add(u);
if (u == end)
{
return(GenAstarResult(end, start, gScore));
}
foreach (Edge e in this.OutEdges(u))
{
if (closedset.Contains(e.To))
{
continue;
}
if (!openset.Contains(e.To))
{
gScore.Add(e.To, (double.MaxValue, -1));
openset.Enqueue(e.To, double.MaxValue);
}
double ndist = gScore[u].distance + e.Value * this.HeuristicDistance(this.Nodes[u], this.Nodes[e.To]);
if (gScore[e.To].distance > ndist)
{
gScore[e.To] = (ndist, u);
openset.UpdatePriority(e.To, ndist + this.HeuristicDistance(this.Nodes[e.To], this.Nodes[end]));
}
}
}
throw new Exception("Not Connected Nodes");
}
public static Queue <Point> GetPath(Point start, Point destination)
{
var openSet = new SimplePriorityQueue <Point>();
openSet.Enqueue(start, 0);
var closedSet = new HashSet <Point>();
var cameFrom = new Dictionary <Point, Point>();
var costSoFar = new Dictionary <Point, float>();
costSoFar[start] = 0;
while (openSet.Count > 0)
{
var current = openSet.Dequeue();
if (current == destination)
{
return(ConstructPath(cameFrom, current));
}
closedSet.Add(current);
foreach (var neighbour in GetNeighbours(current))
{
if (closedSet.Contains(neighbour))
{
continue;
}
float cost = costSoFar[current] + Distance(current, neighbour);
if (!costSoFar.ContainsKey(neighbour) || cost < costSoFar[neighbour])
{
costSoFar[neighbour] = cost;
openSet.Enqueue(neighbour, cost);
cameFrom[neighbour] = current;
}
}
}
return(null);
}
/// <summary>
/// Performs an A* search following the Node Array A* implementation
/// </summary>
public Path FindPath(IMap map, int start, int target)
{
this.isGoal = nodeId => nodeId == target;
this.calculateHeuristic = nodeId => map.GetHeuristic(nodeId, target);
this.map = map;
var heuristic = calculateHeuristic(start);
var startNode = new AStarNode(start, 0, heuristic, CellStatus.Open);
var openQueue = new SimplePriorityQueue<int>();
openQueue.Enqueue(start, startNode.F);
// The open list lookup is indexed by the number of nodes in the graph/map,
// and it is useful to check quickly the status of any node that has been processed
var nodeLookup = new AStarNode?[map.NrNodes];
nodeLookup[start] = startNode;
while (openQueue.Count != 0)
{
var nodeId = openQueue.Dequeue();
var node = nodeLookup[nodeId].Value;
if (isGoal(nodeId))
{
return ReconstructPath(nodeId, nodeLookup);
}
ProcessNeighbours(nodeId, node, nodeLookup, openQueue);
// Close the node. I hope some day the will implement something
// like the records in F# with the "with" keyword
nodeLookup[nodeId] = new AStarNode(node.Parent, node.G, node.H, CellStatus.Closed);
}
// No path found. We could return a null, but since I read the book "Code Complete" I decided
// its best to return an empty path, and I'll return a -1 as PathCost
// TODO: Additionally, all those magic numbers like this -1 should be converted to explicit,
// clearer constants
return new Path(new List<int>(), -1);
}
/// <summary>
/// Processes every open or unexplored successor of nodeId
/// </summary>
private void ProcessNeighbours(int nodeId, AStarNode node, AStarNode?[] nodeLookup, SimplePriorityQueue<int> openQueue)
{
var successors = map.GetNeighbours(nodeId);
foreach (var successor in successors)
{
var newg = node.G + successor.Cost;
var successorTarget = successor.Target;
var targetAStarNode = nodeLookup[successorTarget];
if (targetAStarNode.HasValue)
{
// If we already processed the neighbour in the past or we already found in the past
// a better path to reach this node that the current one, just skip it, else create
// and replace a new PathNode
if (targetAStarNode.Value.Status == CellStatus.Closed || newg >= targetAStarNode.Value.G)
continue;
targetAStarNode = new AStarNode(nodeId, newg, targetAStarNode.Value.H, CellStatus.Open);
nodeLookup[successorTarget] = targetAStarNode;
openQueue.UpdatePriority(successorTarget, targetAStarNode.Value.F);
}
else
{
var newHeuristic = calculateHeuristic(successorTarget);
var newAStarNode = new AStarNode(nodeId, newg, newHeuristic, CellStatus.Open);
openQueue.Enqueue(successorTarget, newAStarNode.F);
nodeLookup[successorTarget] = newAStarNode;
}
}
}
public Path_AStar(World world, Tile tileStart, Tile tileEnd)
{
// Check to see if we have a valid tile graph
if(world.tileGraph == null) {
world.tileGraph = new Path_TileGraph(world);
}
// A dictionary of all valid, walkable nodes.
Dictionary<Tile, Path_Node<Tile>> nodes = world.tileGraph.nodes;
// Make sure our start/end tiles are in the list of nodes!
if(nodes.ContainsKey(tileStart) == false) {
Debug.LogError("Path_AStar: The starting tile isn't in the list of nodes!");
return;
}
if(nodes.ContainsKey(tileEnd) == false) {
Debug.LogError("Path_AStar: The ending tile isn't in the list of nodes!");
return;
}
Path_Node<Tile> start = nodes[tileStart];
Path_Node<Tile> goal = nodes[tileEnd];
// Mostly following this pseusocode:
// https://en.wikipedia.org/wiki/A*_search_algorithm
List<Path_Node<Tile>> ClosedSet = new List<Path_Node<Tile>>();
/* List<Path_Node<Tile>> OpenSet = new List<Path_Node<Tile>>();
OpenSet.Add( start );
*/
SimplePriorityQueue<Path_Node<Tile>> OpenSet = new SimplePriorityQueue<Path_Node<Tile>>();
OpenSet.Enqueue( start, 0);
Dictionary<Path_Node<Tile>, Path_Node<Tile>> Came_From = new Dictionary<Path_Node<Tile>, Path_Node<Tile>>();
Dictionary<Path_Node<Tile>, float> g_score = new Dictionary<Path_Node<Tile>, float>();
foreach(Path_Node<Tile> n in nodes.Values) {
g_score[n] = Mathf.Infinity;
}
g_score[ start ] = 0;
Dictionary<Path_Node<Tile>, float> f_score = new Dictionary<Path_Node<Tile>, float>();
foreach(Path_Node<Tile> n in nodes.Values) {
f_score[n] = Mathf.Infinity;
}
f_score[ start ] = heuristic_cost_estimate( start, goal );
while( OpenSet.Count > 0 ) {
Path_Node<Tile> current = OpenSet.Dequeue();
if(current == goal) {
// We have reached our goal!
// Let's convert this into an actual sequene of
// tiles to walk on, then end this constructor function!
reconstruct_path(Came_From, current);
return;
}
ClosedSet.Add(current);
foreach(Path_Edge<Tile> edge_neighbor in current.edges) {
Path_Node<Tile> neighbor = edge_neighbor.node;
if( ClosedSet.Contains(neighbor) == true )
continue; // ignore this already completed neighbor
float movement_cost_to_neighbor = neighbor.data.movementCost * dist_between(current, neighbor);
float tentative_g_score = g_score[current] + movement_cost_to_neighbor;
if(OpenSet.Contains(neighbor) && tentative_g_score >= g_score[neighbor])
continue;
Came_From[neighbor] = current;
g_score[neighbor] = tentative_g_score;
f_score[neighbor] = g_score[neighbor] + heuristic_cost_estimate(neighbor, goal);
if(OpenSet.Contains(neighbor) == false) {
OpenSet.Enqueue(neighbor, f_score[neighbor]);
}
} // foreach neighbour
} // while
// If we reached here, it means that we've burned through the entire
// OpenSet without ever reaching a point where current == goal.
// This happens when there is no path from start to goal
// (so there's a wall or missing floor or something).
// We don't have a failure state, maybe? It's just that the
// path list will be null.
}
/// <summary>
/// Generar y retornar el árbol SPF de la topología en un proyecto, tomando el parámetro como punto de origen
/// </summary>
/// <param name="idRouterOrigen"></param>
/// <param name="idProyecto"></param>
/// <param name="minBW"></param>
/// <returns></returns>
public static List<NodoDijkstra> GenerarRutas(NodoDijkstra idRouterOrigen, int idProyecto, double minBW, int nTipoMetrica = 2, int idAfinidad = 0)
{
idRouterOrigen.nMinDistancia = 0.0;
SimplePriorityQueue<NodoDijkstra> routerQueue = new SimplePriorityQueue<NodoDijkstra>();
routerQueue.Enqueue(idRouterOrigen, 1);
//mantiene el registro de todos los nodos de la topologia por el que se pasa
List<NodoDijkstra> routerList = new List<NodoDijkstra>();
routerList.Add(idRouterOrigen);
while (routerQueue.Count > 0)
{
NodoDijkstra currentRouter = routerQueue.Dequeue();
//Visita cada enlace adyacente al router u
foreach (var enlace in currentRouter.listaEnlacesDijkstra)
{
int idRouterVecino = 0;
//Fix: Asegurandose de que se use el id del router adyacente en el enlace
if (enlace.idRouterB != currentRouter.idRouter)
{
idRouterVecino = enlace.idRouterB;
//enlace.target = enlace.targetB;
}
else
{
idRouterVecino = enlace.idRouterA;
//enlace.target = enlace.targetA;
}
//NodoDijkstra vecino = new NodoDijkstra(idRouterVecino, idProyecto);
NodoDijkstra vecino = enlace.target;
double nPesoBandwidth = 0;
switch(nTipoMetrica) //ignore var name, aqui va lo del tipo de peso
{
case 1: //Pesos Administrativos
nPesoBandwidth = enlace.nPesoAdministrativo;
break;
case 2: //Minima Cantidad de Saltos
nPesoBandwidth = 1;
break;
case 3: // 1/BW Reservado
nPesoBandwidth = 1.00 / (enlace.nBandwidth - enlace.nBandwidthDisponible);
break;
case 4: // 1/BW Disponible
nPesoBandwidth = 1.00 / enlace.nBandwidthDisponible;
break;
default:
nPesoBandwidth = 1;
break;
}
double nDistanciaTotal = currentRouter.nMinDistancia + nPesoBandwidth;
//Aqui ocurre el filtro por afinidad
if (idAfinidad == 0) //No afinidad definida
{
//En este if ocurre el filtro por BW disponible
if (nDistanciaTotal < vecino.nMinDistancia && minBW < enlace.nBandwidth) //Constraint check
{
if (routerQueue.Contains(vecino))
routerQueue.Remove(vecino);
vecino.nMinDistancia = nDistanciaTotal;
vecino.idRouterPrevio = currentRouter;
enlace.nBandwidthDisponible -= minBW; //reservar el BW en el enlace
routerQueue.Enqueue(vecino, 1);
}
}
else //Afinidad definida
{
if (idAfinidad == enlace.idAfinidad) //Afinidad check
{
//En este if ocurre el filtro por BW disponible
if (nDistanciaTotal < vecino.nMinDistancia && minBW < enlace.nBandwidth) //Constraint check
{
if (routerQueue.Contains(vecino))
routerQueue.Remove(vecino);
vecino.nMinDistancia = nDistanciaTotal;
vecino.idRouterPrevio = currentRouter;
enlace.nBandwidthDisponible -= minBW; //reservar el BW en el enlace
routerQueue.Enqueue(vecino, 1);
}
}
}
//Agrega el router (bueno, los 2) al registro
int indexTarget = routerList.FindIndex(n => n.idRouter == vecino.idRouter);
if (indexTarget != -1)
routerList[indexTarget] = vecino;
else
routerList.Add(vecino);
int indexSource = routerList.FindIndex(n => n.idRouter == currentRouter.idRouter);
if (indexSource != -1)
routerList[indexSource] = currentRouter;
else
routerList.Add(currentRouter);
}
}
return routerList;
}
/// <summary>
/// Finds a path through the puzzle originating from startNode and ending at endNode, going across from left to right.
/// Based on A* pathfinding algorithm, it theoretically is supposed to find the shortest possible path, but this is thus far untested/unproven.
///
/// First uses buildGraph to determine all possible and relevant edges between each tile/node, then uses a mostly standard A* algorithm, until
/// an orange tile is encountered and the rules change (because of "scents"). Every time an orange tile is encountered, a nested A*-based loop (referred to as the "orange loop")
/// is ran (but with orange-scented rules) which essentially starts at the orange tile and seeks any tile that will remove the orange scent, or the endNode.
/// Every tile encountered that exits the orange loop is added to the main open set (the open set of the normal A* loop) with their cost to get there through the orange loop.
///
/// </summary>
/// <returns>Returns a list of all possible paths to the endNode, and the shortest one, or the one with the least tree height, is to be considered the answer</returns>
public List<PathTreeNode> solve()
{
resetGraphEdges();
buildGraph();
//A*-based pathfinding algorithm
List<Node> closedSet = new List<Node>();
SimplePriorityQueue<Node> openSet = new SimplePriorityQueue<Node>();
List<Node> closedOrangeSet = new List<Node>();
SimplePriorityQueue<Node> openOrangeSet;
List<PathTreeNode> Leaves = new List<PathTreeNode>();
if(startNode.edges.Count == 0)
{
return Leaves;
}
startNode.g = 0;
openSet.Enqueue(startNode, 0);
PathTreeNode root = new PathTreeNode(startNode.row, -1);
Leaves.Add(root);
while (openSet.Count > 0)
{
Node current = openSet.Dequeue();
PathTreeNode currentStep = null;
Predicate<PathTreeNode> matchingCurrentPos = aStep => aStep.row == currentStep.row && aStep.col == currentStep.col;
if (current == endNode)
{
return Leaves;
}
if(current.edges.Count == 0)
{
continue;
}
foreach (PathTreeNode leaf in Leaves)
{
if(leaf.row == current.row && leaf.col == current.col)
{
if(currentStep == null || currentStep.height > leaf.height)
{
currentStep = leaf;
}
}
}
if (currentStep != null)
{
Leaves.RemoveAll(matchingCurrentPos);
}
if(current.color == 1)
{
openOrangeSet = new SimplePriorityQueue<Node>();
openOrangeSet.Enqueue(current, current.f);
currentStep.isOrangeStep = true;
Leaves.Add(currentStep);
while(openOrangeSet.Count > 0)
{
Node currentOrange = openOrangeSet.Dequeue();
PathTreeNode currentOrangeStep = null;
Predicate<PathTreeNode> matchingCurrentOrangePos = aStep => aStep.isOrangeStep && aStep.row == currentOrangeStep.row && aStep.col == currentOrangeStep.col;
if (currentOrange.edges.Count == 0)
{
continue;
}
foreach (PathTreeNode leaf in Leaves)
{
if (leaf.isOrangeStep && leaf.row == currentOrange.row && leaf.col == currentOrange.col)
{
if (currentOrangeStep == null || currentOrangeStep.height > leaf.height)
{
currentOrangeStep = leaf;
}
}
}
if (currentOrangeStep != null)
{
Leaves.RemoveAll(matchingCurrentOrangePos);
}
closedOrangeSet.Add(currentOrange);
foreach (Edge toOrangeNeighbor in currentOrange.edges)
{
if (closedSet.Contains(toOrangeNeighbor.childNode) || closedOrangeSet.Contains(toOrangeNeighbor.childNode))
{
continue;
}
if (toOrangeNeighbor.childNode.col == cols)
{
toOrangeNeighbor.childNode.row = currentOrange.row;
}
int currentOrangeG = currentOrange.g + Math.Abs((toOrangeNeighbor.childNode.row - currentOrange.row) + (toOrangeNeighbor.childNode.col - currentOrange.col)) + toOrangeNeighbor.childNode.weight;
if (openSet.Contains(toOrangeNeighbor.childNode) && toOrangeNeighbor.childNode.g < currentOrangeG)
{
continue;
}
PathTreeNode aNextStep;
if ((toOrangeNeighbor.isScented && !toOrangeNeighbor.isOrangeScented) || toOrangeNeighbor.childNode == endNode)
{
toOrangeNeighbor.childNode.f = currentOrangeG + heuristic(toOrangeNeighbor.childNode.col);
toOrangeNeighbor.childNode.g = currentOrangeG;
openSet.Enqueue(toOrangeNeighbor.childNode, toOrangeNeighbor.childNode.f);
aNextStep = new PathTreeNode(toOrangeNeighbor.childNode.row, toOrangeNeighbor.childNode.col, currentOrangeStep);
Leaves.Add(aNextStep);
continue;
}
if(toOrangeNeighbor.childNode.color == 4)
{
continue;
}
if (!openOrangeSet.Contains(toOrangeNeighbor.childNode))
{
toOrangeNeighbor.childNode.f = currentOrangeG + heuristic(toOrangeNeighbor.childNode.col);
openOrangeSet.Enqueue(toOrangeNeighbor.childNode, toOrangeNeighbor.childNode.f);
}
else if (currentOrangeG >= toOrangeNeighbor.childNode.g)
{
continue;
}
toOrangeNeighbor.childNode.g = currentOrangeG;
toOrangeNeighbor.childNode.f = currentOrangeG + heuristic(toOrangeNeighbor.childNode.col);
openOrangeSet.UpdatePriority(toOrangeNeighbor.childNode, toOrangeNeighbor.childNode.f);
aNextStep = new PathTreeNode(toOrangeNeighbor.childNode.row, toOrangeNeighbor.childNode.col, currentOrangeStep, true);
Leaves.Add(aNextStep);
}
}
Predicate<PathTreeNode> isOrangeStepLeaf = aStep => aStep.isOrangeStep;
Leaves.RemoveAll(isOrangeStepLeaf);
closedSet.Add(current);
}
else
{
closedSet.Add(current);
foreach (Edge toNeighbor in current.edges)
{
if (closedSet.Contains(toNeighbor.childNode))
{
continue;
}
if(current.col == -1)
{
current.row = toNeighbor.childNode.row;
}
else if(toNeighbor.childNode.col == cols)
{
toNeighbor.childNode.row = current.row;
}
int currentG = current.g + Math.Abs((toNeighbor.childNode.row - current.row) + (toNeighbor.childNode.col - current.col)) + toNeighbor.childNode.weight;
PathTreeNode aNextStep;
if (!openSet.Contains(toNeighbor.childNode))
{
toNeighbor.childNode.f = currentG + heuristic(toNeighbor.childNode.col);
openSet.Enqueue(toNeighbor.childNode, toNeighbor.childNode.f);
}
else if (currentG >= toNeighbor.childNode.g)
{
continue;
}
toNeighbor.childNode.g = currentG;
toNeighbor.childNode.f = currentG + heuristic(toNeighbor.childNode.col);
openSet.UpdatePriority(toNeighbor.childNode, toNeighbor.childNode.f);
aNextStep = new PathTreeNode(toNeighbor.childNode.row, toNeighbor.childNode.col, currentStep);
Leaves.Add(aNextStep);
}
}
}
return Leaves;
}
