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

Published with MATLAB® R2019b