void solve()
{
if (debugMode)
{
String backPath = "..";
char dirSep = System.IO.Path.DirectorySeparatorChar;
String inFilePath = backPath + dirSep + backPath + dirSep + "in.txt";
sc = new Scanner(new System.IO.StreamReader(inFilePath));
}
else
{
sc = new Scanner();
}
Point start = new Point(sc.NextInt(), sc.NextInt(), 0);
Point goal = new Point(sc.NextInt(), sc.NextInt(), 0);
int n = sc.NextInt();
Point[] barrier = new Point[n + 2];
for (int i = 0; i < n; i++)
{
barrier[i + 1] = new Point(sc.NextInt(), sc.NextInt(), sc.NextInt());
}
barrier[0] = start;
barrier[n + 1] = goal;
SkewHeap <State> heap = new SkewHeap <State>();
double[] minDist = new double[n + 2];
double[,] pDist = new double[n + 2, n + 2];
for (int i = 0; i < n + 2; i++)
{
pDist[i, i] = 0;
for (int j = i + 1; j < n + 2; j++)
{
pDist[i, j] = pDist[j, i] = Math.Max(0.0, Point.dist(barrier[i], barrier[j]) - barrier[i].r - barrier[j].r);
}
}
for (int i = 0; i < n + 2; i++)
{
minDist[i] = INF;
}
heap.push(new State(0, 0));
minDist[0] = 0;
while (!heap.isEmpty())
{
State nowState = heap.poll();
//Console.Error.WriteLine(heap.Count);
if (minDist[nowState.idx] < nowState.dist)
{
continue;
}
for (int i = 0; i < n + 2; i++)
{
if (i != nowState.idx)
{
double nextDist = nowState.dist + pDist[i, nowState.idx];
if (nextDist < minDist[i])
{
minDist[i] = nextDist;
heap.push(new State(nextDist, i));
}
}
}
}
Console.WriteLine(minDist[n + 1]);
}
public void merge(SkewHeap <T> otherHeap)
{
this.count_ += otherHeap.Count;
this.topNode = HeapNode.meld(this.topNode, otherHeap.topNode);
}
