% 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)
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;
z=[b(2) b(3) -1]
for i=1:3
   zn(i)=-z(i)/sqrt(z*z');
end
zn
ypom=[0 1 0];
xn=cross(ypom,zn)
yn=cross(zn,xn)
Trot=[xn' yn' zn' [0 0 0]'
      0   0   0    1]
% vyhodnocení støedu kružnice
% transformace do roviny kružnice
xpr=sum(rmt(1,:))/k;
ypr=sum(rmt(2,:))/k; 
zpr=b(2)*xpr+b(3)*ypr+b(1)
Tt=Tx(xpr)*Ty(ypr)*Tz(zpr);
for i=1:k
    r0ck(:,i)=inv(Tt*Trot)*[rmt(:,i)
                                   1];
end
%výpoèet koeficientù pro MNÈ z transformovaných souøadnic
sx2=0; sy2=0; sz2=0; sxx2=0; sxy2=0; syy2=0; sxz2=0; syz2=0;
sxxyy2=0; sxxy2=0; sxyy2=0;
for i=1:k
    sx2=sx2+r0ck(1,i);
    sy2=sy2+r0ck(2,i);
    sz2=sz2+r0ck(3,i);
    sxx2=sxx2+r0ck(1,i)^2;
    sxy2=sxy2+r0ck(1,i)*r0ck(2,i);
    syy2=syy2+r0ck(2,i)^2;
    sxz2=sxz2+r0ck(3,i)*r0ck(1,i);
    syz2=syz2+r0ck(3,i)*r0ck(2,i);
    sxxyy2=sxxyy2+r0ck(1,i)^2+r0ck(2,i)^2;
    sxxy2=sxxy2+(r0ck(1,i)^2+r0ck(2,i)^2)*r0ck(1,i);
    sxyy2=sxyy2+(r0ck(1,i)^2+r0ck(2,i)^2)*r0ck(2,i);
end
% sestavení rovnice MNÈ
E=[k  sx2  sy2
   sx2 sxx2 sxy2
   sy2 sxy2 syy2];
B2=[sz2;sxz2;syz2];
Ep=[-sxxyy2;-sxxy2;-sxyy2];
% výpoèet koeficientù rovnice roviny v transformovaném souøadném systému pro
% ovìøení správnosti 
b2=inv(E)*B2;
% výpoèet koeficientù rovnice kružnice
e=inv(E)*Ep;
% poloha støedu v transformovaném souøadném systému
S2l=[-e(2)/2;-e(3)/2;0;1];
%transformace
S2=Tt*Trot*S2l
R2=0.5*sqrt(e(2)^2+e(3)^2-4*e(1));
%vykreslení výsledné osy kružnice(z namìžených bodù)
t=[-100:1:100];
xp=[];yp=[];zp=[];
for i=1:size(t,2)
    xp2(i)=S2(1)+b(2)*t(i);
    yp2(i)=S2(2)+b(3)*t(i);
    zp2(i)=S2(3)-1*t(i);
end
figure(1)
plot3(rmt(1,:),rmt(2,:),rmt(3,:),'k.');
axis equal;
hold on;
%plot3(S2(1),S2(2),S2(3),'k*');
%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);
legend('namìøené body', 'vyhodnocená rovina')
% zpìtná zámìna souøadnic
if rozz<=min(rozx,rozy)
   o=[b(2) b(3) -1];
   S=[S2(1) S2(2) S2(3)];
   disp('z')
end
if rozy<=min(rozz,rozx)
   o=[b(3) -1 b(2)];
   S=[S2(2) S2(3) S2(1)];
   disp('y')
end
if rozx<=min(rozy,rozz)
   o=[-1 b(2) b(3)];
   S=[S2(3) S2(1) S2(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)
%vykteslení jednotlivých krokù výpoètu
figure(2)
plot3(r0ck(1,:),r0ck(2,:),r0ck(3,:),'k.');
axis equal;
grid on;
legend('po transformaci do souøadného systému v rovinì')
figure(3)
plot(r0ck(1,:),r0ck(2,:),'k.')
axis equal;
hold on;
fi=linspace(0,2*pi,100);
krx=[];kry=[];
for i=1:size(fi,2)
    krx=[krx S2l(1)+R2*sin(fi(i))];
    kry=[kry S2l(2)+R2*cos(fi(i))];
end
plot(krx,kry)
plot(S2l(1),S2l(2),'.r')
axis([min(r0ck(1,:))-20 max(r0ck(1,:))+20 min(r0ck(2,:))-20 max(r0ck(2,:))+20])
legend('namìøené body', 'vyhodnocená kružnice', 'støed kružnice')