main.cpp

一个基于启发式算法的搬运工求解程序

	int x0;
	int x;
	int y0;
	int y;
	int xr;
	int yr;

	x0 = axisp % col;
	y0 = row - axisp / col;
	x = p % col;
	y = row - p / col;
	xr = (x - x0) * zx[angle / 90] - (y - y0) * zy[angle / 90] + x0;
	yr = (y - y0) * zx[angle / 90] + (x - x0) * zy[angle / 90] + y0;

	return (row - yr) * col + xr;
}

CNode *FindInHash(CNode &m)
{
	CNode *p = &hashtable[m.hashvalue % MAX_HASH];

	while(p->peo_pos != -1)
		if(Same(*p, m))
			return p;
		else
			p = p->next;
	return p;
}

bool DeadLock(CNode &m, int newpos)
{	
	int i;
	bool (*f[])(int) = {Match0, Match1, Match2, Match3};
	bool r = false;

	for(i = 0; i < boxnum; i++)
		if(chessboard[m.boxpos[i]] == '.')
			chessboard[m.boxpos[i]] = 'b';
		else
			chessboard[m.boxpos[i]] = '&';
	for(i = 0; i < 4; i++)
		if(f[i](newpos))
		{
			r = true;
			break;
		}

	for(i = 0; i < boxnum; i++)
		if(chessboard[m.boxpos[i]] == 'b')
			chessboard[m.boxpos[i]] = '.';
		else
			chessboard[m.boxpos[i]] = 'g';

		/*if(!r)
		{
			static string s;
			static int index = 0;


			s = AsString(m);
			fout << row << ' ' << col << ' ' << boxnum << endl;
			if(s.at(newpos + newpos / col) == 'g')
				s.at(newpos + newpos / col) = '!';
			else
				s.at(newpos + newpos / col) = '*';
			s.at(m.peo_pos + m.peo_pos / col) = 'p';
			fout << "index: " << index++ << endl;
			fout << s << endl;
			fout << (r ? "dead" : "notdead") << endl << endl;
		}*/
	return r;
}

void Display(CNode m)
{
	int i;
	CNode now;

	for(i = -1; i < m.pathn; i++)
	{
		cout << "step " << i + 1 << endl;
		if(i == -1)
			now = from;
		else
			now.boxpos[find(now.boxpos, now.boxpos + boxnum, m.path[i * 2]) - now.boxpos] += dp[m.path[i * 2 + 1]];
		cout << AsString(now) << endl;
		cout << "press any key: ";
		cin.get();
		system("cls");
	}
}

unsigned __int64 Hash(CNode &m)
{
	unsigned __int64 r = 0;
	int i;

	for(i = 0; i < boxnum; i++)
		r ^= zobrist_table[m.boxpos[i]][i];

	return r;
}

bool Same(CNode &m, CNode &n)
{
	if(m.hashvalue != n.hashvalue)
		return false;

	int i;
	int peo = n.peo_pos;
	bool valid;

	for(i = 0; i < boxnum; i++)
		if(m.boxpos[i] != n.boxpos[i])
			return false;
	BFS(m, m.peo_pos, &peo, 1, &valid, NULL, true);

	return valid;
}

CNode Run()
{
	priority_queue<CNode> queue;
	CNode top;
	CNode next;
	CNode *p = NULL;
	int i;
	int j;
	int *valid = new int[boxnum * 4];
	bool *reach = new bool[boxnum * 4];	
	int v_n;

	AddToHash(from, NULL);
	queue.push(from);
	while(!queue.empty())
	{
		top = queue.top();
		queue.pop();		

		if(Arrive(top))
		{
			node_cal = node_all - (int)queue.size();
			return top;
		}

		v_n = 0;
		// 判断向哪些箱子的哪些方向上推是合理的
		for(i = 0; i < boxnum; i++)
			for(j = 0; j < 4; j++)
				if(NoBalk(top, top.boxpos[i], j, true)) // 判断箱子能否向j方向走
					valid[v_n++] = top.boxpos[i] + dp[(j + 2) % 4]; // 判断人是否能否走到该方向的后面
		BFS(top, top.peo_pos, valid, v_n, reach, NULL, true);

		for(i = 0; i < boxnum; i++)
			for(j = 0; j < 4; j++)
				if(NoBalk(top, top.boxpos[i], j, true) && reach[find(valid, valid + v_n, top.boxpos[i] + dp[(j + 2) % 4]) - valid])
				{
					next = top;
					next.peo_pos = next.boxpos[i];
					next.boxpos[i] = top.boxpos[i] + dp[j];
					if(DeadLock(next, next.boxpos[i]))						
						continue;
					next.g++;
					if(next.g > MAX_STEP)
						throw CError("路径长度越界");
					UpdateH(next);
					next.path[next.pathn * 2] = top.boxpos[i];
					next.path[next.pathn * 2 + 1] = j;
					next.pathn++;		
					sort(next.boxpos, next.boxpos + boxnum);
					next.hashvalue = Hash(next);
					p = FindInHash(next);
					if(p->peo_pos == -1)
					{
						AddToHash(next, p);
						queue.push(next);
						node_all++;		
					}
				}
	}
	return CNode();
}

void AddToHash(CNode &m, CNode *p)
{
	if(p == NULL)
		AddToHash(m, FindInHash(m));
	else
	{
		*p = m;
		p->next = new CNode();
	}
}

void BFS(CNode &m, int f, int *pos, int pos_n, bool *valid, int *dis, bool consider_box)
{
	queue<CPair> q;
	CPair top;
	static int *go = new int[row * col];
	int i;
	int j;
	int next;
	int t = 0;

	for(i = 0; i < row * col; i++)
		go[i] = 0;
	for(i = 0; i < pos_n; i++)
	{
		go[pos[i]] = -1;
		valid[i] = false;
		if(dis != NULL)
			dis[i] = INF;
	}

	if(go[f] == -1)
	{
		j = int(find(pos, pos + pos_n, f) - pos);
		valid[j] = true;
		if(dis != NULL)
			dis[j] = 0;
		t++;
		go[f] = 1;
	}
	q.push(CPair(f, 0));
	while(!q.empty())
	{
		top = q.front();
		q.pop();

		for(i = 0; i < 4; i++)
			if(NoBalk(m, top.a, i, consider_box))
			{
				next = top.a + dp[i];
				if(go[next] == 1)
					continue;
				if(go[next] == -1)
				{
					j = int(find(pos, pos + pos_n, next) - pos);
					valid[j] = true;
					if(dis != NULL)
						dis[j] = int(top.b) + 1;
					t++;
					if(t == pos_n)
						return;
				}
				go[next] = 1;
				q.push(CPair(next, top.b + 1));
			}
	}
}

bool NoBalk(CNode &m, int pos, int dir, bool consider_box)
{
	int r = pos / col;
	int c = pos % col;
	int p;

	switch(dir)
	{
	case 0:
		r--;
		break;
	case 1:
		c++;
		break;
	case 2:
		r++;
		break;
	case 3:
		c--;
		break;
	default:
		return false;
	}
	if(r < 0 || r >= row || c < 0 || c >= col)
		return false;
	p = r * col + c;
	if(ChessBoard(p, '#') || consider_box && find(m.boxpos, m.boxpos + boxnum, p) - m.boxpos < boxnum)
		return false;
	return true;
}

void Read()
{
	int i;
	int j1 = 0;
	int j2 = 0;
	char ch;
	bool *valid;
	int *pos;
	int *dis;	

	if(fin == NULL)
		throw CError("文件无法打开");
	fin >> row >> col >> boxnum;
	if(!(0 < row && row <= MAX_ROW && 0 < col && col <= MAX_COL))
		throw CError("输入的地图尺寸越界");

	valid = new bool[row * col];
	pos = new int[row * col];
	dis = new int[row * col];
	destpos = new int[boxnum];
	chessboard = new char[row * col];
	zobrist_table = new unsigned __int64 *[row * col];
	for(j1 = 0; j1 < row * col; j1++)
	{
		zobrist_table[j1] = new unsigned __int64[boxnum];
		for(j2 = 0; j2 < boxnum; j2++)
			zobrist_table[j1][j2] = Rand64();
	}	
	j1 = j2 = 0;
	for(i = 0; i < row * col; i++)
	{
		fin >> ch;
		if(ch == '#' || ch == '.')
		{
			chessboard[i] = ch;
			continue;
		}
		if(ch == 'b')
		{
			chessboard[i] = '.';
			from.boxpos[j1++] = i;
			continue;
		}
		if(ch == '&')
		{
			chessboard[i] = 'g';
			destpos[j2++] = i;
			from.boxpos[j1++] = i;
			continue;
		}
		if(ch == 'g')
		{
			chessboard[i] = 'g';
			destpos[j2++] = i;
			continue;
		}
		if(ch == 'p')
		{
			chessboard[i] = '.';
			from.peo_pos = i;
		}
	}
	if(!(j1 == boxnum && j2 == boxnum))
		throw CError("地图中箱子数目和目标位置数目不等");
	for(i = 0; i < row * col; i++)
		if(i / col == 0 || i / col == row - 1 || i % col == 0 || i % col == col - 1)
			if(!ChessBoard(i, '#'))
				throw CError("地图的边界上必须是墙");

	dp[0] = -col;
	dp[1] = 1;
	dp[2] = col;
	dp[3] = -1;

	// BFS 计算min_dis
	min_dis = new int *[boxnum];
	for(i = 0; i < boxnum; i++)
	{
		min_dis[i] = new int[row * col];
		for(j1 = 0; j1 < row * col; j1++)
			min_dis[i][j1] = -1;
	}
	for(i = 0; i < row * col; i++)
		pos[i] = i;
	for(j1 = 0; j1 < boxnum; j1++)
	{
		BFS(from, destpos[j1], pos, row * col, valid, dis, false);
		for(j2 = 0; j2 < row * col; j2++)
			if(valid[j2])
				min_dis[j1][j2] = dis[j2];
	}
	from.g = 0;
	UpdateH(from);	
	from.hashvalue = Hash(from);
}

void UpdateH(CNode &m)
{
	static double *s = new double[boxnum * boxnum * 4];
	static int *people = new int[boxnum];
	static int *box = new int[boxnum];
	static int *dis = new int[boxnum];
	static bool *valid = new bool[boxnum];
	int i;
	int j;

	for(i = 0; i < boxnum; i++)
	{
		people[i] = i;
		box[i] = i + boxnum;
	}
	for(i = 0; i < boxnum; i++)
		for(j = 0; j < boxnum; j++)
		{
			s[people[i] * 2 * boxnum + box[j]] = min_dis[j][m.boxpos[i]];
			if(min_dis[j][m.boxpos[i]] == -1)
				throw CError("min计算错误");
		}

	m.h = MUL * CalH(s, boxnum * 2, people, box, boxnum);
	m.h = int(m.h * (1 + e * (1 - m.g / double(N))));
		//fout << m.h << endl;
}

bool Arrive(CNode &m)
{
	int i;
	static unsigned __int64 des = 0;

	if(des == 0)
		for(i = 0; i < boxnum; i++)
			des ^= zobrist_table[destpos[i]][i];

	if(m.hashvalue != des)
		return false;
	for(i = 0; i < boxnum; i++)
		if(m.boxpos[i] != destpos[i])
			return false;
	return true;
}

unsigned __int64 Rand64()
{
	return rand() ^ ((unsigned __int64)rand() << 15) ^ ((unsigned __int64)rand() << 30) ^ ((unsigned __int64)rand() << 45) ^ ((unsigned __int64)rand() << 60);
}

int CalH(double *s, int n, int *people, int *box, int box_n)
{
	static int size = (n + 2);
	static double *price = new double[size * size];
	static double *cap = new double[size * size];
	static double *value = new double[size * size];
	static int source = size - 2;
	static int sink = size - 1;
	static CMinCostFlow minflow;
	int i;
	int j;

	if(n + 2 != size)
	{
		size = n + 2;
		price = new double[size * size];
		cap = new double[size * size];
		value = new double[size * size];
		source = size - 2;
		sink = size - 1;		
	}

	for(i = 0; i < size * size; i++)
		cap[i] = INF;

	for(i = 0; i < box_n; i++)	
		for(j = 0; j < box_n; j++)
		{
			price[people[i] * size + box[j]] = s[people[i] * (size - 2) + box[j]];
			cap[people[i] * size + box[j]] = 1;
		}
		for(i = 0; i < box_n; i++)
		{
			cap[source * size + people[i]] = 1;
			price[source * size + people[i]] = 0;
		}
		for(i = 0; i < box_n; i++)
		{
			cap[box[i] * size + sink] = 1;
			price[box[i] * size + sink] = 0;
		}

		return (int)minflow.Run(cap, price, value, source, sink, size);
}