A*启发式搜索

九宫重排

#include <bits/stdc++.h>
#define DAD(x) ((x)>>1)
#define LE(x) ((x)<<1)
#define RT(x) (((x)<<1)|1)
using namespace std;
const int dx[4]={0,0,1,-1};
const int dy[4]={1,-1,0,0};
string ed = "123804765";
int Cost(string s)//曼哈顿距离 
{
	int ans = 0;
	for(int i = 0; i < s.size(); ++i)
	{
		if (s[i] == '0') continue;
		int j = ed.find(s[i]);
		ans += abs(j/3-i/3) + abs(j%3-i%3);//计算曼哈顿距离  横坐标为/3, 纵坐标为%3 
	}
	return ans;
}
struct node{
	string str;
	int dad;
	int h;
	int go;
	int value;
	node(string _st,int gone,int d):str(_st),go(gone),dad(d){h = Cost(_st);value=go+h;}
	node(){}
};
struct queue{
	node data[10000];
	int tot;
	queue(){tot=0;
	}
	node top(){return data[1];}
	void repb(int now){
		if(DAD(now)){
			if(data[now].value<data[DAD(now)].value){
				swap(data[now],data[DAD(now)]);
				repb(DAD(now));
			}
		}
	}
	void push(node now){
		data[++tot]=now;
		delSame();
		repb(tot);
	}
	void rept(int now){
		int ex;
		if(LE(now)==tot) ex=LE(now);
		else if(LE(now)>tot) return;
		else ex=data[LE(now)].value<data[RT(now)].value?LE(now):RT(now);
		if(data[now].value>data[ex].value){
			swap(data[now],data[ex]);
			rept(ex);
		}
	}
	node pop(){
		node temp = data[1];
		data[1] = data[tot--];
		rept(1);
		return temp;
	}
	int empty(){return !tot;}
	void delSame(){
		for(int i=1;i<tot;i++){
			if(data[i].str.compare(data[tot].str)==0){
				if(data[i].value>data[tot].value)data[i]=data[tot];
				tot--;
			} 
		}
	}
}q;
struct stack{
	node bin[1000];
	int tot;
	stack(){tot=0;}
	void push(node nod){bin[++tot]=nod;}
	node pop(){return bin[tot--];}
	int loc(){return tot;}
	bool canPush(node now){
		for(int i=0;i<tot;i++){
			if(bin[i].str.compare(now.str)==0){
				if(bin[i].go-now.go<=1)return false;
			}
		}
		return true;
	}
}vec;
int astar(string beg){
	q.push(node(beg,0,0));
	while(!q.empty()){
		node now = q.top();
		q.pop();
		vec.push(now);
		int thisdad = vec.loc();
		if(!Cost(now.str)) return 1;
		int p = now.str.find('0');
		int nx = p/3;
		int ny = p%3;
		for(int i=0;i<4;i++){
			int nowx = nx + dx[i];
			int nowy = ny + dy[i];
			if(nowx<0||nowx>2||nowy<0||nowy>2) continue;
			swap(now.str[p],now.str[3*nowx+nowy]);
			if(vec.canPush(now))q.push(node(now.str,now.go+1,thisdad));
			swap(now.str[p],now.str[3*nowx+nowy]);
		}
	}	
}
int main()
{
	string st;
	cin>>st;
	if(astar(st)){
		int dd = vec.loc();
		int num=0;
		while(dd){
			for(char ch:vec.bin[dd].str){
				num++;
				cout.width(4);cout<<ch;
				if(num%3==0)cout<<endl;
			}
			cout<<endl;		
			dd = vec.bin[dd].dad;
		}
	};
	return 0;
}