CBiRRT modified, check comments for adding missing stuff