diff --git a/PRM.m b/PRM.m index b53b34e..1069866 100644 --- a/PRM.m +++ b/PRM.m @@ -339,12 +339,12 @@ function create_roadmap(prm, opt) end new = [x; y]; - % add it to the graph - vnew = prm.graph.add_node(new); - % find the closest node already in the graph [d,v] = prm.graph.distances(new); + % add it to the graph + vnew = prm.graph.add_node(new); + % test neighbours in order of increasing distance and check for a clear % path for i=1:length(d)