perp_script3

intersect two or more lines

clear
close all

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
hold off
grid
axis([5 8 3 6]);
axis equal

% find intersection

[oc, d] = intersect_lines(p,k);

fprintf('closest point at %g %g\n',oc);
disp('d');
disp(d);
fprintf('mean dist %g\n',mean(d));
dsq = sum(d.^2);
fprintf('daq %g\n',dsq);

% 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

closest point at 6.30204 4.44082
d
    0.0730
    0.0370
    0.0981

mean dist 0.0693681
daq 0.0163265

[x, y] = meshgrid(linspace(6,7,21),linspace(4,5,21));

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),dsq,'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)]

fprintf('approximate minimum of %g at %g %g \n',vmin,x(imin,jmin),y(imin,jmin));
fprintf('actual minimum of %g at %g %g\n',dsq,oc(1),oc(2));

approximate minimum of 0.0165122 at 6.3 4.45 
actual minimum of 0.0163265 at 6.30204 4.44082

Published with MATLAB® R2019b