Fixed shortest path so it recognizes obstacles/stairs/walls - still need to test this though