Problem p03866 (python)

Fixed submission:
from heapq import heappush, heappop
from math import sqrt
xs, ys, xt, yt = map(int, raw_input().split())
n = input()
ps = [map(int, raw_input().split()) for i in xrange(n)]
cant = [0]*n
for i in xrange(n):
    x0, y0, r0 = ps[i]
    for j in xrange(n):
        x1, y1, r1 = ps[j]
        if i == j or r0 > r1: continue
        if (x0 - x1)**2 + (y0 - y1)**2 <= (r1 - r0)**2:
            cant[i] = 1
            break

EPS = 1e-10
que = []
dist = [10**18+7]*n
def calc(x0, y0, r0, x1, y1, r1):
    v = (x0 - x1)**2 + (y0 - y1)**2
    if v <= (r0+r1)**2:
        return 0.
    return sqrt(v) - r0 - r1
for i in xrange(n):
    if cant[i]: continue
    x, y, r = ps[i]
    d = dist[i] = calc(xs, ys, 0, x, y, r)
    heappush(que, (d, i))
while que:
    d, v = heappop(que)
    if dist[v] < d:
        continue
    x0, y0, r0 = ps[v]
    for t in xrange(n):
        if v == t or cant[t]: continue
        x1, y1, r1 = ps[t]
        nd = calc(x0, y0, r0, x1, y1, r1)
        if d+nd < dist[t]:
            dist[t] = d+nd
            heappush(que, (d+nd, t))
ans = sqrt((xs - xt)**2 + (ys - yt)**2)
for i in xrange(n):
    if cant[i]: continue
    x, y, r = ps[i]
    d = calc(xt, yt, 0, x, y, r) + dist[i]
    ans = min(ans, d)
print "%.010f" % ans

Buggy submission (verdict: Runtime Error):
from heapq import heappush, heappop, heapify
from math import sqrt
readline = open(0).readline
xs, ys, xt, yt = map(int, readline().split())
N = int(readline())
C = [list(map(int, readline().split())) for i in range(N)]
C.append((xt, yt, 0))

que = []
dist = [10**18]*(N+1)
rest = set(range(N+1))
for i in range(N+1):
    x, y, r = C[i]
    r0 = max(sqrt((xs - x)**2 + (ys - y)**2) - r, 0)
    dist[i] = r0
    que.append((r0, i))
heapify(que)

remove = rest.remove
T = []
pop = T.pop; push = T.append
while que:
    c, i = heappop(que)
    if i not in rest:
        continue
    if i == N:
        break
    push(i)
    while T:
        i = pop()
        xi, yi, ri = C[i]
        remove(i)
        d = dist[i]
        e = dist[N]-d
        for j in rest:
            x, y, r = C[j]
            r0 = sqrt((xi - x)**2 + (yi - y)**2) - ri - r
            if e > r0 < dist[j]-d:
                if r0 > 0:
                    dist[j] = v = d + r0
                    heappush(que, (v, j))
                else:
                    dist[j] = d
                    push(j)
print("%.016f" % dist[N])

Buggy submission (verdict: Runtime Error):
from heapq import heappush, heappop
from math import sqrt
EPS = 1e-10
xs, ys, xt, yt = map(int, raw_input().split())
n = input()
ps = [map(int, raw_input().split()) for i in xrange(n)]
def calc(x0, y0, r0, x1, y1, r1):
    v = (x0 - x1)**2 + (y0 - y1)**2
    if v <= (r0+r1)**2 + EPS:
        return 0.
    return sqrt(v) - r0 - r1
parent = range(n)
def root(x):
    if parent[x] != x:
        x = parent[x] = root(parent[x])
    return x
def unite(x, y):
    px = root(x)
    py = root(y)
    if px < py:
        parent[py] = px
    else:
        parent[px] = py
for i in xrange(n):
    x0, y0, r0 = ps[i]
    for j in xrange(i+1, n):
        if i == j: continue
        x1, y1, r1 = ps[j]
        if calc(x0, y0, r0, x1, y1, r1) < EPS:
            unite(i, j)
que = []
dist = [10**18+7]*n
for i in xrange(n):
    x, y, r = ps[i]
    d = dist[i] = calc(xs, ys, 0, x, y, r)
    heappush(que, (d, i))
while que:
    d, v = heappop(que)
    if dist[v] < d:
        continue
    x0, y0, r0 = ps[v]
    for t in xrange(n):
        if v == t: continue
        if root(v) == root(t) and d < dist[t]:
            dist[t] = d
            heappush(que, (d, t))
        else:
            x1, y1, r1 = ps[t]
            nd = calc(x0, y0, r0, x1, y1, r1)
            if d+nd < dist[t]:
                dist[t] = d+nd
                heappush(que, (d+nd, t))
ans = 10**18+7
for i in xrange(n):
    if cant[i]: continue
    x, y, r = ps[i]
    d = calc(xt, yt, 0, x, y, r) + dist[i]
    ans = min(ans, d)
print "%.010f" % ans