Pathfinding implemented

diagonal and direct connected nodes