Optimizations: Get rid of wrkmem vector (use fixed stack buffer instead) & create even less nodes. Minor other things.
Really don't include starting position in the path + assert it.