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