Refactor algorithms to use `Map` API

[mavenor]

For now, just DT-RRT* and RRT, since that's what we'll use in the near future.