testify.m

用来验证轴瓦对中的时候数据的准确性

function []=testify(a1,b1,a,b,r)
%----这里的a1,b1是第一个psd中心点Op1在轴瓦坐标系XOY下的坐标----
%----a,b表示激光点在轴瓦坐标系XOY下的坐标，r是轴瓦的半径，L是psd中心点到轴瓦边缘的距离----
%----alphi指的是杆与铅垂线的夹角，范围是[-90,90]----
%------alphi的正负确定方法：测量时杆向左边倾斜则为正值，右边倾斜则为负值-------
%----其中：ki指的是直线mi的斜率，（xi,yi）是直线mi与圆的交点的坐标，Opi是psd的中心点，Ai是直线mi与圆的交点----

%------对第一个点给出a1,b1,alphi_1的值，求出L--------
clc;
alphi_1=input('alphi_1=')  %----给出alphi_1的值----
if (alphi_1~=0)&&(alphi_1~=90)&&(alphi_1~=-90)%----除开0度，90度，-90度时的计算流程----
  if alphi_1>0               %----根据给出的alphi_1判断经过Op1点的直线m1的斜率----
      k1=tan(pi/2-alphi_1*pi/180);
  else
      k1=tan(pi/2+abs(alphi_1*pi/180));
  end
  %f1=sym('y-b1=k1*(x-a1)');%----第一条直线的方程----由于numeric函数不可用，所以直接把交点的计算坐标给出----
  %f=sym('x^2+y^2=r^2');       %----圆的方程----
  %[x,y]=solve(f,f1)         %----求出第一条直线与圆的交点,交点有两个，需要相应的判断----
  %pause
  x1=[ 1/2/(1+k1^2)*(2*k1^2*a1-2*b1*k1+2*(r^2-b1^2-k1^2*a1^2+2*b1*k1*a1+k1^2*r^2)^(1/2)),1/2/(1+k1^2)*(2*k1^2*a1-2*b1*k1-2*(r^2-b1^2-k1^2*a1^2+2*b1*k1*a1+k1^2*r^2)^(1/2))];
  y1=[ b1+1/2*k1/(1+k1^2)*(2*k1^2*a1-2*b1*k1+2*(r^2-b1^2-k1^2*a1^2+2*b1*k1*a1+k1^2*r^2)^(1/2))-k1*a1,b1+1/2*k1/(1+k1^2)*(2*k1^2*a1-2*b1*k1-2*(r^2-b1^2-k1^2*a1^2+2*b1*k1*a1+k1^2*r^2)^(1/2))-k1*a1];
                                    %----直线m1与圆的交点----
  double(x1);
  double(y1);
if y1(1)<y1(2)                  %----因为只取圆的下半部分，所以只取纵坐标值小的那个点计算----
    L=sqrt((x1(1)-a1)^2+(y1(1)-b1)^2);
else
    L=sqrt((x1(2)-a1)^2+(y1(2)-b1)^2);
end
end
switch alphi_1               %----三个特殊的角度时的L值计算----
    case 0                   %----夹角0度时的L值----
    L=b1+sqrt(r^2-a1^2);
    case 90                  %----夹角90度时的L值----
    L=a1+sqrt(r^2-b1^2);
    case -90                 %----夹角-90度时的L值----
    L=sqrt(r^2-b1^2)-a1;
end
L

%------对第二个点给出alphi_2,求出相应的a2,b2------
alphi_2=input('alphi_2=')    %----给出alphi_2的值----
if (alphi_2~=0)&&(alphi_2~=90)&&(alphi_2~=-90)   %----除开0度，90度，-90度时的计算流程----
  if alphi_2>0
      k2=tan(pi/2-alphi_2*pi/180);
  else
    k2=tan(pi/2+abs(alphi_2*pi/180));
  end
  %f2=sym('y-b1=k2*(x-a1)');             %----经过Op2点的直线方程m2----
  %[x,y]=solve(f,f2)                     %----算出m2与圆的交点A2有两个，需要相应的判断----
  %pause
  x2=[1/2/(1+k2^2)*(2*k2^2*a1-2*b1*k2+2*(-b1^2-k2^2*a1^2+2*b1*k2*a1+r^2+k2^2*r^2)^(1/2)),1/2/(1+k2^2)*(2*k2^2*a1-2*b1*k2-2*(-b1^2-k2^2*a1^2+2*b1*k2*a1+r^2+k2^2*r^2)^(1/2))];
  y2=[b1+1/2*k2/(1+k2^2)*(2*k2^2*a1-2*b1*k2+2*(-b1^2-k2^2*a1^2+2*b1*k2*a1+r^2+k2^2*r^2)^(1/2))-k2*a1,b1+1/2*k2/(1+k2^2)*(2*k2^2*a1-2*b1*k2-2*(-b1^2-k2^2*a1^2+2*b1*k2*a1+r^2+k2^2*r^2)^(1/2))-k2*a1];
  double(x2);
  double(y2);
  if y2(1)<y2(2)                                    %----取纵坐标值小的那个点计算----
      Op1A2=sqrt((x2(1)-a1)^2+(y2(1)-b1)^2);         %----Op1点到A2点的距离----
  else
      Op1A2=sqrt((x2(2)-a1)^2+(y2(2)-b1)^2);
  end
  Op1Op2=abs(Op1A2-L);                         %----两个psd中心的距离----
  if (y2(1)<y2(2))&&(Op1A2>L)     %----如果Op1A2大于L，说明杆往下移动以保证Op2A2=L,并取纵坐标值小的那个点计算----
      a2=a1-Op1Op2*(a1-x2(1))/Op1A2;          %----利用等比定理计算Op2点的坐标----
      b2=b1-Op1Op2*(b1-y2(1))/Op1A2;
  elseif (y2(1)>y2(2))&&(Op1A2>L)
      a2=a1-Op1Op2*(a1-x2(2))/Op1A2;
      b2=b1-Op1Op2*(b1-y2(2))/Op1A2;
  elseif (y2(1)<y2(2))&&(Op1A2<L) %----如果Op1A2小于L，说明杆往上移动以保证Op2A2=L,并取纵坐标值小的那个点计算----
      a2=(a1*L-Op1Op2*x2(1))/(L-Op1Op2);     %----利用等比定理计算Op2点的坐标----
      b2=(b1*L-Op1Op2*y2(1))/(L-Op1Op2);
  else 
      a2=(a1*L-Op1Op2*x2(2))/(L-Op1Op2);
      b2=(b1*L-Op1Op2*y2(2))/(L-Op1Op2);
  end
end

switch alphi_2                     %----三个特殊的角度的(a2,b2)坐标计算----
    case 0                         %----alphi_2 为0度----
    Op1A2=b1+sqrt(r^2-a1^2);
    Op1Op2=abs(Op1A2-L);
    a2=a1;                         %----横坐标不变----
    y2(1)=-sqrt(r^2-a1^2);         %----求出交点的纵坐标----
    if Op1A2>L                     %----如果Op1A2大于L，说明杆往下移动以保证Op2A2=L----
        b2=b1-Op1Op2*(b1-y2(1))/Op1A2;
    else
        b2=(b1*L-Op1Op2*y2(1))/(L-Op1Op2); %----如果Op1A2小于L，说明杆往上移动以保证Op2A2=L----  
    end
    case 90                        %----alphi_2 为90度----
    Op1A2=a1+sqrt(r^2-b1^2);
    Op1Op2=abs(Op1A2-L);
    b2=b1;                         %----纵坐标不变----
    x2(1)=-sqrt(r^2-b1^2);         %----求出交点的横坐标----
    if Op1A2>L                     %----原理同上----
       a2=a1-Op1Op2*(a1-x2(1))/Op1A2;
    else
       a2=(a1*L-Op1Op2*x2(1))/(L-Op1Op2);
    end
    case -90                       %----alphi_2 为-90度----
    Op1A2=sqrt(r^2-b1^2)-a1;
    Op1Op2=abs(Op1A2-L);
    b2=b1;                         %----纵坐标不变----
    x2(1)=sqrt(r^2-b1^2);          %----求出交点的横坐标----
    if Op1A2>L                     %----原理同上----
       a2=a1-Op1Op2*(a1-x2(1))/Op1A2;
    else
       a2=(a1*L-Op1Op2*x2(1))/(L-Op1Op2);
    end
end

    a2=a-a2
    b2=b-b2


%------对第三个点给出alphi_3,求出相应的a3,b3------
alphi_3=input('alphi_3=')
if (alphi_3~=0)&&(alphi_3~=90)&&(alphi_3~=-90)
  if alphi_3>0
      k3=tan(pi/2-alphi_3*pi/180);
  else
      k3=tan(pi/2+abs(alphi_3*pi/180));
  end
  %f3=sym('y-b1=k3*(x-a1)');  %----经过Op3点的直线方程m3----
  %[x,y]=solve(f,f3);         %----算出m3与圆的交点A3有两个，需要相应的判断----
  x3=[1/2/(1+k3^2)*(2*k3^2*a1-2*b1*k3+2*(-b1^2-k3^2*a1^2+2*b1*k3*a1+r^2+k3^2*r^2)^(1/2)),1/2/(1+k3^2)*(2*k3^2*a1-2*b1*k3-2*(-b1^2-k3^2*a1^2+2*b1*k3*a1+r^2+k3^2*r^2)^(1/2))];
  y3=[b1+1/2*k3/(1+k3^2)*(2*k3^2*a1-2*b1*k3+2*(-b1^2-k3^2*a1^2+2*b1*k3*a1+r^2+k3^2*r^2)^(1/2))-k3*a1,b1+1/2*k3/(1+k3^2)*(2*k3^2*a1-2*b1*k3-2*(-b1^2-k3^2*a1^2+2*b1*k3*a1+r^2+k3^2*r^2)^(1/2))-k3*a1];
  double(x3);
  double(y3);
  if y3(1)<y3(2)                 %----取纵坐标值小的那个点计算----
      Op1A3=sqrt((x3(1)-a1)^2+(y3(1)-b1)^2);%----Op1点到A3点的距离----
  else
      Op1A3=sqrt((x3(2)-a1)^2+(y3(2)-b1)^2);
  end
  Op1Op3=abs(Op1A3-L);                    %----两个psd中心的距离----
  if (y3(1)<y3(2))&&(Op1A3>L)     %----如果Op1A3大于L，说明杆往下移动以保证Op3A3=L,并取纵坐标值小的那个点计算----
      a3=a1-Op1Op3*(a1-x3(1))/Op1A3;      %----利用等比定理计算Op3点的坐标----
      b3=b1-Op1Op3*(b1-y3(1))/Op1A3;
  elseif (y3(1)>y3(2))&&(Op1A3>L)
      a3=a1-Op1Op3*(a1-x3(2))/Op1A3;
      b3=b1-Op1Op3*(b1-y3(2))/Op1A3;
  elseif (y3(1)<y3(2))&&(Op1A3<L) %----如果Op1A3小于L，说明杆往上移动以保证Op3A3=L,并取纵坐标值小的那个点计算----
      a3=(a1*L-Op1Op3*x3(1))/(L-Op1Op3);  %----利用等比定理计算Op3点的坐标----
      b3=(b1*L-Op1Op3*y3(1))/(L-Op1Op3);
  else 
      a3=(a1*L-Op1Op3*x3(2))/(L-Op1Op3);
      b3=(b1*L-Op1Op3*y3(2))/(L-Op1Op3);
  end
end
switch alphi_3
    case 0                        %----三个特殊的角度的坐标计算----
    Op1A3=b1+sqrt(r^2-a1^2);
    Op1Op3=abs(Op1A3-L);
    a3=a1;
    y3(1)=-sqrt(r^2-a1^2);
    if Op1A3>L
        b3=b1-Op1Op3*(b1-y3(1))/Op1A3;
    else
        b3=(b1*L-Op1Op3*y3(1))/(L-Op1Op3);
    end
    case 90
    Op1A3=a1+sqrt(r^2-b1^2);
    Op1Op3=abs(Op1A3-L);
    b3=b1;
    x3(1)=-sqrt(r^2-b1^2);
    if Op1A3>L
       a3=a1-Op1Op3*(a1-x3(1))/Op1A3;
    else
       a3=(a1*L-Op1Op3*x3(1))/(L-Op1Op3);
    end
    case -90
    Op1A3=sqrt(r^2-b1^2)-a1;
    Op1Op3=abs(Op1A3-L);
    b3=b1;
    x3(1)=sqrt(r^2-b1^2);
    if Op1A3>L
       a3=a1-Op1Op3*(a1-x3(1))/Op1A3;
    else
       a3=(a1*L-Op1Op3*x3(1))/(L-Op1Op3);
    end
end

    a3=a-a3
    b3=b-b3


%------对第四个点给出alphi_4,求出相应的a4,b4------
alphi_4=input('alphi_4=')
if (alphi_4~=0)&&(alphi_4~=90)&&(alphi_4~=-90)
  if alphi_4>0
      k4=tan(pi/2-alphi_4*pi/180);
  else
      k4=tan(pi/2+abs(alphi_4*pi/180));
  end
  %f4=sym('y-b1=k4*(x-a1)');  %----经过Op4点的直线方程m4----
  %[x,y]=solve(f,f4);         %----算出m4与圆的交点A4有两个，需要相应的判断----
  x4=[1/2/(1+k4^2)*(2*k4^2*a1-2*b1*k4+2*(-b1^2-k4^2*a1^2+2*b1*k4*a1+r^2+k4^2*r^2)^(1/2)),1/2/(1+k4^2)*(2*k4^2*a1-2*b1*k4-2*(-b1^2-k4^2*a1^2+2*b1*k4*a1+r^2+k4^2*r^2)^(1/2))];
  y4=[b1+1/2*k4/(1+k4^2)*(2*k4^2*a1-2*b1*k4+2*(-b1^2-k4^2*a1^2+2*b1*k4*a1+r^2+k4^2*r^2)^(1/2))-k4*a1,b1+1/2*k4/(1+k4^2)*(2*k4^2*a1-2*b1*k4-2*(-b1^2-k4^2*a1^2+2*b1*k4*a1+r^2+k4^2*r^2)^(1/2))-k4*a1];
  double(x4);
  double(y4);
  if y4(1)<y4(2)                 %----取纵坐标值小的那个点计算----
      Op1A4=sqrt((x4(1)-a1)^2+(y4(1)-b1)^2);%----Op1点到A4点的距离----
  else
      Op1A4=sqrt((x4(2)-a1)^2+(y4(2)-b1)^2);
  end
  Op1Op4=abs(Op1A4-L);                    %----两个psd中心的距离----
  if (y4(1)<y4(2))&&(Op1A4>L)     %----如果Op1A4大于L，说明杆往下移动以保证Op4A4=L,并取纵坐标值小的那个点计算----
      a4=a1-Op1Op4*(a1-x4(1))/Op1A4;      %----利用等比定理计算Op4点的坐标----
      b4=b1-Op1Op4*(b1-y4(1))/Op1A4;
  elseif (y4(1)>y4(2))&&(Op1A4>L)
      a4=a1-Op1Op4*(a1-x4(2))/Op1A4;
      b4=b1-Op1Op4*(b1-y4(2))/Op1A4;
  elseif (y4(1)<y4(2))&&(Op1A4<L) %----如果Op1A4小于L，说明杆往上移动以保证Op4A4=L,并取纵坐标值小的那个点计算----
      a4=(a1*L-Op1Op4*x4(1))/(L-Op1Op4);  %----利用等比定理计算Op4点的坐标----
      b4=(b1*L-Op1Op4*y4(1))/(L-Op1Op4);
  else 
      a4=(a1*L-Op1Op4*x4(2))/(L-Op1Op4);
      b4=(b1*L-Op1Op4*y4(2))/(L-Op1Op4);
  end
end

switch alphi_4
    case 0                       %----三个特殊的角度的坐标计算----
    Op1A4=b1+sqrt(r^2-a1^2);
    Op1Op4=abs(Op1A4-L);
    a4=a1;
    y4(1)=-sqrt(r^2-a1^2);
    if Op1A4>L
        b4=b1-Op1Op4*(b1-y4(1))/Op1A4;
    else
        b4=(b1*L-Op1Op4*y4(1))/(L-Op1Op4);
    end
    case 90
    Op1A4=a1+sqrt(r^2-b1^2);
    Op1Op4=abs(Op1A4-L);
    b4=b1;
    x4(1)=-sqrt(r^2-b1^2);
    if Op1A4>L
       a4=a1-Op1Op4*(a1-x4(1))/Op1A4;
    else
       a4=(a1*L-Op1Op4*x4(1))/(L-Op1Op4);
    end
    case -90
    Op1A4=sqrt(r^2-b1^2)-a1;
    Op1Op4=abs(Op1A4-L);
    b4=b1;
    x4(1)=sqrt(r^2-b1^2);
    if Op1A4>L
       a4=a1-Op1Op4*(a1-x4(1))/Op1A4;
    else
       a4=(a1*L-Op1Op4*x4(1))/(L-Op1Op4);
    end
end
 
    a4=a-a4
    b4=b-b4


%----利用原来的方程验证求出的a2,b2,a3,b3,a4,b4能否满足方程-----
r1=sqrt((L*sin(alphi_1*pi/180)-a1)^2+(L*cos(alphi_1*pi/180)-b1)^2)
r2=sqrt((L*sin(alphi_2*pi/180)+a2-a)^2+(L*cos(alphi_2*pi/180)+b2-b)^2)
r3=sqrt((L*sin(alphi_3*pi/180)+a3-a)^2+(L*cos(alphi_3*pi/180)+b3-b)^2)
r4=sqrt((L*sin(alphi_4*pi/180)+a4-a)^2+(L*cos(alphi_4*pi/180)+b4-b)^2)