Basic RRT done, also changed it so that s=0 is at the middle of the object so it goes from -LO to +LO