Just made Dstar extend LPAstar, and then called reconstruct path