Fixed a stupid bug with SBA-RRT* algorithm. Played around with various things.