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表示可以通过。起点和终点也由它们在二维数组中的坐标表示。程序输出最短距离和最优路径。