public void PriorityQueueTest1()
{
PriorityQueue<string> a = new PriorityQueue<string>();
a.Push("World", 7);
a.Push("Hello", 2);
Assert.AreEqual("Hello", a.Pop());
Assert.AreEqual("World", a.Pop());
a.Push("Hello", 2);
a.Push("World", 7);
Assert.AreEqual("Hello", a.Pop());
Assert.AreEqual("World", a.Pop());
}
public void PriorityQueueTest2()
{
PriorityQueue<string> a = new PriorityQueue<string>(new[] { Tuple.Create("Hello", 4), Tuple.Create("World", 9), Tuple.Create("Test", 1) });
a.Push("Test2", 2);
Assert.AreEqual("Test", a.Pop());
int p2;
Assert.AreEqual("Test2", a.Pop(out p2));
Assert.AreEqual(2, p2);
a.Push("First", 0);
a.Push("Second", 5);
Assert.AreEqual("First", a.Pop());
Assert.AreEqual("Hello", a.Pop());
Assert.AreEqual("Second", a.Pop());
Assert.AreEqual("World", a.Pop());
a.Pop();
}
static int FindPath(char[,] map, int row, int col, Point start, Point end)
{
int[,] gscore = new int[row, col];
int[,] fscore = new int[row, col];
Color[,] status = new Color[row, col];
PriorityQueue<Point> queue = new PriorityQueue<Point>();
queue.Comparer = Compare;
gscore[start.Y, start.X] = 0;
start.F = fscore[start.Y, start.X] = Heuristic(start, end);
queue.Push(start);
status[start.Y, start.X] = Color.Grey;
while (!queue.IsEmpty())
{
Point cur = queue.Pop();
if (cur.X == end.X && cur.Y == end.Y)
return fscore[cur.Y, cur.X];
foreach (var n in GetNeighbors(cur, map, row, col))
{
if (status[n.Y, n.X] == Color.Black)
continue;
int tentative = gscore[cur.Y, cur.X] + 1;
bool useTentative = true;
if (status[n.Y, n.X] == Color.Grey && gscore[n.Y, n.X] <= tentative)
useTentative = false;
if (useTentative)
{
gscore[n.Y, n.X] = tentative;
n.F = fscore[n.Y, n.X] = tentative + Heuristic(n, end);
queue.Push(n);
n.P = cur;
}
}
}
return int.MinValue;
}
public static void Main()
{
PriorityQueue<int> queue = new PriorityQueue<int>();
queue.Push(1);
queue.Push(2);
queue.Push(12);
queue.Push(34);
queue.Push(6);
queue.Push(9);
Console.WriteLine(queue.Pop());
Console.WriteLine(queue.Pop());
}
public void PriorityQueueSize()
{
PriorityQueue<int> p = new PriorityQueue<int>();
Assert.AreEqual(0, p.Size);
p.Push(5, 4);
Assert.AreEqual(1, p.Size);
p.Pop();
Assert.AreEqual(0, p.Size);
p = new PriorityQueue<int>(new[] { Tuple.Create(1, 1), Tuple.Create(1, 7) });
Assert.AreEqual(2, p.Size);
p.Push(9, 12);
Assert.AreEqual(3, p.Size);
p.Pop();
Assert.AreEqual(2, p.Size);
}
public bool searchPath(Dictionary<string, string> pathMap)
{
PriorityQueue<Node> priorityQueue;
priorityQueue = new PriorityQueue<Node>();
priorityQueue.Push(this.begainNode);
while (!priorityQueue.Empty())
{
Node topNode = priorityQueue.Pop();
#region 判断是否找到目状态
if (matched(topNode, this.targetNode))
{
MessageBox.Show("Finished!");
return true;
}
#endregion
int row = topNode.row_0;
int col = topNode.col_0;
if (row > 0 && topNode.cannotAct != Direction.up)
{
Node curNode = new Node(topNode);
exchange(curNode, row, col, row - 1, col);
curNode.ToString();
curNode.cannotAct = Direction.down;
if (!pathMap.ContainsKey(curNode.state))
{
curNode.deepth = topNode.deepth + 1;
curNode.value = getValue(curNode, this.targetNode);
curNode.row_0 = row - 1;
curNode.col_0 = col;
priorityQueue.Push(curNode);
pathMap.Add(curNode.state, topNode.state);
}
}
if (row < 2 && topNode.cannotAct != Direction.down)
{
Node curNode = new Node(topNode);
exchange(curNode, row, col, row + 1, col);
curNode.ToString();
curNode.cannotAct = Direction.up;
if (!pathMap.ContainsKey(curNode.state))
{
curNode.deepth = topNode.deepth + 1;
curNode.value = getValue(curNode, this.targetNode);
curNode.row_0 = row + 1;
curNode.col_0 = col;
priorityQueue.Push(curNode);
pathMap.Add(curNode.state, topNode.state);
}
}
if (col > 0 && topNode.cannotAct != Direction.left)
{
Node curNode = new Node(topNode);
exchange(curNode, row, col, row, col - 1);
curNode.ToString();
curNode.cannotAct = Direction.left;
if (!pathMap.ContainsKey(curNode.state))
{
curNode.deepth = topNode.deepth + 1;
curNode.value = getValue(curNode, this.targetNode);
curNode.row_0 = row;
curNode.col_0 = col - 1;
priorityQueue.Push(curNode);
pathMap.Add(curNode.state, topNode.state);
}
}
if (col < 2 && topNode.cannotAct != Direction.right)
{
Node curNode = new Node(topNode);
exchange(curNode, row, col, row, col + 1);
curNode.ToString();
curNode.cannotAct = Direction.right;
if (!pathMap.ContainsKey(curNode.state))
{
curNode.deepth = topNode.deepth + 1;
curNode.value = getValue(curNode, this.targetNode);
curNode.row_0 = row;
curNode.col_0 = col + 1;
priorityQueue.Push(curNode);
pathMap.Add(curNode.state, topNode.state);
}
}
}
return false;
}
public static Cell DijkstraNextCell(int startI, int startJ, int endI, int endJ, Cell[] forbidden)
{
if (_distMap == null)
{
_distMap = new double[world.Height, world.Width];
_distPrev = new Cell[world.Height, world.Width];
}
var q = new PriorityQueue<Pair<double, Cell>>();
q.Push(new Pair<double, Cell>(0.0, new Cell(endI, endJ)));
for (var i = 0; i < world.Height; i++)
for (var j = 0; j < world.Width; j++)
_distMap[i, j] = Infinity;
_distMap[endI, endJ] = 0;
while (q.Count > 0)
{
var cur = q.Top().Second;
var minDist = -q.Top().First;
q.Pop();
if (minDist > _distMap[cur.I, cur.J])
continue;
EnumerateNeigbours(cur, to =>
{
if (!CanPass(cur.I, cur.J, to.I, to.J) || forbidden.Any(x => x.Equals(to.I, to.J)))
return;
var distTo = _distMap[cur.I, cur.J] + GetCost(to);
if (distTo < _distMap[to.I, to.J])
{
_distMap[to.I, to.J] = distTo;
_distPrev[to.I, to.J] = cur;
q.Push(new Pair<double, Cell>(-distTo, to));
}
});
}
if (_distPrev[startI, startJ] == null)
{
if (forbidden.Length == 0)
throw new Exception("path not found");
return DijkstraNextCell(startI, startJ, endI, endJ, new Cell[] {});
}
return _distPrev[startI, startJ];
}
public static String Solve( Puzzle map )
{
Puzzle initialNode = map;
Puzzle end = null;
var moves = Enum.GetValues(typeof(Puzzle.Position));
PriorityQueue<Puzzle> nodes = new PriorityQueue<Puzzle>();
nodes.Push(initialNode);
while (nodes.Count != 0)
{
Puzzle currentNode = nodes.Top;
nodes.Pop();
if (currentNode.MD == 0)
{
end = currentNode;
break;
}
if (currentNode.Blank != -1)
{
foreach (var move in moves)
{
if (currentNode.NowStatus.answer.Count != 0)
{
Puzzle.Position now = currentNode.NowStatus.answer[currentNode.NowStatus.answer.Count - 1];
if (currentNode.ReversePosition(now) == (Puzzle.Position)move)
continue;
}
Puzzle newPuzzle = (Puzzle)currentNode.Clone();
if (newPuzzle.DoMove((Puzzle.Position)move))
{
newPuzzle.CalculateMD();
nodes.Push(newPuzzle);
}
}
}
if (currentNode.maxChoice != 0 && ( currentNode.NowStatus.answer.Count != 0 || currentNode.Blank == -1 ))
{
for (int i = 0; i < currentNode.Data.Count(); i++)
{
if (i == currentNode.Blank)
continue;
Puzzle newPuzzle = (Puzzle)currentNode.Clone();
newPuzzle.Choice(i);
newPuzzle.CalculateMD();
nodes.Push(newPuzzle);
}
}
}
if (end != null){
return end.GetSolution();
}
else
return "";
}
static void Main(string[] args)
{
search begainSearch = new search();
//测试优先队列功能
PriorityQueue<int> que;
que = new PriorityQueue<int>();
que.Push(12);
que.Push(132);
que.Push(123);
que.Push(212);
que.Push(322);
que.Push(126);
que.Push(13);
que.Push(189);
while (!que.Empty())
{
Console.Write(que.Pop().ToString()+" ");
}
}
bool searchPath(Dictionary<string, string> pathMap, Node result)
{
PriorityQueue<Node> priorityQueue;
Stack<Node> pathStack;
priorityQueue = new PriorityQueue<Node>();
pathStack = new Stack<Node>();
priorityQueue.Push(this.begainNode);
pathStack.Push(this.begainNode);
int cycle = 0;
while (!priorityQueue.Empty())
{
cycle++;
// Console.WriteLine("第 "+cycle.ToString()+" 步");
// Console.WriteLine("队列中的元素 " + priorityQueue.Count);
Node topNode = priorityQueue.Top();
priorityQueue.Pop();
#region 判断是否找到目状态
if (matched(topNode, this.targetNode))
{
printState(targetNode);
Console.WriteLine("搜索完成");
printState(topNode);
result = topNode;
return true;
}
#endregion
int row = topNode.row_0;
int col = topNode.col_0;
if (row > 0 && topNode.cannotAct != Direction.up)
{
Node curNode = new Node(topNode);
// Console.WriteLine("当前状态");
// printState(topNode);
// Console.WriteLine(row.ToString()+" "+col.ToString()+" 空格上移后状态");
exchange(curNode, row, col, row - 1, col);
curNode.ToString();
curNode.cannotAct = Direction.down;
if (!pathMap.ContainsKey(curNode.state))
{
// printState(curNode);
curNode.deepth = topNode.deepth + 1;
curNode.value = getValue(curNode, this.targetNode);
// Console.WriteLine("当前代价值："+(curNode.value + curNode.deepth).ToString());
curNode.father = topNode;
curNode.row_0 = row - 1;
curNode.col_0 = col;
priorityQueue.Push(curNode);
pathStack.Push(curNode);
pathMap.Add(curNode.state, topNode.state);
}
}
if (row < 2 && topNode.cannotAct != Direction.down)
{
Node curNode = new Node(topNode);
// Console.WriteLine("当前状态");
// printState(topNode);
// Console.WriteLine(row.ToString()+" "+col.ToString()+" 下移后状态");
exchange(curNode, row, col, row + 1, col);
curNode.ToString();
curNode.cannotAct = Direction.up;
if (!pathMap.ContainsKey(curNode.state))
{
// printState(curNode);
curNode.deepth = topNode.deepth + 1;
curNode.value = getValue(curNode, this.targetNode);
// Console.WriteLine("当前代价值："+(curNode.value + curNode.deepth).ToString());
curNode.father = topNode;
curNode.row_0 = row + 1;
curNode.col_0 = col;
priorityQueue.Push(curNode);
pathStack.Push(curNode);
pathMap.Add(curNode.state, topNode.state);
}
}
if (col > 0 && topNode.cannotAct != Direction.left)
{
Node curNode = new Node(topNode);
// Console.WriteLine("当前状态");
// printState(topNode);
// Console.WriteLine(row.ToString()+" "+col.ToString()+" 左移之后的状态");
exchange(curNode, row, col, row, col - 1);
curNode.ToString();
curNode.cannotAct = Direction.left;
if (!pathMap.ContainsKey(curNode.state))
{
// printState(curNode);
curNode.deepth = topNode.deepth + 1;
curNode.value = getValue(curNode, this.targetNode);
// Console.WriteLine("当前代价值："+(curNode.value + curNode.deepth).ToString());
curNode.father = topNode;
curNode.row_0 = row;
curNode.col_0 = col - 1;
priorityQueue.Push(curNode);
pathStack.Push(curNode);
pathMap.Add(curNode.state, topNode.state);
}
}
if (col < 2 && topNode .cannotAct != Direction.right)
{
Node curNode = new Node(topNode);
// Console.WriteLine("当前状态");
// printState(topNode);
// Console.WriteLine(row.ToString()+" "+col.ToString()+" 右移后状态");
exchange(curNode, row, col, row, col + 1);
curNode.ToString();
curNode.cannotAct = Direction.right;
if (!pathMap.ContainsKey(curNode.state))
{
// printState(curNode);
curNode.deepth = topNode.deepth + 1;
curNode.value = getValue(curNode, this.targetNode);
// Console.WriteLine("当前代价值："+(curNode.value+curNode.deepth).ToString());
curNode.father = topNode;
curNode.row_0 = row;
curNode.col_0 = col + 1;
priorityQueue.Push(curNode);
pathStack.Push(curNode);
pathMap.Add(curNode.state, topNode.state);
}
}
}
return false;
}
public List<Waypoint> FindPath(Waypoint start, Waypoint goal, OccupancyGrid2D og, out bool success)
{
List<Waypoint> path = new List<Waypoint>();
//added by aaron (sort of a hack)
if (og == null || goal.Coordinate.DistanceTo(start.Coordinate) == 0)
{
path.Add(new Waypoint(start.Coordinate, true, 0));
success = true;
return path;
}
int xIdx, yIdx;
success = true;
Vector2[] NESWVector = new Vector2[4];
Vector2[] diagVector = new Vector2[4];
bool[] NESW = new bool[4];
Vector2 startV = start.Coordinate; // Start Vector2
Vector2 goalV = goal.Coordinate; // Goal Vector2
PriorityQueue open = new PriorityQueue();
closed = new OccupancyGrid2D(resX, resY, extentX, extentY);
opened = new OccupancyGrid2D(resX, resY, extentX, extentY);
GetIndicies(startV.X, startV.Y, out xIdx, out yIdx);
startV = new Vector2(xIdx, yIdx);
GetIndicies(goalV.X, goalV.Y, out xIdx, out yIdx);
goalV = new Vector2(xIdx, yIdx);
Node root = new Node(goalV, goalV.DistanceTo(startV), 0, null);
Node current = root;
open.Push(current);
// Do the spreading/discovering stuff until we discover a path.
while (current.xy != startV)
{
if (open.q.Count == 0 || open.q.Count > MAX_OPEN)
{
Console.WriteLine("Failure in DSstar. Open count is: " + open.q.Count);
success = false;
break;
}
current = open.Pop();
NESWVector[0] = new Vector2(current.xy.X, current.xy.Y - 1);
NESWVector[1] = new Vector2(current.xy.X + 1, current.xy.Y);
NESWVector[2] = new Vector2(current.xy.X, current.xy.Y + 1);
NESWVector[3] = new Vector2(current.xy.X - 1, current.xy.Y);
diagVector[0] = new Vector2(current.xy.X + 1, current.xy.Y - 1);
diagVector[1] = new Vector2(current.xy.X + 1, current.xy.Y + 1);
diagVector[2] = new Vector2(current.xy.X - 1, current.xy.Y + 1);
diagVector[3] = new Vector2(current.xy.X - 1, current.xy.Y - 1);
for (int i = 0; i < 4; i++)
{
if ((int)og.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) < 255)
{
if (closed.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) == 0)
{
NESW[i] = true;
if (opened.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) == 0)
{
open.Push(new Node(NESWVector[i], NESWVector[i].DistanceTo(startV), current.h + 1
+ og.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) / blurWeight, current));
opened.SetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y, 1);
}
}
}
}
for (int i = 0; i < 4; i++)
{
if (NESW[i % 4] && NESW[(i + 1) % 4])
{
if (og.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) < 255)
{
if (closed.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) == 0)
{
if (opened.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) == 0)
{
open.Push(new Node(diagVector[i], diagVector[i].DistanceTo(startV), current.h + 1.4
+ og.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) / blurWeight, current));
opened.SetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y, 1);
}
}
}
}
}
for (int i = 0; i < 4; i++) NESW[i] = false;
closed.SetCellByIdx((int) current.xy.X, (int) current.xy.Y, 1);
}
// Build a path using the discovered path.
double x, y;
Waypoint waypoint;
// First waypoint is a user waypoint
GetReals((int)current.xy.X, (int)current.xy.Y, out x, out y);
waypoint = new Waypoint(x + resX / 2, y + resY / 2, true, og.GetCellByIdx((int)current.xy.X, (int)current.xy.Y));
path.Add(waypoint);
current = current.dad;
// Middle waypoints are path waypoints
while (current != root && current != null)
{
GetReals((int) current.xy.X, (int) current.xy.Y, out x, out y);
waypoint = new Waypoint(x + resX / 2, y + resY / 2, false, og.GetCellByIdx((int)current.xy.X, (int)current.xy.Y));
path.Add(waypoint);
current = current.dad;
}
// Last waypoint is a user waypoint
if (current != null)
{
GetReals((int)current.xy.X, (int)current.xy.Y, out x, out y);
waypoint = new Waypoint(x + resX / 2, y + resY / 2, true, og.GetCellByIdx((int)current.xy.X, (int)current.xy.Y));
path.Add(waypoint);
}
return path;
}
private void requeue()
{
_queue = new PriorityQueue<Node>();
for (int i = 0; i < 9; i++)
for (int j = 0; j < 9; j++)
{
_queue.Push(_board[i, j]);
}
}
//Given seed probability P, find the best k nodes that can maximize influence spread
public Tuple<List<int>, double> Greedy(int k, List<double> P)
{
HashSet<int> seeds = new HashSet<int> ();
List<int> seedSet = new List<int> ();
List<double> edgeW = new List<double> ();
for (int h = 0; h < numS; ++h)
edgeW.Add (1.0);
//CELF Algorithm
PriorityQueue<VNode> pq = new PriorityQueue<VNode> (numV+1, new VNodeComparer ());
List<bool> update = new List<bool> ();
for (int u = 0; u < numV; ++u)
{
VNode node = new VNode (u, numS);
pq.Push (node);
update.Add (false);
}
double total_gain = 0.0;
for (int i = 0; i < k; ++i)
{
for (int u = 0; u < numV; ++u)
update [u] = false;
int next = 0;
double gain = 0;
while (true)
{
VNode node = pq.Pop ();
int max_u = node.id;
if (update [max_u])
{
next = max_u;
gain = node.val;
break;
}
else
{
double sum = 0;
if (i == 0)
sum = V2S[max_u].Count * P[max_u];
else
{
foreach (int sid in V2S[max_u])
sum += edgeW[sid] * P[max_u];
}
VNode n1 = new VNode (max_u, sum);
pq.Push (n1);
update [max_u] = true;
}
}
total_gain += gain;
foreach (int sid in V2S[next])
edgeW [sid] = edgeW [sid] * (1 - P [next]);
seeds.Add (next);
seedSet.Add (next);
}
return new Tuple<List<int>, double> (seedSet, total_gain*numV/numS);
}
public static Map.Box AStar(Map m, Map.Box start, Vector3 target, out int pathLength)
{
Dictionary<Map.Box, int> closed = new Dictionary<Map.Box, int>();
PriorityQueue<AStarEntry> queue = new PriorityQueue<AStarEntry>(new LambdaComparer<AStarEntry>((x, y) => x.F.CompareTo(y.F)));
Dictionary<Map.Box, AStarEntry> queueLookup = new Dictionary<Map.Box, AStarEntry>();
AStarEntry startEntry = new AStarEntry
{
Parent = null,
Box = start,
G = 0,
F = (target - start.GetCenter()).Length(),
BoxSize = Math.Max(start.Width, Math.Max(start.Height, start.Depth)),
PathIndex = 0,
};
queue.Push(startEntry);
queueLookup[start] = startEntry;
const float thresholdFCoefficient = 0.6f;
const int iterationLimit = 10;
int iteration = 0;
while (queue.Count > 0)
{
AStarEntry entry = queue.Pop();
if (iteration >= iterationLimit || entry.F < entry.BoxSize * thresholdFCoefficient)
return VoxelChaseAI.reconstructPath(entry, out pathLength);
iteration++;
queueLookup.Remove(entry.Box);
closed[entry.Box] = entry.G;
foreach (Map.Box adjacent in entry.Box.Adjacent.ToList())
{
if (adjacent == null)
continue;
int boxSize = Math.Max(adjacent.Width, Math.Max(adjacent.Height, adjacent.Depth));
int tentativeGScore = entry.G + boxSize;
int previousGScore;
bool hasPreviousGScore = closed.TryGetValue(adjacent, out previousGScore);
if (hasPreviousGScore && tentativeGScore > previousGScore)
continue;
AStarEntry alreadyInQueue;
bool throwaway = queueLookup.TryGetValue(adjacent, out alreadyInQueue);
if (alreadyInQueue == null || tentativeGScore < previousGScore)
{
AStarEntry newEntry = alreadyInQueue != null ? alreadyInQueue : new AStarEntry();
newEntry.Parent = entry;
newEntry.G = tentativeGScore;
newEntry.F = tentativeGScore + (target - adjacent.GetCenter()).Length();
newEntry.PathIndex = entry.PathIndex + 1;
if (alreadyInQueue == null)
{
newEntry.Box = adjacent;
newEntry.BoxSize = boxSize;
queue.Push(newEntry);
queueLookup[adjacent] = newEntry;
}
}
}
}
pathLength = 0;
return null;
}
