Optimize toolpath by connecting non colliding starts and ends