Partially fix issue #64

This commit fixes a bug in the Util.smoothenPath function. The
smoothened paths now are much better than before but when a path
segment crosses a corner it still overlaps with non walkable nodes