Optimization: Instead of traversing once for getting occupied, once for getting frontier etc: Traverse once and put into different lists.

[hjpvandijk]

See if this works, as the search spaces are different (sizes)