Efficient Configuration Space Construction and Optimization for Motion Planning. (March 2015)