clear,clc
l_1=3;
l_2=1;
l_3=3;
l_4=2;
l_5=0.5;
%%%%%%%%D&H-Tabelle definieren!!! %%%%%%%%%
% 1.Pos.:alpha
% 2.Pos.:a
% 3.Pos.:d
% 4.Pos.:tau_min
% 5.Pos.:tau_max
Tabelle=[0,0,0,0,0;-pi()/2,0,2,0,0;-pi()/4,0,2,0,2*pi()];

%%%%%%%%Tabelle auswerten zu T verarbeiten %%%%%%%%%
G=size(Tabelle);              
g=G(1,1);               %Anz. Zeilen der D&H-Tabelle  
T=zeros(1:g,1:4);
T(1:g,1:3)=Tabelle(1:g,1:3);

%%%%%%%%Intervall definieren %%%%%%%%%
intervall=32;
l=0;

%%%%%%%%Main-Loop%%%%%%%%%
while(l<=intervall)
    
    %%%%%%%%Wert eines Schrittes bestimmen %%%%%%%%%
    h=1;
    while(h<=g)
        T(h,4)=(Tabelle(h,5)-Tabelle(h,4))/intervall*l;
        h=h+1;
    end 
    
    %%%%%%%%Initialisieren von TFM, TF_TOT, O %%%%%%%%%
    TFM=zeros(4,4*g);       %Transfermatrix pro Zeile 
    TF_TOT=zeros(4,4*g);    %ganze Transfermatrix
    O=zeros(4,g+1);
    
    %%%%%%%%Transformationsmatrix generieren %%%%%%%%%
    i=1;                    %Integrator                     
    while(i<=g)
        %Transfermatrix von i-1 bis i
        TFM(1:4,(4*i-3):4*i)=DoMatrix(T,i);
    
        %Transfermatrixen von 0 bis i    
        if(i==1)
            TF_TOT(1:4,(4*i-3):4*i)=TFM(1:4,(4*i-3):4*i);
        else
            TF_TOT(1:4,(4*i-3):4*i)=TF_TOT(1:4,(4*(i-1)-3):4*(i-1))*TFM(1:4,(4*i-3):4*i);
        end
        i=i+1;
    end
    
    %%%%%%%%Plotten %%%%%%%%%
    i=1;
    O(1:4,1)=[0;0;0;1];
    clf('reset')
    while(i<=g)
        A=TF_TOT(1:4,(4*i-3):4*i);
        O(1:4,i+1)=inv(A)*O(1:4,i);
    
        ax(i,1:100)=linspace(O(1,i),O(1,i+1));
        ay(i,1:100)=linspace(O(2,i),O(2,i+1));
        az(i,1:100)=linspace(O(3,i),O(3,i+1));
        
        figure(1)
        plot3(ax(i,1:100),ay(i,1:100),az(i,1:100))
        axis([-10 10 -10 10 -10 10]),grid on
        hold on
        i=i+1;        
    end
    %%%%%%%%Warteschleife %%%%%%%%%
    l=l+1;
    pause(0.05)
end
    
disp('end')