private void AssertHeap()
{
if (_vertexQueue.Count == 0)
{
return;
}
TVertex top = _vertexQueue.Peek();
TVertex[] vertices = _vertexQueue.ToArray();
for (int i = 1; i < vertices.Length; ++i)
{
if (GetVertexDistance(top) > GetVertexDistance(vertices[i]))
{
Debug.Assert(false);
}
}
}