Motion Planning in Medicine: Optimization and Simulation Algorithms for Image-Guided Procedures