function d = tnp_dist(mot1, mot2, i, j) tmp = mot1(:,i) - mot2(:,j); d = tmp' * tmp;