Problem p03866 (C++)

Buggy submission (status: Time Limit Exceeded):
#include <bits/stdc++.h>
using namespace std;

#define int long long
typedef vector<int> vint;
typedef pair<int, int> pint;
typedef vector<pint> vpint;
#define rep(i, n) for (int i = 0; i < (n); i++)
#define reps(i, f, n) for (int i = (f); i < (n); i++)
#define all(v) (v).begin(), (v).end()
#define each(it, v)                                                            \
  for (__typeof((v).begin()) it = (v).begin(); it != (v).end(); it++)
#define pb push_back
#define fi first
#define se second
template <typename A, typename B> inline void chmin(A &a, B b) {
  if (a > b)
    a = b;
}
template <typename A, typename B> inline void chmax(A &a, B b) {
  if (a < b)
    a = b;
}

#define D double
typedef pair<D, int> T;
int N;
int X[1111], Y[1111], R[1111];
D dist[1111];
D INF = 1e10;
D EPS = 1e-9;

int table2[1111][1111];
D table[1111][1111];

signed main() {

  cin >> X[0] >> Y[0] >> X[1] >> Y[1];
  cin >> N;
  rep(i, N) cin >> X[i + 2] >> Y[i + 2] >> R[i + 2];
  N += 2;

  rep(i, N) rep(j, N) {
    table2[i][j] =
        (X[i] - X[j]) * (X[i] - X[j]) + (Y[i] - Y[j]) * (Y[i] - Y[j]);
    table[i][j] = sqrt(1.0 * table2[i][j]);
  }

  fill_n(dist, 1111, INF);
  dist[0] = 0;
  priority_queue<T, vector<T>, greater<T>> que;
  que.push(make_pair(0, 0));
  while (que.size()) {
    D d;
    int idx;
    tie(d, idx) = que.top();
    que.pop();
    if (dist[idx] != d)
      continue;
    rep(i, N) {
      int foo = table2[i][idx];
      int bar = R[i] + R[idx];
      D cost = d;
      if (foo > bar * bar)
        cost += sqrt(1.0 * foo) - bar;
      if (cost < d)
        cost = d;
      if (dist[i] < EPS + cost)
        continue;
      dist[i] = cost;
      que.push(T(dist[i], i));
    }
  }
  printf("%.20f\n", dist[1]);
  return 0;
}


Fixed submission:
#include <bits/stdc++.h>
using namespace std;

#define int long long
typedef vector<int> vint;
typedef pair<int, int> pint;
typedef vector<pint> vpint;
#define rep(i, n) for (int i = 0; i < (n); i++)
#define reps(i, f, n) for (int i = (f); i < (n); i++)
#define all(v) (v).begin(), (v).end()
#define each(it, v)                                                            \
  for (__typeof((v).begin()) it = (v).begin(); it != (v).end(); it++)
#define pb push_back
#define fi first
#define se second
template <typename A, typename B> inline void chmin(A &a, B b) {
  if (a > b)
    a = b;
}
template <typename A, typename B> inline void chmax(A &a, B b) {
  if (a < b)
    a = b;
}

#define D double
typedef pair<D, int> T;
int N;
int X[1111], Y[1111], R[1111];
D dist[1111];
D INF = 1e10;
D EPS = 1e-9;

int table2[1111][1111];
D table[1111][1111];

signed main() {

  cin >> X[0] >> Y[0] >> X[1] >> Y[1];
  cin >> N;
  rep(i, N) cin >> X[i + 2] >> Y[i + 2] >> R[i + 2];
  N += 2;

  rep(i, N) rep(j, N) {
    table2[i][j] =
        (X[i] - X[j]) * (X[i] - X[j]) + (Y[i] - Y[j]) * (Y[i] - Y[j]);
    table[i][j] = sqrt(1.0 * table2[i][j]);
  }

  fill_n(dist, 1111, INF);
  dist[0] = 0;
  priority_queue<T, vector<T>, greater<T>> que;
  que.push(make_pair(0, 0));
  while (que.size()) {
    D d;
    int idx;
    tie(d, idx) = que.top();
    que.pop();
    if (dist[idx] != d)
      continue;
    rep(i, N) {
      int foo = table2[i][idx];
      int bar = R[i] + R[idx];
      D cost = d;
      if (foo > bar * bar)
        cost += sqrt(1.0 * foo) - bar;
      if (cost < d)
        cost = d;
      if (dist[i] <= cost)
        continue;
      dist[i] = cost;
      que.push(T(dist[i], i));
    }
  }
  printf("%.20f\n", dist[1]);
  return 0;
}