Removed debug path storing code from shortestpath

...and increasing the speed by a lot for large shortest path queries. Got to love free speed boosts.

Oh and this also fixes a bug where immediate neighbor nodes were not being set into the reachable vertex collection.