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;
}
}
}
}
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;
}
public static HexCell[] Path(int from, int to, HexCell[] cells)
{
SimplePriorityQueue <HexCell> frontier = new SimplePriorityQueue <HexCell>();
Dictionary <HexCell, HexCell> visited = new Dictionary <HexCell, HexCell>();
Dictionary <HexCell, float> costAcc = new Dictionary <HexCell, float>();
HexCell start = cells[from];
HexCell goal = cells[to];
VisitCell(start, null, 0, ref frontier, ref visited);
costAcc[start] = 0;
while (frontier.Count > 0)
{
HexCell curr = frontier.Dequeue();
if (curr == goal)
{
break;
}
foreach (HexCell cell in curr.GetAllNeighbors())
{
float nextCost = costAcc[curr] + cell.MoveCost;
if (!visited.ContainsKey(cell) || nextCost < costAcc[cell])
{
costAcc[cell] = nextCost + HeuristicCost(cell, goal);
VisitCell(cell, curr, nextCost, ref frontier, ref visited);
}
}
}
return(PathfindingBase.ConstructPath(start, goal, visited));
}
void AddOneRingToQueue(Dictionary <int, HalfEdgeCounter> counter, SimplePriorityQueue <PQElement> Q, Vertex v, EnergyFun efun, Dictionary <int, int> flipCounter, int time)
{
foreach (var he in v.Circulate())
{
AddToQueue(counter, Q, he, efun, flipCounter, time);
}
}
private SimplePriorityQueue <ChatQueueItem, HigherVersionWinsComparerChat> GetQueueForDataContext(TeamsDataContext ctx)
{
if (!chatsToRetrieve.TryGetValue(ctx, out var queue))
{
queue = new SimplePriorityQueue <ChatQueueItem, HigherVersionWinsComparerChat>(new LowerVersionWinsChatComparer());
// try to add new queue; this might fail if another thread already did this
if (!chatsToRetrieve.TryAdd(ctx, queue))
{
// in case another thread added a queue use it
queue = chatsToRetrieve[ctx];
}
else
{
logger.Debug("[{TenantName}] Created thread retrieval queue for tenant", ctx.Tenant.TenantName);
}
}
lock (threads)
{
if (!threads.TryGetValue(ctx, out var thread))
{
thread = new Thread(() => ThreadFunc(ctx))
{
IsBackground = true
};
thread.Start();
logger.Debug("[{TenantName}] Starting retrieval thread for tenant", ctx.Tenant.TenantName);
threads.Add(ctx, thread);
}
}
return(queue);
}
public void ResetDistances(SimplePriorityQueue <Node> nodes)
{
foreach (Node node in nodes)
{
node.Distance = 100000000;
}
}
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.
}
// 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);
}
}
}
}
}
public List <Node> Run(List <List <Node> > grid, Node startNode, Node finishNode)
{
UnvisitedNodes = GetAllNodes(grid);
ResetDistances(UnvisitedNodes);
List <Node> visitedNodesInOrder = new List <Node>();
startNode.Distance = 0;
UnvisitedNodes.UpdatePriority(startNode, 0);
while (true)
{
// unvisitedNodes = SortNodesByDistance(unvisitedNodes);
Node closestNode = UnvisitedNodes.Dequeue();
if (closestNode.IsWall)
{
closestNode.IsVisited = true;
continue;
}
if (closestNode.Distance == 100000000)
{
return(visitedNodesInOrder);
}
closestNode.IsVisited = true;
visitedNodesInOrder.Add(closestNode);
if (closestNode.IsFinish == true)
{
return(visitedNodesInOrder);
}
Console.WriteLine(closestNode.Id);
UpdateUnvisitedNeighbors(closestNode, grid);
}
}
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);
}
private void InitializeClientData()
{
_pendingPackets = new Dictionary <int, PendingOutgoingPacket>();
for (int i = 0; i < _maxPending; ++i)
{
_pendingPackets.Add(i, null);
}
_timeoutQueue = new SimplePriorityQueue <PendingOutgoingPacket, float>();
_packetHandlers = new Dictionary <int, PacketHandler>()
{
{ (int)ServerPackets.welcome, ClientHandle.Welcome },
{ (int)ServerPackets.udpTest, ClientHandle.UdpTest },
{ (int)ServerPackets.spawnPlayer, ClientHandle.SpawnPlayer },
{ (int)ServerPackets.playerPosition, ClientHandle.PlayerPosition },
{ (int)ServerPackets.playerRotation, ClientHandle.PlayerRotation },
{ (int)ServerPackets.playerDisconnected, ClientHandle.PlayerDisconnected },
{ (int)ServerPackets.playerHealth, ClientHandle.PlayerHealth },
{ (int)ServerPackets.playerRespawned, ClientHandle.PlayerRespawned },
{ (int)ServerPackets.createItemSpawner, ClientHandle.CreateItemSpawner },
{ (int)ServerPackets.itemSpawned, ClientHandle.ItemSpawned },
{ (int)ServerPackets.itemPickedUp, ClientHandle.ItemPickedUp },
{ (int)ServerPackets.spawnProjectile, ClientHandle.SpawnProjectile },
{ (int)ServerPackets.projectilePosition, ClientHandle.ProjectilePosition },
{ (int)ServerPackets.projectileExploded, ClientHandle.ProjectileExploded }
};
Debug.Log("Initilized packet handlers!");
}
/// <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 void backToMapGeneratorHandler()
{
foreach (RectTransform rect in AliasDragAreas)
{
GeneratorUIManager.Instance.deleteMapOnUI(rect.GetChild(0));
rect.GetComponent <MapListManager>().dictionaryMap.Clear();
}
genMan.inAliasGenerator = false;
K_CollisionSet = null;
SimilarMapsQueue = null;
foreach (var t in ToggleLinesContainer.GetComponentsInChildren <Toggle>())
{
t.isOn = true;
}
deleteBestWorstUILines();
foreach (Transform child in GeneratorManager.Instance.Content.transform)
{
GameObject.Destroy(child.gameObject);
}
GeneratorUIManager.Instance.gameObject.GetComponent <UIParametersValueChange>().refreshUIParams();
}
private List <ObjAndNode <NodeType> > FindNearestRectToPoint(Point p, NodeType node)
{
List <ObjAndNode <NodeType> > answer_list = new List <ObjAndNode <NodeType> >();
SimplePriorityQueue <NodeType> searching_nodes = new SimplePriorityQueue <NodeType>();
searching_nodes.Enqueue(node, 0);
RectangleObj answer = null;
NodeType answer_node = default(NodeType);
double min_distance_sq = region_width * region_width;
while (searching_nodes.Count > 0)
{
NodeType current_node = searching_nodes.Dequeue();
Dictionary <RectangleObj, double> nearest_rect = current_node.GetNearestRectangle(p);
if (nearest_rect.Count > 0)
{
foreach (KeyValuePair <RectangleObj, double> entry in nearest_rect)
{
if (entry.Value <= min_distance_sq || entry.Value < 0.01)
{
min_distance_sq = entry.Value;
answer = entry.Key;
answer_node = current_node;
if (min_distance_sq < 0.01)
{
ObjAndNode <NodeType> ans = new ObjAndNode <NodeType>();
ans.node = answer_node;
ans.rect = answer;
answer_list.Add(ans);
answer = null;
}
}
}
}
if (current_node.HasChildren())
{
foreach (KeyValuePair <int, NodeType> child in current_node.getChildern())
{
NodeType child_node = child.Value;
RectangleObj field_rect = new RectangleObj();
field_rect.SetRectangleByCenter(child_node.getCenter(), child_node.getNodeSize(), child_node.getNodeSize());
double field_dist = field_rect.GetDistanceSqToPoint(p);
if (field_dist <= min_distance_sq)
{
searching_nodes.Enqueue(child_node, (float)field_dist);
}
}
}
}
if (answer != null)
{
ObjAndNode <NodeType> ans = new ObjAndNode <NodeType>();
ans.node = answer_node;
ans.rect = answer;
answer_list.Add(ans);
}
return(answer_list);
}
public override void StartAlgorithm(TDTile start, TDTile end, TGMap map)
{
algoSteps.Clear();
SimplePriorityQueue <TDTile> frontier = new SimplePriorityQueue <TDTile>();
frontier.Enqueue(start, 0);
Dictionary <TDTile, TDTile> cameFrom = new Dictionary <TDTile, TDTile>();
cameFrom.Add(start, null);
Dictionary <TDTile, float> costSoFar = new Dictionary <TDTile, float>();
costSoFar.Add(start, 0);
float priority = 0.0f;
while (frontier.Count > 0)
{
TDTile currentTile = frontier.Dequeue();
if (currentTile.GetTileType() == (int)TILE_TYPE.ENDPOINT)
{
break;
}
AlgorithmStep algoStep = new AlgorithmStep(currentTile);
algoSteps.Add(algoStep);
foreach (TDTile nextTile in currentTile.neighbours)
{
if (nextTile == null || nextTile.GetTileType() == (int)TILE_TYPE.WATER || nextTile.GetTileType() == (int)TILE_TYPE.WALL)
{
continue;
}
// diagonal step check: if neighbour is diagonal but diagonal step not allowed --> we skip
if (IsDiagonalNeighbour(currentTile, nextTile) && !IsDiagonalStepAllowed())
{
continue;
}
algoTiles.Add(nextTile);
float newCost = costSoFar[currentTile] + map.GetCostByTileType(nextTile.GetTileType());
if (!costSoFar.ContainsKey(nextTile) || newCost < costSoFar[nextTile])
{
costSoFar[nextTile] = newCost;
priority = newCost;
frontier.Enqueue(nextTile, priority);
cameFrom.Add(nextTile, currentTile);
algoStep.NeighbourTiles.Add(nextTile);
}
}
}
GeneratePath(end, cameFrom);
}
public TNode Evaluate <TNode, TKey>(TNode start, TKey key) where TNode : Node <TNode, TKey>
{
TNode bestComplete = null;
var bestNodes = new Dictionary <TKey, TNode>();
var toEvaluate = new SimplePriorityQueue <TKey, decimal>();
var evaluated = new HashSet <TKey>();
bestNodes[start.Key] = start;
toEvaluate.Enqueue(start.Key, start.EstimatedCost);
while (true)
{
if (toEvaluate.Count == 0)
{
//var x = bestNodes.Select(n => new { node = n.Value, next = n.Value.GetAdjacent().ToArray() })
// .ToArray();
var bestLeft = bestNodes.Values.OrderBy(i => i.CurrentCost).First();
Console.WriteLine("Best of the rest....");
return(bestLeft);
}
var workKey = toEvaluate.Dequeue();
var work = bestNodes[workKey];
if (bestComplete != null && bestComplete.CurrentCost <= work.EstimatedCost)
{
return(bestComplete);
}
evaluated.Add(work.Key);
foreach (var next in work.GetAdjacent())
{
if (!next.IsValid)
{
continue;
}
if (next.IsComplete)
{
if (null == bestComplete || next.CurrentCost < bestComplete.CurrentCost)
{
// new best - remember it
bestComplete = next;
}
// no need to continue to evaluate complete nodes
continue;
}
if (bestNodes.TryGetValue(next.Key, out var existing))
{
// we've already seen this node - update the cost if better, but no need to process further
if (next.CurrentCost < existing.CurrentCost)
{
bestNodes[next.Key] = next;
toEvaluate.TryUpdatePriority(next.Key, next.EstimatedCost);
}
continue;
}
// never seen this node before - track it and queue it up
bestNodes.Add(next.Key, next);
toEvaluate.Enqueue(next.Key, next.EstimatedCost);
}
}
}
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);
}
private static int Reduce(List <Rule> rules, string molecule)
{
// Dictionary with number of steps to molecule
Dictionary <string, int> moleculeToSteps = new Dictionary <string, int>()
{
{ molecule, 0 }
};
// Remember processed molecules
HashSet <string> seen = new HashSet <string>();
// Queue with molecules to process
SimplePriorityQueue <string> toProcess = new SimplePriorityQueue <string>();
toProcess.Enqueue(molecule, molecule.Length);
// Randomizator, because otherwise we get stuck
Random rng = new Random();
while (toProcess.Any())
{
// Get another molecule to process
string oldMolecule = toProcess.Dequeue();
//Console.WriteLine(oldMolecule);
if (oldMolecule == "e")
{
break;
}
// Do all possible reductions
foreach (Rule rule in rules.OrderBy(x => rng.Next()))
{
foreach (int index in oldMolecule.IndiciesOf(rule.To))
{
// Do reduction
string newMolecule = oldMolecule.Substring(0, index) + rule.From + oldMolecule.Substring(index + rule.To.Length);
// Calculate steps
int steps = moleculeToSteps[oldMolecule] + 1;
// Save progress
if (!moleculeToSteps.TryGetValue(newMolecule, out int oldSteps) || oldSteps > steps)
{
moleculeToSteps[newMolecule] = steps;
}
// Do more reductions
if (!seen.Contains(newMolecule))
{
toProcess.Enqueue(newMolecule, newMolecule.Length);
seen.Add(newMolecule);
}
}
}
}
return(moleculeToSteps["e"]);
}
public static List <GridTile> SearchByGridPosition(GridTile start, int maxReach, bool WalkableTilesOnly = true, bool unoccupiedTilesOnly = true, bool square = true, bool ignoreHeight = false, bool includeStartingTile = false, int MinReach = 1)
{
List <GridTile> range = new List <GridTile>();
// Start is goal
if (maxReach <= 0)
{
range.Add(start);
return(range);
}
if (maxReach < MinReach)
{
return(range);
}
Dictionary <GridTile, float> cost_so_far = new Dictionary <GridTile, float>();
SimplePriorityQueue <GridTile> frontier = new SimplePriorityQueue <GridTile>();
frontier.Enqueue(start, 0);
cost_so_far.Add(start, 0);
GridTile current = start;
while (frontier.Count > 0)
{
current = frontier.Dequeue();
if (cost_so_far[current] <= maxReach)
{
var neighbors = WalkableTilesOnly == true?GridManager.Instance.WalkableNeighbors(current, ignoreHeight, unoccupiedTilesOnly, null, GridManager.defaultRectangle8Directions) : GridManager.Instance.Neighbors(current, ignoreHeight, GridManager.defaultRectangle8Directions);
foreach (GridTile next in neighbors)
{
float new_cost = cost_so_far[current] + (square == true ? 1 : Utilities.Heuristic(current, next));
if (!cost_so_far.ContainsKey(next))
{
cost_so_far[next] = new_cost;
float priority = new_cost;
frontier.Enqueue(next, priority);
if (!range.Contains(next) && new_cost >= MinReach && new_cost <= maxReach)
{
range.Add(next);
}
}
}
}
}
// remove the starting tile if required
if (!includeStartingTile)
{
if (range.Contains(start))
{
range.Remove(start);
}
}
return(range);
}
public static LinkedList <Edge> FindPath(Node start, Node dest, Boundaries boundaries = null)
{
HashSet <GridTile> Visited = new HashSet <GridTile>();
Dictionary <GridTile, Edge> Parent = new Dictionary <GridTile, Edge>();
Dictionary <GridTile, float> gScore = new Dictionary <GridTile, float>();
SimplePriorityQueue <Node, float> pq = new SimplePriorityQueue <Node, float>();
float temp_gCost, prev_gCost;
gScore[start.pos] = 0;
pq.Enqueue(start, EuclidianDistance(start, dest));
Node current;
while (pq.Count > 0)
{
current = pq.Dequeue();
if (current.pos.Equals(dest.pos))
{
//Rebuild path and return it
return(RebuildPath(Parent, current));
}
Visited.Add(current.pos);
//Visit all neighbours through edges going out of node
foreach (Edge e in current.edges)
{
//If we defined boundaries, check if it crosses it
if (boundaries != null && IsOutOfGrid(e.end.pos, boundaries))
{
continue;
}
//Check if we visited the outer end of the edge
if (Visited.Contains(e.end.pos))
{
continue;
}
temp_gCost = gScore[current.pos] + e.weight;
//If new value is not better then do nothing
if (gScore.TryGetValue(e.end.pos, out prev_gCost) && temp_gCost >= prev_gCost)
{
continue;
}
//Otherwise store the new value and add the destination into the queue
Parent[e.end.pos] = e;
gScore[e.end.pos] = temp_gCost;
pq.Enqueue(e.end, temp_gCost + EuclidianDistance(e.end, dest));
}
}
return(new LinkedList <Edge>());
}
public VLCTaskQueue(MediaPlayer mediaPlayer)
{
MediaPlayer = mediaPlayer;
SimplePriorityQueue <int, Action <MediaPlayer> > queue = new SimplePriorityQueue <int, Action <MediaPlayer> >(2);
_bc = new BlockingCollection <KeyValuePair <int, Action <MediaPlayer> > >();
_loopTask = Task.Factory.StartNew(() => executeLoop(cts.Token), CancellationToken.None, TaskCreationOptions.LongRunning, TaskScheduler.Default);
}
public static List <MessagePrioritiesModel> SortInbox(List <Message> inbox)
{
List <string> senders = GetSenderNames(inbox);
SimplePriorityQueue <string> queue = PopulatePriorityQueue(senders);
List <MessagePrioritiesModel> sortedSenders = PriorityQueueToList(queue);
return(sortedSenders);
}
public void SimplePriorityQueueFunctionalityTest()
{
// create the queue and specify the comparison func. This is a simple lambda which returns the comparison on priorities
SimplePriorityQueue <QueueElement> queue = new SimplePriorityQueue <QueueElement>((a, b) => a.Priority.CompareTo(b.Priority));
// use the generic method for all queues.
PriorityQueueFunctionalityTest(queue, 100, true);
}
public void Initialize(NodeController node0, NodeController node1, int initialState)
{
startNode = node0;
endNode = node1;
this.initialState = initialState;
prioQueue = new SimplePriorityQueue <HelperNodeController>();
InitializeAllDistances();
}
public static bool Search(GraphNode source, GraphNode destination, ref List <GraphNode> path, int maxSteps)
{
bool found = false;
path = new List <GraphNode>();
//djkstra algorithm
SimplePriorityQueue <GraphNode> nodes = new SimplePriorityQueue <GraphNode>();
source.Cost = 0;
nodes.Enqueue(source, source.Cost);
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;
nodes.EnqueueWithoutDuplicates(edge.nodeB, cost);
}
}
}
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);
}
/// Optimize in a greedy fashion.
public void PriorityQueueOptimization(HMesh m, EnergyFun efun)
{
//HalfEdgeAttributeVector<HalfEdgeCounter> counter(m.allocated_halfedges(), HalfEdgeCounter{0, false});
// VertexAttributeVector<int> flipCounter(m.allocated_vertices(), 0);
Dictionary <int, HalfEdgeCounter> counter = new Dictionary <int, HalfEdgeCounter>();
Dictionary <int, int> flipCounter = new Dictionary <int, int>();
Priority_Queue.SimplePriorityQueue <PQElement> Q = new SimplePriorityQueue <PQElement>();
//priority_queue<PQElement> Q;
//cout << "Building priority queue"<< endl;
int time = 1;
foreach (var h in m.GetHalfedgesRaw())
{
if (!counter.ContainsKey(h.id))
{
counter.Add(h.id, new HalfEdgeCounter());
}
AddToQueue(counter, Q, h, efun, flipCounter, time);
}
//cout << "Emptying priority queue of size: " << Q.size() << " ";
while (Q.Count > 0)
{
PQElement elem = Q.Dequeue();
//Walker w = m.walker(elem.h);
if (counter[elem.h.id].isRemovedFromQueue) // if item already has been processed continue
{
continue;
}
counter[elem.h.id].isRemovedFromQueue = true;
if (counter[elem.h.id].touched != elem.time)
{
if (efun.DeltaEnergy(elem.h) >= 0)
{
continue;
}
}
if (!PrecondFlipEdge(elem.h))
{
continue;
}
flipCounter[elem.h.vert.id]++;
elem.h.Flip();
AddOneRingToQueue(counter, Q, elem.h.vert, efun, flipCounter, time);
AddOneRingToQueue(counter, Q, elem.h.next.vert, efun, flipCounter, time);
AddOneRingToQueue(counter, Q, elem.h.opp.vert, efun, flipCounter, time);
AddOneRingToQueue(counter, Q, elem.h.opp.next.vert, efun, flipCounter, time);
}
}
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);
}
/// <summary>
/// Finds a path between two <see cref="PathfindingCell"/>s.
/// </summary>
/// <returns>
/// If start == end: An empty enumeration
/// If no path is possible: An single null item
/// Otherwise: A lazy enumeration with the steps in the path, including <paramref name="start"/> end <paramref name="end"/>.
/// </returns>
public override IEnumerable <TCell> FindPath <TCell>(TCell start, TCell end)
{
if (start == end)
{
yield break;
}
// The PriorityQueue library has a FastPriorityQueue that has tons of optimizations but there's a big problem: you need to define the max size and it doesn't resize itself.
// SimplyPriorityQueue isn't as fast, but still pretty fast and it has no maximum size.
var frontier = new SimplePriorityQueue <TCell>();
var cameFrom = new Dictionary <TCell, TCell>();
var costSoFar = new Dictionary <TCell, int>();
// We start searching at the end and look for the start.
// We do it this way because when we create a path, it will be backwards.
// So if we do this backwards to begin with, then the returned list will actually be forwards.
frontier.Enqueue(end, end.Cost);
costSoFar[end] = end.Cost;
// This is a local function so we can break out of the nested loop more easily
bool traverse()
{
while (frontier.TryDequeue(out TCell current))
{
foreach (TCell next in current.Neighbors.Except(cameFrom.Keys))
{
int newCost = costSoFar[current] + next.Cost;
if (!costSoFar.TryGetValue(next, out int cost) || newCost < cost)
{
cameFrom[next] = current;
if (next == start)
{
return(true);
}
costSoFar[next] = newCost;
frontier.Enqueue(next, newCost);
}
}
}
return(false);
}
if (traverse())
{
TCell current = start;
while (current != end)
{
current = cameFrom[current];
yield return(current);
}
}
else
{
yield return(null);
}
}
//Dijkstra's algorithm.
//Populates IList<Vector3> path with a valid solution to the goalPosition.
//Returns the goalPosition if a solution is found.
//Returns the startPosition if no solution is found.
Vector3 FindShortestPathDijkstra(Vector3 startPosition, Vector3 goalPosition)
{
uint nodeVisitCount = 0;
float timeNow = Time.realtimeSinceStartup;
//A priority queue containing the shortest distance so far from the start to a given node
IPriorityQueue <Vector3, int> priority = new SimplePriorityQueue <Vector3, int>();
//A list of all nodes that are walkable, initialized to have infinity distance from start
IDictionary <Vector3, int> distances = walkablePositions
.Where(x => x.Value == true)
.ToDictionary(x => x.Key, x => int.MaxValue);
//Our distance from the start to itself is 0
distances[startPosition] = 0;
priority.Enqueue(startPosition, 0);
while (priority.Count > 0)
{
Vector3 curr = priority.Dequeue();
nodeVisitCount++;
if (curr == goalPosition)
{
// If the goal position is the lowest position in the priority queue then there are
// no other nodes that could possibly have a shorter path.
print("Dijkstra: " + distances[goalPosition]);
print("Dijkstra time: " + (Time.realtimeSinceStartup - timeNow).ToString());
print(string.Format("Dijkstra visits: {0} ({1:F2}%)", nodeVisitCount, (nodeVisitCount / (double)walkablePositions.Count) * 100));
return(goalPosition);
}
IList <Vector3> nodes = GetWalkableNodes(curr);
//Look at each neighbor to the node
foreach (Vector3 node in nodes)
{
int dist = distances[curr] + Weight(node);
//If the distance to the parent, PLUS the distance added by the neighbor,
//is less than the current distance to the neighbor,
//update the neighbor's paent to curr, update its current best distance
if (dist < distances [node])
{
distances [node] = dist;
nodeParents [node] = curr;
if (!priority.Contains(node))
{
priority.Enqueue(node, dist);
}
}
}
}
return(startPosition);
}
public static DijsktraTable MakeDijkstra(T1 from, T1 to, Dictionary <T1, Node> Nodes)
{
SimplePriorityQueue <NodePriorityQueue, T2> queue = new SimplePriorityQueue <NodePriorityQueue, T2>();
Dictionary <T1, T2> distances = new Dictionary <T1, T2>();
Dictionary <T1, T1> perviosNodes = new Dictionary <T1, T1>();
ISet <T1> visited = new HashSet <T1>();
var start = Nodes[from];
//dynamic tmp = 0;
distances.Add(start.Value, default(T2));
perviosNodes.Add(start.Value, default(T1));
queue.Enqueue(new NodePriorityQueue(start, null, default(T2)), default(T2));
while (queue.Count != 0)
{
var queueNode = queue.Dequeue();
var node = queueNode.Node;
if (visited.Contains(node.Value))
{
continue;
}
dynamic tmpDistance = queueNode.Weight;
if (distances.ContainsKey(node.Value))
{
if (tmpDistance < distances[node.Value])
{
distances[node.Value] = (T2)tmpDistance;
perviosNodes[node.Value] = queueNode.Source.Value; //node.Edges[
//node.Edges[node.Value].Where(n => n.From.Value.Equals(node.Value)).FirstOrDefault().To.Value;
}
}
else
{
distances.Add(node.Value, (T2)tmpDistance);
perviosNodes.Add(node.Value, queueNode.Source.Value);
}
foreach (var list in node.Edges.Values)
{
foreach (var item in list)
{
dynamic dist = item.Weigth;
dist += distances[node.Value];
T2 x = dist;
//float distanceFloat = BitConverter.ToSingle(dist);
queue.Enqueue(new NodePriorityQueue(item.To, item.From, x), x);//BitConverter.GetBytes((dist)));
}
}
visited.Add(node.Value);
}
return(new DijsktraTable {
Distances = distances, PerviosNodes = perviosNodes
});
}
public static PathResult GetPath(Vector2Int startPosition, Vector2Int endPosition)
{
TileProperty start = Loki.map[startPosition];
TileProperty end = Loki.map[endPosition];
bool success = false;
Vector2Int[] path = new Vector2Int[0];
start.parent = start;
if (!start.blockPath && !end.blockPath)
{
SimplePriorityQueue <TileProperty> openSet = new SimplePriorityQueue <TileProperty>();
HashSet <TileProperty> closedSet = new HashSet <TileProperty>();
openSet.Enqueue(start, start.fCost);
while (openSet.Count > 0)
{
TileProperty current = openSet.Dequeue();
if (current == end)
{
success = true;
break;
}
closedSet.Add(current);
for (int i = 0; i < 8; i++)
{
TileProperty neighbour = Loki.map[current.position + DirectionUtils.neighbours[i]];
if (neighbour == null || neighbour.blockPath || closedSet.Contains(neighbour))
{
continue;
}
float neighbourCost = current.gCost + Utils.Distance(current.position, neighbour.position) + neighbour.pathCost;
if (neighbourCost > neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = neighbourCost;
neighbour.hCost = Utils.Distance(neighbour.position, end.position);
neighbour.parent = current;
if (!openSet.Contains(neighbour))
{
openSet.Enqueue(neighbour, neighbour.fCost);
}
else
{
openSet.UpdatePriority(neighbour, neighbour.fCost);
}
}
}
}
}
if (success)
{
path = PathFinder.CalcPath(start, end);
success = path.Length > 0;
}
return(new PathResult(path, success));
}
public static Queue <RobotAction> GetRouteTo(Point start, Point goal, Map map, int maxDistance = int.MaxValue)
{
if (start == goal)
{
return(new Queue <RobotAction>());
}
var closed_set = new HashSet <Point>();
var came_from = new Dictionary <Point, Point>();
var g_score = new Dictionary <Point, float> {
{ start, 0 }
};
var open_set = new SimplePriorityQueue <Point, float>();
open_set.Enqueue(start, GetDistance(start, goal));
while (open_set.Count != 0)
{
var current = open_set.Dequeue();
if (current == goal)
{
return(ReconstructPath(came_from, start, goal));
}
closed_set.Add(current);
var tentative_g_score = g_score[current] + 1;
if (tentative_g_score > maxDistance)
{
continue;
}
foreach (var neighbor in map.Neighbors(current))
{
if (closed_set.Contains(neighbor))
{
continue;
}
if (!open_set.Contains(neighbor))
{
open_set.Enqueue(neighbor, tentative_g_score + GetDistance(neighbor, goal));
}
else if (tentative_g_score >= g_score[neighbor])
{
continue;
}
came_from[neighbor] = current;
g_score[neighbor] = tentative_g_score;
open_set.UpdatePriority(neighbor, tentative_g_score + GetDistance(neighbor, goal));
}
}
return(null);
}
public SimplePriorityQueue <StorageBuilding> FindStorageBuildingToAccept(ItemOrder io)
{
int num = io.amount;
int item = io.item;
ItemType type = io.type;
GameObject[] objs = GameObject.FindGameObjectsWithTag("StorageBuilding");
SimplePriorityQueue <StorageBuilding> queue = new SimplePriorityQueue <StorageBuilding>();
if (objs.Length == 0)
{
return(queue);
}
foreach (GameObject go in objs)
{
StorageBuilding strg = go.GetComponent <StorageBuilding>();
//if null, continue
if (strg == null)
{
continue;
}
//only add to list if it stores this type
if (strg.typeStored != type)
{
continue;
}
if (!strg.Operational)
{
continue;
}
//only add to list if it has an entrance
List <Node> entrancesHere = GetAdjRoadTiles();
List <Node> entrancesThere = strg.GetAdjRoadTiles();
if (entrancesHere.Count == 0 || entrancesThere.Count == 0)
{
continue;
}
//only add to list if it can accept amount
if (!strg.CanAcceptAmount(num, item))
{
continue;
}
float distance = entrancesHere[0].DistanceTo(entrancesThere[0]);
queue.Enqueue(strg, distance);
}
return(queue);
}
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;
}
/// <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);
}
public void SimplePriorityQueueFunctionalityTest()
{
// create the queue and specify the comparison func. This is a simple lambda which returns the comparison on priorities
SimplePriorityQueue<QueueElement> queue = new SimplePriorityQueue<QueueElement>((a, b) => a.Priority.CompareTo(b.Priority));
// use the generic method for all queues.
PriorityQueueFunctionalityTest(queue, 100, true);
}
/// <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;
}
public ActionResult _CrearCSPF(CSPFViewModel newModel)
{
//Proyecto proyectoActual = new Proyecto(newModel.idProyecto);
NodoDijkstra RouterOrigen = new NodoDijkstra(newModel.nRouterOrigen, newModel.idProyecto);
SimplePriorityQueue<NodoDijkstra> routerQueue = new SimplePriorityQueue<NodoDijkstra>();
routerQueue = Dijkstra.GenerarRutas(RouterOrigen, newModel.idProyecto);
NodoDijkstra RouterDestino = routerQueue.FirstOrDefault(x => x.idRouter == newModel.nRouterDestino);
List<NodoDijkstra> result = new List<NodoDijkstra>();
result = Dijkstra.GetRutaMasCortaHasta(RouterDestino);
return Json(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;
}
}
}
/// <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;
}
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.
}
