struct HeapStructure{	GridCell* MapElement;	// The type of data stored in the heap	long HeapPosition;		// This is used to determine where in the heap and oject exists, and if/when it is updated this value is used to help 							// reposition it in calles to updateHeap()};class BinaryHeap{public:	BinaryHeap()	{		m_MAX_ELEMENTS = 100000;		m_ElementsInList = 0;		p_mBinaryHeap = new HeapStructure*[m_MAX_ELEMENTS];	}	~BinaryHeap()	{		m_CellsInList.RemoveAll();		for (long LoopCtr = 0; LoopCtr < m_ElementsInList; LoopCtr++)		{			delete p_mBinaryHeap[LoopCtr]->MapElement;			delete p_mBinaryHeap[LoopCtr];		}		delete[] p_mBinaryHeap;	}	int m_MAX_ELEMENTS;	int left(int root) const;	int right(int root) const;	int parent(int child) const;	void insertElement(GridCell* DataElement);	GridCell* removeMin();	// Removes and returns the minimum object from the heap and reheaps the set	void reHeap(int root);	// Reheaps the heap from the root downwards	int count() const;	void clearHeap();		// Clears out the heap	bool checkElementExists(GridCell* DataElement, long *FScore);	const GridCell* getMin();	// Returns the minimum object from the heap, the heap is left unaffected	bool checkHeapValidity();	// Checks that the heap is valid. Ensures that no element is less than the root and the element positions are correct	void updateHeap(int child);	// This will update the heap from a child, bubbling upwards to the root in the case of an in heap updateprivate:	HeapStructure** p_mBinaryHeap;				// The data structure of the heap itself, it holds pointers to HeapStructure objects	C3DHashtable<HeapStructure*> m_CellsInList;	// Hash table used to determine whether an object is already in the heap	long m_ElementsInList;						// Number of elements in the heap};

// Binary Heap implemetation to improve the A* Path finding algrothim's performance for Open and Closed list maintaiance.#include "BinaryHeap.h"#include <vector>using namespace std;int BinaryHeap::left(int root) const{	return (root * 2) + 1;}int BinaryHeap::right(int root) const{	return (root * 2) + 2;}// Calculates the position of a parent of a child int BinaryHeap::parent(int child) const{	return (child - 1) / 2;}// Determines if an element exists in the heapbool BinaryHeap::checkElementExists(GridCell* DataElement, long *FScore){	HeapStructure* lExistingHeapRecordDataItem = NULL;	if (m_CellsInList.Exists(static_cast<int>(DataElement->m_X), static_cast<int>(DataElement->m_Y), 0) == true)	{		lExistingHeapRecordDataItem = m_CellsInList.Retrieve(static_cast<int>(DataElement->m_X), static_cast<int>(DataElement->m_Y), 0);		*FScore =  lExistingHeapRecordDataItem->MapElement->m_FScore;		return true;	}	return false;}// This method will either insert a new element into the heap, or if it finds that the element already exists then it will update // that element and maintain the heap to ensure is the properties are still correct.// It updates both the actual heap and the m_CellsInList, which is just a hash dictionary to record the position and existance of // elements in the heapvoid BinaryHeap::insertElement(GridCell* DataElement){	int new_pos;	HeapStructure* lHeapRecordDataItem = NULL;	HeapStructure* lExistingHeapRecordDataItem = NULL;	if (DataElement->m_X == 6 && DataElement->m_Y == -3)	{		printf("Test Element %d", DataElement->m_FScore);	}	if (m_ElementsInList+1 >= m_MAX_ELEMENTS)	{		printf("Max Array Error %d", m_MAX_ELEMENTS);	}	lExistingHeapRecordDataItem = m_CellsInList.Retrieve(static_cast<int>(DataElement->m_X), static_cast<int>(DataElement->m_Y), 0);	if (lExistingHeapRecordDataItem == NULL)	{		if (m_ElementsInList == 0)		{			lHeapRecordDataItem = new HeapStructure;			lHeapRecordDataItem->MapElement = new GridCell(static_cast<long>(DataElement->m_X), static_cast<long>(DataElement->m_Y), DataElement->m_FScore); 			p_mBinaryHeap[m_ElementsInList] = lHeapRecordDataItem;			lHeapRecordDataItem->HeapPosition = m_ElementsInList;			m_ElementsInList++;		}		else		{			lHeapRecordDataItem = new HeapStructure;			lHeapRecordDataItem->MapElement = new GridCell(static_cast<long>(DataElement->m_X), static_cast<long>(DataElement->m_Y), DataElement->m_FScore); 			p_mBinaryHeap[m_ElementsInList] = lHeapRecordDataItem;			lHeapRecordDataItem->HeapPosition = m_ElementsInList;			new_pos = m_ElementsInList;			m_ElementsInList++;			while((new_pos != 0) && (p_mBinaryHeap[new_pos]->MapElement->m_FScore <= p_mBinaryHeap[parent(new_pos)]->MapElement->m_FScore)) //loop while the item has not become the main root, and while its value is less than its parent			{				swap(p_mBinaryHeap[new_pos], p_mBinaryHeap[parent(new_pos)]); //swap the value of item with its lesser parent				p_mBinaryHeap[new_pos]->HeapPosition = new_pos;				p_mBinaryHeap[parent(new_pos)]->HeapPosition = parent(new_pos);				new_pos = parent(new_pos); //update the item's positions			}			p_mBinaryHeap[new_pos]->HeapPosition = new_pos;			p_mBinaryHeap[parent(new_pos)]->HeapPosition = parent(new_pos);		}		m_CellsInList.Insert(static_cast<int>(lHeapRecordDataItem->MapElement->m_X), static_cast<int>(lHeapRecordDataItem->MapElement->m_Y), 0, lHeapRecordDataItem);	}	else	{		// We're inserting a cell that already exists - in this case we need to re-heap to ensure the heap's		// properly is maintained.		long lTestFScore;				bool lElementExists = checkElementExists(DataElement, &lTestFScore);		if (lElementExists == true)		{			lExistingHeapRecordDataItem = NULL;			lExistingHeapRecordDataItem = m_CellsInList.Retrieve(static_cast<int>(DataElement->m_X), static_cast<int>(DataElement->m_Y), 0);			if ( lExistingHeapRecordDataItem != NULL)			{				// Update the F Score of the Map Element in the search list. This will then be resorted by the call to reHeap() if it is any better then before				//if (lExistingHeapRecordDataItem->MapElement->m_FScore != DataElement->m_FScore)				{					printf("F Score Change Found");					lExistingHeapRecordDataItem->MapElement->m_FScore = DataElement->m_FScore;					if ( p_mBinaryHeap[lExistingHeapRecordDataItem->HeapPosition]->MapElement->m_FScore < p_mBinaryHeap[parent(lExistingHeapRecordDataItem->HeapPosition)]->MapElement->m_FScore )					{						swap(p_mBinaryHeap[parent(lExistingHeapRecordDataItem->HeapPosition)], p_mBinaryHeap[lExistingHeapRecordDataItem->HeapPosition]);						p_mBinaryHeap[lExistingHeapRecordDataItem->HeapPosition]->HeapPosition = lExistingHeapRecordDataItem->HeapPosition;						p_mBinaryHeap[parent(lExistingHeapRecordDataItem->HeapPosition)]->HeapPosition = parent(lExistingHeapRecordDataItem->HeapPosition);												// Perform Heap Maintainance to correct the heap following an items update.						updateHeap(lExistingHeapRecordDataItem->HeapPosition);					}				}			}			else			{				// Not good - we think we have a record of an element, but it wasn't found!				printf("Null Pointer Error on existing element!");			}		}		else		{			printf("Element Does Not Exist, Error!");		}	}}// The purpose of this method is to determine if the heap is valid. There are two things that are checked://// 1. That for ever item in the heap it is less than the root element [0]// 2. That the recorded position of an element is the heap is correct and corresponds to the array position//	  ONLY USE FOR DEBUGGING - obviously completly ruins any performance gains//bool BinaryHeap::checkHeapValidity(){	bool lHeapValid = true;	// For the heap to be valid the zeroth element must be the lowest in the set and 	// the HeapPosition of each element must be correct	for (long loopIndex = 0; loopIndex < m_ElementsInList; loopIndex++)	{		if (p_mBinaryHeap[loopIndex]->MapElement->m_FScore < p_mBinaryHeap[0]->MapElement->m_FScore)		{			lHeapValid = false;			break;		}		if (p_mBinaryHeap[loopIndex]->HeapPosition != loopIndex)		{			lHeapValid = false;			break;		}			}	return lHeapValid;}// Simply returns the minumum cell in the heap. This does not affect the listconst GridCell* BinaryHeap::getMin(){	return p_mBinaryHeap[0]->MapElement;}// This method will remove the minimum element from the heap. The caller must delete the returned GridCell pointer.GridCell* BinaryHeap::removeMin(){	m_ElementsInList--; //update the amount of elements in heap	if(m_ElementsInList != 0) //if we didn't delete the root	{		swap(p_mBinaryHeap[0], p_mBinaryHeap[m_ElementsInList]);		p_mBinaryHeap[0]->HeapPosition = 0;		p_mBinaryHeap[m_ElementsInList]->HeapPosition = -1;	// Using -1 as this is not a valid heap position value, this it's not in use!		reHeap(0);	}	 if ( m_CellsInList.Remove(static_cast<int>(p_mBinaryHeap[m_ElementsInList]->MapElement->m_X), static_cast<int>(p_mBinaryHeap[m_ElementsInList]->MapElement->m_Y), 0) == false)	 {		printf("Error deleting object");		return NULL;	 }	return p_mBinaryHeap[m_ElementsInList]->MapElement;}void BinaryHeap::reHeap(int root){	int child = left(root);	if (child <= (m_ElementsInList-1))	{		if((p_mBinaryHeap[child]->MapElement->m_FScore > p_mBinaryHeap[child+1]->MapElement->m_FScore) && child < (m_ElementsInList-1 )) //if a right child exists, and it's bigger than the left child, it will be used			child++;		if(p_mBinaryHeap[root]->MapElement->m_FScore <= p_mBinaryHeap[child]->MapElement->m_FScore) //if root is bigger than its largest child, stop.				{			p_mBinaryHeap[root]->HeapPosition = root;//parent(new_pos);			p_mBinaryHeap[child]->HeapPosition = child;			return;		}		swap(p_mBinaryHeap[root], p_mBinaryHeap[child]); //swap root and its biggest child		p_mBinaryHeap[root]->HeapPosition = root;//parent(new_pos);		p_mBinaryHeap[child]->HeapPosition = child;		reHeap(child); //continue the process on root's new children	}	else	{		return;	}}// A key method when updating an already existing object in the heap. This will ensure it is corrected following the updatevoid BinaryHeap::updateHeap(int child){	int root = parent(child);	if (child <= (m_ElementsInList-1))	{		//if((p_mBinaryHeap[child]->MapElement->m_FScore > p_mBinaryHeap[child+1]->MapElement->m_FScore) && child < (m_ElementsInList-1 )) //if a right child exists, and it's bigger than the left child, it will be used		//	child++;		if(p_mBinaryHeap[root]->MapElement->m_FScore <= p_mBinaryHeap[child]->MapElement->m_FScore) //if root is bigger than its largest child, stop.				{			p_mBinaryHeap[root]->HeapPosition = root;//parent(new_pos);			p_mBinaryHeap[child]->HeapPosition = child;			return;		}		swap(p_mBinaryHeap[root], p_mBinaryHeap[child]); //swap root and its biggest child		p_mBinaryHeap[root]->HeapPosition = root;//parent(new_pos);		p_mBinaryHeap[child]->HeapPosition = child;		updateHeap(root); //continue the process on root's new children	}	else	{		return;	}}int BinaryHeap::count() const{	return m_ElementsInList;}void BinaryHeap::clearHeap(){	m_ElementsInList = 0;	delete p_mBinaryHeap; }