function [pt, d] = intersect_lines(p,k) n = size(p,1); A = zeros(2); b = zeros([2,1]); for n = 1:n A(1,1)=A(1,1)+k(n,2)^2; A(1,2)=A(1,2)-k(n,1)*k(n,2); A(2,2)=A(2,2)+k(n,1)^2; A(2,1)=A(1,2); b(1) = b(1) + p(n,1)*k(n,2)^2-p(n,2)*k(n,1)*k(n,2); b(2) = b(2) + p(n,2)*k(n,1)^2-p(n,1)*k(n,1)*k(n,2); end oc = A\b; pt = oc'; d = zeros([n 1]); for n=1:n rv = pt - p(n,:); q = dot(rv,k(n,:)); dv = rv - q*k(n,:); d(n) = sqrt(dv(1)^2+dv(2)^2); end