最短路径，迪杰斯特拉算法，三维坐标

题目传送门

题目描述

已知了飞行器的起点和终点以及n个休息站的坐标，问起点到终点的最短路径是多少？

限制：飞行器不能长期飞行，必须中途在某结点下停下休息。（即连续飞行距离应不大于m）

欧涛师兄很想在师妹面前大展身手，你能帮助他解决这个问题吗？

输入描述

第一行输入两个数，整数n和浮点数m

第二行输入六个浮点数x1,y1,z1,x2,y2,z2。分别代表起点坐标（x1,y1,z1）和终点坐标(x2,y2,z2)

紧接着下面n行，每行依次输入三个浮点数，代表休息站的坐标（ai,bi,ci），休息站编码依次为1,2……n。

输出描述

输出满足条件的起点到终点的最短距离长度（结果保留三位小数）。

依次输出飞行器经过站台的编码（休息站编码为1到n，起点编码Start，终点编码End）

若不能到达终点输出“-1”（无双引号）

示例

输入
4 5
0 0 0 6 6 0
-1 1 0
5 6 0
3 4 0
6 1 0
输出
8.606
Start 3 End
注
n<=600

代码如下：

#include<iostream>
#include<algorithm>
#include<cstring>
#include<cmath>
using namespace std;
const int maxv = 610;
const int inf = 10000000;
double G[maxv][maxv];
double x[maxv],y[maxv],z[maxv];
double d[maxv];
int vis[maxv];
int n;
double m;
int pre[maxv];
void dijketra(){
    fill(d,d+maxv,inf*1.0);
    d[0] = 0;
    for(int i = 0;i <= n+1; i++){
        pre[i] = i;
    }
    for(int i = 0;i <= n+1; i++){
        int u = -1;
        double min = inf*1.0;
        for(int j = 0;j <= n+1; j++){
            if(!vis[j]&&d[j]<min){
                u = j;
                min = d[j];
            }
        }
        if(u==-1) return;
        vis[u] = 1;
        for(int v = 0;v <= n+1; v++){
            if(!vis[v]&&G[u][v]+d[u]<d[v]){
                d[v] = G[u][v] + d[u];
                pre[v] = u;
            }
        }
    }
}
void DFS(int s,int v){
    if(s==v){
        printf("Start");
        return;
    }
    DFS(s,pre[v]);
    if(v==n+1){
        printf(" End");
    }else{
        printf(" %d",v);
    }
}
int main(){
    scanf("%d%lf",&n,&m);
    fill(G[0],G[0]+maxv*maxv,inf*1.0);
    memset(vis,0,sizeof(vis));
    scanf("%lf%lf%lf",&x[0],&y[0],&z[0]);
    scanf("%lf%lf%lf",&x[n+1],&y[n+1],&z[n+1]);
    double tempx,tempy,tempz,temp;
    for(int i = 1;i <= n; i++)
        scanf("%lf%lf%lf",&x[i],&y[i],&z[i]);
    for(int i = 0;i <= n+1; i++){
        for(int j = 0;j <= n+1; j++){
            tempx = (x[i]-x[j])*(x[i]-x[j]);
            tempy = (y[i]-y[j])*(y[i]-y[j]);
            tempz = (z[i]-z[j])*(z[i]-z[j]);
            temp = sqrt(tempx+tempy+tempz);
            if(temp<=m*1.0000001&&i!=j){
                G[i][j] = G[j][i] = temp;
            }else if(i==j){
                G[i][j] = G[j][i] = 0;
            }
        }
    }
    dijketra();
    if(d[n+1]==inf*1.0){
        printf("-1\n");
    }else{
        printf("%.3lf\n",d[n+1]);
        DFS(0,n+1);
    }
    system("pause");
    return 0;
}