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 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 static SearchResult Search(AbstractNode initialNode, IKnowledgeBase kb)
{
var frontier = new PriorityQueue<AbstractNode>();
var explored = new List<AbstractState>();
var statesSearched = 0; //Bliver kun brugt af os af ren interesse
var end = initialNode.Target;
frontier.Add(initialNode);
explored.Add(initialNode.State);
while (frontier.Count > 0)
{
// Chooses the lowest-cost node in the frontier
var currentNode = frontier.Pop();
statesSearched++;
if (currentNode.State.Equals(end))
return new SearchResult(currentNode, statesSearched, true);
var actions = kb.ActionsForNode(currentNode);
//Explore /expand the current node
foreach (var action in actions)
{
var child = kb.Resolve(currentNode, action, end);
//System.Console.WriteLine("Frontier.Count: " + frontier.Count);
if (!explored.Contains(child.State) && !frontier.Contains(child))
{
explored.Add(child.State);
frontier.Add(child);
}
else if(true)
{
for (int i = 0; i < frontier.Count; i++)
{
var frontierNode = frontier[i];
if (frontierNode.State.Equals(child.State) && frontierNode.PathCost > child.PathCost)
{
frontier[i] = child;
break;
}
}
}
}
}
return new SearchResult(null, statesSearched, false);
}
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 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 static int Search(List<State> states, List<Action> actions, State start, Tile target)
{
var found = 0;
PriorityQueue<BFNode> frontier = new PriorityQueue<BFNode>();
List<State> explored = new List<State>();
frontier.Add(new BFNode(start));
while (frontier.Count > 0)
{
// Chooses the lowest-cost node in the frontier
BFNode currentBFNode = frontier.Pop();
// Win condition
if (currentBFNode.State.Type.Equals(target))
found++;
explored.Add(currentBFNode.State);
// Filter actions to the ones connected to the current node
foreach (Action action in actions.Where(a => a.StateA.Equals(currentBFNode.State)))
{
// One of A or B will be the currentBFNode's action
// but it won't be added to the frontier since it
// is already in explored
var childA = new BFNode(currentBFNode, action, action.StateA);
var childB = new BFNode(currentBFNode, action, action.StateB);
if (!explored.Contains(childA.State) && !frontier.Any(n => n.State == childA.State))
frontier.Add(childA);
if (!explored.Contains(childB.State) && !frontier.Any(n => n.State == childB.State))
frontier.Add(childB);
}
}
return found;
}
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();
}
public static INode Search(List<State> states, List<Action> actions, State start, State end)
{
PriorityQueue<Node> frontier = new PriorityQueue<Node>();
List<State> explored = new List<State>();
frontier.Add(new Node(start, end));
while (frontier.Count > 0)
{
// Chooses the lowest-cost node in the frontier
Node currentNode = frontier.Pop();
// Win condition
if (currentNode.State.Equals(end))
return currentNode;
// Add currentNode to list of explored
explored.Add(currentNode.State);
// Filter actions to the ones connected to the current node
foreach (Action action in actions.Where(a => a.StateA.Equals(currentNode.State) || a.StateB.Equals(currentNode.State)))
{
// One of A or B will be the currentNode's action
// but it won't be added to the frontier since it
// is already in explored
var childA = new Node(currentNode, action, action.StateA, end);
if (!explored.Contains(childA.State))
frontier.Add(childA);
var childB = new Node(currentNode, action, action.StateB, end);
if (!explored.Contains(childB.State))
frontier.Add(childB);
}
}
return null;
}
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 "";
}
public static void UpdateDijkstraMap(PosArray<int> map,IntLocationDelegate get_cost)
{
PriorityQueue<cell> frontier = new PriorityQueue<cell>(c => -c.value);
int height = map.objs.GetLength(0);
int width = map.objs.GetLength(1);
for(int i=0;i<height;++i){
for(int j=0;j<width;++j){
if(map[i,j] != U.DijkstraMin){
int v = map[i,j];
bool good = true;
for(int s=-1;s<=1 && good;++s){
for(int t=-1;t<=1 && good;++t){
if(i+s >= 0 && i+s < height && j+t >= 0 && j+t < width){
if(map[i+s,j+t] < v && map[i+s,j+t] != U.DijkstraMin){
good = false;
}
}
}
}
if(good){ //find local minima and add them to the frontier
frontier.Add(new cell(i,j,v));
}
}
}
}
while(frontier.list.Count > 0){
cell c = frontier.Pop();
for(int s=-1;s<=1;++s){
for(int t=-1;t<=1;++t){
if(c.row+s >= 0 && c.row+s < height && c.col+t >= 0 && c.col+t < width){
int cost = get_cost(c.row+s,c.col+t);
if(map[c.row+s,c.col+t] > c.value+cost){
map[c.row+s,c.col+t] = c.value+cost;
frontier.Add(new cell(c.row+s,c.col+t,c.value+cost));
}
}
}
}
}
}
public void AstarRun()
{
Console.WriteLine("A* starts!");
NavigateNode start = new NavigateNode(2, 2, NavigateNode.StateEnum.NAVIGABLE);
NavigateNode end = new NavigateNode(goalX, goalY, NavigateNode.StateEnum.NAVIGABLE);
PriorityQueue<NavigateNode> openSet = new PriorityQueue<NavigateNode>();
PriorityQueue<NavigateNode> closeSet = new PriorityQueue<NavigateNode>();
openSet.Add(start);
while (!openSet.Empty) {
// get from open set
NavigateNode current = openSet.Pop();
// add to close set
closeSet.Add(current);
// goal found
if (current.IsSameLocation(end)) {
while (current.Parent != null) {
mMap[current.X, current.Y].State = NavigateNode.StateEnum.PATH;
current = current.Parent;
}
return;
}
else {
List<NavigateNode> neighbors = GetNeighbors(current);
foreach (NavigateNode n in neighbors) {
if (closeSet.IsMember(n)) {
continue;
}
else {
if (!openSet.IsMember(n)) {
n.Parent = current;
n.DirectCost = current.DirectCost + GetDirectCost(current, n);
n.HeuristicCost = GetHeuristicCost(n, end);
n.TotalCost = n.DirectCost + n.HeuristicCost;
// add to open set
openSet.Add(n);
}
else {
double costFromThisPathToM = current.DirectCost + GetDirectCost(current, n);
// we found a better path
if (costFromThisPathToM < n.DirectCost) {
n.Parent = current; // change parent to n
n.DirectCost = costFromThisPathToM; // recalculate direct cost
n.TotalCost = n.HeuristicCost + n.DirectCost; // recalculate total cost
}
}
}
}
}
}
Console.WriteLine("end here?");
}
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;
}
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()+" ");
}
}
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;
}
//todo: remove dijkstra from this file, use the one in Utility? (do I have UpdateDijkstra in Utility?)
public static PosArray<int> GetDijkstraMap(int height,int width,BooleanLocationDelegate is_blocked,IntLocationDelegate get_cost,List<cell> sources)
{
PriorityQueue<cell> frontier = new PriorityQueue<cell>(c => -c.value);
PosArray<int> map = new PosArray<int>(height,width);
for(int i=0;i<height;++i){
for(int j=0;j<width;++j){
if(is_blocked(i,j)){
map[i,j] = U.DijkstraMin;
}
else{
map[i,j] = U.DijkstraMax;
}
}
}
foreach(cell c in sources){
map[c.row,c.col] = c.value;
frontier.Add(c);
}
while(frontier.list.Count > 0){
cell c = frontier.Pop();
for(int s=-1;s<=1;++s){
for(int t=-1;t<=1;++t){
if(c.row+s >= 0 && c.row+s < height && c.col+t >= 0 && c.col+t < width){
int cost = get_cost(c.row+s,c.col+t);
if(map[c.row+s,c.col+t] > c.value+cost){
map[c.row+s,c.col+t] = c.value+cost;
frontier.Add(new cell(c.row+s,c.col+t,c.value+cost));
}
}
}
}
}
for(int i=0;i<height;++i){
for(int j=0;j<width;++j){
if(map[i,j] == U.DijkstraMax){
map[i,j] = U.DijkstraMin; //any unreachable areas are marked unpassable
}
}
}
return map;
}
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;
}
//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 void PriorityQueueEmpty()
{
PriorityQueue<string> a = new PriorityQueue<string>();
a.Pop();
}
/// <summary>
/// ----------------
/// - A* Algorithm -
/// ----------------
///
/// OPEN = priority queue contain START
/// CLOSED = empty set
///
/// while lowest rank in OPEN is not the GOAL:
/// current = remove lowest rank item from OPEN
/// add current to CLOSED
///
/// for neighbors of current
/// cost = g(current) + movement_cost(current, neighbor)
///
/// if neighbor in OPEN and cost less than g(neighbor)
/// remove neighbor from OPEN, because new path is better
///
/// if neighbor in CLOSED and cost less than g(neighbor)
/// remove neighbor from CLOSED
///
/// if neighbor not in OPEN and neighbor not in CLOSED
/// set g(neighbor) to cost
/// add neighbor to OPEN
/// set priority queue rank to g(neighbor) + h(neighbor)
/// set neighbor's parent to current
///
///
/// reconstruct reverse path from goal to start
/// by following parent pointers
///
/// </summary>
/// <param name="startPostion">start</param>
/// <param name="goalPosition">destination</param>
/// <param name="nodeType">color of node</param>
/// <returns></returns>
private List<NavNode> MakeAStarPath(Vector3 startPostion, Vector3 goalPosition, NavNode.NavNodeEnum nodeType)
{
/**
* A* implementation
* Summary:
* 1) Add the starting node to the open set
* 2) Repeat the following:
* a) Look for the lowest F cost on the open set.
* b) Move it to the closed set
* c) For each of the 8 adjacency node to the current node:
* + If it is NOT walkable or if it is on the CLOSED SET, just ignore it.
* + Otherwise:
* - If it is NOT on the OPEN SET, add it to the OPEN SET. Make the "current node" as
* parent of this adjacency node. Record F, G, H for this node.
* - If it is on the OPEN SET already, check to see if this path to that square is
* better using G cost as the measure. A lower G means that this is a better path.
* If so, change the parent of the node to the "current node", and recalculate
* the G and F cost of the node.
* d) Stop when we:
* + Add the goal node to the closed set, in which case the path has been found.
* + Fail to find the goal node, and the open set is empty. In this case, there is NO path.
*/
// spacing between node on map (= 150)
int spacing = stage.Terrain.Spacing;
/**
* A* path
* this is our final path
*/
List<NavNode> path = new List<NavNode>();
/**
* The starting point
*/
NavNode start = new NavNode(startPostion);
start.DistanceFromSource = 0.0;
start.DistanceToGoal = 0.0;
start.Distance = 0.0;
start.Parent = null;
/**
* The goal
*/
NavNode goal = new NavNode(goalPosition);
goal.DistanceFromSource = 0.0;
goal.DistanceToGoal = 0.0;
goal.Distance = 0.0;
goal.Parent = null;
// open set
PriorityQueue<NavNode> openSet = new PriorityQueue<NavNode>();
// close set
PriorityQueue<NavNode> closedSet = new PriorityQueue<NavNode>();
// add starting point to open set (part 1)
openSet.Add(start);
while (!openSet.Empty) {
// get the current node with lowest cost F and remove it from open set
NavNode current = openSet.Pop();
// add current to close set
closedSet.Add(current);
// if it's equal to our goal, we're done (part d)
if (current.IsSameLocation(goal)) {
while (current.Parent != null) {
path.Add(current);
current.Navigatable = nodeType;
current = current.Parent;
}
path.Reverse();
return path;
}
else {
// for each of the 8 adjacency neighbors
// NOTE: the neighbor list already removed un-walkable nodes
List<NavNode> neighbors = GetNeighbors(current.Translation);
foreach (NavNode n in neighbors) {
// if it's on the closed set, just ignore it
if (IsNodeIn(n, closedSet)) {
continue;
}
else {
if (!IsNodeIn(n, openSet)) {
// make the "current node" as parent of this neighbor
n.Parent = current;
// record new F, G, H
n.DistanceFromSource = current.DistanceFromSource + CalculateDistanceFromSource(current, n);
n.DistanceToGoal = CalculateHeuristicDinstanceToGoal(n, goal);
n.Distance = n.DistanceFromSource + n.DistanceToGoal;
// add this neighbor to the OPEN SET
openSet.Add(n);
}
else { // it's already on the OPEN SET
double costFromThisPathToN = current.DistanceFromSource + CalculateDistanceFromSource(current, n);
// we have a better path, going from "current node"
if (costFromThisPathToN < n.DistanceFromSource) {
// recalculate G and F for this neighbor
n.Parent = current;
n.DistanceFromSource = costFromThisPathToN;
n.Distance = n.DistanceFromSource + n.DistanceToGoal;
}
}
}
}
}
}
return path;
}
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;
}
