Added features:
Break XY and Z movements based on the height of points during 5 axis positioning. The tool can retract (Z+) first then move XY - or move XY first than plunge (Z-).
Retract tool along tool axis using linear interpolation when over-travel (Instead of rapid retract). Rapid retract can cause collision when using head-head machine.