Fixed bugs with path finding overlap.

I added another method and made some changes to the original algorithm to fix some broken cases.