shiliang2.m

矢量算法

function [ro,thet,p,q]=shiliang2(pos,ang,dl)%pos为三个换能器距原点长度，ang为三个换能器的角度，dl为事先计算出的发射源距换能器长度
phi=linspace(0,pi,1000);
l=pos(1);
dil=dl(1);
l1=zeros(1,1000);
ang1=zeros(1,1000);
for i=1:1000
l1(i)=sqrt(l^2+dil^2-2*cos(phi(i))*l*dil);
ang1(i)=(l^2+l1(i)^2-dil^2)/(2*l*l1(i));
ang1(i)=acos(ang1(i));
end
l1=[l1,flipud(l1)];
ang1=[ang1+ang(1),ang(1)-ang1];
l=pos(2);
dil=dl(2);
l2=zeros(1,1000);
ang2=zeros(1,1000);
for i=1:1000
l2(i)=sqrt(l^2+dil^2-2*cos(phi(i))*l*dil);
ang2(i)=(l^2+l2(i)^2-dil^2)/(2*l*l2(i));
ang2(i)=acos(ang2(i));
end
l2=[l2,flipud(l2)];
ang2=[ang2+ang(2),ang(2)-ang2];
ddl=sqrt(l1.^2+pos(2).^2-2.*cos(ang1-ang(2)).*pos(2).*l1);
% plot(ddl)% 因为这条指令太耗时所以关闭，但是可以观看最后结果误差大小。若结果不对，可以打开，观看计算圆1圆周上点与圆心2的距离
ddl=abs(ddl-dil);
% figure
% plot(ddl)%
% 因为这条指令太耗时所以关闭，但是可以观看最后结果误差大小。若结果不对，可以打开，观看计算圆1圆周上点与圆心2的距离与理论距离的差值。与26行作用相
% 同，可以选择其中之一观看。
[p,q]=sort(ddl);
pp=find(p>p(1));
q=q(1:pp-1);
x1=l1.*cos(ang1);
x2=l2.*cos(ang2);
y2=l2.*sin(ang2);
y1=l1.*sin(ang1);

% figure%若打开前面几行被隐蔽的命令，则需打开此处，以开出新的画图窗口
plot(x1,y1,'*',x2,y2,'*',x1(q),y1(q),'+');
% hold on
% plot(x1(q(1)),y1(q(1)),'+')
ro=sqrt(x1(q).^2+y1(q).^2);
thet=atan(y1(q)./x1(q));
end