Create custom linear heap for A*

[sebschmi]

Simply a vec of vecs where each inner vec holds all items of a specific cost. Possibly use VecDeques if it makes things easier.