C++ RRT (Rapidly-exploring Random Tree) implementation