Refactoring and cleanup of RRTstar and new RRTstar+Dubins test cases according to comments on pull request