Problem p03866 (C++)

Buggy submission (status: Runtime Error):
#include <bits/stdc++.h>

using namespace std;

#define pb push_back
#define mp make_pair
#define mt make_tuple
#define fi first
#define se second
#define all(a) (a).begin(), (a).end()
#define endl "\n"
#define rep(i, a, b) for (auto(i) = (a); (i) < (b); (i)++)
#define rrep(i, a, b) for (auto(i) = (a); (i) > (b); (i)--)

#define aim_cpo                                                                \
  do {                                                                         \
    ios::sync_with_stdio(false);                                               \
    cin.tie(nullptr);                                                          \
    cout.precision(12);                                                        \
    cout << fixed;                                                             \
  } while (0)

#define LOCAL_INPUT                                                            \
  do {                                                                         \
    FILE *stream1;                                                             \
    stream1 = freopen("/Users/aim_cpo/CLionProjects/competitive/in.txt", "r",  \
                      stdin);                                                  \
    if (stream1 == nullptr)                                                    \
      return 0;                                                                \
  } while (0)
#define LOCAL_OUTPUT                                                           \
  do {                                                                         \
    FILE *stream2;                                                             \
    stream2 = freopen("out.txt", "w", stdout);                                 \
    if (stream2 == nullptr)                                                    \
      return 0;                                                                \
  } while (0)

#ifdef LOCAL_DEFINE
#define show(x) cerr << #x << " = " << (x) << endl
#define dump(x)                                                                \
  cerr << #x << " = " << (x) << " (line " << __LINE__ << ")" << endl
#define showV(v, n)                                                            \
  do {                                                                         \
    for (int _i_t_e_ = 0; _i_t_e_ < (n); _i_t_e_++)                            \
      cerr << "(" << _i_t_e_ << " = " << (v)[_i_t_e_] << ") ";                 \
    cerr << endl;                                                              \
  } while (0)
#define showA(a, n)                                                            \
  do {                                                                         \
    for (int _i_t_e_ = 0; _i_t_e_ < (n); _i_t_e_++)                            \
      cerr << "(" << _i_t_e_ << " = " << (a)[_i_t_e_] << ") ";                 \
    cerr << endl;                                                              \
  } while (0)
#define showA2(a, n, m)                                                        \
  do {                                                                         \
    for (int _i_t_e_ = 0; _i_t_e_ < (n); _i_t_e_++) {                          \
      for (int _i_t_e_2 = 0; _i_t_e_2 < (m); _i_t_e_2++) {                     \
        cerr << "(" << _i_t_e_ << ", " << _i_t_e_2 << " = "                    \
             << (a)[_i_t_e_][_i_t_e_2] << ") ";                                \
      }                                                                        \
      cerr << endl;                                                            \
    }                                                                          \
    cerr << endl;                                                              \
  } while (0)
#else
#define show(x)
#define dump(x)
#define showV(v, n)
#define showA(a, n)
#define showA2(a, n, m)
#endif

typedef long long ll;
typedef unsigned long long ull;

constexpr const int INT_INF = 0x3f3f3f3f;       // 1061109567
constexpr const ll LL_INF = 0x3f3f3f3f3f3f3f3f; // 4557430888798830399

template <typename T> bool chmin(T &a, T b) {
  return a > b ? (a = b, true) : false;
}
template <typename T> bool chmax(T &a, T b) {
  return a < b ? (a = b, true) : false;
}
template <typename T> void ln(T i, T n) { cout << (i == n - 1 ? "\n" : " "); }
template <typename T, typename S>
ostream &operator<<(ostream &out, const pair<T, S> &pair1) {
  out << '(' << pair1.fi << ", " << pair1.se << ')';
  return out;
}
template <typename T>
ostream &operator<<(ostream &out, const vector<T> &vector1) {
  out << '[';
  for (unsigned int i = 0; i < vector1.size(); i++) {
    out << vector1[i];
    if (i == vector1.size() - 1)
      out << "]";
    else
      out << ", ";
  }
  return out;
}

// INT
#define GCD(a, b) __gcd(a, b)
template <typename T> T LCM(T a, T b) { return a / GCD(a, b) * b; }
template <typename T> T EXTGCD(T a, T b, T &x, T &y) {
  T d = a;
  if (b != 0) {
    d = EXTGCD(b, a % b, y, x);
    y -= (a / b) * x;
  } else
    x = 1, y = 0;
  return d;
}
template <typename T> bool is_prime(T a) {
  for (int i = 2; i * i <= a; i++)
    if (a % i == 0)
      return 1;
  return 0;
}
template <typename T, typename S> T Pow(T a, S b) {
  T res = 1, now = a;
  while (b) {
    if (b & 1)
      res *= now;
    b >>= 1;
    now *= now;
  }
  return res;
}

// MOD
ll MOD = 1000000000L + 7L;
#define add(a, b) ((a % MOD) + (b % MOD)) % MOD
#define mul(a, b) ((a % MOD) * (b % MOD)) % MOD
#define sub(a, b) ((a % MOD) + MOD - (b % MOD)) % MOD
template <typename T, typename S> T ModPow(T a, S b) {
  T res = 1, now = a;
  while (b) {
    if (b & 1)
      res = mul(res, now);
    b >>= 1;
    now = mul(now, now);
  }
  return res;
}
template <typename T>
T mod_inverse(T a, T mod, bool prime) { // if mod is prime, "prime" is true.
  if (prime)
    return ModPow(a, mod - 2);
  else {
    T x, y;
    EXTGCD(a, mod, x, y);
    return (mod + x % mod) % mod;
  }
}
#define divide(a, b) ((a % MOD) * (mod_inverse(b, MOD, true))) % MOD

// LLの数値をつかう時は最後にLをつける癖をつけよう
//
//   ┏┓┏┓　┓　┏┓
//   ┏┛┃┃　┃　┗┫
//   ┗┛┗┛　┻　┗┛
//
//    謹┃賀┃新┃年┃
//    ━┛━┛━┛━┛
// WWWWWWWWWWWWWWWWWWWWWW

vector<pair<pair<int, int>, int>> v;
vector<pair<int, double>> g[1001];
double d[1011];

int main() {
  aim_cpo; // インタラクティブのときは消すように.
#ifdef LOCAL_DEFINE
  LOCAL_INPUT; // インタラクティブのときは消すように.
               //  LOCAL_OUTPUT; // ファイルに出力したいときのみ
  show(MOD);
#endif
  int sx, sy;
  cin >> sx >> sy;
  int gx, gy;
  cin >> gx >> gy;
  v.push_back(mp(mp(sx, sy), 0));
  v.push_back(mp(mp(gx, gy), 0));
  int n;
  cin >> n;
  rep(i, 0, n) {
    int x, y, r;
    cin >> x >> y >> r;
    v.push_back(mp(mp(x, y), r));
  }
  rep(i, 0, (int)v.size()) {
    rep(j, 0, (int)v.size()) {
      if (i == j)
        continue;
      double cost = sqrt(Pow((double)v[i].fi.fi - v[j].fi.fi, 2) +
                         Pow((double)v[i].fi.se - v[j].fi.se, 2));
      if (cost - v[i].se - v[j].se < 0)
        cost = 0.;
      else
        cost -= v[i].se + v[j].se;
      g[i].emplace_back(j, cost);
      //      show(i);
      //      show(j);
      //      show(cost);
    }
  }
  priority_queue<pair<double, int>> q;
  fill(d, d + n + 2, 1e17);
  d[0] = 0.;
  q.push(mp(0., 0));
  while (!q.empty()) {
    int now = q.top().se;
    double cost = -q.top().fi;
    q.pop();
    show(now);
    show(cost);
    if (d[now] < cost)
      continue;
    rep(i, 0, (int)g[now].size()) {
      if (d[g[now][i].fi] > d[now] + g[now][i].se) {
        d[g[now][i].fi] = d[now] + g[now][i].se;
        q.push(mp(-d[g[now][i].fi], g[now][i].fi));
      }
    }
  }
  showA(d, n + 2);
  cout << d[1] << endl;
#ifdef LOCAL_DEFINE
  cerr << "Time elapsed: " << 1.0 * clock() / CLOCKS_PER_SEC << "s." << endl;
#endif
  return 0;
}

Fixed submission:
#include <bits/stdc++.h>

using namespace std;

#define pb push_back
#define mp make_pair
#define mt make_tuple
#define fi first
#define se second
#define all(a) (a).begin(), (a).end()
#define endl "\n"
#define rep(i, a, b) for (auto(i) = (a); (i) < (b); (i)++)
#define rrep(i, a, b) for (auto(i) = (a); (i) > (b); (i)--)

#define aim_cpo                                                                \
  do {                                                                         \
    ios::sync_with_stdio(false);                                               \
    cin.tie(nullptr);                                                          \
    cout.precision(12);                                                        \
    cout << fixed;                                                             \
  } while (0)

#define LOCAL_INPUT                                                            \
  do {                                                                         \
    FILE *stream1;                                                             \
    stream1 = freopen("/Users/aim_cpo/CLionProjects/competitive/in.txt", "r",  \
                      stdin);                                                  \
    if (stream1 == nullptr)                                                    \
      return 0;                                                                \
  } while (0)
#define LOCAL_OUTPUT                                                           \
  do {                                                                         \
    FILE *stream2;                                                             \
    stream2 = freopen("out.txt", "w", stdout);                                 \
    if (stream2 == nullptr)                                                    \
      return 0;                                                                \
  } while (0)

#ifdef LOCAL_DEFINE
#define show(x) cerr << #x << " = " << (x) << endl
#define dump(x)                                                                \
  cerr << #x << " = " << (x) << " (line " << __LINE__ << ")" << endl
#define showV(v, n)                                                            \
  do {                                                                         \
    for (int _i_t_e_ = 0; _i_t_e_ < (n); _i_t_e_++)                            \
      cerr << "(" << _i_t_e_ << " = " << (v)[_i_t_e_] << ") ";                 \
    cerr << endl;                                                              \
  } while (0)
#define showA(a, n)                                                            \
  do {                                                                         \
    for (int _i_t_e_ = 0; _i_t_e_ < (n); _i_t_e_++)                            \
      cerr << "(" << _i_t_e_ << " = " << (a)[_i_t_e_] << ") ";                 \
    cerr << endl;                                                              \
  } while (0)
#define showA2(a, n, m)                                                        \
  do {                                                                         \
    for (int _i_t_e_ = 0; _i_t_e_ < (n); _i_t_e_++) {                          \
      for (int _i_t_e_2 = 0; _i_t_e_2 < (m); _i_t_e_2++) {                     \
        cerr << "(" << _i_t_e_ << ", " << _i_t_e_2 << " = "                    \
             << (a)[_i_t_e_][_i_t_e_2] << ") ";                                \
      }                                                                        \
      cerr << endl;                                                            \
    }                                                                          \
    cerr << endl;                                                              \
  } while (0)
#else
#define show(x)
#define dump(x)
#define showV(v, n)
#define showA(a, n)
#define showA2(a, n, m)
#endif

typedef long long ll;
typedef unsigned long long ull;

constexpr const int INT_INF = 0x3f3f3f3f;       // 1061109567
constexpr const ll LL_INF = 0x3f3f3f3f3f3f3f3f; // 4557430888798830399

template <typename T> bool chmin(T &a, T b) {
  return a > b ? (a = b, true) : false;
}
template <typename T> bool chmax(T &a, T b) {
  return a < b ? (a = b, true) : false;
}
template <typename T> void ln(T i, T n) { cout << (i == n - 1 ? "\n" : " "); }
template <typename T, typename S>
ostream &operator<<(ostream &out, const pair<T, S> &pair1) {
  out << '(' << pair1.fi << ", " << pair1.se << ')';
  return out;
}
template <typename T>
ostream &operator<<(ostream &out, const vector<T> &vector1) {
  out << '[';
  for (unsigned int i = 0; i < vector1.size(); i++) {
    out << vector1[i];
    if (i == vector1.size() - 1)
      out << "]";
    else
      out << ", ";
  }
  return out;
}

// INT
#define GCD(a, b) __gcd(a, b)
template <typename T> T LCM(T a, T b) { return a / GCD(a, b) * b; }
template <typename T> T EXTGCD(T a, T b, T &x, T &y) {
  T d = a;
  if (b != 0) {
    d = EXTGCD(b, a % b, y, x);
    y -= (a / b) * x;
  } else
    x = 1, y = 0;
  return d;
}
template <typename T> bool is_prime(T a) {
  for (int i = 2; i * i <= a; i++)
    if (a % i == 0)
      return 1;
  return 0;
}
template <typename T, typename S> T Pow(T a, S b) {
  T res = 1, now = a;
  while (b) {
    if (b & 1)
      res *= now;
    b >>= 1;
    now *= now;
  }
  return res;
}

// MOD
ll MOD = 1000000000L + 7L;
#define add(a, b) ((a % MOD) + (b % MOD)) % MOD
#define mul(a, b) ((a % MOD) * (b % MOD)) % MOD
#define sub(a, b) ((a % MOD) + MOD - (b % MOD)) % MOD
template <typename T, typename S> T ModPow(T a, S b) {
  T res = 1, now = a;
  while (b) {
    if (b & 1)
      res = mul(res, now);
    b >>= 1;
    now = mul(now, now);
  }
  return res;
}
template <typename T>
T mod_inverse(T a, T mod, bool prime) { // if mod is prime, "prime" is true.
  if (prime)
    return ModPow(a, mod - 2);
  else {
    T x, y;
    EXTGCD(a, mod, x, y);
    return (mod + x % mod) % mod;
  }
}
#define divide(a, b) ((a % MOD) * (mod_inverse(b, MOD, true))) % MOD

// LLの数値をつかう時は最後にLをつける癖をつけよう
//
//   ┏┓┏┓　┓　┏┓
//   ┏┛┃┃　┃　┗┫
//   ┗┛┗┛　┻　┗┛
//
//    謹┃賀┃新┃年┃
//    ━┛━┛━┛━┛
// WWWWWWWWWWWWWWWWWWWWWW

vector<pair<pair<int, int>, int>> v;
vector<pair<int, double>> g[1011];
double d[1011];

int main() {
  aim_cpo; // インタラクティブのときは消すように.
#ifdef LOCAL_DEFINE
  LOCAL_INPUT; // インタラクティブのときは消すように.
               //  LOCAL_OUTPUT; // ファイルに出力したいときのみ
  show(MOD);
#endif
  int sx, sy;
  cin >> sx >> sy;
  int gx, gy;
  cin >> gx >> gy;
  v.push_back(mp(mp(sx, sy), 0));
  v.push_back(mp(mp(gx, gy), 0));
  int n;
  cin >> n;
  rep(i, 0, n) {
    int x, y, r;
    cin >> x >> y >> r;
    v.push_back(mp(mp(x, y), r));
  }
  rep(i, 0, (int)v.size()) {
    rep(j, 0, (int)v.size()) {
      if (i == j)
        continue;
      double cost = sqrt(Pow((double)v[i].fi.fi - v[j].fi.fi, 2) +
                         Pow((double)v[i].fi.se - v[j].fi.se, 2));
      if (cost - v[i].se - v[j].se < 0)
        cost = 0.;
      else
        cost -= v[i].se + v[j].se;
      g[i].emplace_back(j, cost);
      //      show(i);
      //      show(j);
      //      show(cost);
    }
  }
  priority_queue<pair<double, int>> q;
  fill(d, d + n + 2, 1e17);
  d[0] = 0.;
  q.push(mp(0., 0));
  while (!q.empty()) {
    int now = q.top().se;
    double cost = -q.top().fi;
    q.pop();
    show(now);
    show(cost);
    if (d[now] < cost)
      continue;
    rep(i, 0, (int)g[now].size()) {
      if (d[g[now][i].fi] > d[now] + g[now][i].se) {
        d[g[now][i].fi] = d[now] + g[now][i].se;
        q.push(mp(-d[g[now][i].fi], g[now][i].fi));
      }
    }
  }
  showA(d, n + 2);
  cout << d[1] << endl;
#ifdef LOCAL_DEFINE
  cerr << "Time elapsed: " << 1.0 * clock() / CLOCKS_PER_SEC << "s." << endl;
#endif
  return 0;
}