clear all;
[filename,pathname]=uigetfile('*.txt');
fit1=fopen(strcat(pathname,filename),'rt');
if(fit1==-1)
    error('input file or path is not correct','Warning','warn')
end
for i=1:8
    ID(i)=fscanf(fit1,'%d',1);
    x1(i)=fscanf(fit1,'%f',1);
    y1(i)=fscanf(fit1,'%f',1);
    x2(i)=fscanf(fit1,'%f',1);
    y2(i)=fscanf(fit1,'%f',1);
end
for i=1:8
    x1(i)=x1(i)-225;
    y1(i)=225-y1(i);
    x2(i)=x2(i)-225;
    y2(i)=225-y2(i);
end
f=1150;
Xs1=240413.6;Ys1=1189391.5;Zs1=3088.3;
Xs2=239619.7;Ys2=1189568.3;Zs2=3075.5;
fi1=-0.0511735;w1=-0.0211784;k1=0.0034949;
fi2=0.0459680;w2=-0.0789097;k2=0.0037945;

Bx=Xs2-Xs1;
By=Ys2-Ys1;
Bz=Zs2-Zs1;

a1=cos(fi1)*cos(k1)-sin(fi1)*sin(w1)*sin(k1);
a2=-cos(fi1)*sin(k1)-sin(fi1)*sin(w1)*cos(k1);
a3=-sin(fi1)*cos(w1);
b1=cos(w1)*sin(k1);
b2=cos(w1)*cos(k1);
b3=-sin(w1);
c1=sin(fi1)*cos(k1)+cos(fi1)*sin(w1)*sin(k1);
c2=-sin(fi1)*sin(k1)+cos(fi1)*sin(w1)*cos(k1);
c3=cos(fi1)*cos(w1);

a1p=cos(fi2)*cos(k2)-sin(fi2)*sin(w2)*sin(k2);
a2p=-cos(fi2)*sin(k2)-sin(fi2)*sin(w2)*cos(k2);
a3p=-sin(fi2)*cos(w2);
b1p=cos(w2)*sin(k2);
b2p=cos(w2)*cos(k2);
b3p=-sin(w2);
c1p=sin(fi2)*cos(k2)+cos(fi2)*sin(w2)*sin(k2);
c2p=-sin(fi2)*sin(k2)+cos(fi2)*sin(w2)*cos(k2);
c3p=cos(fi2)*cos(w2);

for i=1:8
    X1(i)=a1*x1(i)+a2*y1(i)+a3*(-f);
    Y1(i)=b1*x1(i)+b2*y1(i)+b3*(-f);
    Z1(i)=c1*x1(i)+c2*y1(i)+c3*(-f);
    X2(i)=a1p*x2(i)+a2p*y2(i)+a3p*(-f);
    Y2(i)=b1p*x2(i)+b2p*y2(i)+b3p*(-f);
    Z2(i)=c1p*x2(i)+c2p*y2(i)+c3p*(-f);
end

for i=1:8
    N1(i)=(Bx*Z2(i)-Bz*X2(i))/(X1(i)*Z2(i)-X2(i)*Z1(i));
    N2(i)=(Bx*Z1(i)-Bz*X1(i))/(X1(i)*Z2(i)-X2(i)*Z1(i));
end

for i=1:8
    XA(i)=Xs1+N1(i)*X1(i);
    YA(i)=(Ys1+N1(i)*Y1(i)+Ys2+N2(i)*Y2(i))/2;    %YA（i）取平均值是考慮相對定向中殘余上下視差的存在。
    ZA(i)=Zs1+N1(i)*Z1(i);
end



for i=1:8
    fprintf('XA(%d)=%f\nYA(%d)=%f\nZA(%d)=%f\n',i,XA(i),i,YA(i),i,ZA(i));
end