public Route GetShortestPath(Vertex from, Vertex to)
{
ResetExploration();
var queue = new PriorityQueue<double, Route>();
queue.Enqueue(0, new Route { @from });
while (!queue.IsEmpty)
{
var route = queue.Dequeue();
if (route.Last().IsExplored)
continue;
if (route.Last().Equals(to))
return route;
route.Last().IsExplored = true;
foreach (Edge n in route.Last().AdjacentEdges)
{
var newRoute = route.Clone();
newRoute.Add(n.Tail);
queue.Enqueue(newRoute.Cost, newRoute);
}
}
return null;
}
public void TestFirstTakePrecedence1()
{
PriorityQueue<Char> queue = new PriorityQueue<Char>();
queue.Enqueue ('b', 1);
queue.Enqueue ('a', 0);
String output = queue.ToString();
Assert.IsTrue ("[ 0. a, 1. b ]".Equals(output));
}
public void TestFirstTakePrecedence2()
{
PriorityQueue<Char> queue = new PriorityQueue<Char>();
queue.Enqueue ('d', 3);
queue.Enqueue ('b', 2);
queue.Enqueue ('c', 2);
queue.Enqueue ('a', 1);
String output = queue.ToString();
Assert.IsTrue ("[ 1. a, 2. b, 2. c, 3. d ]".Equals(output));
}
public void Clear()
{
var pq = new PriorityQueue<int>();
pq.Clear();
Assert.AreEqual(0, pq.Count);
pq.Enqueue(777);
pq.Enqueue(234);
pq.Enqueue(13);
pq.Clear();
Assert.AreEqual(0, pq.Count);
}
public void TestDequeueNotEmptyQueue()
{
PriorityQueue<Char> queue = new PriorityQueue<Char>();
queue.Enqueue ('a', 1);
queue.Enqueue ('b', 1);
queue.Enqueue ('c', 1);
queue.Enqueue ('d', 1);
Char firstElement = queue.Dequeue();
String output = queue.ToString();
Assert.IsTrue ('a' == firstElement);
Assert.IsTrue ("[ 1. b, 1. c, 1. d ]".Equals(output));
}
public static Path FindPath(VGArea mMap, PGNode mStart, PGNode mEnd)
{
foreach(VGTile tile in mMap.Tiles) tile.Node.Clear();
var closed = new HashSet<PGNode>();
var queue = new PriorityQueue<double, Path>();
queue.Enqueue(0, new Path(mStart));
while (!queue.IsEmpty)
{
var path = queue.Dequeue();
if (!path.LastStep.Passable || closed.Contains(path.LastStep))
continue;
if (path.LastStep.Equals(mEnd))
{
return path;
}
closed.Add(path.LastStep);
foreach (PGNode n in mMap.GetTileNodeNeighbors(path.LastStep.Tile.X, path.LastStep.Tile.Y))
{
double d = PGUtils.GetNodeDiagonalDistance(path.LastStep, n);
var newPath = path.AddStep(n, d);
queue.Enqueue(newPath.TotalCost + PGUtils.GetNodeManhattanDistance(n, mEnd), newPath);
}
}
return null;
}
static void Main()
{
// example structure as binary heap
// -4444 parent root at the beginning with children -5 and 1
// -5 with children 5 and 50, 1 with children 4 and 8
// 5 with children 999 and 19
// -4444
// -5 1
// 5 50 4 8
// 999 19
PriorityQueue<int> queue = new PriorityQueue<int>();
queue.Enqueue(5);
queue.Enqueue(19);
queue.Enqueue(-4444);
queue.Enqueue(1);
queue.Enqueue(50);
queue.Enqueue(4);
queue.Enqueue(8);
queue.Enqueue(999);
queue.Enqueue(-5);
Print(queue);
Console.WriteLine(queue.Dequeue());
// all are for testing
// Console.WriteLine();
// Print(queue);
Console.WriteLine(queue.Dequeue());
// Console.WriteLine();
// Print(queue);
Console.WriteLine(queue.Dequeue());
Console.WriteLine("----------------");
// Console.WriteLine();
// Print(queue);
Console.WriteLine("Peek: " + queue.Peek());
Console.WriteLine(queue.Count);
queue.Clear();
Console.WriteLine(queue.Count);
}
public void CopyTo()
{
var pq = new PriorityQueue<int>();
pq.Enqueue(1);
pq.Enqueue(1);
pq.Enqueue(1);
int[] array = new int[10];
pq.CopyTo(array, 3);
Assert.AreEqual(1, array[3]);
Assert.AreEqual(1, array[4]);
Assert.AreEqual(1, array[5]);
array = new int[10];
((ICollection)pq).CopyTo(array, 3);
Assert.AreEqual(1, array[3]);
Assert.AreEqual(1, array[4]);
Assert.AreEqual(1, array[5]);
}
public void TestEnqueueEmptyQueue()
{
PriorityQueue<Char> queue = new PriorityQueue<Char>();
queue.Enqueue ('a', 1);
String output = queue.ToString();
Assert.IsTrue ("[ 1. a ]".Equals(output));
}
// This merges a bunch of temporary flat files
// @param files
// @param output file
// @return The number of lines sorted. (P. Beaudoin)
public static int MergeSortedFiles(List<FileStream> aFiles, FileStream aOutputFile, IComparer<long> aComparer)
{
IComparer<BinaryFileBuffer> lComp = Comparer<BinaryFileBuffer>.Create(new Comparison<BinaryFileBuffer>((BinaryFileBuffer x, BinaryFileBuffer y) =>
{
long x_peek = Convert.ToInt64(x.Peek());
long y_peek = Convert.ToInt64(y.Peek());
int lResult = aComparer.Compare(x_peek, y_peek);
return x_peek.CompareTo(y_peek);// Result;
}));
PriorityQueue<BinaryFileBuffer> lPq = new PriorityQueue<BinaryFileBuffer>(lComp, 11);
foreach (FileStream lFile in aFiles) {
BinaryFileBuffer lBfb = new BinaryFileBuffer(lFile);
lPq.Enqueue(lBfb); //add
}
StreamWriter lBufWriter = new StreamWriter(new BufferedStream(aOutputFile));
int lRowCounter = 0;
try {
while (lPq.Count > 0) {
BinaryFileBuffer lBfb = lPq.Dequeue();
string lLine = lBfb.Pop();
lBufWriter.WriteLine(lLine);
++lRowCounter;
if (lBfb.Empty) {
lBfb.BufReader.Close();
lBfb.OriginalFile.Close();// we don't need you anymore
} else {
lPq.Enqueue(lBfb); // add it back
}
}
} finally {
lBufWriter.Close();
//foreach (BinaryFileBuffer lBfb in lPq)
//{
// lBfb.Close();
//}
}
return lRowCounter;
}
public void Contains()
{
var pq = new PriorityQueue<object>();
for (int i = 0; i < 20; i++)
pq.Enqueue(i);
Assert.IsTrue(pq.Contains(0));
Assert.IsTrue(pq.Contains(14));
Assert.IsTrue(pq.Contains(19));
Assert.IsFalse(pq.Contains(-1));
Assert.IsFalse(pq.Contains(20));
pq.Dequeue();
Assert.IsFalse(pq.Contains(19));
}
/// <summary>
/// Finds a path between two graph nodes using the A* algorithm
/// Based on http://blogs.msdn.com/b/ericlippert/archive/2007/10/10/path-finding-using-a-in-c-3-0-part-four.aspx
/// </summary>
/// <param name="start">The starting node to path from</param>
/// <param name="end">The ending node to path from</param>
/// <param name="distance">The function used to find the distance between two points</param>
/// <param name="estimate">The function used to estimate the distance between the current node and another node</param>
/// <returns>A list of nodes representing the path between the two nodes</returns>
public static List<IGraphNode> FindPath(IGraphNode start, IGraphNode end)
{
//A PriorityQueue of open paths to evaluate. the priority is the total cost of the current node plus the estimated distance to the end node
PriorityQueue<double, List<IGraphNode>> openQueue = new PriorityQueue<double, List<IGraphNode>>();
//A hashset set of nodes we have already looked at
//Is a hashset because it has O(1) time Add and Contains
HashSet<IGraphNode> closedSet = new HashSet<IGraphNode>();
//The initial path, from which all other paths will be calculated
List<IGraphNode> initialPath = new List<IGraphNode>();
//Add the start node to the initial path, and the initial path to the queue of things to be evaluated
initialPath.Add(start);
openQueue.Enqueue(ManhattanDistance(start, end), initialPath);
//While we stll have objects in the queue and have not exited the queue yet
while (!openQueue.IsEmpty)
{
//Get the current node to be evaluated
List<IGraphNode> currentPath = openQueue.Dequeue();
IGraphNode currentNode = currentPath.Last();
//If the current node has already been evaluated, then this path is definitely not the shortest path and we can skip evaluating it.
if (!closedSet.Contains(currentNode))
{
//If the current node is the end node, then we're done and can return the current path.
if (currentNode.Equals(end))
return currentPath;
//Add the current node to the closed set so that we know it's already been evaluated.
closedSet.Add(currentNode);
//For each of the object's neighbors, create a new path and add it to the queue
foreach (var node in currentNode.Neighbors)
{
//Create a new path, contiaining the old path plus this path.
List<IGraphNode> newPath = new List<IGraphNode>(currentPath);
newPath.Add(node);
//Add this path to the queue
openQueue.Enqueue(CalculatePathCost(currentPath) + ManhattanDistance(currentNode, node) + ManhattanDistance(node, end), newPath);
}
}
}
//if no valid path exists, return null.
return null;
}
static void Main(string[] args)
{
PriorityQueue<int> priorityQueue = new PriorityQueue<int>();
priorityQueue.Enqueue(10);
priorityQueue.Enqueue(12);
priorityQueue.Enqueue(13);
priorityQueue.Enqueue(14);
priorityQueue.Enqueue(15);
Console.WriteLine("Peek at biggest element: {0} ", priorityQueue.Peek);
priorityQueue.Dequeue();
Console.WriteLine("Peek after dequeue: {0}", priorityQueue.Peek);
priorityQueue.Enqueue(15);
priorityQueue.Enqueue(20);
Console.WriteLine("Biggest element after enqueue: {0} ", priorityQueue.Peek);
}
static void Main(string[] args)
{
PriorityQueue<AlarmEvent, AlarmEventType> pq = new PriorityQueue<AlarmEvent, AlarmEventType>();
// Add a bunch of events to the queue
pq.Enqueue(new AlarmEvent(AlarmEventType.Test, "Testing 1"), AlarmEventType.Test);
pq.Enqueue(new AlarmEvent(AlarmEventType.Fire, "Fire alarm 1"), AlarmEventType.Fire);
pq.Enqueue(new AlarmEvent(AlarmEventType.Trouble, "Battery low"), AlarmEventType.Trouble);
pq.Enqueue(new AlarmEvent(AlarmEventType.Panic, "I've fallen and I can't get up!"), AlarmEventType.Panic);
pq.Enqueue(new AlarmEvent(AlarmEventType.Test, "Another test."), AlarmEventType.Test);
pq.Enqueue(new AlarmEvent(AlarmEventType.Alert, "Oops, I forgot the reset code."), AlarmEventType.Alert);
Console.WriteLine("The queue contains {0} events", pq.Count);
// Now remove the items in priority order
Console.WriteLine();
while (pq.Count > 0)
{
PriorityQueueItem<AlarmEvent, AlarmEventType> item = pq.Dequeue();
Console.WriteLine("{0}: {1}", item.Priority, item.Value.Message);
}
}
public static LinkedList<CestyGraf.Hrana> doDijkstra(CestyGraf graf, Auto startPozice, CestyGraf.Hrana cil)
{
int pocetProhledanych = 0;
Stopwatch stopky = new Stopwatch();
stopky.Start();
double vzdalenost = 0; ;
LinkedList<CestyGraf.Hrana> cilovaCesta = new LinkedList<CestyGraf.Hrana>();
DVrchol cilPosledni = new DVrchol();
PriorityQueue<double, DVrchol, CestyGraf.IBod> otevrene = new PriorityQueue<double, DVrchol, CestyGraf.IBod>(graf.CountVrcholy());
Dictionary<CestyGraf.IBod, DVrchol> uzavrene = new Dictionary<CestyGraf.IBod, DVrchol>(graf.CountVrcholy());
DVrchol zkoumany = new DVrchol();
otevrene.Enqueue(startPozice.VzdalenostOdV1, new DVrchol(startPozice.HranaPoloha.Vrchol1 as CestyGraf.Vrchol, null, startPozice.HranaPoloha, startPozice.VzdalenostOdV1), startPozice.HranaPoloha.Vrchol1.Souradnice);
otevrene.Enqueue(startPozice.VzdalenostOdV2, new DVrchol(startPozice.HranaPoloha.Vrchol2 as CestyGraf.Vrchol, null, startPozice.HranaPoloha, startPozice.VzdalenostOdV2), startPozice.HranaPoloha.Vrchol2.Souradnice);
while (otevrene.Count != 0) {
zkoumany = otevrene.Dequeue().Value;
if (uzavrene.ContainsKey(zkoumany.Data.Souradnice)) {
if (uzavrene[zkoumany.Data.Souradnice].MetrikaOdStartu < zkoumany.MetrikaOdStartu) {
// zahození horší cesty (zlepšilo o několik % procházení - méně procházených cest)
continue;
} else { // pokud by náhodou nastalo, odebrat starý vrchol (kvůli jedinečnosti): O(1)
uzavrene.Remove(zkoumany.Data.Souradnice);
}
}
uzavrene.Add(zkoumany.Data.Souradnice, zkoumany);
if (zkoumany.Data == cil.Vrchol1 || zkoumany.Data == cil.Vrchol2) { // je nalezen cíl
if (cilPosledni.Data != null) {
if (cilPosledni.MetrikaOdStartu > zkoumany.MetrikaOdStartu) {
cilPosledni = zkoumany; // nebude pouze ukazatel, který se neustále mění?
Debug.WriteLine("Další výskyt: " + cilPosledni.MetrikaOdStartu + ", vrchol: " + cilPosledni.Data.Data);
} else {
Debug.WriteLine("Nepoužitý výskyt: " + zkoumany.MetrikaOdStartu + ", vrchol: " + zkoumany.Data.Data);
}
} else {
cilPosledni = zkoumany;
Debug.WriteLine("První výskyt: " + cilPosledni.MetrikaOdStartu + ", vrchol: " + cilPosledni.Data.Data);
//break; // pokud chceme ukončit po nalezení prvního vrcholu
}
}
foreach (CestyGraf.Hrana item in zkoumany.Data.DejHrany()) { // z zkoum. pro každou hranu přidat do otevřených koncový bod
if (!item.Sjizdna) {
continue;
}
pocetProhledanych++;
DVrchol v1 = new DVrchol(item.Vrchol1 as CestyGraf.Vrchol, zkoumany, item, zkoumany.MetrikaOdStartu + item.Metrika);
DVrchol v2 = new DVrchol(item.Vrchol2 as CestyGraf.Vrchol, zkoumany, item, zkoumany.MetrikaOdStartu + item.Metrika);
if (zkoumany.Data.Equals(item.Vrchol1)) {
otevrene.Enqueue(v2.MetrikaOdStartu, v2, v2.Data.Souradnice);
} else {
otevrene.Enqueue(v1.MetrikaOdStartu, v1, v1.Data.Souradnice);
}
}
}
Debug.WriteLine("Poslední výskyt: " + cilPosledni.MetrikaOdStartu + ", vrchol: " + cilPosledni.Data.Data);
while (cilPosledni.Predchudce != null) {
vzdalenost += cilPosledni.SilniceDoPredchudce.Metrika;
cilovaCesta.AddFirst(cilPosledni.SilniceDoPredchudce);
cilPosledni = cilPosledni.Predchudce;
}
CestyGraf.Hrana hrStart = new CestyGraf.Hrana(null, startPozice.DejPolohu(), cilPosledni.Data, 0, true);
cilovaCesta.AddFirst(hrStart);
stopky.Stop();
Debug.WriteLine("Milisekund: " + stopky.ElapsedMilliseconds + ", Tiků: " + stopky.ElapsedTicks + ", Prohledaných stavů celkem: " + pocetProhledanych);
return cilovaCesta;
}
public void Enumerator()
{
var pq = new PriorityQueue<int>();
for (int i = 0; i < 10; i++)
pq.Enqueue(i);
var list = new List<int>();
foreach (var i in pq)
{
Assert.IsTrue(!list.Contains(i));
list.Add(i);
}
Assert.AreEqual(pq.Count, list.Count);
Assert.Throws(typeof(InvalidOperationException), () =>
{
foreach (var i in pq)
{
pq.Enqueue(1);
}
});
Assert.Throws(typeof(InvalidOperationException), () =>
{
foreach (var i in pq)
{
pq.Dequeue();
}
});
Assert.Throws(typeof(InvalidOperationException), () =>
{
foreach (var i in pq)
{
pq.Remove(1);
}
});
Assert.Throws(typeof(InvalidOperationException), () =>
{
foreach (var i in pq)
{
pq.Clear();
}
});
}
static void Dijkstra(Dictionary<Node, List<Connection>> map, Node source)
{
PriorityQueue<long, Node> queue = new PriorityQueue<long, Node>();
foreach (var node in map)
{
node.Key.DijkstraDistance = long.MaxValue;
}
source.DijkstraDistance = 0;
queue.Enqueue(source.DijkstraDistance, source);
while (queue.Count > 0)
{
Node currentNode = queue.Dequeue().Value;
if (currentNode.DijkstraDistance == long.MaxValue)
{
break;
}
foreach (var connection in map[currentNode])
{
var distance = currentNode.DijkstraDistance + connection.Distance;
if (distance < connection.ToNode.DijkstraDistance)
{
connection.ToNode.DijkstraDistance = distance;
queue.Enqueue(connection.ToNode.DijkstraDistance, connection.ToNode);
}
}
}
}
public void MinPriorityQueue()
{
var pq = new PriorityQueue<int>((x, y) => y - x);
pq.Enqueue(2);
pq.Enqueue(1);
pq.Enqueue(3);
Assert.AreEqual(1, pq.Dequeue());
Assert.AreEqual(2, pq.Dequeue());
Assert.AreEqual(3, pq.Dequeue());
}
public void TestLessPriority()
{
PriorityQueue<Char> queue = new PriorityQueue<Char>();
queue.Enqueue ('a', 1);
queue.Enqueue ('b', 1);
queue.Enqueue ('c', 1);
queue.Enqueue ('d', 1);
String output = queue.ToString();
Assert.IsTrue ("[ 1. a, 1. b, 1. c, 1. d ]".Equals(output));
}
public void Test()
{
var pq = new PriorityQueue<int>();
var random = new Random();
for (int i = 0; i < 100; i++)
{
var value = random.Next(0, 20);
pq.Enqueue(value);
pq.Validate();
}
var max = int.MaxValue;
for (int i = 0; i < 100; i++)
{
Assert.AreEqual(100 - i, pq.Count);
var peek = pq.Peek();
var value = pq.Dequeue();
Assert.AreEqual(peek, value);
Assert.IsTrue(max >= value);
max = value;
pq.TrimExcess();
}
}
public void Remove()
{
var pq = new PriorityQueue<object>();
for (int i = 0; i < 20; i++)
pq.Enqueue(i);
Assert.IsTrue(pq.Remove(0));
Assert.IsTrue(pq.Remove(14));
Assert.IsTrue(pq.Remove(19));
Assert.IsFalse(pq.Remove(-1));
Assert.IsFalse(pq.Remove(20));
Assert.AreEqual(17, pq.Count);
}
public void TestNullAndDuplicates()
{
var pq = new PriorityQueue<object>();
pq.Enqueue(null);
pq.Enqueue(1);
pq.Enqueue(null);
pq.Enqueue(2);
pq.Enqueue(2);
Assert.AreEqual(2, pq.Dequeue());
Assert.AreEqual(2, pq.Dequeue());
Assert.AreEqual(1, pq.Dequeue());
Assert.AreEqual(null, pq.Dequeue());
Assert.AreEqual(null, pq.Dequeue());
}
public void TestToString()
{
PriorityQueue<Char> queue = new PriorityQueue<Char>();
queue.Enqueue ('a', 1);
queue.Enqueue ('c', 1);
queue.Enqueue ('c', 1);
queue.Enqueue ('d', 1);
String output = queue.ToString();
System.Console.WriteLine(output);
Assert.IsTrue ("[ 1. a, 1. c, 1. c, 1. d ]".Equals(output));
}
public void Count()
{
var pq = new PriorityQueue<int>();
Assert.AreEqual(0, pq.Count);
pq.Enqueue(123);
Assert.AreEqual(1, pq.Count);
pq.Enqueue(31);
pq.Enqueue(64);
Assert.AreEqual(3, pq.Count);
for (int i = 0; i < 30; i++)
pq.Enqueue(i);
pq.Peek();
pq.Dequeue();
pq.Dequeue();
Assert.AreEqual(31, pq.Count);
pq.Clear();
Assert.AreEqual(0, pq.Count);
}
/**
* Solve the TSP using an include/exclude B&B strategy
*
* First, get an upper bound, bssf.
*
* Generate the initial Reduced Cost Matrix, (RCM) which
* will give us the initial lower bound.
*
*
*/
public void solveBranchAndBound()
{
// Stats
int totalStatesGenerated = 0, statesPruned = 0, maxStatesStored = 0;
bool initialBssfIsFinal = true;
// Get an upper bound
getGreedyRoute();
bssf = new TSPSolution(Route);
// Generate the RCM, and get it's lower bound from the reduction
double[,] rcm = generateRCM();
double lowerBound = reduceCM(ref rcm);
// Now we need to start throwing states on the queue and processing them..
PriorityQueue<BBState> stateQueue = new PriorityQueue<BBState>();
// Create initial state
BBState initialState = new BBState(rcm, lowerBound);
stateQueue.Enqueue(initialState, initialState.bound);
totalStatesGenerated = maxStatesStored = 1;
// Ok, now we kick off the process!
// Start the timer..
Stopwatch timer = new Stopwatch();
timer.Start();
BBState curState = null;
while (stateQueue.Count > 0)
{
/*if (timer.ElapsedMilliseconds > 30000) // 30 seconds
break; */
curState = stateQueue.Dequeue();
// If this state's lower bound is greater than BSSF, then we
// prune it out
if (curState.bound > costOfBssf())
{
statesPruned++;
continue;
}
// If it's not, then see if it's a complete solution. If it is,
// then update BSSF.
if (curState.isCompleteSolution(ref Cities))
{
bssf = new TSPSolution(curState.getRoute(ref Cities));
initialBssfIsFinal = false;
continue;
}
// If it's not a complete solution, but it's within range, then
// expand it into two child states, one with an included edge, one
// with the same edge excluded.
Tuple<int, int> edge = curState.getNextEdge();
if (edge == null)
continue;
// Ok, now we have the next edge to include and exclude in different states to maximize
// the difference in bounds. So we need to create states corresponding to each and put
// them in the queue.
BBState incState = new BBState(curState.cm, curState.bound, curState.includedEdges);
incState.includedEdges.Add(edge);
incState.cm[edge.Item1, edge.Item2] = double.PositiveInfinity;
incState.cm[edge.Item2, edge.Item1] = double.PositiveInfinity;
for (int t = 0; t < incState.cm.GetLength(0); t++)
incState.cm[t, edge.Item2] = double.PositiveInfinity;
for (int t = 0; t < incState.cm.GetLength(1); t++)
incState.cm[edge.Item1, t] = double.PositiveInfinity;
// Need to take out edges in incState that could be used to complete a premature cycle
if (incState.includedEdges.Count < incState.cm.GetLength(0) - 1)
{
int start = edge.Item1, end = edge.Item2, city;
city = getCityExited(start, incState.includedEdges);
while (city != -1)
{
start = city;
city = getCityExited(start, incState.includedEdges);
}
city = getCityEntered(end, incState.includedEdges);
while (city != -1)
{
end = city;
city = getCityEntered(end, incState.includedEdges);
}
while (start != edge.Item2)
{
incState.cm[end, start] = double.PositiveInfinity;
incState.cm[edge.Item2, start] = double.PositiveInfinity;
start = getCityEntered(start, incState.includedEdges);
}
}
// finish setting up the state and put it in the queue
incState.bound = curState.bound + reduceCM(ref incState.cm);
totalStatesGenerated++;
if (incState.bound > costOfBssf())
{
statesPruned++;
}
else
{
stateQueue.Enqueue(incState, incState.bound);
if (stateQueue.Count > maxStatesStored)
maxStatesStored = stateQueue.Count;
}
BBState exState = new BBState(curState.cm, curState.bound, curState.includedEdges);
exState.cm[edge.Item1, edge.Item2] = double.PositiveInfinity;
exState.bound = curState.bound + reduceCM(ref exState.cm);
totalStatesGenerated++;
if (exState.bound > costOfBssf())
{
statesPruned++;
}
else
{
stateQueue.Enqueue(exState, exState.bound);
if (stateQueue.Count > maxStatesStored)
maxStatesStored = stateQueue.Count;
}
}
timer.Stop();
Program.MainForm.tbElapsedTime.Text = " " + timer.Elapsed;
// update the cost of the tour.
Program.MainForm.tbCostOfTour.Text = " " + bssf.costOfRoute();
// do a refresh.
Program.MainForm.Invalidate();
String msg = "\tB&B RESULTS\n";
if (timer.ElapsedMilliseconds > 30000)
msg += "\nSearch timed out, 30 seconds expired.";
if (initialBssfIsFinal)
msg += "\nInitial BSSF (Greedy) is final solution.";
else
msg += "\nA better BSSF than the initial was found.";
msg += "\n\n\tSTATS:";
msg += "\nTotal States Created:\t" + totalStatesGenerated;
msg += "\nTotal States Pruned:\t" + statesPruned;
msg += "\nMax States Stored: \t" + maxStatesStored;
MessageBox.Show(msg);
return;
}
/// <summary>
/// Computes the voronoi graph.
/// </summary>
/// <returns>
/// The voronoi graph.
/// </returns>
/// <param name='Datapoints'>
/// Datapoints.
/// </param>
/// <exception cref='Exception'>
/// Represents errors that occur during application execution.
/// </exception>
public static VoronoiGraph ComputeVoronoiGraph(IEnumerable Datapoints)
{
PriorityQueue<VEvent> PQ = new PriorityQueue<VEvent>(PriorityQueueType.Minimum);
Hashtable CurrentCircles = new Hashtable();
VoronoiGraph VG = new VoronoiGraph();
VNode RootNode = null;
foreach(Vector2D V in Datapoints)
{
PQ.Enqueue(new VDataEvent(V));
}
while(PQ.Count>0)
{
VEvent VE = PQ.Dequeue() as VEvent;
VDataNode[] CircleCheckList;
if(VE is VDataEvent)
{
RootNode = VNode.ProcessDataEvent(VE as VDataEvent,RootNode,VG,VE.Y,out CircleCheckList);
}
else if(VE is VCircleEvent)
{
CurrentCircles.Remove(((VCircleEvent)VE).NodeN);
if(!((VCircleEvent)VE).Valid)
continue;
RootNode = VNode.ProcessCircleEvent(VE as VCircleEvent,RootNode,VG,VE.Y,out CircleCheckList);
}
else {
throw new Exception("Got event of type "+VE.GetType().ToString()+"!");
}
foreach(VDataNode VD in CircleCheckList)
{
if(CurrentCircles.ContainsKey(VD))
{
((VCircleEvent)CurrentCircles[VD]).Valid=false;
CurrentCircles.Remove(VD);
}
VCircleEvent VCE = VNode.CircleCheckDataNode(VD,VE.Y);
if(VCE!=null)
{
PQ.Enqueue(VCE);
CurrentCircles[VD]=VCE;
}
}
if(VE is VDataEvent)
{
Vector2D DP = ((VDataEvent)VE).DataPoint;
foreach(VCircleEvent VCE in CurrentCircles.Values)
{
if(MathTools.Dist(DP.X,DP.Y,VCE.Center.X,VCE.Center.Y)<VCE.Y-VCE.Center.Y && Math.Abs(MathTools.Dist(DP.X,DP.Y,VCE.Center.X,VCE.Center.Y)-(VCE.Y-VCE.Center.Y))>1e-10) {
VCE.Valid = false;
}
}
}
}
return VG;
}
public PriorityQueue<Group> calculateGroupsQueue()
{
Grid temp = this.copy();
PriorityQueue<Group> result = new PriorityQueue<Group>();
for (int x = 1; x <= this.x; x++)
{
for (int y = 1; y <= this.x; y++)
{
if (temp.grid[x, y] != "E")
{
int count = temp.currentRemoved;
temp.removeGroup(x, y);
count = temp.currentRemoved - count;
result.Enqueue(new Group(x, y, count));
}
}
}
return result;
}
public int ShortestRoute(int s,int f, out string p)
{
pq = new PriorityQueue<int,Node>();
Node cn = this.GetNode(s);
cn.Visit();
foreach (AdjNode an in cn.neighbors)
{
try
{
cn.d.Add("n" + cn.n + "r" + an.route, 0);
}
catch { }
}
cn.len = 0;
pq.Enqueue(0, cn);
while (pq.Count > 0)
{
//Console.ReadKey();
cn = pq.Dequeue().Value;
//LookAroundNode(cn);
LookAt(cn);
}
// восстанавливаем путь по меткам
string path = "";
Node start = GetNode(s);
Node current = GetNode(f);
Console.WriteLine(current.Info);
Node prev = null;
#region old
/*
while (current != start)
{
if (prev !=null && current.route.n != prev.r )
{
path = "до "+current.n +" \n пересесть на маршрут " + prev.r.ToString() + " "+ path;
}
//path = "по (" + current.r + ")" + " в " + current.n + path + "\n";
//path = ", ехать в " + current.n +" "+ path;
nodestomove.Enqueue(current);
prev = current;
if (current.route.n !=-1)
if (((" "+routes[current.route.n].rs+" ").IndexOf(" " + current.n.ToString() + " ")) > (((" "+current.route.rs+" ").IndexOf(" " +GetNode(current.prevn).n.ToString() + " "))))
{
current = current.Neighbor(current.route.n, -1);
}
else current = current.Neighbor(current.r, 1);
}
*/
#endregion
//path = "Инструкция: на " + prev.r + " маршруте " + path+" до "+f;
//nodestomove.Enqueue(start);
Node finish = GetNode(f);
//p = GetNode(f).d.Keys
var items = from item in finish.d
orderby item.Value ascending
select item.Key;
path = items.ToArray().First();
p = path;
return finish.d.Values.Min();
}
public override IEnumerator FindPath(Vector3 startPos, Vector3 targetPos, DistanceHeuristic distanceType, bool simplified)
{
this.myProperties.sw = new Stopwatch();
this.myProperties.sw.Start();
#region Init
Node startTile = this.myProperties.myGraph.GetNode(startPos);
Node targetTile = this.myProperties.myGraph.GetNode(targetPos);
Vector3[] wayPoints = new Vector3[0];
bool success = false;
//List<Tile> open = new List<Tile>();
PriorityQueue<Node> open = new PriorityQueue<Node>(this.myProperties.myGraph.Size);
HashSet<Node> close = new HashSet<Node>();
open.Enqueue(startTile); //Add the starting tile to be processed
#endregion Init
//if (startTile.walkable) {
while (open.Count > 0)
{
Node currentTile = open.Dequeue(); //Set the currentTile to the next elem in open
//If we got to the target, the create the path to it
//and exit the loop
if (currentTile == targetTile)
{
this.myProperties.sw.Stop();
print("A*: " + this.myProperties.sw.ElapsedMilliseconds + " ms");
success = true;
break;
}
close.Add(currentTile);
//
foreach (Node adjacent in this.myProperties.myGraph.GetAdjacents(currentTile))
{
//Ignore the adjacent neightbor which is already evaluated
if (!adjacent.walkable || close.Contains(adjacent)) continue;
//Length of this path
this.myProperties.tentativeGScore = currentTile.gScore + this.GetDistance(currentTile, adjacent, distanceType);
//Find new tiles
if (this.myProperties.tentativeGScore < adjacent.gScore || !open.Contains(adjacent))
{
adjacent.gScore = this.myProperties.tentativeGScore;
adjacent.hScore = this.GetDistance(adjacent, targetTile, distanceType);
adjacent.myParent = currentTile;
if (!open.Contains(adjacent))
open.Enqueue(adjacent);
else
open.UpdateElement(adjacent);
}
}
}
yield return new WaitForSeconds(0.0000001f);
if (success)
{
wayPoints = CreatePath(startTile, targetTile, simplified);
}
this.myProperties.myManager.DoneProcessing(wayPoints, success);
//}
}
/// <summary>
/// Builds the shortest path tree using a priority queue for given vertex
/// </summary>
/// <remarks>Negative edges are ignored</remarks>
private void BuildShortestPathTree()
{
ResetGraphState(); // Reset all distances to endpoint and previous shortest path
Vertex v = this.EndVertex;
v.Distance = 0; // Set distance to 0 for endpoint vertex
// Creates a fringe (queue) for storing edge pending to be processed
PriorityQueue<SP_Node> fringe = new PriorityQueue<SP_Node>(this.Vertices.Count);
// Main loop
do
{
if (v!=null)
foreach (Edge _e in v.RelatedEdges) // Evaluate all incoming edges
if (_e.Head==v && _e.Weight>=0) // Ignore negative edges
fringe.Enqueue(new SP_Node(_e, _e.Weight + _e.Head.Distance));
SP_Node node = fringe.Dequeue(); // Extracts next element in queue
if (node == null) // No pending edges to evaluate, finished
break;
Edge e = node.Edge;
v = e.Tail;
if (v.Distance == int.MinValue) // Vertex distance to endpoint not calculated yet
{
v.Distance = e.Weight + e.Head.Distance;
v.EdgeToPath = e;
}
else
v = null;
} while (true);
}
