A better algorithm for finding a path DStar algorithm