working rrt with some results for 2d