Added optimization to A* to stop adding children that have already been reached by a shorter path