c语言智能小车避障最优路线代码
时间: 2023-10-12 13:20:05 浏览: 83
以下是一个简单的C语言智能小车避障最优路线代码示例,仅供参考:
```c
#include <stdio.h>
#include <stdlib.h>
#define ROW 10
#define COL 10
int map[ROW][COL] = { {0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 1, 1, 0, 1, 1, 1, 0, 1, 0},
{0, 1, 1, 0, 1, 1, 1, 0, 1, 0},
{0, 1, 1, 1, 1, 0, 0, 1, 1, 0},
{0, 1, 0, 1, 1, 0, 0, 1, 1, 0},
{0, 1, 1, 1, 0, 0, 1, 1, 1, 0},
{0, 1, 0, 1, 1, 1, 1, 0, 1, 0},
{0, 1, 0, 0, 1, 0, 1, 0, 1, 0},
{0, 1, 1, 1, 1, 1, 1, 1, 1, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0} };
int visited[ROW][COL];
int dist[ROW][COL];
struct Queue {
int x, y;
} queue[ROW * COL];
int front = 0, rear = 0;
void enqueue(int x, int y) {
queue[rear].x = x;
queue[rear].y = y;
rear++;
}
struct Queue dequeue() {
front++;
return queue[front - 1];
}
int is_empty() {
return front == rear;
}
void bfs(int sx, int sy, int tx, int ty) {
int dx[4] = { 1, 0, -1, 0 };
int dy[4] = { 0, 1, 0, -1 };
visited[sx][sy] = 1;
dist[sx][sy] = 0;
enqueue(sx, sy);
while (!is_empty()) {
struct Queue cur = dequeue();
for (int i = 0; i < 4; i++) {
int nx = cur.x + dx[i];
int ny = cur.y + dy[i];
if (nx < 0 || nx >= ROW || ny < 0 || ny >= COL)
continue;
if (visited[nx][ny] || map[nx][ny] == 0)
continue;
visited[nx][ny] = 1;
dist[nx][ny] = dist[cur.x][cur.y] + 1;
if (nx == tx && ny == ty)
return;
enqueue(nx, ny);
}
}
}
void print_path(int sx, int sy, int tx, int ty) {
printf("Path: (%d, %d)", sx, sy);
while (sx != tx || sy != ty) {
int dx[4] = { 1, 0, -1, 0 };
int dy[4] = { 0, 1, 0, -1 };
for (int i = 0; i < 4; i++) {
int nx = sx + dx[i];
int ny = sy + dy[i];
if (nx < 0 || nx >= ROW || ny < 0 || ny >= COL)
continue;
if (visited[nx][ny] && dist[nx][ny] == dist[sx][sy] - 1) {
printf(" -> (%d, %d)", nx, ny);
sx = nx;
sy = ny;
break;
}
}
}
printf("\n");
}
int main() {
int sx = 1, sy = 1;
int tx = 8, ty = 8;
bfs(sx, sy, tx, ty);
printf("Shortest distance: %d\n", dist[tx][ty]);
print_path(sx, sy, tx, ty);
return 0;
}
```
这个示例代码使用BFS算法来查找从起点到终点的最短路径。在这个示例中,地图由一个二维数组表示,其中0表示障碍物,1表示可以通过。起点和终点也由它们在二维数组中的坐标表示。程序输出最短距离和最优路径。
相关推荐
![-](https://csdnimg.cn/download_wenku/file_type_column_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)