Code clean up

Cleaned up Pathfinding and PathNode to get rid of old commented out
code.