B-Matrix distance function from matlab code