% zpracování namìøených dat-analýza os pohybu
clear all;
%naètení zmìøených hodnot
load('sim_mereni.mat')
% pokud do promìnné rm uložíme rmi probìhne vyhodnocení
% bez uvažování chyby mìøení
%um=umi
% urèení poètu mìøených bodù
n=size(um,2);
k=size(um,3);
% vykreslení mechanismu
r0=[0;0;0;1];
DH=[[fi0] DH];
rteor=[0;0;0;1];
for i=1:6
    T(:,:,i)=Tfiz(DH(i,1))*Tz(DH(i,2))*Tx(DH(i,3))*Tfix(DH(i,4))*Tfiy(DH(i,5));
end
T16(:,:)=T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5)*T(:,:,6);
rteor=[T(:,:,1)*r0 rteor];
rteor=[T(:,:,1)*Tfiz(DH(2,1))*Tz(DH(2,2))*Tx(DH(2,3))*r0 rteor];
rteor=[T(:,:,1)*T(:,:,2)*r0 rteor];
rteor=[T(:,:,1)*T(:,:,2)*T(:,:,3)*r0 rteor];
rteor=[T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*r0 rteor];
rteor=[T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5)*r0 rteor];
rteor=[T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5)*T(:,:,6)*r0 rteor];
figure(1)
li=plot3(rteor(1,:),rteor(2,:),rteor(3,:));
axis equal
axis([-50 250 -100 100 -50 2650])
xlabel('x [mm]');ylabel('y [mm]');zlabel('z [mm]');
set(li,'LineWidth',2);
% pøíprava promìných pro výsledek
Str=[];
o=[];
% získání støedù bodù a smìrových vektorù k popisu jednotlivých os pohybu
for j=1:5
    %zjištìní nezávislé promìnné pro rovnici roviny z=f(x,y) a zámìna
    %souøadnic
    rozx=max(um(1,:,j))-min(um(1,:,j));
    rozy=max(um(2,:,j))-min(um(2,:,j));
    rozz=max(um(3,:,j))-min(um(3,:,j));
    if rozz<=min(rozx,rozy)
        rmt=[um(1,:,j)
             um(2,:,j)
             um(3,:,j)];
    end
    if rozy<=min(rozz,rozx)
        rmt=[um(3,:,j)
             um(1,:,j)
             um(2,:,j)];
    end
    if rozx<=min(rozy,rozz)
        rmt=[um(2,:,j)
             um(3,:,j)
             um(1,:,j)];
    end
    %výpoèet koeficientù pro MNÈ
    sx=0; sy=0; sz=0; sxx=0; sxy=0; syy=0; sxz=0; syz=0;
    for i=1:n
        sx=sx+rmt(1,i);
        sy=sy+rmt(2,i);
        sz=sz+rmt(3,i);
        sxx=sxx+rmt(1,i)^2;
        sxy=sxy+rmt(1,i)*rmt(2,i);
        syy=syy+rmt(2,i)^2;
        sxz=sxz+rmt(3,i)*rmt(1,i);
        syz=syz+rmt(3,i)*rmt(2,i);
    end
    %sestavení rovnic pro MNÈ
    B=[n  sx  sy
       sx sxx sxy
       sy sxy syy];
    Bp=[sz;sxz;syz];
    %proložení dat rovinou
    znj=inv(B)*Bp;
    % vyhodnocení støedu kružnice
    %výpoèet koeficientù pro MNÈ
    sxb=0; syb=0; sxxb=0; sxyb=0; syyb=0; sxyzb=0; sxyzbx=0; sxyzby=0;
    for i=1:n
        sxb=sxb+rmt(1,i)+znj(2)*rmt(3,i);
        syb=syb+rmt(2,i)+znj(3)*rmt(3,i);
        sxxb=sxxb+(rmt(1,i)+znj(2)*rmt(3,i))^2;
        sxyb=sxyb+(rmt(1,i)+znj(2)*rmt(3,i))*(rmt(2,i)+znj(3)*rmt(3,i));
        syyb=syyb+(rmt(2,i)+znj(3)*rmt(3,i))^2;
        sxyzb=sxyzb-rmt(1,i)^2-rmt(2,i)^2-rmt(3,i)^2+2*znj(1)*rmt(3,i);
        sxyzbx=sxyzbx+(-rmt(1,i)^2-rmt(2,i)^2-rmt(3,i)^2+2*znj(1)*rmt(3,i))*(rmt(1,i)+znj(2)*rmt(3,i));
        sxyzby=sxyzby+(-rmt(1,i)^2-rmt(2,i)^2-rmt(3,i)^2+2*znj(1)*rmt(3,i))*(rmt(2,i)+znj(3)*rmt(3,i));
    end
   C=[n  sxb  syb
       sxb sxxb sxyb
       syb sxyb syyb];
    Cp=[sxyzb;sxyzbx;sxyzby];
    c=inv(C)*Cp;
    % souøadnice støedu kružnice
    St=[-c(2)/2;-c(3)/2;-znj(2)*c(2)/2-znj(3)*c(3)/2+znj(1);1];
    %zpìtná zámìna souøadnic
    if rozz<=min(rozx,rozy)
       oz=[znj(2) znj(3) -1];
       Sz=[St(1) St(2) St(3)];
    end
    if rozy<=min(rozz,rozx)
       oz=[znj(3) -1 znj(2)];
       Sz=[St(2) St(3) St(1)];
    end
    if rozx<=min(rozy,rozz)
       oz=[-1 znj(2) znj(3)];
       Sz=[St(3) St(1) St(2)];
    end
    Str=[Str
         Sz];
    o=[o
       oz];
end
% vyhodnocení roviny pøíruby
okp=[];
Bpr=[];
Stp=[];
for j=6:k
    %zjištìní nezávislé promìnné pro rovnici roviny z=f(x,y) a zámìna
    %souøadnic
    rozx=max(um(1,:,j))-min(um(1,:,j));
    rozy=max(um(2,:,j))-min(um(2,:,j));
    rozz=max(um(3,:,j))-min(um(3,:,j));
    if rozz<=min(rozx,rozy)
        rmt=[um(1,:,j)
             um(2,:,j)
             um(3,:,j)];
    end
    if rozy<=min(rozz,rozx)
        rmt=[um(3,:,j)
             um(1,:,j)
             um(2,:,j)];
    end
    if rozx<=min(rozy,rozz)
        rmt=[um(2,:,j)
             um(3,:,j)
             um(1,:,j)];
    end
    %výpoèet koeficientù pro MNÈ
    sx=0; sy=0; sz=0; sxx=0; sxy=0; syy=0; sxz=0; syz=0;
    for i=1:n
        sx=sx+rmt(1,i);
        sy=sy+rmt(2,i);
        sz=sz+rmt(3,i);
        sxx=sxx+rmt(1,i)^2;
        sxy=sxy+rmt(1,i)*rmt(2,i);
        syy=syy+rmt(2,i)^2;
        sxz=sxz+rmt(3,i)*rmt(1,i);
        syz=syz+rmt(3,i)*rmt(2,i);
    end
    %sestavení rovnic pro MNÈ
    B=[n  sx  sy
       sx sxx sxy
       sy sxy syy];
    Bp=[sz;sxz;syz];
    %proložení dat rovinou
    b=inv(B)*Bp;
    % vyhodnocení støedu kružnice
    %výpoèet koeficientù pro MNÈ
    sxb=0; syb=0; sxxb=0; sxyb=0; syyb=0; sxyzb=0; sxyzbx=0; sxyzby=0;
    for i=1:n
        sxb=sxb+rmt(1,i)+b(2)*rmt(3,i);
        syb=syb+rmt(2,i)+b(3)*rmt(3,i);
        sxxb=sxxb+(rmt(1,i)+b(2)*rmt(3,i))^2;
        sxyb=sxyb+(rmt(1,i)+b(2)*rmt(3,i))*(rmt(2,i)+b(3)*rmt(3,i));
        syyb=syyb+(rmt(2,i)+b(3)*rmt(3,i))^2;
        sxyzb=sxyzb-rmt(1,i)^2-rmt(2,i)^2-rmt(3,i)^2+2*b(1)*rmt(3,i);
        sxyzbx=sxyzbx+(-rmt(1,i)^2-rmt(2,i)^2-rmt(3,i)^2+2*b(1)*rmt(3,i))*(rmt(1,i)+b(2)*rmt(3,i));
        sxyzby=sxyzby+(-rmt(1,i)^2-rmt(2,i)^2-rmt(3,i)^2+2*b(1)*rmt(3,i))*(rmt(2,i)+b(3)*rmt(3,i));
    end
    C=[n  sxb  syb
       sxb sxxb sxyb
       syb sxyb syyb];
    Cp=[sxyzb;sxyzbx;sxyzby];
    c=inv(C)*Cp;
    St=[-c(2)/2;-c(3)/2;-b(2)*c(2)/2-b(3)*c(3)/2+b(1);1];
    % polomìr kulové plochy
    rpr(j-5)=1/2*sqrt(c(2)^2+c(3)^2+(b(2)*c(2)+b(3)*c(3)-2*b(1))^2-4*c(1));
    % bod na pøírubì (dasazení 2 souøadnic 1. bodu mìøení do vyhodnocené
    % roviny roviny)
    Bprt=[rmt(1,1) rmt(2,1) rmt(1,1)*b(2)+rmt(2,1)*b(3)+b(1)];
    % zpìtná zámìna souøadnic
    if rozz<=min(rozx,rozy)
       okp=[okp
            b(2) b(3) -1];
       Bpr=[Bpr
            Bprt(1) Bprt(2) Bprt(3)];
       Stp=[Stp
            St(1) St(2) St(3)];
    end
    if rozy<=min(rozz,rozx)
       okp=[okp
            b(3) -1 b(2)];
       Bpr=[Bpr
            Bprt(2) Bprt(3) Bprt(1)];
       Stp=[Stp
            St(2) St(3) St(1)];
    end
    if rozx<=min(rozy,rozz)
       okp=[okp
            -1 b(2) b(3)];
       Bpr=[Bpr
            Bprt(3) Bprt(1) Bprt(2)];
       Stp=[Stp
            St(3) St(1) St(2)];
    end
    end
%vyhodnocení smìrovéo vektoru a støedu 6.osy ze kružnic namìøených na
%pøírubì (prùmìrováním)
o=[o
   sum(okp(:,1))/(k-5) sum(okp(:,2))/(k-5) sum(okp(:,3))/(k-5)]
Str=[Str
     sum(Stp(:,1))/(k-5) sum(Stp(:,2))/(k-5) sum(Stp(:,3))/(k-5)]
%normování smìrových vektorù na jednotkovou velikost
for j=1:6
    for h=1:3
    on(j,h)=o(j,h)/sqrt(o(j,:)*o(j,:)');
    end
end
  
%vykteslení nalezených os
hold on
for j=1:6
    ta=[-80:1:80];   
    ax=[];ay=[];az=[];bx=[];by=[];bz=[];
    for i=1:size(ta,2)
        ax(i)=Str(j,1)+on(j,1)*ta(i);
        ay(i)=Str(j,2)+on(j,2)*ta(i);
        az(i)=Str(j,3)+on(j,3)*ta(i);
    end
    plot3(ax,ay,az,'k-.');
end

%nalezení roviny pøíruby
%výpoèet koeficientù pro MNÈ
sx=0; sy=0; sz=0; sxx=0; sxy=0; syy=0; sxz=0; syz=0;
for i=1:(k-5)
    sx=sx+Bpr(i,1);
    sy=sy+Bpr(i,2);
    sz=sz+Bpr(i,3);
    sxx=sxx+Bpr(i,1)^2;
    sxy=sxy+Bpr(i,1)*Bpr(i,2);
    syy=syy+Bpr(i,2)^2;
    sxz=sxz+Bpr(i,3)*Bpr(i,1);
    syz=syz+Bpr(i,3)*Bpr(i,2);
end
%nalezení roviny pøíruby
%sestavení rovnic pro MNÈ
B=[k-5  sx  sy
   sx sxx sxy
   sy sxy syy];
Bp=[sz;sxz;syz];
%proložení dat rovinou
b=inv(B)*Bp;
op=[b(2) b(3) -1];
for h=1:3
    opn(h)=op(h)/sqrt(op*op');
end
on=[on
    opn];
% úprava smìru jednotlivých vyhodnocených os (aby smìr odpovídal definici)
% pøi zmìnì zákl. polohy mìøení do jiného kvadrantu nutná úprava
on=[-on(1,:)
    on(2,:)
    on(3,:)
    -on(4,:)
    on(5,:)
    on(6,:)
    -on(7,:)];
% vyhodnocení DH parametrù
DHn=[];
x=[];
%poèátek souøadného systému 1
PSS=[Str(1,1)-Str(1,3)/on(1,3)*on(1,1) Str(1,2)-Str(1,3)/on(1,3)*on(1,2)  0];
for j=1:5
    % vyhodnocení 2 sady parametrù (vyhodnocení 2 a 3 osa probíhá odlišnì-parametr
    % beta namísto d)
    if j==2
        %definice os a poèátkù souøadných systémù
        zn=on(j,:);
        znj=on(j+1,:);
        Sn=PSS(j,:);
        Snj=Str(j+1,:);
        p=-(dot(zn,Snj)-dot(zn,Sn))/dot(zn,znj);
        Sb=Snj+p*znj; %poèátek souøadného systému na ose 3
        PSS=[PSS
             Sb];
        xn=x(j,:);
        ab=cross(cross((Sb-Sn),znj),(Sb-Sn)); %vektor osy 2 promítnutý do roviny kolmé ke spojnici
        xp=Sb-Sn;
        w=cross(xn,xp);
        wp=w./zn;
        if max(wp)>0
            DHth=acos(dot(xn,xp)/(sqrt(dot(xn,xn))*sqrt(dot(xp,xp))));
        else
            DHth=-acos(dot(xn,xp)/(sqrt(dot(xn,xn))*sqrt(dot(xp,xp))));
        end
        DHa=sqrt(xp*xp');
        w2=cross(zn,ab);
        wp2=w2./xp;
        if max(wp2)>0
            DHal=acos(dot(zn,ab)/(sqrt(dot(zn,zn))*sqrt(dot(ab,ab))));
        else
            DHal=-acos(dot(zn,ab)/(sqrt(dot(zn,zn))*sqrt(dot(ab,ab))));
        end
        w3=cross(ab,znj);
        y2=cross(znj,xp);
        wp3=w3./y2;
        if max(wp3)>0
            DHbe=acos(dot(ab,znj)/(sqrt(dot(ab,ab))*sqrt(dot(znj,znj))));
        else
            DHbe=-acos(dot(ab,znj)/(sqrt(dot(ab,ab))*sqrt(dot(znj,znj))));
        end
        xnj=cross(cross(ab,znj),znj);
        x=[x
           xp];
        DHd=0;
    else
    AB=Str(j,:)-Str(j+1,:);
    zn=on(j,:);
    znj=on(j+1,:);
    % získání bodù kolmé pøíèky na obou osách
    Cp=[zn*zn' -zn*znj'
    zn*znj' -znj*znj'];
    Dp=[-AB*zn';-AB*znj'];
    pp=inv(Cp)*Dp; %hodnoty parametru na pøímkách pro body kolmé pøíèky
    Pp1=Str(j,:)+pp(1)*zn; %body kolmé pøíèky na pøímkách
    Pp2=Str(j+1,:)+pp(2)*znj;
    PSS=[PSS
         Pp2];
    if j==1
        x=[1 0 0];
    end
    x=[x
       Pp2-Pp1];
    xn=x(j,:);
    xnj=x(j+1,:);
    DHa=sqrt(xnj*xnj');
    DHd=sqrt((Pp1-PSS(j,:))*(Pp1-PSS(j,:))');
    if max((Pp1-PSS(j,:))/zn)>0
        DHd=DHd;
    else
        DHd=-DHd;
    end
    w=cross(xn,xnj);
    wp=w./zn;
    cDHth=dot(xn,xnj)/(sqrt(dot(xn,xn))*sqrt(dot(xnj,xnj)));
    if abs(cDHth)>1
        cDHth=round(cDHth);
    end
    if max(wp)>0
        DHth=acos(cDHth);
    else
        DHth=-acos(cDHth);
    end
    w2=cross(zn,znj);
    wp2=w2./xnj;
    if max(wp2)>0
       DHal=acos(dot(zn,znj)/(sqrt(dot(zn,zn))*sqrt(dot(znj,znj))));
    else
       DHal=-acos(dot(zn,znj)/(sqrt(dot(zn,zn))*sqrt(dot(znj,znj))));
    end
    DHbe=0;
    end
    
    DHn=[DHn
         DHth DHd DHa DHal DHbe];
end
%definice vektorù pro vyhodnocení parametrù pøíruby
zn=on(6,:);
znj=on(7,:);
xn=x(6,:);
Sn=PSS(6,:);
Snj=Str(6,:);
ab=cross(cross(xn,znj),xn);
% vyhodnocení poslední sady DH parametrù
p=-(dot(znj,Sn)-dot(znj,Snj))/dot(znj,zn);
Sb=Sn+p*zn;
PSS=[PSS
     Sb];
DHd6=sqrt((Sn-Sb)*(Sn-Sb)');
w=cross(zn,ab);
wp=w./xn;
if max(wp)>0
    DHal=acos(dot(zn,ab)/(sqrt(dot(zn,zn))*sqrt(dot(ab,ab))));
else
    DHal=-acos(dot(zn,ab)/(sqrt(dot(zn,zn))*sqrt(dot(ab,ab))));
end
w2=cross(ab,znj);
y2=cross(znj,xn);
wp2=w2./y2;
if max(wp2)>0
    DHbe=acos(dot(ab,znj)/(sqrt(dot(ab,ab))*sqrt(dot(znj,znj))));
else
    DHbe=-acos(dot(ab,znj)/(sqrt(dot(ab,ab))*sqrt(dot(znj,znj))));
end
DHn=[DHn
     0 DHd6 0 DHal DHbe];
plot3(PSS(:,1),PSS(:,2),PSS(:,3),'r.');
DHer=DH-DHn