C++ 三维点群数据 经过每个点最小路径 源代码

以下是基于Dijkstra算法实现的C++代码，用于计算三维点云中每个点到其他点的最短路径：

#include <iostream>
#include <vector>
#include <queue>
#include <cmath>

using namespace std;

// 定义三维点结构体
struct Point3d {
    double x, y, z;
};

// 定义边结构体
struct Edge {
    int from, to;
    double dist;
};

// 定义比较函数，用于优先队列排序
struct cmp {
    bool operator() (Edge a, Edge b) {
        return a.dist > b.dist;
    }
};

// 计算两点之间的距离
double distance(Point3d a, Point3d b) {
    return sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2) + pow(a.z - b.z, 2));
}

// Dijkstra算法求解最短路径
void dijkstra(int n, vector<vector<double>> &graph, vector<vector<Edge>> &edges, int s, vector<double> &dist, vector<int> &prev) {
    priority_queue<Edge, vector<Edge>, cmp> pq;    // 定义优先队列，存储边
    dist.resize(n, INFINITY);    // 初始化距离为无穷大
    prev.resize(n, -1);    // 初始化前驱为-1
    dist[s] = 0;    // 起点到起点的距离为0
    pq.push({-1, s, 0});    // 将起点入队

    while (!pq.empty()) {
        Edge e = pq.top();
        pq.pop();
        if (prev[e.to] != -1) continue;    // 如果到该点的最短路径已经确定，则跳过

        prev[e.to] = e.from;    // 更新前驱
        dist[e.to] = e.dist;    // 更新距离

        for (auto next : edges[e.to]) {    // 遍历该点的相邻节点
            if (prev[next.to] == -1) {    // 如果该节点还没有确定最短路径
                pq.push({next.from, next.to, e.dist + next.dist});    // 将该边加入优先队列
            }
        }
    }
}

int main() {
    int n;    // 点的数量
    cin >> n;

    vector<Point3d> points(n);    // 存储点的坐标
    for (int i = 0; i < n; i++) {
        cin >> points[i].x >> points[i].y >> points[i].z;
    }

    vector<vector<Edge>> edges(n);    // 存储边
    vector<vector<double>> graph(n, vector<double>(n, 0));    // 存储邻接矩阵

    // 构建图
    for (int i = 0; i < n; i++) {
        for (int j = i + 1; j < n; j++) {
            double d = distance(points[i], points[j]);    // 计算两点之间的距离
            graph[i][j] = graph[j][i] = d;    // 更新邻接矩阵
            edges[i].push_back({i, j, d});    // 添加边
            edges[j].push_back({j, i, d});
        }
    }

    // 计算每个点到其他点的最短路径
    for (int i = 0; i < n; i++) {
        vector<double> dist;
        vector<int> prev;
        dijkstra(n, graph, edges, i, dist, prev);
        cout << "Point " << i << ":" << endl;
        for (int j = 0; j < n; j++) {
            if (j != i) {
                cout << "    To point " << j << ": " << dist[j] << endl;
            }
        }
    }

    return 0;
}

该代码中，首先读入三维点云中的点坐标，并根据每两个点之间的距离构建邻接矩阵和边列表。然后，对于每个点，使用Dijkstra算法计算其到其他点的最短路径，并将结果输出。