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