Some refactoring, but mostly trials and errors trying to find something that works well for SBA*-RRT* density metric / criteria.