Improved Efficiency of Pathfinding Code

Changed the implementation of how pathnodes are processed
- Pathfinding can be done significantly faster
- Greatly reduced (effectively eliminated?) the slowdown associated with
a large queue of pathnodes