sam-route（C++）

待施工

#include <iostream>
using namespace std;

//车的状态
struct State {
	int x;
	int y;
	int step;
};

//建筑物属性
struct Building {
    //左上角的位置
	int x;
	int y;
	//宽高
	int width;
	int height;
	//停车场位置
	int px;
	int py;
};

int dir[4][2] = {
	0,1,
	0,-1,
	1,0,
	-1,0
};

int visited[50][50] = { 0 };
int map[50][50] = { 0 };

//建筑的停车场位置
pair<int, int> buildings[10];
int buildID = 0;//当前建筑的ID

//增加建筑：建筑所在位置设为2，并将停车场位置记录下来
void addBuilding(Building b) {
	for (int i = b.x; i < b.x + b.width; i++) {
		for (int j = b.y; j < b.y + b.height; j++) {
			map[i][j] = 2;
		}
	}
	map[b.px][b.py] = 1;
	buildings[buildID++] = make_pair(b.px, b.py);
}

//获取两建筑间行驶距离
int getDistance(pair<int, int> p1, pair<int, int> p2) {
	State queue[50] = { 0 };
	int head = 0;
	int tail = 0;

	visited[p1.first][p1.second] = 1;
	State init = { p1.first,p1.second,0 };
	queue[tail] = init;
	tail = (tail + 1) % 50;
	while (tail != head) {
		State cur = queue[head];
		head = (head + 1) % 50;

		cout << cur.x << " " << cur.y << " " << cur.step << endl;

		if (cur.x == p2.first && cur.y == p2.second) {
			return cur.step;
		}

		int x, y;
		int step = cur.step + 1;

        //走右边：下边有房子或右下角有房子
		if (map[cur.x + 1][cur.y] == 2 || map[cur.x + 1][cur.y + 1] == 2) {
			x = cur.x;
			y = cur.y + 1;

			if (visited[x][y] == 1) continue;
			visited[x][y] = 1;//这里有问题
			if (map[x][y] != -1 && map[x][y] != 2) {
				State next = { x,y,step };
				queue[tail] = next;
				tail = (tail + 1) % 50;
			}
		}
		//走左边
		if (map[cur.x - 1][cur.y] == 2 || map[cur.x - 1][cur.y - 1] == 2) {
			x = cur.x;
			y = cur.y - 1;
			if (visited[x][y] == 1) continue;
			visited[x][y] = 1;
			if (map[x][y] != -1 && map[x][y] != 2) {
				State next = { x,y,step };
				queue[tail] = next;
				tail = (tail + 1) % 50;
			}
		}
		//走下边
		if (map[cur.x][cur.y - 1] == 2 || map[cur.x + 1][cur.y - 1] == 2) {
			x = cur.x + 1;
			y = cur.y;
			if (visited[x][y] == 1) continue;
			visited[x][y] = 1;
			if (map[x][y] != -1 && map[x][y] != 2) {
				State next = { x,y,step };
				queue[tail] = next;
				tail = (tail + 1) % 50;
			}
		}
		//走上边
		if (map[cur.x][cur.y + 1] == 2 || map[cur.x - 1][cur.y + 1] == 2) {
			x = cur.x - 1;
			y = cur.y;
			if (visited[x][y] == 1) continue;
			visited[x][y] = 1;
			if (map[x][y] != -1 && map[x][y] != 2) {
				State next = { x,y,step };
				queue[tail] = next;
				tail = (tail + 1) % 50;
			}
		}

        //如果在十字路口
		if (map[cur.x + 1][cur.y] != 2 && map[cur.x - 1][cur.y] != 2 && map[cur.x][cur.y + 1] != 2 && map[cur.x][cur.y - 1] != 2)
			for (int i = 0; i < 4; i++) {
				x = cur.x + dir[i][0];
				y = cur.y + dir[i][1];

				if (map[x][y] == -1) continue;
				if (visited[x][y] == 1) continue;

				visited[x][y] = 1;
				State next = { x,y,step };
				queue[tail] = next;
				tail = (tail + 1) % 50;
			}
	}
	return -1;
}

int main() {
	for (int i = 0; i < 50; i++) {
		map[0][i] = -1;
		map[49][i] = -1;
	}
	for (int i = 0; i < 50; i++) {
		map[i][0] = -1;
		map[i][49] = -1;
	}

	while (1) {
		cout << "1：添加建筑" << endl;
		cout << "2：求最短路径" << endl;
		char n;
		cin >> n;
		switch (n)
		{
		case '1':
			Building b;
			cin >> b.x >> b.y >> b.width >> b.height >> b.px >> b.py;
			addBuilding(b);
			break;
		case '2':
			int b1index, b2index;
			cin >> b1index >> b2index;
			cout << getDistance(buildings[b1index], buildings[b2index]);
			break;
		default:
			break;
		}
	}
}