Motion Planning in Medicine:Optimization and Simulation Alg