CForest first version

Sorry about the (void) convention in functions, this is what happens when you copy and paste ignoring the other changes :)  Other RRTstar tweaks have been also overwritten.
RRTstar::addMotion(Motion *) would just contain
```
nn_->add(motion);
 motion->parent->children.push_back(motion);
```
The derived method would call this method and then have whatever is inside the if statement.