% zpracování namìøených dat-analýza os pohybu
clear all; close all;
%naètení zmìøených dat
load sim_1osa
%zjištìní poètu mìøených bodù
k=size(um,2);
%zjištìní nezávislé promìnné pro rovnici roviny z=f(x,y) 
% zámìna souøadnic
rozx=max(um(1,:))-min(um(1,:))
rozy=max(um(2,:))-min(um(2,:))
rozz=max(um(3,:))-min(um(3,:))
if rozz<=min(rozx,rozy)
    disp('z')
    rmt=[um(1,:)
         um(2,:)
         um(3,:)];
end
if rozy<=min(rozz,rozx)
    disp('y')
    rmt=[um(3,:)
         um(1,:)
         um(2,:)];
end
if rozx<=min(rozy,rozz)
    disp('x')
    rmt=[um(2,:)
         um(3,:)
         um(1,:)];
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:k
    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=[k  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:k
    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
% sestavení rovnice MNÈ
C=[k  sxb  syb
   sxb sxxb sxyb
   syb sxyb syyb];
Cp=[sxyzb;sxyzbx;sxyzby];
%výpoèet koeficientù
c=inv(C)*Cp;
St=[-c(2)/2;-c(3)/2;-b(2)*c(2)/2-b(3)*c(3)/2+b(1);1];
%vykreslení výsledné osy kružnice(z namìžených bodù)
t=[-200:1:200];
xp=[];yp=[];zp=[];
for i=1:size(t,2)
    xp2(i)=St(1)+b(2)*t(i);
    yp2(i)=St(2)+b(3)*t(i);
    zp2(i)=St(3)-1*t(i);
end
figure(1)
plot3(rmt(1,:),rmt(2,:),rmt(3,:),'k.');
axis equal;
hold on;
plot3(St(1),St(2),St(3),'k*');
%plot3(OS(2,1),OS(2,2),OS(2,3),'r*');
plot3(xp2,yp2,zp2,'r-.','LineWidth',2);
lim=[min(rmt(1,:))-20 max(rmt(1,:))+20 min(rmt(2,:))-20 max(rmt(2,:))+20];
ezmesh(@(x,y) rovina(x,y,b(1),b(2),b(3)),lim);
R=sqrt(c(2)^2+c(3)^2+(b(2)*c(2)+b(3)*c(3)-2*b(1))^2-4*c(1))/2
ezmesh(@(x,y) koule(x,y,R,St(1),St(2),St(3)),lim);
axis([min(rmt(1,:))-100 max(rmt(1,:))+100 min(rmt(2,:))-100 max(rmt(2,:))+100 min(rmt(3,:))-100 max(rmt(3,:))+100])
legend('namìøené body','støed vyhodnocené kulové plochy','vyhodnocená poloha pohybové osy','vyhodnocená rovina','vyhodnocená kulová plocha') 
% zpìtná zámìna souøadnic
if rozz<=min(rozx,rozy)
   o=[b(2) b(3) -1];
   S=[St(1) St(2) St(3)];
   disp('z')
end
if rozy<=min(rozz,rozx)
   o=[b(3) -1 b(2)];
   S=[St(2) St(3) St(1)];
   disp('y')
end
if rozx<=min(rozy,rozz)
   o=[-1 b(2) b(3)];
   S=[St(3) St(1) St(2)];
   disp('x')
end
for i=1:3
    on(i)=o(i)/sqrt(o*o');  
end
% chyba vyhodnocení smìrového vektoru [stupnì]
er=min(acos(on*OS(1,1:3)')*180/pi,acos((-on)*OS(1,1:3)')*180/pi)
% chyba vyhodnocení støedu kružnice [mm]
erS=((S(1)-OS(2,1))^2+(S(2)-OS(2,2))^2+(S(3)-OS(2,3))^2)^(1/2)