Steal most of dubins implementation from OMPL