Integrate Optimal Embedded Surface Into Planner

[rland93]

This code was originally written as a companion to the research code in c7d5270. Now that the method has been fleshed out, we can integrate it into this repository.

[rland93]

Going to start working on this.

This wasn't integrated before, because the solution was not robust and quite slow.

I think I can fix those issues. If not, my old (slow, bad, high error) implementation will be added, but maybe on an experimental branch. If so, then I'll try to integrate it into the planner in a way that makes sense.

[rland93]

Leaving this issue open until we have documentation supporting the surface API.