clc, clear all, close all, format short

tic
%mame zadane kartezske souradnice XYZ
mat_sour=load('input_CoM.txt');
m=9.05; %[kg]
 
%mame zadane sfericke souradnice r, fi, psi
%  A=load('input_teziste_polarni.txt'); 
%  [x,y,z] = sph2cart(A(:,2),A(:,3),A(:,1));
%  mat_sour=[x,y,z];
 
XYZ=mat_sour; %matice souradnic x, y, z z matice souradnic
 %na kazdou primku bude potreba 5 bodu 02, A, B, G1, G2
T_21M=[];
G1M=[];
G2M=[];
i_M=[];
j_M=[];
k_M=[];
S_12M=[];
cos_aM=[];
cos_bM=[];
cos_gM=[];
vel=size(mat_sour);
cyk=vel(1);
 
for i=1:5:cyk;
    i;
 XYZ_u5=[XYZ(i,:);XYZ(i+1,:);XYZ(i+2,:);XYZ(i+3,:);XYZ(i+4,:)];
[T_21,ii,jj,kk,G1,G2,S_12,cos_a,cos_b,cos_g] = transformace12(XYZ_u5);
T_21M=[T_21M, T_21];
G1M=[G1M;G1];
G2M=[G2M;G2];
i_M=[i_M; ii];
j_M=[j_M; jj];
k_M=[j_M; kk];
S_12M=[S_12M; S_12];
cos_aM=[cos_aM; cos_a];
cos_bM=[cos_bM; cos_b];
cos_gM=[cos_gM; cos_g];
end

cosM=[cos_aM cos_bM cos_gM];

%matice pocatecnich bodu
% PA :          Nx3-matrix containing starting point of N lines
% PB :          Nx3-matrix containing end point of N lines

PA = G1M;  % odd matrix
PB = G2M;  % even matrix


[P_intersect,distances] = lineIntersect3D(PA,PB)

pomoc=size(G1M);
k=pomoc(1);
XYZ2=[];
for i=1:k

    XYZ2=[XYZ2; G1M(i,:); G2M(i,:)] ;

end

XYZ3=[];
for i=1:2:size(XYZ2)
    for j=1:3
    a=[XYZ2(i,j);XYZ2(i+1,j)];
    XYZ3=[XYZ3 a];
    end
end

p=1; %prodlouzeni primky
s=[XYZ3(2,:)-XYZ3(1,:)];
XYZ4=[XYZ3(1,:)-p.*s;XYZ3(1,:)+p.*2*s];

xxx=XYZ4;
size=size(xxx);
yyy=size(2);
figure(2)
hold on

for i=1:3:yyy;

plot3(xxx(:,i),xxx(:,i+1),xxx(:,i+2))

end

plot3(P_intersect(1),P_intersect(2),P_intersect(3),'*')
grid on
hold off
xlabel('osa x')
ylabel('osa y')
zlabel('osa z')
legend('1','2','3','4','5','6')

  [I]=mat_set_fce(cos_aM,cos_bM, cos_gM);
  
  IxT=I(1);
  IyT=I(2);
  IzT=I(3);
  DxyT=I(4);
  DyzT=I(5);
  DzxT=I(6);
  
  xT=P_intersect(1);
  yT=P_intersect(2);
  zT=P_intersect(3);
   
  roz_steiner=[yT^2+zT^2 -xT*yT -xT*zT;
               -xT*yT xT^2+zT^2 -yT*zT;
               -xT*zT yT*zT xT^2+yT^2]/1000000;
  
 %
 I_T=[IxT -DxyT -DzxT
     -DxyT  IyT  -DyzT
     -DzxT -DyzT  IzT]
 
 I=I_T+roz_steiner
 
 toc