small fixes to make Dubins interpolation slightly more robust
small fixes to make Dubins interpolation slightly more robust

utiasASRL/batch-informed-trees,utiasASRL/batch-informed-trees,utiasASRL/batch-informed-trees,utiasASRL/batch-informed-trees,utiasASRL/batch-informed-trees,utiasASRL/batch-informed-trees