3D trajectory planning algorithm for organ groups and irregular obstacles with application for prostate brachytherapy.