public void AStar(Coord2D from, Coord2D to)
{
Stopwatch stopWatch = new Stopwatch();
stopWatch.Start();
// Initialize start node
int initialHeuristic = EuclideanDistance(from, to);
TileNode startNode = new TileNode(from.x, from.y, 0, initialHeuristic);
// Store start node
nodeQueue.Enqueue(startNode, startNode.totalCost);
nodeTracker[startNode.y, startNode.x] = startNode;
int timeC = 0;
while (true)
{
// timeC++;
// if (timeC > 14)
// {
// yield return null;
// timeC = 0;
// }
if (nodeQueue.Count == 0) // If the queue is empty, exit and fail
{
print("UNABLE TO FIND PATH!");
break;
}
TileNode bestNode = nodeQueue.Dequeue();
transform.position = new Vector3(bestNode.x, bestNode.y, 1);
GameObject.Instantiate(frontierMarker).transform.position = transform.position;
// Set closed state and break if dest. reached
if (SetClosedState(bestNode))
{
ShowBestPath(bestNode);
break;
}
AScanAdjacent(bestNode);
}
stopWatch.Stop();
// Get the elapsed time as a TimeSpan value.
TimeSpan ts = stopWatch.Elapsed;
// Format and display the TimeSpan value.
string elapsedTime = String.Format("{0:00}:{1:00}:{2:00}.{3:00}",
ts.Hours, ts.Minutes, ts.Seconds,
ts.Milliseconds / 10);
print("RunTime " + elapsedTime);
}
public override void Execute(State state, FastPriorityQueue <Event> eventQueue)
{
// Free the platform
station.Free(platform);
// Reset ready for departure
tram.ResetReadyForDeparture();
// Claim lane
SwitchLane lane = Switch.ExitLaneFor(platform);
System.Diagnostics.Debug.WriteLine($"DepartureStartstation: tram {tram.id}, station: {station.name}, {platform}, {lane}, time: {state.time}");
// Clear the lane it's leaving over in 60s
eventQueue.Enqueue(new ClearSwitchLane(station, lane), state.time + Config.c.switchClearanceTime);
// Queue next arrival
int stationIndex = state.stations.IndexOf(station);
int newStationIndex = stationIndex + 1;
// Make sure trams do not take over each other
int drivingTime = Sampling.drivingTime(Config.c.transferTimes[stationIndex].averageTime);
int arrivalTime = station.SignalNextArrival(state.time + drivingTime);
eventQueue.Enqueue(new ExpectedTramArrival(tram, newStationIndex), arrivalTime);
}
public List <Vector2Int> FindPath(Vector2Int start, Vector2Int end)
{
if (!_grid.CanMove(start) || !_grid.CanMove(end))
{
return(null);
}
_grid.Clear();
_start = start;
_end = end;
_openSet.Clear();
// 后面大括号内为 初始化列表
var startNode = new Jps_Node(_start)
{
Cost = 0, IsOpen = false, IsClose = false, IsForce = false
};
_openSet.Enqueue(startNode, startNode.Cost);
// 每次取 估值函数最小的节点 检测
while (_openSet.Count > 0)
{
Jps_Node cur = _openSet.Dequeue();
cur.IsClose = true;
if (cur.Pos == _end)
{
return(Trace(cur));
}
IdentitySuccessors(cur);
}
return(null);
}
public override void Execute(State state, FastPriorityQueue <Event> eventQueue)
{
// Clear switch (no lane cleared if coming from depot)
SwitchLane lane = fromDepot ? SwitchLane.None : Switch.ArrivalLaneFor(platform);
eventQueue.Enqueue(new ClearSwitchLane(station, lane), state.time + Config.c.switchClearanceTime);
// Log
System.Diagnostics.Debug.WriteLine($"ArrivalEndstation: tram {tram.id}, station: {station.name}, {platform}, {lane}, time: {state.time}");
// Check if tram can do another round trip if it is at endstation with depot
if (!station.TramToDepot(state.time))
{
// Board and unboard
(int pOut, int pIn, List <int> e) = station.UnboardAndBoard(tram, tram.passengerCount);
entrances = e;
// Calculate when to schedule departure
// If boarding/unboarding takes shorter than the turnaround, take the turnaround time
int passengerTransferTime = state.time + Math.Max(Sampling.passengerExchangeTime(pOut, pIn), Config.c.turnAroundTimeFor(station));
int nextDepartureTime = station.NextDeparture();
int nextEventTime = Math.Max(passengerTransferTime, nextDepartureTime);
// Queue event
eventQueue.Enqueue(new ExpectedDepartureStartstation(tram, station, platform, nextDepartureTime), nextEventTime);
}
// Transfer tram to depot
else
{
station.Free(platform);
}
}
private void AStar(ByteVector2 from, ByteVector2 to)
{
// Initialize start node
int initialHeuristic = EuclideanDistance(from, to);
TileNode startNode = new TileNode(from.X, from.Y, 0, initialHeuristic);
// Store start node
nodeQueue.Enqueue(startNode, startNode.TotalCost);
nodeTracker[startNode.Y, startNode.X] = startNode;
// Run A*
while (true)
{
if (nodeQueue.Count == 0) // If the queue is empty, exit and fail
{
break;
}
// Get the next best node
TileNode bestNode = nodeQueue.Dequeue();
// Set closed state and break if dest. reached
if (SetClosedState(bestNode))
{
SetBestPath(bestNode);
break;
}
// Scan all adjacent nodes
AStarScanAdjacent(bestNode);
}
}
void Predict(Ball a, double limit)
{
if (a == null)
{
return;
}
for (int i = 0; i < _balls.Length; i++)
{
double dt = a.TimeToHit(_balls[i]);
if (t + dt <= limit)
{
_pq.Enqueue(new Event(t + dt, a, _balls[i]), (float)(t + dt));
}
}
double dtX = a.TimeToHitVerticalWall();
double dtY = a.TimeToHitHorizontalWall();
if (t + dtX <= limit + 1)
{
_pq.Enqueue(new Event(t + dtX, a, null), (float)(t + dtX));
}
if (t + dtY <= limit + 1)
{
_pq.Enqueue(new Event(t + dtY, null, a), (float)(t + dtY));
}
}
public override IEnumerable <Node> AStar(Node from, Node to)
{
var openSet = new FastPriorityQueue <Node>(Width * Height);
openSet.Enqueue(from, 0f);
while (openSet.Count > 0)
{
var currentNode = openSet.Dequeue();
if (currentNode == to)
{
var path = ReconstructPath(currentNode);
foreach (var node in this)
{
node.Reset();
}
return(path);
}
currentNode.InClosedSet = true;
foreach (var neighbor in currentNode.Neighbors)
{
if (neighbor.InClosedSet)
{
continue;
}
var tentativeGScore = currentNode.GScore + currentNode.Distance(neighbor);
if (openSet.Contains(neighbor) && tentativeGScore >= neighbor.GScore)
{
continue;
}
neighbor.CameFrom = currentNode;
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(new List <Node>());
}
public void ScheduleEvent(SimEvent eSimEvent, double time)
{
//if (CurrentTime > time)
// throw new Exception("Scheadule Event is not possible. Current time > time.");
// _timeLine.Enqueue(eSimEvent, time);
_timeLine.Enqueue(eSimEvent, (float)time);
}
public override object Part2()
{
var map = Parse();
var queue = new FastPriorityQueue <QueueNode>(20000);
queue.Enqueue(new QueueNode((0, 0), Tool.Torch, 0), 0);
var shortestPath = new Dictionary <(int x, int y, Tool tool), int>();
while (true)
{
var cur = queue.Dequeue();
if (shortestPath.TryGetValue(cur.Key, out var shortest) && shortest <= cur.Len)
{
continue;
}
shortestPath[cur.Key] = cur.Len;
if (cur.Key == map.Target)
{
return(cur.Len);
}
foreach (var t in map.Tools(cur.Coord).Where(x => x != cur.Tool))
{
queue.Enqueue(new QueueNode(cur.Coord, t, cur.Len + 7), cur.Len + 7);
}
foreach (var nc in cur.Neighbours().Where(nc => map.Tools(nc).Contains(cur.Tool)))
{
queue.Enqueue(new QueueNode(nc, cur.Tool, cur.Len + 1), cur.Len + 1);
}
}
}
// A* pathfinding on tilemap
public static List <Vector3> FindPath(Vector2 start, Vector2 end, Tilemap t, bool useDiagonals)
{
List <Vector3> ret = new List <Vector3>();
Dictionary <Vector2, Vector2> cameFrom = new Dictionary <Vector2, Vector2>();
Dictionary <Vector2, float> cost = new Dictionary <Vector2, float>();
Vector2 current;
float newcost = 0;
float dist = 0;
FastPriorityQueue <QueueNode> queue = new FastPriorityQueue <QueueNode>(64);
queue.Enqueue(new QueueNode(start), 0);
cost.Add(start, 0);
while (queue.Count != 0)
{
current = queue.Dequeue().data;
if (Vector2.SqrMagnitude(end - current) < 0.5f)
{
if (!cameFrom.ContainsKey(end) && cameFrom.ContainsKey(current))
{
cameFrom.Add(end, cameFrom[current]);
}
break;
}
List <Vector3> neighbors;
if (useDiagonals)
{
neighbors = GetTileNeighborDiagonal(t, current);
}
else
{
neighbors = GetTileNeighbor(t, current);
}
foreach (Vector3 neighbor in neighbors)
{
newcost = cost[current] + 0.5f;
if (!cost.ContainsKey(neighbor) || newcost < cost[neighbor])
{
cost[neighbor] = newcost;
if (useDiagonals)
{
dist = Mathf.Max(Mathf.Abs(neighbor.x - end.x), Mathf.Abs(neighbor.y - end.y));
}
else
{
dist = Vector2.Distance(neighbor, end);
}
queue.Enqueue(new QueueNode(neighbor), newcost + dist);
cameFrom[neighbor] = current;
}
}
}
TraverseCameFrom(end, cameFrom, ret);
return(ret);
}
public override void Execute(State state, FastPriorityQueue <Event> eventQueue)
{
System.Diagnostics.Debug.WriteLine($"ExpectedDepartureStartstation: tram {tram.id}, station: {station.name}, {platform}, Timetable: {timeTableTime}, time: {state.time}");
// Close doors if it is time to leave
if (state.time >= timeTableTime)
{
// Mark tram as ready for departure
tram.ReadyForDeparture();
// Start leaving directly when possible
SwitchLane lane = Switch.ExitLaneFor(platform);
bool isFirst = station.first == platform;
if (isFirst && station._switch.SwitchLaneFree(lane))
{
station._switch.UseSwitchLane(lane);
eventQueue.Enqueue(new DepartureStartstation(tram, station, platform), state.time);
}
}
// Otherwise let extra passengers enter
else
{
(int pInExtra, List <int> e) = station.BoardPassengers(tram);
entrances = e;
int extraTime = Sampling.passengerExchangeTime(0, pInExtra);
eventQueue.Enqueue(new ExpectedDepartureStartstation(tram, station, platform, timeTableTime), state.time + extraTime);
}
}
public override void Execute(State state, FastPriorityQueue <Event> eventQueue)
{
var station = state.stations[stationIndex];
station.Free();
// Enqueue current tram
if (station.HasQueue())
{
Tram newOccupant = station.OccupyFromQueue();
eventQueue.Enqueue(new TramArrival(newOccupant, stationIndex), state.time + Config.c.stationClearanceTime);
}
// Schedule arrival at next station
int newStationIndex = stationIndex + 1 == state.stations.Count ? 0 : stationIndex + 1;
int drivingTime = Sampling.drivingTime(Config.c.transferTimes[stationIndex].averageTime);
Debug.WriteLine($"TramDeparture: tram {tram.id}: {station.name}, dir: {station.direction}, time: {state.time}");
// Make sure trams do not take over each other
int arrivalTime = station.SignalNextArrival(state.time + drivingTime);
if (state.stations[newStationIndex] is Endstation endstation)
{
eventQueue.Enqueue(new ExpectedArrivalEndstation(tram, endstation), arrivalTime);
}
else
{
eventQueue.Enqueue(new ExpectedTramArrival(tram, newStationIndex), arrivalTime);
}
}
public void DijkstraShortestPath(int source, int exitIndex)
{
int length = width * height;
int[] preCopy = new int[length * 4];
for (int i = 0; i < preCopy.Length; ++i)
{
preCopy[i] = -1;
}
initVar();
distance[source] = 0;
queue.Enqueue(new PathNode(source, distance[source]), distance[source]);
while (queue.Count != 0)
{
PathNode pathNode = queue.Dequeue();
if (visit[pathNode.point])
{
continue; //已计算
}
visit[pathNode.point] = true; //k节点已遍历
for (int m = first[pathNode.point]; m != -1; m = next[m])
{
if (value[m] + distance[pathNode.point] < distance[v[m]])
{
distance[v[m]] = value[m] + distance[pathNode.point];
preCopy[v[m]] = pathNode.point;
queue.Enqueue(new PathNode(v[m], value[v[m]]), value[v[m]]);
}
}
}
for (int i = 0; i < preCopy.Length; ++i)
{
pre[exitIndex, i] = preCopy[i];
}
}
// Hay que resetear la Astar cada vez que se llame, reseatear todo las variables a valor del start
public List <Vector2> calculatePath(int [,] tablero, Vector2 pathStart, Vector2 pathEnd)
{
fillMarked(tablero);
fillTableroNode(tablero, pathEnd);
tableroNode [(int)pathStart.x, (int)pathStart.y].g = 0;
tableroNode [(int)pathStart.x, (int)pathStart.y].estado = Node.state.open;
// referencia a un nodo que se va a considerar a continuacion
Node myNode = tableroNode [(int)pathStart.x, (int)pathStart.y];
// Nodo que vamos a usar para guardar el nodo final
Node aux = null;
open.Enqueue(myNode, myNode.g);
while (open.Count != 0)
{
myNode = open.Dequeue();
myNode.estado = Node.state.closed;
marked [(int)myNode.pos.x, (int)myNode.pos.y] = true;
aux = addNeighbours(myNode, pathEnd);
if (aux != null)
{
open.Clear();
return(getPath(aux));
}
}
return(null);
}
private List <NavPath> Dijkstra(Node s)
{
Dictionary <long, QueueNode> touched = new Dictionary <long, QueueNode>();
HashSet <long> done = new HashSet <long>();
FastPriorityQueue <QueueNode> pq = new FastPriorityQueue <QueueNode>(map.Nodes.Count);
List <NavPath> ret = Enumerable.Repeat(NavPath.NoPath, intersections.Count).ToList();
QueueNode start = new QueueNode(s.id);
start.path = new NavPath();
touched[s.id] = start;
pq.Enqueue(start, 0);
while (pq.Count != 0)
{
QueueNode qn = pq.Dequeue();
float distance = qn.Priority;
NavPath p = qn.path;
Node n = map.Nodes[qn.id];
done.Add(n.id);
ret[pathMapping[n.id]] = p;
foreach (var edgeId in n.Edges)
{
Way edge = map.Ways[edgeId];
if (edge.GetNext(n.id) != -1)
{
Node next = map.Nodes[edge.GetNext(n.id)];
float newWeight = distance + edge.weight;
if (!done.Contains(next.id))
{
List <Vector3> points = new List <Vector3>(edge.waypoints);
if (edge.isBack(n.id))
{
points.Reverse();
}
if (touched.ContainsKey(next.id))
{
qn = touched[next.id];
if (qn.Priority > newWeight)
{
pq.UpdatePriority(qn, newWeight);
qn.path = new NavPath(p.getPoints());
qn.path.Add(points);
}
}
else
{
qn = new QueueNode(next.id);
qn.path = new NavPath(p.getPoints());
qn.path.Add(points);
touched[next.id] = qn;
pq.Enqueue(qn, newWeight);
}
}
}
}
}
return(ret);
}
private int GetShortestPathToTarget()
{
var visitedPoints = new Dictionary <Point, HashSet <EquipmentType> >();
var consideredPoints = new FastPriorityQueue <PointNode>((this._furthestPoint.X + 1) * (this._furthestPoint.Y + 1) * 3);
consideredPoints.Enqueue(new PointNode
{
P = new Point(0, 0),
Time = 0,
Equipment = EquipmentType.Torch
}, 0);
while (consideredPoints.Any())
{
var consideredPoint = consideredPoints.Dequeue();
if (!visitedPoints.ContainsKey(consideredPoint.P))
{
visitedPoints.Add(consideredPoint.P, new HashSet <EquipmentType> {
consideredPoint.Equipment
});
}
else
{
if (visitedPoints[consideredPoint.P].Contains(consideredPoint.Equipment))
{
continue;
}
visitedPoints[consideredPoint.P].Add(consideredPoint.Equipment);
}
if (consideredPoint.P == this._targetPosition)
{
if (consideredPoint.Equipment == EquipmentType.Torch)
{
return(consideredPoint.Time);
}
var nextPoint = new PointNode
{
P = consideredPoint.P,
Equipment = EquipmentType.Torch,
Time = consideredPoint.Time + 7
};
consideredPoints.Enqueue(nextPoint, nextPoint.Time);
}
var neighbours = this.GetNextPoints(consideredPoint, visitedPoints);
foreach (var neighbour in neighbours)
{
consideredPoints.Enqueue(neighbour, neighbour.Time);
}
}
return(int.MaxValue);
}
public Path GetShortestPath(string from, string to)
{
if (!_nodes.TryGetValue(from, out var fromNode))
{
throw new Exception("from node not exist");
}
if (!_nodes.TryGetValue(to, out var toNode))
{
throw new Exception("to node not exist");
}
var distances = new Dictionary <Node, int>();
foreach (var node in _nodes.Values)
{
distances.Add(node, int.MaxValue);
}
distances.Remove(fromNode);
distances.Add(fromNode, 0);
var visited = new HashSet <Node>();
var privouseNode = new Dictionary <Node, Node>();
var queue = new FastPriorityQueue <NodeEntry>(10);
queue.Enqueue(node: new NodeEntry(fromNode, 0), priority: 0);
while (queue.Any())
{
var current = queue.Dequeue().GetNode();
visited.Add(current);
foreach (var edge in current.GetEdges())
{
var edgeTo = Get(edge.To());
if (visited.Contains(edgeTo))
{
continue;
}
var newDistance = distances[current] + edge.GetWeight();
if (newDistance >= distances[edgeTo])
{
continue;
}
distances.Remove(edgeTo);
distances.Add(edgeTo, newDistance);
if (privouseNode.TryGetValue(edgeTo, out var currentNode))
{
privouseNode.Remove(edgeTo);
}
privouseNode.Add(edgeTo, current);
queue.Enqueue(new NodeEntry(edgeTo, newDistance), newDistance);
}
}
return(BuildPath(privouseNode, toNode));
}
/// <summary>
/// Benchmark
/// </summary>
public void Enqueue()
{
Queue.Clear();
for (int i = 0; i < QueueSize; i++)
{
Queue.Enqueue(Nodes[i], i);
}
}
void FindPath(Vector3 a_StartPos, Vector3 a_TargetPos)
{
Node StartNode = nodeMesh.NodeFromWorldPoint(a_StartPos); //Gets the node closest to the starting position
Node TargetNode = nodeMesh.NodeFromWorldPoint(a_TargetPos); //Gets the node closest to the target position
if (StartNode != null && TargetNode != null)
{
FastPriorityQueue <Node> OpenList = new FastPriorityQueue <Node>(200);
//SimplePriorityQueue<Node> OpenList = new FastPriorityQueue<Node>(maxN_Calculations);//List of nodes for the open list
HashSet <Node> ClosedList = new HashSet <Node>(); //Hashset of nodes for the closed list
OpenList.Enqueue(StartNode, StartNode.FCost); //Add the starting node to the open list to begin the program
while (OpenList.Count > 0) //Whilst there is something in the open list
{
Node CurrentNode = OpenList.Dequeue(); //Create a node and set it to the first item in the open list
OpenList.ResetNode(CurrentNode);
//OpenList.Remove(CurrentNode);//Remove that from the open list
ClosedList.Add(CurrentNode); //And add it to the closed list
if (CurrentNode == TargetNode) //If the current node is the same as the target node
{
GetFinalPath(StartNode, TargetNode); //Calculate the final path
}
//Debug.Log("el nodo en cuestion tiene x vecinos" + CurrentNode.NeighbNodes.Count);
Node[] nNodes = nodeMesh.GetNeighbNodes(CurrentNode);
for (int i = 0; i < nNodes.Length; i++)
{
Node NeighborNode = nNodes[i];
if (NeighborNode == null)
{
continue;
}
if (NeighborNode.BIsWall || ClosedList.Contains(NeighborNode))
{
continue; //If the neighbor is a wall or has already been checked // Skip it
}
int MoveCost = CurrentNode.IgCost + GetManhattenDistance(CurrentNode, NeighborNode); //Get the F cost of that neighbor
if (MoveCost < NeighborNode.IgCost || !OpenList.Contains(NeighborNode)) //If the f cost is greater than the g cost or it is not in the open list
{
NeighborNode.IgCost = MoveCost; //Set the g cost to the f cost
NeighborNode.IhCost = GetManhattenDistance(NeighborNode, TargetNode); //Set the h cost
NeighborNode.ParentNode1 = CurrentNode; //Set the parent of the node for retracing steps
if (!OpenList.Contains(NeighborNode)) //If the neighbor is not in the openlist
{
//Add it to the list
OpenList.Enqueue(NeighborNode, NeighborNode.FCost);
}
}
}
}
//while (OpenList.Count > 0)
//{
// OpenList.ResetNode(OpenList.Dequeue());
//}
}
}
private static int FindShortestPathTime(int[,] erosionLevelMap, int x, int y, int equip, int time)
{
var queue = new FastPriorityQueue <State>(100000);
var set = new HashSet <(int, int, int)>();
queue.Enqueue(new State(x, y, equip), time);
while (true)
{
var state = queue.Dequeue();
time = (int)state.Priority;
x = state.X;
y = state.Y;
equip = state.Equip;
if (x == TARGET_X &&
y == TARGET_Y &&
equip == TORCH)
{
return(time);
}
if (set.Contains((x, y, equip)))
{
continue;
}
set.Add((x, y, equip));
for (int newEquip = 0; newEquip < 3; newEquip++)
{
if (CorrectEquipForRegionType(newEquip, GetRegionType(erosionLevelMap[x, y])))
{
queue.Enqueue(new State(x, y, newEquip), (time + 7));
}
}
for (int direction = 0; direction < 4; direction++)
{
int dx, dy;
(dx, dy) = GetDirectionVector(direction);
if (x + dx >= DIMENSION_X ||
x + dx < 0 ||
y + dy >= DIMENSION_Y ||
y + dy < 0)
{
continue;
}
if (CorrectEquipForRegionType(equip, GetRegionType(erosionLevelMap[x + dx, y + dy])))
{
queue.Enqueue(new State(x + dx, y + dy, equip), (time + 1));
}
}
}
}
public override bool Find(DirectionGraph graph, Node start, Node end)
{
priorityQueue = new FastPriorityQueue <Node>(graph.VerticeCount());
searchSteps = new Queue <Node>();
System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch();
watch.Start();
costForStartToTarget = new Dictionary <Node, int>();
costForStartToTarget[start] = 0;
priorityQueue.Enqueue(start, 0);
while (priorityQueue.Count > 0)
{
Node min = priorityQueue.Dequeue();
if (min == end)
{
watch.Stop();
executionTimes = watch.Elapsed.TotalMilliseconds;
CalculateCostAndGeneratePath(start, end, graph);
return(true);
}
List <DirectionEdge> minNeighbors = graph.GetNeighborEdge(min);
int costForStartToMin = costForStartToTarget[min];
for (int i = 0, count = minNeighbors.Count; i < count; i++)
{
Node neighbor = minNeighbors[i].to;
int costForStartToNeighbor = int.MaxValue;
if (costForStartToTarget.ContainsKey(neighbor))
{
costForStartToNeighbor = costForStartToTarget[neighbor];
}
int newCostSoFar = costForStartToMin + minNeighbors[i].weight;
if (newCostSoFar < costForStartToNeighbor)
{
costForStartToTarget[neighbor] = newCostSoFar;
nodesComeFrom[neighbor] = min;
if (priorityQueue.Contains(neighbor))
{
priorityQueue.UpdatePriority(neighbor, newCostSoFar);
}
else
{
priorityQueue.Enqueue(neighbor, newCostSoFar);
}
}
}
if (min != start)
{
searchSteps.Enqueue(min);
}
}
watch.Stop();
executionTimes = watch.Elapsed.TotalMilliseconds;
searchSteps.Clear();
return(false);
}
public override IEnumerable <Node> AStar(Node from, Node to)
{
var closedSet = new HashSet <Node>();
var openSet = new FastPriorityQueue <Node>(Width * Height);
openSet.Enqueue(@from, 0f);
var cameFrom = new Dictionary <Node, Node>();
var gScore = new Dictionary <Node, float> {
{ from, 0f }
};
while (openSet.Count > 0)
{
var currentNode = openSet.Dequeue();
if (currentNode == to)
{
return(ReconstructPath(cameFrom, currentNode));
}
closedSet.Add(currentNode);
foreach (var neighbor in currentNode.Neighbors)
{
if (closedSet.Contains(neighbor))
{
continue;
}
var tentativeGScore = gScore[currentNode] + currentNode.Distance(neighbor);
if (openSet.Contains(neighbor) && tentativeGScore >= gScore[neighbor])
{
continue;
}
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>());
}
/// <summary>
/// Finds a shortest path from the <paramref name="start"/> to the <paramref name="end"/>.
/// It uses a Dijkstra's algorithm with a priority queue.
/// </summary>
string Solve(Map map, Cell start, Cell end)
{
var vertices = new FastPriorityQueue <Cell>(map.TotalMapSize);
var distances = new Dictionary <Cell, float>();
var previous = new Dictionary <Cell, Cell>();
foreach (var cell in map.AllCells())
{
if (!cell.Equals(start))
{
distances[cell] = float.PositiveInfinity;
previous[cell] = null;
vertices.Enqueue(cell, float.PositiveInfinity);
}
}
vertices.Enqueue(start, 0);
distances[start] = 0;
int oneTenth = map.TotalMapSize > 10 ? map.TotalMapSize / 10 : 1;
while (vertices.Any())
{
var u = vertices.Dequeue();
if (reportProgress && vertices.Count % oneTenth == 0)
{
Console.WriteLine($"Remaining nodes {vertices.Count}/{map.TotalMapSize}");
}
foreach (var v in map.GetNeighbors(u))
{
if (!vertices.Contains(v))
{
continue;
}
var currentDistance = distances[v];
var newDistance = distances[u] + v.Difficulty;
if (newDistance < currentDistance)
{
vertices.UpdatePriority(v, newDistance);
distances[v] = newDistance;
previous[v] = u;
if (v.Equals(end))
{
vertices.Clear();
break;
}
}
}
}
return(GeneratePath(end, previous, map));
}
public Coord GetClosestAvailablePoint(Coord coord)
{
FastPriorityQueue <DijkstraNode> frontier = new FastPriorityQueue <DijkstraNode>(AvailabilityMap.Height * AvailabilityMap.Width);
DijkstraNode[] explored = new DijkstraNode[AvailabilityMap.Height * AvailabilityMap.Width];
AdjacencyRule adjacencyRule = Distance;
DijkstraNode initial = new DijkstraNode(coord, null);
initial.Distance = 0;
frontier.Enqueue(initial, 0);
while (frontier.Count > 0)
{
DijkstraNode current = frontier.Dequeue();
if (AvailabilityMap[current.Position])
{
return(current.Position);
}
foreach (Coord neighbor in adjacencyRule.Neighbors(current.Position))
{
if (!WalkabilityMap[neighbor])
{
continue;
}
int neighborIndex = neighbor.ToIndex(AvailabilityMap.Width);
bool neighborExplored = explored[neighborIndex] != null;
float neighborDistance = (float)Distance.Calculate(current.Position, neighbor) + current.Distance;
DijkstraNode neighborNode =
neighborExplored ? explored[neighborIndex] : new DijkstraNode(neighbor, current);
explored[neighborIndex] = neighborNode;
if (neighborExplored && neighborDistance < neighborNode.Distance)
{
neighborNode.Distance = neighborDistance;
frontier.UpdatePriority(neighborNode, neighborNode.Distance);
}
else
{
neighborNode.Distance = neighborDistance;
frontier.Enqueue(neighborNode, neighborDistance);
}
}
}
return(Coord.NONE);
}
public void TestIntQueuePriorityVsFloatTest1()
{
FastPriorityQueue <Node> fastQueue = new FastPriorityQueue <Node>(100);
Node n1 = new Node();
Node n2 = new Node();
fastQueue.Enqueue(n1, 1);
fastQueue.Enqueue(n2, .9999999f);
Node node = fastQueue.First;
Assert.IsTrue(node == n2);
}
public override void Execute(State state, FastPriorityQueue <Event> eventQueue)
{
System.Diagnostics.Debug.WriteLine($"ClearSwitchLane: {station.name}, {lane}, time: {state.time}");
/* Kind of a critical point here:
* - We have a possible departing tram
* - We have a possible incoming tram
* - The incoming tram can never go to the departing tram platform (not free yet)
* This means that if the departing tram is on platform B, the arriving tram cannot enter because the cross will be used.
* If the departing tram is on platform A, they can drive past each other, so it's okay to queue the arrival. */
// Clear the switch
station._switch.FreeSwitch(lane);
// Check if tram is in queue at switch, give priority to departing trams
var(departingTram, departingPlatform) = station.GetFirstDepartingTram();
if (departingTram != null)
{
// Queue the departure
SwitchLane departureLane = Switch.ExitLaneFor(departingPlatform);
station._switch.UseSwitchLane(departureLane);
Event e = new DepartureStartstation(departingTram, station, departingPlatform);
eventQueue.Enqueue(e, state.time);
}
// Get the best free platform
Platform arrivalPlatform = station.BestFreePlatform();
if (arrivalPlatform != Platform.None)
{
// Check if there are trams arriving from the depot, if so prefer those.
if (station.depotQueue.Count > 0)
{
Tram arrivingTram = station.OccupyFromDepotQueue(arrivalPlatform);
Event e = new ArrivalEndstation(arrivingTram, station, arrivalPlatform, true);
eventQueue.Enqueue(e, state.time);
}
// Check if we can enqueue an arrival as well
else if (station.HasQueue() && station._switch.SwitchLaneFree(Switch.ArrivalLaneFor(arrivalPlatform)))
{
// Get best available platform && queue
Tram arrivingTram = station.OccupyFromQueue(arrivalPlatform);
SwitchLane lane = Switch.ArrivalLaneFor(arrivalPlatform);
station._switch.UseSwitchLane(lane);
// Queue the arrival
Event e = new ArrivalEndstation(arrivingTram, station, arrivalPlatform);
eventQueue.Enqueue(e, state.time);
}
}
}
public INode <T> Run(INode <T> start, T goal, int maxIterations = 100, bool earlyExit = true)
{
frontier.Clear();
stateToNode.Clear();
explored.Clear();
frontier.Enqueue(start, start.GetCost());
var iterations = 0;
while ((frontier.Count > 0) && (iterations < maxIterations) && (frontier.Count + 1 < frontier.MaxSize))
{
iterations++;
var node = frontier.Dequeue();
if (node.IsGoal(goal))
{
ReGoapLogger.Log("[Astar] Success iterations: " + iterations);
return(node);
}
explored[node.GetState()] = node;
foreach (var child in node.Expand())
{
if (earlyExit && child.IsGoal(goal))
{
ReGoapLogger.Log("[Astar] (early exit) Success iterations: " + iterations);
return(child);
}
var childCost = child.GetCost();
var state = child.GetState();
if (explored.ContainsKey(state))
{
continue;
}
INode <T> similiarNode;
stateToNode.TryGetValue(state, out similiarNode);
if (similiarNode != null)
{
if (similiarNode.GetCost() > childCost)
{
frontier.Remove(similiarNode);
}
else
{
break;
}
}
frontier.Enqueue(child, childCost);
stateToNode[state] = child;
}
}
ReGoapLogger.LogWarning("[Astar] failed.");
return(null);
}
public List <Coordinates> AStar(Coordinates start, Coordinates target)
{
FastPriorityQueue <BreadCrump> nodesQueue = new FastPriorityQueue <BreadCrump>(VerticeAmount);
Dictionary <Coordinates, BreadCrump> guide = new Dictionary <Coordinates, BreadCrump>();
BreadCrump ariadnasThread = new BreadCrump(target, 0);
nodesQueue.Enqueue(ariadnasThread, 0);
guide.Add(target, ariadnasThread);
while (nodesQueue.IsAny())
{
BreadCrump current = nodesQueue.Dequeue();
if (current.Equals(start))
{
ariadnasThread = current;
break;
}
foreach (var neighbour in Neighbours(current))
{
int newCost = 1 + current.PathCost;
if (!guide.ContainsKey(neighbour) || newCost < guide[neighbour].PathCost)
{
BreadCrump next = new BreadCrump(neighbour, newCost);
guide[next] = current;
int priority = newCost + Heuristic(neighbour, start);
nodesQueue.Enqueue(next, priority);
}
}
}
if (ariadnasThread.Equals(start))
{
List <Coordinates> path = new List <Coordinates>();
path.Add(ariadnasThread);
while (!ariadnasThread.Equals(target))
{
ariadnasThread = guide[ariadnasThread];
path.Add(ariadnasThread);
}
return(path);
}
return(null);
}
public static List <Edge> PrimMST(List <int> D)
{
List <Edge> MSTList = new List <Edge>();
// final list contains the MST
FastPriorityQueue <VertexParent> FP = new FastPriorityQueue <VertexParent>(D.Count);
// Priority queue sorts the priority of edges' weights each time .
VertexParent [] VP = new VertexParent[D.Count];
// array holding each node and it's parent node.
VP[0] = new VertexParent(D[0], null); // initializing the first node in the MST.
FP.Enqueue(VP[0], 0); // inserting the first node into the priority queue.
float w;
for (int i = 1; i < D.Count; i++)
{
// intializing all the weights with OO value.
VP[i] = new VertexParent(D[i], null);
FP.Enqueue(VP[i], int.MaxValue);
}
while (FP.Count != 0)
{
VertexParent Top = FP.Dequeue(); // get the minimum priority
if (Top.P != null) // if it is not the starting node.
{
Edge E;
E.V1 = Top.V;
E.V2 = (int)(Top.P);
E.Weight = (float)(Top.Priority);
MSTList.Add(E); // add the minimum weight to the MST.
}
foreach (var unit in FP) // modify the priority each time .
{
w = CalcWeight(unit, Top); // calculates the weight between the current node and the top node.
if (w < unit.Priority)
{
unit.P = Top.V;
FP.UpdatePriority(unit, w);
}
}
}
return(MSTList);
}
private static FastPriorityQueue <Node> PopulateQueue2Y(List <Point2Y> points)
{
var queue = new FastPriorityQueue <Node>(points.Count);
Node prev = null;
for (var i = 0; i < points.Count; i++)
{
var area = i == 0 || i == points.Count - 1 ? float.MaxValue : CalcArea2Y(points[i - 1], points[i], points[i + 1]);
var node = new Node
{
Index = i,
Point = points[i],
Prev = prev
};
if (prev != null)
{
prev.Next = node;
}
prev = node;
queue.Enqueue(node, area);
}
return(queue);
}
static void TestCase4()
{
var random = new Random();
var myHeap = new FibonacciHeap<double>(int.MinValue, Comparer<double>.Default);
var thirdPartyHeap = new FastPriorityQueue<FastPriorityQueueNode>(1000);
for (var i = 0; i < 1000; i++)
{
if (random.Next(3) == 0 && thirdPartyHeap.Any())
{
var myResult = myHeap.ExtractMin();
var otherResult = thirdPartyHeap.Dequeue();
Assert(myResult.Item1);
Assert(Math.Abs(myResult.Item2 - otherResult.Priority) < double.Epsilon);
}
else
{
var value = random.NextDouble()*10;
myHeap.Insert(value);
thirdPartyHeap.Enqueue(new FastPriorityQueueNode(), value);
}
}
while (thirdPartyHeap.Any())
{
var myResult = myHeap.ExtractMin();
var otherResult = thirdPartyHeap.Dequeue();
Assert(myResult.Item1);
Assert(Math.Abs(myResult.Item2 - otherResult.Priority) < double.Epsilon);
}
}
public static void RunExample()
{
//First, we create the priority queue. We'll specify a max of 10 users in the queue
FastPriorityQueue<User> priorityQueue = new FastPriorityQueue<User>(MAX_USERS_IN_QUEUE);
//Next, we'll create 5 users to enqueue
User user1 = new User("1 - Jason");
User user2 = new User("2 - Tyler");
User user3 = new User("3 - Valerie");
User user4 = new User("4 - Joseph");
User user42 = new User("4 - Ryan");
//Now, let's add them all to the queue (in some arbitrary order)!
priorityQueue.Enqueue(user4, 4);
priorityQueue.Enqueue(user2, 0); //Note: Priority = 0 right now!
priorityQueue.Enqueue(user1, 1);
priorityQueue.Enqueue(user42, 4);
priorityQueue.Enqueue(user3, 3);
//Change user2's priority to 2. Since user2 is already in the priority queue, we call UpdatePriority() to do this
priorityQueue.UpdatePriority(user2, 2);
//Finally, we'll dequeue all the users and print out their names
while(priorityQueue.Count != 0)
{
User nextUser = priorityQueue.Dequeue();
Console.WriteLine(nextUser.Name);
}
//Output:
//1 - Jason
//2 - Tyler
//3 - Valerie
//4 - Joseph
//4 - Ryan
//Notice that when two users with the same priority were enqueued, they were dequeued in the same order that they were enqueued.
}
static void TestCase2()
{
var random = new Random();
var myHeap = new BinomialHeap<double>(double.MinValue, Comparer<double>.Default);
var otherQueue = new FastPriorityQueue<FastPriorityQueueNode>(10000);
for (var i = 0; i < 10000; i++)
{
if (otherQueue.Any() && random.Next(3) == 0)
{
Assert(Math.Abs(myHeap.ExtractMin().Item2 - otherQueue.Dequeue().Priority) < double.Epsilon);
}
else
{
var newValue = random.NextDouble()*10;
myHeap.Insert(newValue);
otherQueue.Enqueue(new FastPriorityQueueNode(), newValue);
}
}
while(otherQueue.Any())
Assert(Math.Abs(myHeap.ExtractMin().Item2 - otherQueue.Dequeue().Priority) < double.Epsilon);
}
public void WriteMIDI(string path)
{
HashSet<Note>.Enumerator enumerator = notes.GetEnumerator();
FastPriorityQueue<Note> startQueue = new FastPriorityQueue<Note>(numNotes);
FastPriorityQueue<Note> endQueue = new FastPriorityQueue<Note>(numNotes);
while (enumerator.MoveNext())
{
startQueue.Enqueue(enumerator.Current, enumerator.Current.Priority);
}
using (BinaryWriter writer = new BinaryWriter(File.Open(path+trackName + ".mid", FileMode.Create)))
{
writer.Write(OneTrackHeader());
writer.Write(StandardTrackHeader());
long deltaStart = 0;
do
{
if (endQueue.Peek() == null)
{
Note note = startQueue.Dequeue();
writer.Write(note.MidiStart(deltaStart));
deltaStart = (long) note.Priority;
endQueue.Enqueue(note, note.Priority+note.GetDuration());
} else if (startQueue.Peek() == null)
{
Note note = endQueue.Dequeue();
writer.Write(note.MidiEnd(deltaStart));
deltaStart = (long) note.Priority;
} else
{
if (endQueue.Peek().Priority > startQueue.Peek().Priority)
{
Note note = startQueue.Dequeue();
writer.Write(note.MidiStart(deltaStart));
deltaStart = (long)note.Priority;
endQueue.Enqueue(note, note.Priority+note.GetDuration());
} else
{
Note note = endQueue.Dequeue();
writer.Write(note.MidiEnd(deltaStart));
deltaStart = (long)note.Priority;
}
}
} while (startQueue.Count + endQueue.Count > 0);
writer.Write(TrackEnd());
}
}