added distanceOut to RRT::Tree->nearest()
refactored tree drawing code into its own function

This way it can be called twice, once for each tree when
we begin doing bidirectional RRTs
mostly implemented bidirectional RRTs

it segfaults right now :(
but its almost done!
fixed segfault issue and improved logic
increased GUI window size (400,300) -> (600,450)