Tool geometry compensation, keep contact of tool on desired curve