First version of optimal partitioning algorithm included
added cpp11 support
some regressions
merged
Merge branch 'pfosten-aware' of https://github.com/asl-beachbot/pathmaker into pfosten-aware
changed some matlab specific bits