第四个点老是错，太离谱了！

萌新求助！

#include<bits/stdc++.h>
#define int long long
#define inf 1e10
using namespace std;
struct node{
	int index;
	double data;
};
int x[3010], y[3010], r[3010];
double dis[3010], vis[3010];
double mp[3010][3010];
int n; // n <= 1000 直接连边 
void Dijkstra(int s) {
	for(int i=0; i<=n+11; ++i) {
		dis[i] = 2147483647;
	}
	dis[s] = 0;
	priority_queue<pair<double,int> > que;
	que.push({0,s});
	while(!que.empty()) {
		pair<int,int> temp = que.top(); que.pop();
//		cout << temp.second << " " << temp.first << endl;
		if(vis[temp.second]){
			continue;
		}
		vis[temp.second] = 1;
		for(int i=0; i<=n+1; ++i) {
			if(!vis[i]) {
				if(dis[temp.second] + mp[temp.second][i] < dis[i]) {
					dis[i] = dis[temp.second] + mp[temp.second][i];
					que.push({-dis[i], i});
				}
			}
		}
	}
}
signed main() {
	double xs, ys, xt, yt;
	cin >> xs >> ys >> xt >> yt;
	x[0] = xs, y[0] = ys;
	cin >> n;
	x[n+1] = xt, y[n+1] = yt;
	r[0] = r[n+1] = 0;
	for(int i=1; i<=n; ++i) {
		cin >> x[i] >> y[i] >> r[i];
	}
	for(int i=0; i<=n+1; ++i) {
		for(int j=0; j<=n+1; ++j) {
			double l = sqrt((x[i] - x[j]) * (x[i] - x[j]) + (y[i] - y[j]) * (y[i] - y[j]));
			if(l <= r[i]+r[j]) {
				mp[i][j] = 0;
			}
			else
			{
				mp[i][j] = l - r[i] - r[j];
			}
		}
	}
	Dijkstra(0);
	printf("%.10lf", dis[n+1]);
	return 0;
}