perp3

intersect two or more lines

p = [5 2;  2 4; 4 2];
k1 = [0.4 0.8];
k2 = [0.9 0.1];
k3 = [1 1];
k1 = k1/norm(k1);
k2 = k2/norm(k2);
k3 = k3/norm(k3);
k = [k1; k2; k3];
for n = 1:3
    pl = [p(n,:); p(n,:)+10*k(n,:)];
plot(pl(:,1),pl(:,2),'r');
hold on
end
grid
hold off

%

A = zeros(2);
b = zeros([2,1]);
for n = 1:3
    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

hold on
plot(oc(1),oc(2),'rx','MarkerSize',11);
hold off
oc =

    6.3020
    4.4408

pt= oc';


dtot = 0.0;
for n=1:3
    rv = pt - p(n,:);
    q = dot(rv,k(n,:));
    dv = rv - q*k(n,:);
    dtot = dtot + dv(1)^2+dv(2)^2;

    pl = [p(n,:); p(n,:)+8*k(n,:)];
    p2 = [pt; p(n,:)+q*k(n,:)];
    plot(pl(:,1),pl(:,2),'r');
    hold on
    plot(p2(:,1),p2(:,2),'g');
    plot(pt(1),pt(2),'ko','MarkerSize',7);
end
hold off
grid
axis([5 8 3 6]);
axis equal

fprintf('evaluated at %g %g\n',pt);
fprintf('sq dist %g\n',dtot);
evaluated at 6.30204 4.44082
sq dist 0.0163265
[x y] = meshgrid(linspace(6,7,31),linspace(4,5,31));

pt = [x(:) y(:)];
sz = size(pt);
N = sz(1);

dist = zeros([N 1]);
for n=1:3
    rv = pt - repmat(p(n,:),[N 1]);
    q = rv*k(n,:)';
    dv = rv - q*k(n,:);
    dist = dist + dv(:,1).^2 + dv(:,2).^2;
end

z = reshape(dist,size(x));
mesh(x,y,z);
hold on
plot3(oc(1),oc(2),dtot,'k*');
hold off
[vmin, imin] = min(z);
[vmin, jmin] = min(vmin);
imin = imin(jmin);
% two values below should agree
[min(min(z)) z(imin,jmin)]
vmin
fprintf('minimum at %g %g \n',x(imin,jmin),y(imin,jmin));
dtot
ans =

    0.0164    0.0164


vmin =

    0.0164

minimum at 6.3 4.43333 

dtot =

    0.0163