public void setStartAndGoal(int x0, int y0, int xn, int yn)
{
_nodeStart = new heapNode(x0, y0);
_nodeGoal = new heapNode(xn, yn);
_heap = new HeapSort(ref _openList);
// add a new element, and re-build the heap
_heap.push_heap(_nodeStart);
}
public double getScoreGain(heapNode preNode, heapNode node)
{
double preNodeScore = _lattice.getMap(preNode._x, preNode._y);
double nodeScoreGain = _lattice.getMap(node._x, node._y);
double edgeScoreGain = _lattice.getMap(node._x, preNode._y, node._y);
if (preNodeScore <= -1e10 || nodeScoreGain <= -1e10 || edgeScoreGain <= -1e10)//anything touching with -1e10 will die..
{
return(-1e10);
}
return(nodeScoreGain + edgeScoreGain);
}
public double doOneStep(ref List <int> tags)
{
double score = -1;
// Pop the best node (max heap)
heapNode n = _heap.pop_heap();
if (n == null)
{
return(-2);//heap empty, search failure
}
// Check for the goal, once we pop that we're done
if (n._x == _nodeGoal._x && n._y == _nodeGoal._y)
{
score = n._f;
_nodeGoal._parent = n._parent;
//build the "solution-node-sequence" in a backward way
heapNode nodeParent = _nodeGoal._parent;
while (nodeParent != _nodeStart)
{
tags.Add(nodeParent._y);
nodeParent = nodeParent._parent;
}
tags.Reverse();
return(score);
}
else //not goal
{
//get successors
int xNew = n._x + 1;
for (int yNew = 0; yNew < _h; yNew++)
{
if (xNew >= _w)
{
if (_lattice.getMap(xNew, yNew) <= -1e10)
{
continue;
}
}
heapNode successor = new heapNode(xNew, yNew);
successor._parent = n;
successor._g = n._g + getScoreGain(n, successor);
successor._h = _lattice.getHeuMap(xNew, yNew);
successor._f = successor._g + successor._h;//_f is necessary for building the heap! don't remove it
//add element, and re-build heap
_heap.push_heap(successor);
}
return(-1);
}
}
public heapNode pop_heap()
{
if (_a.Count == 0)
{
return(null);
}
heapNode n = _a[0];
_a[0] = _a[_a.Count - 1];
_a.RemoveAt(_a.Count - 1);
siftDown(0, _a.Count - 1);
return(n);
}
public void push_heap(heapNode n)
{
//prune
if (Global.beamSearch && _a.Count > Global.beamSize)
{
if (n < _a[_a.Count - 1])
{
return;
}
}
_a.Add(n);
int loc = _a.Count - 1;
int root;
heapNode temp;
while (loc != 0)
{
//check n is left child or right child
if (loc % 2 == 0)//right
{
root = (loc - 2) / 2;
}
else
{
root = (loc - 1) / 2;//left
}
if (_a[root] < _a[loc])
{
temp = _a[root];
_a[root] = _a[loc];
_a[loc] = temp;
loc = root;
}
else
{
break;
}
}
//prune
if (Global.beamSearch && _a.Count > Global.beamSize)
{
_a.RemoveAt(_a.Count - 1);
}
}
//a simple test for debug
public static void debug()
{
heapNode a = new heapNode();
a._f = 9;
heapNode b = new heapNode();
b._f = 50;
heapNode c = new heapNode();
c._f = 21;
heapNode d = new heapNode();
d._f = 98;
heapNode e = new heapNode();
e._f = 55;
List <heapNode> tmpList = new List <heapNode>();
tmpList.Add(a);
HeapSort hs = new HeapSort(ref tmpList);
hs.push_heap(b);
hs.push_heap(c);
hs.push_heap(d);
hs.push_heap(e);
hs.pruneToSize(2);
heapNode x = hs.pop_heap();
Console.WriteLine(x._f);
x = hs.pop_heap();
Console.WriteLine(x._f);
x = hs.pop_heap();
Console.WriteLine(x._f);
x = hs.pop_heap();
Console.WriteLine(x._f);
x = hs.pop_heap();
Console.WriteLine(x._f);
}
//should be slow...to improve
public void push_heap_slow(heapNode n)
{
_a.Add(n);
buildHeap();//this should be improved for efficiency!! no need to build half
}