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); }
//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); }
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); } }