cpp implementation of simplify path