Introduce optFlag so that one can choose not to build the optimization-related cell of matrices called B. It is too computationally costly and should probably be done in a different way.

% Get the index of the points p within the tall array of points ps
function [I, ok] = pointIndex(p, ps)
    [ok, I] = ismember(p,  ps, 'rows');
end