rrt code snippets in matlab