c++写一段群体寻路的代码

群体寻路是指一组个体根据一定规则一起移动，达到特定目的地。以下是一个简单的 C 语言代码示例，实现了群体寻路的功能：

#include <stdio.h>
#include <stdlib.h>
#include <time.h>

#define N 10
#define M 10
#define MAX_STEP 10

int map[N][M]; // 地图
int start_x, start_y; // 起点坐标
int end_x, end_y; // 终点坐标

// 初始化地图
void init_map() {
    srand((unsigned int)time(NULL));
    for (int i = 0; i < N; i++) {
        for (int j = 0; j < M; j++) {
            map[i][j] = rand() % 2; // 随机生成障碍
        }
    }
    start_x = rand() % N; // 随机生成起点
    start_y = rand() % M;
    end_x = rand() % N; // 随机生成终点
    end_y = rand() % M;
    map[start_x][start_y] = 0; // 起点和终点不可能是障碍
    map[end_x][end_y] = 0;
}

// 输出地图
void print_map() {
    for (int i = 0; i < N; i++) {
        for (int j = 0; j < M; j++) {
            if (i == start_x && j == start_y) printf("S "); // 起点
            else if (i == end_x && j == end_y) printf("E "); // 终点
            else if (map[i][j] == 1) printf("# "); // 障碍
            else printf(". "); // 空地
        }
        printf("\n");
    }
}

// 判断一个点是否在地图内
int is_inside(int x, int y) {
    return x >= 0 && x < N && y >= 0 && y < M;
}

// 计算两个点之间的距离
int distance(int x1, int y1, int x2, int y2) {
    return abs(x1 - x2) + abs(y1 - y2);
}

// 寻找下一步的移动方向
void find_next_step(int x, int y, int *dx, int *dy) {
    int min_distance = N * M;
    for (int i = -1; i <= 1; i++) {
        for (int j = -1; j <= 1; j++) {
            if (i == 0 && j == 0) continue; // 不考虑原地不动
            int nx = x + i;
            int ny = y + j;
            if (!is_inside(nx, ny) || map[nx][ny] == 1) continue; // 不考虑越界或障碍
            int d = distance(nx, ny, end_x, end_y); // 计算到终点的距离
            if (d < min_distance) { // 取距离最小的点
                min_distance = d;
                *dx = i;
                *dy = j;
            }
        }
    }
}

// 移动群体
void move_group() {
    int x[N], y[N], dx[N], dy[N], step[N]; // 保存每个个体的坐标、移动方向和步数
    for (int i = 0; i < N; i++) {
        x[i] = start_x;
        y[i] = start_y;
        step[i] = 0;
        dx[i] = dy[i] = 0;
    }
    while (1) {
        // 输出每个个体的位置
        for (int i = 0; i < N; i++) {
            printf("(%d, %d) ", x[i], y[i]);
        }
        printf("\n");
        // 判断是否到达终点
        int all_reached = 1;
        for (int i = 0; i < N; i++) {
            if (x[i] != end_x || y[i] != end_y) {
                all_reached = 0;
                break;
            }
        }
        if (all_reached) {
            printf("All reached the end!\n");
            return;
        }
        // 移动每个个体
        for (int i = 0; i < N; i++) {
            if (step[i] < MAX_STEP) { // 没有走到最大步数
                find_next_step(x[i], y[i], &dx[i], &dy[i]); // 寻找下一步的移动方向
                if (dx[i] != 0 || dy[i] != 0) { // 有可行的移动方向
                    x[i] += dx[i];
                    y[i] += dy[i];
                    step[i]++;
                }
            }
        }
    }
}

int main() {
    init_map();
    print_map();
    move_group();
    return 0;
}

在上面的代码中，我们首先初始化了一个 $10 \times 10$ 的地图，其中随机生成了一些障碍、一个起点和一个终点；然后输出了地图，并在控制台中展示了每个个体的位置；接着循环移动每个个体，直到所有个体都到达终点或走到了最大步数为止。在移动每个个体时，我们调用了 `find_next_step` 函数来寻找下一步的移动方向，该函数会考虑到每个个体到终点的距离，并选择距离最近的点作为移动方向。注意，我们在移动每个个体时，需要判断它是否已经达到最大步数，以及它是否有可行的移动方向。