In the step of the PRM when the road map is created, the new vertice is added to the graph, and then the graph is investigated to find all vertices within a certain distance from the new vertice. This function will then also return the newly added vertice. As a result the set of edges will include edges of length zero from each vertice to itself. Later this becomes an issue when querying the road map: The returned path is certainly not optimal, as it should be, since the algorithm used is A*.
The pull request fixes this bug, by simply getting the closest vertices BEFORE adding the new vertice to the graph.
In the step of the PRM when the road map is created, the new vertice is added to the graph, and then the graph is investigated to find all vertices within a certain distance from the new vertice. This function will then also return the newly added vertice. As a result the set of edges will include edges of length zero from each vertice to itself. Later this becomes an issue when querying the road map: The returned path is certainly not optimal, as it should be, since the algorithm used is A*.
The pull request fixes this bug, by simply getting the closest vertices BEFORE adding the new vertice to the graph.