Problem p03866 (C++)

Buggy submission (status: Time Limit Exceeded):
#include <algorithm> // min, max, swap, sort, reverse, lower_bound, upper_bound
#include <bitset>    // bitset
#include <cctype>    // isupper, islower, isdigit, toupper, tolower
#include <complex>   // complex
#include <cstdint>   // int64_t, int*_t
#include <cstdio>    // printf
#include <deque>     // deque
#include <iomanip>   // setprecision
#include <iostream>  // cout, endl, cin
#include <map>       // map
#include <math.h>
#include <queue>         // queue, priority_queue
#include <set>           // set
#include <stack>         // stack
#include <string>        // string, to_string, stoi
#include <tuple>         // tuple, make_tuple
#include <unordered_map> // unordered_map
#include <unordered_set> // unordered_set
#include <utility>       // pair, make_pair
#include <vector>        // vector
#define enld '\n'
#define rep(i, n) for (int i = 0; i < (n); i++)
using namespace std;
using ll = long long;
// using P = pair<int,int>;
#pragma GCC target("sse,sse2,sse3,ssse3,sse4,popcnt,abm,mmx,avx,tune=native")
#pragma GCC optimize("Ofast")
constexpr ll INF = 1e18;
constexpr int inf = 1e9;
constexpr ll mod = 1000000007;
constexpr ll mod2 = 998244353;
const double PI = 3.1415926535897932384626433832795028841971;
const int dx[8] = {1, 0, -1, 0, 1, 1, -1, -1};
const int dy[8] = {0, 1, 0, -1, 1, -1, 1, -1};
template <class T> inline bool chmax(T &a, T b) {
  if (a < b) {
    a = b;
    return 1;
  }
  return 0;
}
template <class T> inline bool chmin(T &a, T b) {
  if (a > b) {
    a = b;
    return 1;
  }
  return 0;
}
// ---------------------------------------------------------------------------

typedef double D;
typedef complex<D> P;
using pdi = pair<double, int>;

int main() {
  ios::sync_with_stdio(false);
  cin.tie(nullptr);
  int sx, sy, gx, gy;
  cin >> sx >> sy >> gx >> gy;
  int N;
  cin >> N;
  N += 2;
  vector<P> p(N);
  vector<D> r(N);
  p[0] = P(sx, sy);
  r[0] = 0;
  p[N - 1] = P(gx, gy);
  r[N - 1] = 0;
  for (int i = 1; i < N - 1; i++) {
    double x, y;
    cin >> x >> y >> r[i];
    p[i] = P(x, y);
  }
  vector<vector<D>> G(N, vector<D>(N));
  for (int i = 0; i < N; i++) {
    for (int j = i; j < N; j++) {
      double now = abs(p[i] - p[j]) - (r[i] + r[j]);
      G[i][j] = max(0.0, now);
      G[j][i] = max(0.0, now);
    }
  }
  priority_queue<pdi, vector<pdi>, greater<pdi>> que;
  que.emplace(0, 0);
  vector<double> dist(N, INF);
  dist[0] = 0;
  while (que.size()) {
    double cost;
    int v;
    tie(cost, v) = que.top();
    que.pop();
    for (int i = 0; i < N; i++) {
      double ncost = G[v][i];
      int nv = i;
      if (ncost + cost < dist[nv]) {
        dist[nv] = ncost + cost;
        que.emplace(dist[nv], nv);
      }
    }
  }
  cout << fixed << setprecision(15) << dist[N - 1] << "\n";
  return 0;
}

Fixed submission:
#include <algorithm> // min, max, swap, sort, reverse, lower_bound, upper_bound
#include <bitset>    // bitset
#include <cctype>    // isupper, islower, isdigit, toupper, tolower
#include <complex>   // complex
#include <cstdint>   // int64_t, int*_t
#include <cstdio>    // printf
#include <deque>     // deque
#include <iomanip>   // setprecision
#include <iostream>  // cout, endl, cin
#include <map>       // map
#include <math.h>
#include <queue>         // queue, priority_queue
#include <set>           // set
#include <stack>         // stack
#include <string>        // string, to_string, stoi
#include <tuple>         // tuple, make_tuple
#include <unordered_map> // unordered_map
#include <unordered_set> // unordered_set
#include <utility>       // pair, make_pair
#include <vector>        // vector
#define enld '\n'
#define rep(i, n) for (int i = 0; i < (n); i++)
using namespace std;
using ll = long long;
// using P = pair<int,int>;
#pragma GCC target("sse,sse2,sse3,ssse3,sse4,popcnt,abm,mmx,avx,tune=native")
#pragma GCC optimize("Ofast")
constexpr ll INF = 1e18;
constexpr int inf = 1e9;
constexpr ll mod = 1000000007;
constexpr ll mod2 = 998244353;
const double PI = 3.1415926535897932384626433832795028841971;
const int dx[8] = {1, 0, -1, 0, 1, 1, -1, -1};
const int dy[8] = {0, 1, 0, -1, 1, -1, 1, -1};
template <class T> inline bool chmax(T &a, T b) {
  if (a < b) {
    a = b;
    return 1;
  }
  return 0;
}
template <class T> inline bool chmin(T &a, T b) {
  if (a > b) {
    a = b;
    return 1;
  }
  return 0;
}
// ---------------------------------------------------------------------------

typedef double D;
typedef complex<D> P;
using pdi = pair<double, int>;

int main() {
  ios::sync_with_stdio(false);
  cin.tie(nullptr);
  int sx, sy, gx, gy;
  cin >> sx >> sy >> gx >> gy;
  int N;
  cin >> N;
  N += 2;
  vector<P> p(N);
  vector<D> r(N);
  p[0] = P(sx, sy);
  r[0] = 0;
  p[N - 1] = P(gx, gy);
  r[N - 1] = 0;
  for (int i = 1; i < N - 1; i++) {
    double x, y;
    cin >> x >> y >> r[i];
    p[i] = P(x, y);
  }
  vector<vector<D>> G(N, vector<D>(N));
  for (int i = 0; i < N; i++) {
    for (int j = i; j < N; j++) {
      double now = abs(p[i] - p[j]) - (r[i] + r[j]);
      G[i][j] = max(0.0, now);
      G[j][i] = max(0.0, now);
    }
  }
  priority_queue<pdi, vector<pdi>, greater<pdi>> que;
  que.emplace(0, 0);
  vector<double> dist(N, INF);
  dist[0] = 0;
  while (que.size()) {
    double cost;
    int v;
    tie(cost, v) = que.top();
    que.pop();
    if (dist[v] < cost)
      continue;
    for (int i = 0; i < N; i++) {
      double ncost = G[v][i];
      int nv = i;
      if (ncost + cost < dist[nv]) {
        dist[nv] = ncost + cost;
        que.emplace(dist[nv], nv);
      }
    }
  }
  cout << fixed << setprecision(15) << dist[N - 1] << "\n";
  return 0;
}