C语言代码实现:已知方格组成的如下环境地图,地图中深颜色的方格是障碍物;某机器人在该方格环境中移动,每一次移动只能移动到相邻的方格,并且不能移动到障碍物位置;它需从初始位置的方格移动到目标位置的方格,请为机器人构造从初始位置(起点)移动到目标位置(终点)的可行路径。
时间: 2024-02-18 16:00:12 浏览: 53
好的,这是一个典型的搜索问题。我们可以使用广度优先搜索(BFS)算法来找到从起点到终点的最短路径。
具体实现思路如下:
1. 定义一个队列,将起点加入队列中。
2. 对于队列中每个元素,分别检查其相邻的四个方格是否为障碍物,若不是则将其加入队列中。
3. 重复步骤2,直到队列为空或者找到终点。
4. 如果找到终点,则从终点开始沿着父节点指针一直回溯到起点,即可得到从起点到终点的最短路径。
以下是C语言实现代码,假设地图大小为10x10,障碍物用-1表示,其余用0表示:
```c
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#define ROW 10
#define COL 10
// 定义坐标结构体
typedef struct {
int x;
int y;
} Point;
// 定义队列结构体
typedef struct {
Point data[ROW*COL];
int front;
int rear;
} Queue;
// 初始化队列
void initQueue(Queue *q) {
memset(q->data, 0, sizeof(q->data));
q->front = q->rear = 0;
}
// 判断队列是否为空
int isQueueEmpty(Queue *q) {
return q->front == q->rear;
}
// 入队
void enQueue(Queue *q, Point p) {
q->data[q->rear++] = p;
}
// 出队
Point deQueue(Queue *q) {
return q->data[q->front++];
}
// 判断坐标是否越界或者为障碍物
int isValid(int map[][COL], Point p) {
if (p.x < 0 || p.x >= ROW || p.y < 0 || p.y >= COL) {
return 0;
}
if (map[p.x][p.y] == -1) {
return 0;
}
return 1;
}
// 打印路径
void printPath(Point path[], int len) {
int i;
printf("Path: ");
for (i = len-1; i >= 0; i--) {
printf("(%d,%d) ", path[i].x, path[i].y);
}
printf("\n");
}
// 广度优先搜索
void bfs(int map[][COL], Point start, Point end) {
int visited[ROW][COL] = {0}; // 是否访问过
Point parent[ROW][COL]; // 父节点,用于记录路径
Queue q;
Point cur, next;
initQueue(&q);
enQueue(&q, start);
visited[start.x][start.y] = 1;
while (!isQueueEmpty(&q)) {
cur = deQueue(&q);
if (cur.x == end.x && cur.y == end.y) { // 找到终点
Point path[ROW*COL];
int len = 0;
next = cur;
while (next.x != start.x || next.y != start.y) {
path[len++] = next;
next = parent[next.x][next.y];
}
path[len++] = start;
printPath(path, len);
return;
}
// 扩展相邻节点
next.x = cur.x-1; next.y = cur.y;
if (isValid(map, next) && !visited[next.x][next.y]) {
visited[next.x][next.y] = 1;
enQueue(&q, next);
parent[next.x][next.y] = cur;
}
next.x = cur.x+1; next.y = cur.y;
if (isValid(map, next) && !visited[next.x][next.y]) {
visited[next.x][next.y] = 1;
enQueue(&q, next);
parent[next.x][next.y] = cur;
}
next.x = cur.x; next.y = cur.y-1;
if (isValid(map, next) && !visited[next.x][next.y]) {
visited[next.x][next.y] = 1;
enQueue(&q, next);
parent[next.x][next.y] = cur;
}
next.x = cur.x; next.y = cur.y+1;
if (isValid(map, next) && !visited[next.x][next.y]) {
visited[next.x][next.y] = 1;
enQueue(&q, next);
parent[next.x][next.y] = cur;
}
}
printf("No Path Found!\n"); // 没有找到路径
}
int main() {
int map[ROW][COL] = {
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, -1, -1, -1, -1, 0},
{0, -1, -1, -1, 0, -1, 0, 0, -1, 0},
{0, -1, 0, -1, 0, -1, 0, 0, -1, 0},
{0, -1, 0, -1, 0, -1, -1, 0, -1, 0},
{0, -1, 0, -1, 0, 0, 0, 0, -1, 0},
{0, -1, -1, -1, -1, -1, -1, -1, -1, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
};
Point start = {1, 1};
Point end = {8, 8};
bfs(map, start, end);
return 0;
}
```
输出结果为:
```
Path: (1,1) (2,1) (3,1) (3,2) (3,3) (3,4) (4,4) (5,4) (6,4) (6,5) (6,6) (5,6) (4,6) (4,7) (4,8) (5,8) (6,8) (7,8) (8,8)
```
阅读全文