A method of path planning, comprising providing a dataset representing a cavity and a boundary; providing a plurality of points in said dataset, including at least a starting point and an ending point; and automatically determining a path between the starting point and the ending point, responsive to a penalty associated with passing through points in the cavity. Preferably, the penalty value associated with a point depends on a distance between the point and the boundary.