public override IEnumerable <Node> AStar(Node from, Node to)
{
var openSet = new SimplePriorityQueue <Node>();
openSet.Enqueue(from, 0f);
IEnumerable <Node> path = new List <Node>();
while (openSet.Count > 0)
{
Parallel.For(0, Math.Min(4, openSet.Count), i =>
{
var currentNode = openSet.Dequeue();
if (currentNode == to)
{
path = ReconstructPath(currentNode);
}
currentNode.ClosedSet = 1;
foreach (var neighbor in currentNode.Neighbors)
{
{
if (neighbor.ClosedSet != 0)
{
continue;
}
var tentativeGScore = currentNode.GScore + currentNode.Distance(neighbor);
if (openSet.Contains(neighbor) && tentativeGScore >= neighbor.GScore)
{
continue;
}
Interlocked.Exchange(ref neighbor.CameFrom, currentNode);
Interlocked.Exchange(ref neighbor.GScore, tentativeGScore);
var fs = tentativeGScore + HeuristicCostEstimate(neighbor, to);
if (openSet.Contains(neighbor))
{
openSet.UpdatePriority(neighbor, fs);
}
else
{
openSet.Enqueue(neighbor, fs);
}
}
}
});
}
foreach (var node in this)
{
node.Reset();
}
return(path);
}
public void RegisterEntity(ITimeNode node)
{
if (!turnQueue.Contains(node))
{
turnQueue.Enqueue(node, node.Tick);
}
}
private void ExpandNode(Point currentNode, Func <Point, float> heuristic)
{
foreach (var dir in UnobstructedDirections(currentNode))
{
var nextNode = new Point(currentNode.X + (int)dir.X, currentNode.Y + (int)dir.Y);
if (mVisited.Contains(nextNode))
{
continue;
}
var nextWeight = mWeights[currentNode] + dir.Length();
if (mPaths.Contains(nextNode) && nextWeight >= mWeights[nextNode])
{
continue;
}
mPathRecord[nextNode] = currentNode;
mWeights[nextNode] = nextWeight;
var priority = nextWeight + heuristic(nextNode);
if (mPaths.Contains(nextNode))
{
mPaths.UpdatePriority(nextNode, priority);
}
else
{
mPaths.Enqueue(nextNode, priority);
}
}
}
public static void updateVertex(SimplePriorityQueue <Spot> openSet, Spot s, Spot succ, Spot start, Spot goal)
{
double angle = theta(s.pos, s.parent.pos, succ.pos);
if (s != start && s.lb <= angle && s.ub >= angle)
{
// Path - 2
if (s.parent.g + s.parent.pos.DistanceTo(succ.pos) < succ.g && s.los)
{
succ.g = s.parent.g + (float)s.parent.pos.DistanceTo(succ.pos);
succ.parent = s.parent;
if (openSet.Contains(succ))
{
openSet.Remove(succ);
}
succ.f = succ.g + calcHeuristic(succ, goal);
openSet.Enqueue(succ, succ.f);
}
}
else
{
// Path - 1
if (s.g + s.pos.DistanceTo(succ.pos) < succ.g && s.los)
{
succ.g = s.g + (float)s.pos.DistanceTo(succ.pos);
succ.parent = s;
if (openSet.Contains(succ))
{
openSet.Remove(succ);
}
succ.f = succ.g + calcHeuristic(succ, goal);
openSet.Enqueue(succ, succ.f);
}
}
}
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));
}
// javadoc:
// "Returns true if and only if this queue contained the specified element"
public bool remove(T t)
{
if(!simplePriorityQueue.Contains(t))
{
return false;
}
simplePriorityQueue.Remove(t);
return true;
}
public override IEnumerable <Node> AStar(Node from, Node to)
{
var closedSet = new ConcurrentBag <Node>();
var openSet = new SimplePriorityQueue <Node>();
openSet.Enqueue(from, 0f);
var cameFrom = new ConcurrentDictionary <Node, Node>();
var gScore = new ConcurrentDictionary <Node, float>();
gScore.TryAdd(from, 0f);
while (openSet.Count > 0)
{
var currentNode = openSet.Dequeue();
if (currentNode == to)
{
return(ReconstructPath(cameFrom, currentNode));
}
closedSet.Add(currentNode);
Parallel.ForEach(currentNode.Neighbors, neighbor =>
{
if (closedSet.Contains(neighbor))
{
return;
}
var tentativeGScore = gScore[currentNode] + currentNode.Distance(neighbor);
if (openSet.Contains(neighbor) && tentativeGScore >= gScore[neighbor])
{
return;
}
cameFrom[neighbor] = currentNode;
gScore[neighbor] = tentativeGScore;
var fs = tentativeGScore + HeuristicCostEstimate(neighbor, to);
if (openSet.Contains(neighbor))
{
openSet.UpdatePriority(neighbor, fs);
}
else
{
openSet.Enqueue(neighbor, fs);
}
});
}
return(new List <Node>());
}
public override void Add(Vector3Int incoming, ChunkStageData stageData)
{
if (!TerminateHereCondition(stageData))
{
if (queue.Contains(incoming))//Update priority if item existed
{
queue.UpdatePriority(incoming, getPriority(incoming));
}
else
{//Add to queue otherwise
queue.Enqueue(incoming, getPriority(incoming));
}
}
//incoming terminates here and so does not need to be added to the queue
}
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);
}
}
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);
}
// 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);
}
}
}
}
}
internal bool UnSubscribeBindModule(Module moudle)
{
using (new LockWait(ref _lock))
{
Type type = moudle.GetType();
if (!_dic.ContainsKey(type))
{
Log.E(string.Format("01,Have Not Bind Module | Type {0} Name {1}", type, moudle.name));
return(false);
}
else
{
var list = _dic[type];
if (!list.ContainsKey(moudle.name))
{
Log.E(string.Format("02,Have Not Bind Module | Type {0} Name {1}", type, moudle.name));
return(false);
}
else
{
_dic[type].Remove(moudle.name);
if (_queue.Contains(moudle))
{
_queue.Remove(moudle);
}
return(true);
}
}
}
}
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 void UpdateVertex(int x, int y, int nx, int ny)
{
worldNode current = worldNodes[x, y];
worldNode next = worldNodes[nx, ny];
float css = cost(x, y, nx, ny);
if (css == 0)
{
closedList.Add(next);
return;
}
if (current.g + css < next.g)
{
next.g = current.g + css;
next.h = computeHeuristic(nx, ny);
next.f = next.g + next.h;
next.parent = current;
if (fringe.Contains(next))
{
fringe.UpdatePriority(next, next.f);
}
else
{
fringe.Enqueue(next, next.f); // f- c*g where c = 200
}
//next.g + h - 200*next.g
}
}
/// <summary>
/// Creates a minimum spanning tree using Prim's algorithm. The graph this is called on has to be connected.
/// </summary>
/// <param name="random">Random object for getting random starter element. As parameter to keep random seed predictability.</param>
/// <returns>The minimum spanning tree.</returns>
public RoomGraph MinSpanningTree(Random random, Func <Room, Room, double> DistFunc)
{
if (Count == 0)
{
return(new RoomGraph());
}
SimplePriorityQueue <Room> queue = new SimplePriorityQueue <Room>();
Dictionary <Room, double> distanceToSpannTree = new Dictionary <Room, double>(Count);
Dictionary <Room, Room> parentRoom = new Dictionary <Room, Room>(Count);
foreach (Room r in rooms)
{
queue.Enqueue(r, double.MaxValue);
distanceToSpannTree.Add(r, double.MaxValue);
parentRoom.Add(r, null);
}
int start = random.Next(Count);
distanceToSpannTree[rooms[start]] = 0;
queue.UpdatePriority(rooms[start], 0);
while (queue.Count > 0)
{
Room r = queue.Dequeue();
foreach (Room other in connections[r])
{
if (queue.Contains(other)) //could keep track of this separatly (HashSet) because this is linear.
{
double dist = DistFunc(r, other);
if (dist < distanceToSpannTree[other])
{
parentRoom[other] = r;
distanceToSpannTree[other] = dist;
queue.UpdatePriority(other, dist);
}
}
}
}
//insert the parentNode pairs into a new graph
RoomGraph minSpanTree = new RoomGraph();
foreach (Room r in rooms)
{
minSpanTree.rooms.Add(r);
minSpanTree.connections.Add(r, new List <Room>(8));
}
foreach (var p in parentRoom)
{
if (p.Key != null && p.Value != null)
{
minSpanTree.AddConnectionBothWays(p.Key, p.Value);
}
}
return(minSpanTree);
}
private void Insert(SearchNode X, float newH)
{
if (X.IsNew)
{
X.Key = newH;
}
else if (X.Opened)
{
X.Key = Mathf.Min(X.Key, newH);
}
else if (X.Closed)
{
X.Key = Mathf.Min(X.H, newH);
}
X.Opened = true;
X.H = newH;
if (m_openQueue.Contains(X))
{
m_openQueue.UpdatePriority(X, X.Key);
}
else
{
m_openQueue.Enqueue(X, X.Key);
}
}
public void AdvanceFrontier()
{
if (frontier.Count != 0 && !frontier.Contains(destination))
{
int cycles = frontier.Count;
for (int i = 0; i < cycles; i++)
{
//check each neighbor
SearchLoop();
}
}
else
{
Debug.Log("Search End");
}
}
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);
}
}
public void Search()
{
previous[source] = source;
//we have to continue until we pop the target
while (Unvisited.Contains(target) || Unvisited.Count == 0)
{
// If we don't have any nodes left to find a path to, then stop
if (Unvisited.Count == 0)
{
return;
}
//before we mess with it we need to get the distance to the node at the front of the priority queue
dist[Unvisited.First] = Unvisited.GetPriority(Unvisited.First);
//take it off the top, let's see what we can do with it.
//the int refers to the id of the node we dequeue
int lastChosen = Unvisited.Dequeue();
//now that we have it, what are its neighbors?
//we need to update the previous and distances to all of these objects
foreach (GraphEdge e in graph.edges[lastChosen])
{
// I need to refrain from looking at nodes already visited, otherwise ignore per the foreach
if (Unvisited.Contains(e.to))
{
// put in words: if the cost of the edge + the distance from the source to the "from" of the current edge < the previously known cost to the "to" of the edge.
if (dist[lastChosen] + e.cost < Unvisited.GetPriority(e.to))
{
// if we've made it into this loop, then we've found an improvement for the path to this node
//first, update what is pointing at the current node
previous[e.to] = e.from;
//update the priority with the new calculated cost
Unvisited.UpdatePriority(e.to, dist[lastChosen] + e.cost);
}
}
}
}
//TODO : if unvisited.count == 0...return;///////
//else
//we are done and have the data...
}
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);
}
//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);
}
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 bool Enqueue(RemindedTask task)
{
if (List.Contains(task))
{
return(false);
}
List.Enqueue(task, task.unixTime);
return(true);
}
public Vector3[] FindPath(Vector3 startPos, Vector3 targetPos)
{
var path = new Vector3[0];
var pathFound = false;
//Debug.Log(startPos + " " + targetPos);
var startNode = _grid.getNodeFromPos(startPos);
var targetNode = _grid.getNodeFromPos(targetPos);
var fringe = new SimplePriorityQueue <Node>();
var visited = new HashSet <Node>();
startNode.gCost = 0;
startNode.hCost = startNode.GetDistance(targetNode);
fringe.Enqueue(startNode, (float)(0 + startNode.hCost));
while (fringe.Count > 0)
{
var currentNode = fringe.Dequeue();
visited.Add(currentNode);
if (Equals(currentNode, targetNode))
{
pathFound = true;
break;
}
foreach (var neighbour in _grid.GetNeighbours(currentNode))
// .Where(neighbour => neighbour.walkable && !visited.Contains(neighbour))
// .Where(neighbour => !fringe.Contains(neighbour)))
{
if (neighbour.walkable && !visited.Contains(neighbour))
{
if (!fringe.Contains(neighbour))
{
neighbour.gCost = currentNode.gCost + currentNode.GetDistance(neighbour);
neighbour.hCost = neighbour.GetDistance(targetNode);
neighbour.parent = currentNode;
fringe.Enqueue(neighbour, (float)neighbour.fCost);
}
/*else
* {
* var cost = currentNode.gCost + currentNode.GetDistance(neighbour);
* cost+=neighbour.GetDistance(targetNode);
* if (cost < neighbour.fCost)
* {
* fringe.UpdatePriority(neighbour, (float)cost);
* }
* }*/
}
}
}
if (pathFound)
{
path = GetPath(startNode, targetNode);
}
return(path);
}
/// <summary>
/// Do not call directly, use Chunk.CreateVBO().
/// </summary>
/// <param name="ch"></param>
public void NeedToRender(Chunk ch)
{
lock (RenderingNow)
{
if (!NeedsRendering.Contains(ch.WorldPosition))
{
NeedsRendering.Enqueue(ch.WorldPosition, (ch.WorldPosition.ToLocation() * Chunk.CHUNK_SIZE).DistanceSquared(TheClient.Player.GetPosition()));
}
}
}
public List <Vector2> Astar(Vector2 start, Vector2 goal)
{
SimplePriorityQueue <int> openSet = new SimplePriorityQueue <int>();
Dictionary <int, Vector2> cameFrom = new Dictionary <int, Vector2>();
Dictionary <int, float> gScore = new Dictionary <int, float>();
Dictionary <int, float> fScore = new Dictionary <int, float>();
foreach (int key in inner.Keys)
{
gScore[key] = float.MaxValue;
fScore[key] = float.MaxValue;
}
int startHash = Utils.HashVector2(start);
int goalHash = Utils.HashVector2(goal);
gScore[startHash] = 0f;
fScore[startHash] = h(start, goal);
openSet.Enqueue(startHash, fScore[startHash]);
while (openSet.Count > 0)
{
int current = openSet.First;
if (current == goalHash)
{
return(reconstructPath(cameFrom, current));
}
openSet.Dequeue();
foreach (Vector2 neib in inner[current])
{
// d = h :)
float tentativeGScore = gScore[current] + h(hashToVec[current], neib);
int hashNeib = Utils.HashVector2(neib);
if (tentativeGScore < gScore[hashNeib])
{
cameFrom[hashNeib] = hashToVec[current];
gScore[hashNeib] = tentativeGScore;
fScore[hashNeib] = gScore[hashNeib] + h(start, neib);
if (!openSet.Contains(hashNeib))
{
openSet.Enqueue(hashNeib, fScore[hashNeib]);
}
}
}
}
Debug.LogWarning("Astar failed to find a path");
return(null);
}
public static List <Node> DoPathFind_AStar(Node startNode, Node endNode, Node[,] maps)
{
// clear everything
foreach (Node n in maps)
{
n.cameFrom = null;
}
SimplePriorityQueue <Node> nodes = new SimplePriorityQueue <Node> ();
nodes.Enqueue(startNode, startNode.costSoFar);
while (nodes.Count > 0)
{
Node currentNode = nodes.Dequeue();
if (currentNode == endNode)
{
// we found the path break the loop !!!
return(RetracePath(endNode, startNode));
}
Node[] neighbour = NeighbourNode(currentNode, maps);
foreach (Node n in neighbour)
{
float newCost = currentNode.costSoFar + ((n.movementCost == 0) ? 10000 : n.movementCost * ((n.isDiag) ? 1.414f : 1));
if (n.cameFrom == null || newCost < n.costSoFar)
{
n.costSoFar = newCost;
float priority = n.costSoFar + HeuristicDistance(endNode, n);
if (n.cameFrom != null)
{
if (nodes.Contains(n))
{
nodes.UpdatePriority(n, priority);
}
else
{
nodes.Enqueue(n, priority);
}
}
else
{
nodes.Enqueue(n, priority);
}
n.cameFrom = currentNode;
}
}
}
// if we reach her mean's we were unable to find path
Debug.LogError("Unable to find Path");
return(null);
}
/* Function: deleteCircleEvent
* ---------------------------
* Deletes a potential circle event associated with a site event
* from the event queue (it is no longer correct).
*/
private void deleteCircleEvent(BeachArc arc)
{
CircleEvent ce = arc.circleEvent;
arc.circleEvent = null;
if (ce != null && sweepLine.Contains(ce))
{
sweepLine.Remove(ce);
usedCircleEvents.Add(ce);
}
}
public static void Build(Node source, Node destination, out List <Node> path)
{
SimplePriorityQueue <Node> nodes = new SimplePriorityQueue <Node>();
source.visited = true;
source.cost = 0.0f;
source.heuristic = (destination.transform.position - source.transform.position).magnitude;
nodes.Enqueue(source, source.cost + source.heuristic);
bool found = false;
while (nodes.Count > 0) //looking at the top
{
Node node = nodes.Dequeue();
if (node == destination)
{
found = true;
}
foreach (Node.Edge edge in node.edges)
{
Node childNode = edge.nodeB;
if (childNode.visited)
{
continue;
}
float nodeCost = node.cost + (node.transform.position - childNode.transform.position).magnitude;
if (nodeCost < childNode.cost)
{
childNode.parentNode = node;
childNode.cost = node.cost;
childNode.heuristic = (destination.transform.position - childNode.transform.position).magnitude;
if (nodes.Contains(childNode) == false)
{
nodes.Enqueue(childNode, childNode.cost);
}
}
}
}
path = new List <Node>();
if (found)
{
Node node = destination;
while (node)
{
path.Add(node);
node = node.parentNode;
}
path.Reverse();
}
}
public static IDictionary <GraphNode <T>, GraphNode <T> > FindDistancesFrom(Graph <T> graph, GraphNode <T> start)
{
IDictionary <GraphNode <T>, int> distances = new Dictionary <GraphNode <T>, int>();
IDictionary <GraphNode <T>, GraphNode <T> > nodeParents = new Dictionary <GraphNode <T>, GraphNode <T> >();
SimplePriorityQueue <GraphNode <T>, int> unexploredNodes = new SimplePriorityQueue <GraphNode <T>, int>();
HashSet <GraphNode <T> > exploredNodes = new HashSet <GraphNode <T> >();
unexploredNodes.Enqueue(start, 0);
foreach (var node in graph.Nodes.Values)
{
//nodeParents.Add(new KeyValuePair<GraphNode<T>, GraphNode<T>>(node, null));
//unexploredNodes.Add(node);
distances.Add(new KeyValuePair <GraphNode <T>, int>(node, int.MaxValue));
}
distances[start] = 0;
while (unexploredNodes.Count > 0)
{
var currentNode = unexploredNodes.Dequeue();
exploredNodes.Add(currentNode);
foreach (var edge in currentNode.Edges)
{
var otherNode = edge.GetOther(currentNode);
if (exploredNodes.Contains(otherNode))
{
continue;
}
if (!unexploredNodes.Contains(otherNode))
{
unexploredNodes.Enqueue(otherNode, int.MaxValue);
}
if (!nodeParents.ContainsKey(otherNode))
{
nodeParents.Add(otherNode, currentNode);
}
int dist = distances[currentNode] + edge.Weight;
if (dist >= distances[otherNode])
{
continue;
}
distances[otherNode] = dist;
nodeParents[otherNode] = currentNode;
}
exploredNodes.Add(currentNode);
}
return(nodeParents);
}
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>
/// 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.
}
/// <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;
}
