added obstacles and adapted wavefront to work for obstacles