基于二叉堆的ASTAR算法演示程序

最新推荐文章于 2021-10-21 11:23:49 发布

ahccom

最新推荐文章于 2021-10-21 11:23:49 发布

阅读量1k

点赞数 1

文章标签：算法 null delete random list output

本文链接：https://blog.csdn.net/ahccom/article/details/8026172

版权

具体原理可以参考

http://www.policyalmanac.org/games/binaryHeaps.htm,花了大半天时间做的，有点简陋，内存泄漏问题请自行解决。

以下是我的一个简单实现：

附代码：
AStar.h

#ifndef _ASTART_H_
#define _ASTART_H_

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>

#include "binaryheap.h"

class AStarNode
{
public:
	AStarNode(void)
		: parent(NULL)
		, child(NULL)
		, G(0)
		, H(0)
		, F(0)
		, b(0)
		, w(0)
		, h(0)
		, id(0)
		, s(0)
	{

	}
	AStarNode(int blocked, int width, int height, int id)
		: parent(NULL)
		, child(NULL)
		, G(0)
		, H(0)
		, F(0)
		, b(blocked)
		, w(width)
		, h(height)
		, id(id)
		, s(0)
	{

	}
	~AStarNode(void)
	{

	}
/*
	AStarNode& operator=(AStarNode& r)
	{
		if( this!=&r )
		{

		}
	}
*/
	AStarNode *parent;
	AStarNode *child;

	int G;
	int H;
	int F;

	int b; //0:non blocked; 1:blocked
	int w;
	int h;
	int id;
	int s; //0:free; 1:openlist 2:closelist
};

#define COST 10
#define MAX_WIDTH_SIZE  9
#define MAX_HEIGHT_SIZE 9
class AStarAlgorithm
{
public:
	AStarAlgorithm(void)
		: openArray(this)
		, closeArray(this)
		, currNode(NULL)
		, startNode(NULL)
		, endNode(NULL)
		, searchTime(0)
		, isStop(false)
	{
		map = new AStarNode*[MAX_WIDTH_SIZE*MAX_HEIGHT_SIZE];
	}
	~AStarAlgorithm(void)
	{
		for(int i=0; i<MAX_WIDTH_SIZE*MAX_HEIGHT_SIZE; i++)
			delete map[i];
		delete[] map;
	}

	// return the Target if its has been searched,either return NULL.
	AStarNode* AStarSearchRun(AStarNode* start, AStarNode* end);
	
	void AStarSearchInit(AStarNode* start, AStarNode* end);
	void AStarSearchStep(void);

	//
	void TestCreateMap(int w, int h, int rand);

	void TravelOutputResult(void);
	AStarNode* getCurrNode(void);

	AStarNode* getNode(int w, int h);
	AStarNode* getNode(int id);
	bool IsStop(void) { return isStop; }
private:
	int getF(int w, int h);
	int getF(int id);

	AStarNode* allocNode(int b, int w, int h, int id);
	void calculateNode(AStarNode* node);
	// choose the lowest F node.
	AStarNode* getLowestOpenNode(void);

	AStarNode* getOpenNode(AStarNode* node);
	AStarNode* addOpenNode(AStarNode* node);
	AStarNode* delOpenNode(AStarNode* node);
	AStarNode* getCloseNode(AStarNode* node);
	AStarNode* addCloseNode(AStarNode* node);
	AStarNode* delCloseNode(AStarNode* node);

	void outputInfo(const char* extra);

	BinaryHeap openArray;
	BinaryHeap closeArray;

	AStarNode* currNode;
	AStarNode* startNode;
	AStarNode* endNode;

	AStarNode** map;

	int searchTime;
	int isStop;
};

#endif

AStar.cpp

#include "stdafx.h"
#include "AStar.h"

int AStarAlgorithm::getF(int w, int h)
{
	if( w>=0&&w<MAX_WIDTH_SIZE && h>=0&&h<MAX_HEIGHT_SIZE)
	{
		return map[h*MAX_WIDTH_SIZE+w]->F;
	}
	return 0;
}

int AStarAlgorithm::getF(int id)
{
	if( id<MAX_WIDTH_SIZE*MAX_HEIGHT_SIZE )
	{
		return map[id]->F;
	}
	return 0;
}

AStarNode* AStarAlgorithm::getNode(int w, int h)
{
	if( w>=0&&w<MAX_WIDTH_SIZE && h>=0&&h<MAX_HEIGHT_SIZE)
	{
		return map[h*MAX_WIDTH_SIZE+w];
	}
	return NULL;
}

AStarNode* AStarAlgorithm::getNode(int id)
{
	if( id<MAX_WIDTH_SIZE*MAX_HEIGHT_SIZE && 0<=id )
	{
		return map[id];
	}
	return NULL;
}

AStarNode* AStarAlgorithm::allocNode(int blocked, int width, int height, int id)
{
	return new AStarNode(blocked, width, height, id);
}

// choose the lowest F node.
AStarNode* AStarAlgorithm::getLowestOpenNode(void)
{
	AStarNode* r = getNode(openArray.Minimum());
	delOpenNode(r);
	addCloseNode(r);
	return r;
}

AStarNode* AStarAlgorithm::getOpenNode(AStarNode* node)
{
	return node->s==1?node:NULL;
}

AStarNode* AStarAlgorithm::addOpenNode(AStarNode* node)
{
	node->s = 1;
	openArray.MinInsert(openArray.length+1, node->id);
	return node;
}

AStarNode* AStarAlgorithm::delOpenNode(AStarNode* node)
{
	if( !node ) return NULL;
	node->s = 0;
	int id = openArray.MinExtract();
	return getNode(id);
}

AStarNode* AStarAlgorithm::getCloseNode(AStarNode* node)
{
	return node->s==2?node:NULL;
}

AStarNode* AStarAlgorithm::addCloseNode(AStarNode* node)
{
	if( !node ) return NULL;
	node->s = 2;
	closeArray.MinInsert(closeArray.length+1, node->id);
	return node;
}

AStarNode* AStarAlgorithm::delCloseNode(AStarNode* node)
{
	node->s = 0;
	int id = closeArray.MinExtract();
	return getNode(id);
}
AStarNode* AStarAlgorithm::getCurrNode(void)
{
	return currNode;
}
void AStarAlgorithm::AStarSearchInit(AStarNode* start, AStarNode* end)
{
	searchTime = 0;
	if( !start || !end ) return;
	if( start==end ) return;
	startNode = start;
	endNode = end;

	//1.initiate the start node to open list.
	addOpenNode(startNode);
}
void AStarAlgorithm::AStarSearchStep(void)
{
	if( !startNode || !endNode ) return;
	currNode = getLowestOpenNode();
	if( currNode && currNode!=endNode )
	{
		searchTime++;
		//2.traversal all around start node
		for(int i=-1; i<=1; i++) // width
		{
			for(int j=-1; j<=1; j++) // height
			{
				if(currNode->w+i>=0 && currNode->w+i<MAX_WIDTH_SIZE 
					&& currNode->h+j>=0 && currNode->h+j<MAX_HEIGHT_SIZE)
				{
					AStarNode* n = getNode(currNode->w+i, currNode->h+j);
					if( n && !n->b && !getCloseNode(n) )
					{
						//3.calc the G,H,F
						calculateNode(n);
					}
				}
			}
		}

		char info[128];
		sprintf(info, "step:%d", searchTime);
		outputInfo(info);
	}
	else
		isStop = true;
}
AStarNode* AStarAlgorithm::AStarSearchRun(AStarNode* start, AStarNode* end)
{
	searchTime = 0;
	if( !start || !end ) return NULL;
	if( start==end ) return end;
	startNode = start;
	endNode = end;

	//1.initiate the start node to open list.
	addOpenNode(startNode);
	currNode = getLowestOpenNode();
	while( currNode && currNode!=endNode )
	{
		searchTime++;
		//2.traversal all around start node
		for(int i=-1; i<=1; i++) // width
		{
			for(int j=-1; j<=1; j++) // height
			{
				if(currNode->w+i>=0 && currNode->w+i<MAX_WIDTH_SIZE 
					&& currNode->h+j>=0 && currNode->h+j<MAX_HEIGHT_SIZE)
				{
					AStarNode* n = getNode(currNode->w+i, currNode->h+j);
					if( n && !n->b && !getCloseNode(n) )
					{
						//3.calc the G,H,F
						calculateNode(n);
					}
				}
			}
		}
		currNode = getLowestOpenNode();
		outputInfo("step 3");
	}
	return 0;
}

void AStarAlgorithm::calculateNode(AStarNode* node)
{
	if( !node ) return;

	int delta = 0;
	if( abs(currNode->w-node->w)+abs(currNode->h-node->h)>1 )		//Diagonal distance
		delta = 4;

	
	if( getOpenNode(node) )
	{
		int g = currNode->G + COST + delta;
		if( g<=node->G )
		{
			node->G = g;
			node->parent = currNode;
			node->H = abs(node->w - endNode->w)*COST + abs(node->h - endNode->h)*COST;
			node->F = node->G + node->H;
		}
	}
	else
	{
		node->G = currNode->G + COST + delta;
		node->parent = currNode;
		node->H = abs(node->w - endNode->w)*COST + abs(node->h - endNode->h)*COST;
		node->F = node->G + node->H;
		// add to open list
		addOpenNode(node);
	}
}

void AStarAlgorithm::outputInfo(const char* extra)
{
	printf("%s:\n", extra);
	printf("start(id:%d,F:%d,G:%d,H:%d),end(id:%d,F:%d,G:%d,H:%d), curr(id:%d,F:%d,G:%d,H:%d)\n",
		startNode->id,startNode->F,startNode->G,startNode->H,
		endNode->id,endNode->F,endNode->G,endNode->H,
		!currNode?-1:currNode->id,!currNode?-1:currNode->F,!currNode?-1:currNode->G,!currNode?-1:currNode->H);

	printf("open list:\n");
	for(int i=0; i<openArray.length; i++)
	{
		AStarNode* an = getNode(openArray.array[i]);
		printf("node(id:%d,F:%d,G:%d,H:%d)\n",
			an->id,an->F,an->G,an->H);
	}
	printf("close list:\n");
	for(int i=0; i<closeArray.length; i++)
	{
		AStarNode* an = getNode(closeArray.array[i]);
		printf("node(id:%d,F:%d,G:%d,H:%d)\n",
			an->id,an->F,an->G,an->H);
	}
}

void AStarAlgorithm::TestCreateMap(int w, int h, int random)
{
	srand((unsigned int)time(NULL));

	for(int i=0; i<w; i++)
	{
		for(int j=0; j<h; j++)
		{
			int blocked = 0;
			int rnd = rand() % random;
		//	if( rnd==(random-1) )
		//		blocked = 1;
		//	else
		//		blocked = 0;
			AStarNode* node = allocNode(blocked, i, j, j*MAX_WIDTH_SIZE+i);
			map[j*MAX_WIDTH_SIZE+i] = node;
			if( node->b )
				printf("node:%d is blocked.\n", node->id);
		}
	}
	
	map[3*MAX_WIDTH_SIZE+4]->b = 1;
	map[4*MAX_WIDTH_SIZE+4]->b = 1;
	map[5*MAX_WIDTH_SIZE+4]->b = 1;
}

void AStarAlgorithm::TravelOutputResult(void)
{
	printf("travel time:%d output node:\n", searchTime);
	AStarNode* p = endNode->parent;
	printf("back node(id:%d,F:%d,G:%d,H:%d)\n",
		endNode->id,endNode->F,endNode->G,endNode->H);
	while( p )
	{
		printf("back node(id:%d,F:%d,G:%d,H:%d)\n",
			p->id,p->F,p->G,p->H);
		p = p->parent;
	}
}

binaryheap

#ifndef BINARY_HEAP_H
#define BINARY_HEAP_H

class AStarNode;
class AStarAlgorithm;

class BinaryHeap
{
	public:
		BinaryHeap(AStarAlgorithm* a);
		~BinaryHeap(void);
		
		void Initiate(int len);
		int Left(int idx);
		int Right(int idx);
		int Parent(int idx);
		int value(int idx);
		
		void MaxHeapify(int idx);
		void BuildMaxHeap(void);
		void MaxInsert(int idx, int id);
		int	 Maximum(void);
		int	 MaxExtract(void);
		void IncreaseKey(int idx, int id);
		
		void MinHeapify(int idx);
		void BuildMinHeap(void);
		void MinInsert(int idx, int id);
		int	 Minimum(void);
		int	 MinExtract(void);
		int	 MinRemove(int idx);
		void DecreaseKey(int idx, int id);

		void HeapSort(bool max=true);
		
		int  getF(int id);
		void setF(int id, int F);

		AStarAlgorithm* algo;
		int* array;
		int length;
};
#endif

binaryheap.cpp

#include "stdafx.h"
#include "binaryheap.h"
#include "AStar.h"

BinaryHeap::BinaryHeap(AStarAlgorithm* a)
: algo(a)
, array(0)
, length(0)
{
	Initiate(MAX_WIDTH_SIZE*MAX_HEIGHT_SIZE);
}
BinaryHeap::~BinaryHeap(void)
{
	delete[] array;
}
		
void BinaryHeap::Initiate(int len)
{
	if( len<1 ) return;
	
	array = new int[len];
	length = 0;
	memset(array, 0, sizeof(int)*len);
}

int BinaryHeap::Left(int index)
{
	index = index;
//	if( index<0 || index*2>=length ) return 0;
	return index*2;
}

int BinaryHeap::Right(int index)
{
//	if( index<0 || (index*2+1)>=length ) return 0;
	return (index*2+1);
}

int BinaryHeap::Parent(int index)
{
//	if( index<1 || index >=length ) return 0;
	return index/2;
}

int BinaryHeap::value(int index)
{
//	if( index<0 || index >=length ) return 0;
	return array[index-1];
}

int  BinaryHeap::getF(int id)
{
	AStarNode* n = algo->getNode(id);
	if( n ) return n->F;
	return 0;
}

void BinaryHeap::setF(int id, int F)
{
	AStarNode* n = algo->getNode(id);
	if( n ) n->F = F;
}

void BinaryHeap::MaxHeapify(int idx)
{
	int l = Left(idx);
	int r = Right(idx);
	int largest = 0;
	if( l<=length && getF(array[l-1])>getF(array[idx-1]) )
	{
		largest = l;
	}
	else
	{
		largest = idx;
	}
	if( r<=length && getF(array[r-1])>getF(array[largest-1]) )
	{
		largest = r;
	}
	if( largest!=idx )
	{
		int tmp = array[idx-1];
		array[idx-1] = array[largest-1];
		array[largest-1] = tmp;
		MaxHeapify(largest);
	}
}
void BinaryHeap::BuildMaxHeap(void)
{
	for(int i=length/2; i>0; i--)
	{
		MaxHeapify(i);
	}
}

void BinaryHeap::MaxInsert(int idx, int id)
{
	length++;
	array[length] = id;
	IncreaseKey(idx, id);
}
int	 BinaryHeap::Maximum(void)
{
	return array[0];
}
int	 BinaryHeap::MaxExtract(void)
{
	if( length<1 ) return 0;
	int max = array[0];
	array[0] = array[length-1];
	length--;
	MaxHeapify(1);
	return max;
}
void BinaryHeap::IncreaseKey(int idx, int id)
{
	if( getF(id)<getF(array[idx-1]) ) return;
	array[idx-1] = id;
	while( idx>1 && getF(array[Parent(idx)-1])<getF(array[idx-1]) )
	{
		int tmp = array[idx-1];
		array[idx-1] = array[Parent(idx)-1];
		array[Parent(idx)-1] = tmp;
		idx = Parent(idx);
	}
}

void BinaryHeap::MinHeapify(int idx)
{
	int l = Left(idx);
	int r = Right(idx);
	int smallest = 0;
	if( l<=length && getF(array[l-1])<getF(array[idx-1]) )
	{
		smallest = l;
	}
	else
	{
		smallest = idx;
	}
	if( r<=length && getF(array[r-1])<getF(array[smallest-1]) )
	{
		smallest = r;
	}
	if( smallest!=idx )
	{
		int tmp = array[idx-1];
		array[idx-1] = array[smallest-1];
		array[smallest-1] = tmp;
		MinHeapify(smallest);
	}
}
void BinaryHeap::BuildMinHeap(void)
{
	for(int i=length/2; i>0; i--)
	{
		MinHeapify(i);
	}
}
void BinaryHeap::MinInsert(int idx, int id)
{
	array[length] = id;
	length++;
	DecreaseKey(idx, id);
}
int	 BinaryHeap::Minimum(void)
{
	return length?array[0]:-1;
}
int	 BinaryHeap::MinExtract(void)
{
	if( length<1 ) return 0;
	int min = array[0];
	array[0] = array[length-1];
	length--;
	MinHeapify(1);
	return min;
}
int	 BinaryHeap::MinRemove(int idx)
{
	if( length<1 ) return 0;
	int min = array[idx-1];
	array[idx-1] = array[length-1];
	length--;
	MinHeapify(1);
	return min;
}
void BinaryHeap::DecreaseKey(int idx, int id)
{
	if( getF(id)>getF(array[idx-1]) ) return;
	array[idx-1] = id;
	while( idx>1 && getF(array[Parent(idx)-1])>getF(array[idx-1]) )
	{
		int tmp = array[idx-1];
		array[idx-1] = array[Parent(idx)-1];
		array[Parent(idx)-1] = tmp;
		idx = Parent(idx);
	}
}

void BinaryHeap::HeapSort(bool max)
{
	if( max )
	{
		BuildMaxHeap();
		for(int i=length; i>=2; i--)
		{
			int tmp = array[1-1];
			array[1-1] = array[i-1];
			array[i-1] = tmp;
			length--;
			MaxHeapify(1);
		}
	}
	else
	{
		BuildMinHeap();
		for(int i=length; i>=2; i--)
		{
			int tmp = array[1-1];
			array[1-1] = array[i-1];
			array[i-1] = tmp;
			length--;
			MinHeapify(1);
		}
	}
}