public PQitem RemoveFirst()
{
PQitem first = queue[0];
queue.RemoveAt(0);
return(first);
}
public void AddConnections(PQitem origin)
{
foreach (GraphNode connection in origin.node.connections)
{
if (!finished.Contains(connection))
{
InsertToPQ(origin, connection);
}
}
finished.Add(origin.node);
}
public void InsertToPQ(PQitem origin, GraphNode node)
{
PQitem pqi = new PQitem(origin.node, node);
pqi.totalDist = 1 + Vector3.Distance(node.pos, end) + origin.totalDist;
for (var i = 0; i < queue.Count; i++)
{
if (pqi.totalDist < queue[i].totalDist)
{
queue.Insert(i, pqi);
return;
}
}
queue.Add(pqi);
}