Some parameters changed. Added some functionality for the visual evaluation of the method. filterObstacles() method improved.