Fixed a memory leak

The pathfinding code did not release all the allocated objects.
Other minor changes...