https://jack.kawell.us/posts/ompl-cost-functions/