A star

最新推荐文章于 2023-10-28 18:24:51 发布

T_W_S

最新推荐文章于 2023-10-28 18:24:51 发布

阅读量720

点赞数

CC 4.0 BY-SA版权

分类专栏：算法导论

本文链接：https://blog.youkuaiyun.com/T_W_S/article/details/8709384

算法导论专栏收录该内容

14 篇文章

订阅专栏

本文介绍了一种基于A*算法的智能寻路策略，适用于解决在障碍物密集的环境中快速找到从起点到终点的最短路径的问题。通过详细解析算法原理、实现步骤以及在实际场景中的应用案例，旨在为开发者提供一种高效、实用的路径规划解决方案。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

#include <iostream>
#include <list>
#include <math.h>
#include <LIMITS>
using namespace std;

#define OPEN 0
#define BREAK 1
#define START_END 5
#define PATH_NODE 8

struct point{
	int x;
	int y;
};

struct node{
	point pt;
	int f;//total
	int h;//real
	int g;//guji
	node* father;
};

	const static point startP={2,0};
	const static point endP={2,6};

	list<node*> pOpenList;
	list<node*> pColseList;

	int data[5][7];



void showData()
{
	for(int i=0; i<5; i++)
	{
		for(int j=0; j<7; j++)
			if(data[i][j]==BREAK)
				cout<<(char)3<<" ";
			else if(data[i][j]==START_END)
				cout<<(char)2<<" ";
			else if(data[i][j]==PATH_NODE)
				cout<<(char)2<<" ";
			else
				cout<<"|"<<" ";
		cout<<endl;
	}
		cout<<endl;
}


void readData( int a[5][7])
{
		for(int i=0; i<5; i++)
			for(int j=0; j<7; j++)
				a[i][j]=0;
		a[1][4]=a[2][4]=a[3][4]=a[4][4]=1;
		a[2][0]=a[2][6]=5;

		showData();
}

node* createNode( point p, node* father) 
{
	node* q=new node;
	q->father=father;
	q->pt=p;
	

	int r=(p.x - father->pt.x)+(p.y - father->pt.y);
	if(r==1 || r==-1)
		q->h=father->h+10;
	else
		q->h=father->h+14;
	

	int rest=fabs(endP.x-p.x)*10+fabs(endP.y-p.y)*10;
	q->g=rest;
	

	q->f=q->g+q->h;
	
	return q;
}


void addToOpenList(int x, int y, node* s)
{
	//地图越界		
	if(x<0 || x>4)return;
	if(y<0 || y>6)return;

	//todo.....
	//在这个地方添加障碍物的判断信息
	//
	//
	if(data[x][y]==BREAK)return;

	//在close列表中
	for(list<node*>::iterator a=pColseList.begin(); a!=pColseList.end(); a++)
	{
		if((*a)->pt.x==x && (*a)->pt.y==y)
			return;
	}

	int h;
	int add;
	fabs(s->pt.x+s->pt.y-x-y)==1 ? add=10 : add=14;
	h=s->h+add;

	for(list<node*>::iterator b=pOpenList.begin(); b!=pOpenList.end(); b++)
	{
		if((*b)->pt.x==x && (*b)->pt.y==y)
		{
			
			if(h < (*b)->h)
			{
				(*b)->h=h;
				(*b)->f=(*b)->h + (*b)->g;
				(*b)->father=s;
			}
			return;
		}
	}

	point t={x,y};
	node* pp=createNode(t , s);
	pOpenList.push_front(pp);

}

node* search(node* s)
{
	pOpenList.push_front(s);
	while(!pOpenList.empty())
	{
		int minlen=INT_MAX;
		node* nowp;

		for(list<node*>::iterator f=pOpenList.begin(); f!=pOpenList.end(); f++)
		{
			if((*f)->pt.x==endP.x && (*f)->pt.y==endP.y)
				return (*f);
			if((*f)->f<minlen)
			{
				minlen=(*f)->f;
				nowp=*f;
			}
		}
		
		pOpenList.remove(nowp);
		pColseList.push_front(nowp);
		addToOpenList(nowp->pt.x-1,nowp->pt.y-1,nowp);
		addToOpenList(nowp->pt.x-1,nowp->pt.y,nowp);
		addToOpenList(nowp->pt.x-1,nowp->pt.y+1,nowp);
		addToOpenList(nowp->pt.x,nowp->pt.y-1,nowp);
		addToOpenList(nowp->pt.x,nowp->pt.y+1,nowp);
		addToOpenList(nowp->pt.x+1,nowp->pt.y-1,nowp);
		addToOpenList(nowp->pt.x+1,nowp->pt.y,nowp);
		addToOpenList(nowp->pt.x+1,nowp->pt.y+1,nowp);
	}
	return false;
}



void main()
{
	readData(data);

	node* startNode=new node;
	startNode->father=NULL;
	startNode->pt=startP;
	startNode->g=fabs(startP.x-endP.x)*10+fabs(startP.y-endP.y)*10;
	startNode->h=0;
	startNode->f=startNode->g;

	node* fff=search(startNode);


	if(fff)
	{
		node* ppt=fff;
		while(ppt)
		{
			cout<<"("<<ppt->pt.x<<","<<ppt->pt.y<<")"<<endl;;
			data[ppt->pt.x][ppt->pt.y]=PATH_NODE;
			ppt=ppt->father;
		}
	}
	showData();
}