function Animacao3D( Q,L,R,M,XYRef,Sl,COPy,GAIT )

%--------------------------------------------------------------------------------------------
%-- Simulaçao offline ----------------------------------------------------------------------
%--------------------------------------------------------------------------------------------
for i=1:size(Q,3), %-- Movimento dividido em ?size? fases ----------------------------------
for k=1:size(Q,2), %-- ... ao longo do tempo -----------------------------------------------
    
   %-- Posiçoes angulares em funçao do tempo -----------------------------------------------
    Q_t = [ Q(1,k,i)   Q(2,k,i)   Q(3,k,i)   Q(4,k,i)    Q(5,k,i)   Q(6,k,i) ...
            Q(7,k,i)   Q(8,k,i)   Q(9,k,i)   Q(10,k,i)   Q(11,k,i)  Q(12,k,i) ];  
   %-- Posiçao das juntas do robo ----------------------------------------------------------  
    Ai = CinDir12dof( Q_t,L,XYRef );   %-- SupLeg = 1 -> RL
    [P,C,F] = CoordJuntas( Ai,XYRef );
    P_AUX(:,:,k) = P; C_AUX(:,:,:,k) = C; F_AUX(:,:,:,k) = F;
   %-- Rasto da trajectoria da anca --------------------------------------------------------
    Rasto_AUX(1,:,k) = P(9,:);
    Rasto_AUX(2,:,k) = P(18,:);      
    %-- Centro de Massa --------------------------------------------------------------------- 
    COG(:,k,i) = RobCOG( Q_t,L,R,M );
     %COG(1,:,i) = COGi(1); COG(2,:,i) = COGi(2); COG(3,:,i) = COGi(3);
end
   %-- Guardar fase SS e DS ----------------------------------------------------------------
    if i == 1, P_ON    = P_AUX; C_ON    = C_AUX; F_ON    = F_AUX; Rasto_ON    = Rasto_AUX; end 
    if i == 2, P_RL    = P_AUX; C_RL    = C_AUX; F_RL    = F_AUX; Rasto_RL    = Rasto_AUX; end 
    if i == 3, P_RL2LL = P_AUX; C_RL2LL = C_AUX; F_RL2LL = F_AUX; Rasto_RL2LL = Rasto_AUX; end
   clear P_AUX C_AUX F_AUX Rasto_AUX
end
COG_ON = COG(:,:,1); COG_RL = COG(:,:,2); COG_RL2LL = COG(:,:,3); 
%size(P_RL), size(C_RL), size(F_RL), size(Rasto_RL), pause

%--------------------------------------------------------------------------------------------
%-- Formaçao do ciclo completo --------------------------------------------------------------
%--------------------------------------------------------------------------------------------
%-- Coordenadas das juntas ------------------------------------------------------------------
 for k=1:size(P_RL,1),
     if k ~=9 & k ~= 10,
        P_AUX_LL(k,1,:) =  P_RL(size(P_RL,1)-k+1,1,:)+Sl;           P_AUX_LL2RL(k,1,:) =  P_RL2LL(size(P_RL,1)-k+1,1,:)+Sl;  
        if GAIT == 1,
        P_AUX_LL(k,2,:) = -P_RL(size(P_RL,1)-k+1,2,:)+COPy+L(9)/2;  P_AUX_LL2RL(k,2,:) = -P_RL2LL(size(P_RL,1)-k+1,2,:)+COPy+L(9)/2;  
        else
        P_AUX_LL(k,2,:) = -P_RL(size(P_RL,1)-k+1,2,:)+L(9);         P_AUX_LL2RL(k,2,:) = -P_RL2LL(size(P_RL,1)-k+1,2,:)+L(9);
        end
        P_AUX_LL(k,3,:) =  P_RL(size(P_RL,1)-k+1,3,:);              P_AUX_LL2RL(k,3,:) =  P_RL2LL(size(P_RL,1)-k+1,3,:);
     else
         P_AUX_LL(k,1,:) =  P_RL(k,1,:)+Sl;                         P_AUX_LL2RL(k,1,:) =  P_RL2LL(k,1,:)+Sl;      
         if GAIT == 1,
         P_AUX_LL(k,2,:) = -P_RL(k,2,:)+COPy+L(9)/2;                P_AUX_LL2RL(k,2,:) = -P_RL2LL(k,2,:)+COPy+L(9)/2;   
         else
         P_AUX_LL(k,2,:) = -P_RL(k,2,:)+L(9);                       P_AUX_LL2RL(k,2,:) = -P_RL2LL(k,2,:)+L(9); 
         end         
         P_AUX_LL(k,3,:) =  P_RL(k,3,:);                            P_AUX_LL2RL(k,3,:) =  P_RL2LL(k,3,:);
     end
 end
 P_LL = P_AUX_LL;  P_LL2RL = P_AUX_LL2RL;
%-- Coordenadas dos cilindros --------------------------------------------------------------
 for k=1:size(C_RL,3),
     C_AUX_LL(1:2,:,k,:) =  C_RL(1:2,:,size(C_RL,3)-k+1,:)+Sl;           C_AUX_LL2RL(1:2,:,k,:) =  C_RL2LL(1:2,:,size(C_RL,3)-k+1,:)+Sl;  
     if GAIT == 1,
     C_AUX_LL(3:4,:,k,:) = -C_RL(3:4,:,size(C_RL,3)-k+1,:)+COPy+L(9)/2;  C_AUX_LL2RL(3:4,:,k,:) = -C_RL2LL(3:4,:,size(C_RL,3)-k+1,:)+COPy+L(9)/2;   
     else
     C_AUX_LL(3:4,:,k,:) = -C_RL(3:4,:,size(C_RL,3)-k+1,:)+L(9);         C_AUX_LL2RL(3:4,:,k,:) = -C_RL2LL(3:4,:,size(C_RL,3)-k+1,:)+L(9); 
     end       
     C_AUX_LL(5:6,:,k,:) =  C_RL(5:6,:,size(C_RL,3)-k+1,:);              C_AUX_LL2RL(5:6,:,k,:) =  C_RL2LL(5:6,:,size(C_RL,3)-k+1,:);  
 end
 C_LL = C_AUX_LL;  C_LL2RL = C_AUX_LL2RL;
%-- Coordenadas do pe livre -----------------------------------------------------------------
 F_LL(1,:,:,:) =  F_RL(1,:,:,:)+Sl;           F_LL2RL(1,:,:,:) =  F_RL2LL(1,:,:,:)+Sl;
 if GAIT == 1,
 F_LL(2,:,:,:) = -F_RL(2,:,:,:)+COPy+L(9)/2;  F_LL2RL(2,:,:,:) = -F_RL2LL(2,:,:,:)+COPy+L(9)/2;
 else
 F_LL(2,:,:,:) = -F_RL(2,:,:,:)+L(9);         F_LL2RL(2,:,:,:) = -F_RL2LL(2,:,:,:)+L(9);
 end 
 F_LL(3,:,:,:) =  F_RL(3,:,:,:);              F_LL2RL(3,:,:,:) =  F_RL2LL(3,:,:,:);
%-- Coordenadas do pe livre -----------------------------------------------------------------
 Rasto_LL(:,1,:) =  Rasto_RL(:,1,:)+Sl;           Rasto_LL2RL(:,1,:) =  Rasto_RL2LL(:,1,:)+Sl;
 if GAIT == 1,
 Rasto_LL(:,2,:) = -Rasto_RL(:,2,:)+COPy+L(9)/2;  Rasto_LL2RL(:,2,:) = -Rasto_RL2LL(:,2,:)+COPy+L(9)/2;   
 else
 Rasto_LL(:,2,:) = -Rasto_RL(:,2,:)+L(9);         Rasto_LL2RL(:,2,:) = -Rasto_RL2LL(:,2,:)+L(9); 
 end 
 Rasto_LL(:,3,:) =  Rasto_RL(:,3,:);              Rasto_LL2RL(:,3,:) =  Rasto_RL2LL(:,3,:);
%-- Centro de massa -------------------------------------------------------------------------
 COG_LL(1,:) = COG_RL(1,:)+Sl;           COG_LL2RL(1,:) =  COG_RL2LL(1,:)+Sl;
 if GAIT == 1,  
 COG_LL(2,:) = -COG_RL(2,:)+COPy+L(9)/2; COG_LL2RL(2,:) = -COG_RL2LL(2,:)+COPy+L(9)/2;
 else
 COG_LL(2,:) = -COG_RL(2,:)+L(9);        COG_LL2RL(2,:) = -COG_RL2LL(2,:)+L(9);
 end
 COG_LL(3,:) =  COG_RL(3,:);             COG_LL2RL(3,:) =  COG_RL2LL(3,:);
%figure(2), plot(1:125,[COG_ON(2,:) COG_RL(2,:) COG_RL2LL(2,:) COG_LL(2,:) COG_LL2RL(2,:)]), pause
%--------------------------------------------------------------------------------------------
%-- Simulaçao online ------------------------------------------------------------------------
%--------------------------------------------------------------------------------------------
%mov = avifile('humanimate.avi');
NR_ciclos = 1;
for k=1:size(Q,2), %-- ... ao longo do tempo -----------------------------------------------
%-- Fase1: Arranque -------------------------------------------------------------------------  
 DesenharRobo( P_ON(:,:,k),C_ON(:,:,:,k),F_ON(:,:,:,k),Rasto_ON(:,:,1:k),XYRef, COG_ON(:,k) )
% if NR_ciclos>=2, F = getframe; mov = addframe(mov,F); end
end

while 1,  %-- Ciclo infinito ---------------------------------------------------------------
for k=1:size(Q,2), %-- ... ao longo do tempo -----------------------------------------------
%-- Fase2: SS, RL ---------------------------------------------------------------------------  
 DesenharRobo( P_RL(:,:,k),C_RL(:,:,:,k),F_RL(:,:,:,k),Rasto_RL(:,:,1:k),XYRef, COG_RL(:,k) )
% if NR_ciclos>=2, F = getframe; mov = addframe(mov,F); end
end
%-- Fase3: DS, RL2LL ------------------------------------------------------------------------  
for k=1:size(Q,2), %-- ... ao longo do tempo ------------------------------------------------
 DesenharRobo( P_RL2LL(:,:,k),C_RL2LL(:,:,:,k),F_RL2LL(:,:,:,k),Rasto_RL2LL(:,:,1:k),XYRef, COG_RL2LL(:,k) )
% if NR_ciclos>=2, F = getframe; mov = addframe(mov,F); end
end
%-- Fase4: SS, LL --------------------------------------------------------------------------
if GAIT == 1, XYRef = XYRef + [Sl COPy+L(9)/2];
else          XYRef = XYRef + [Sl L(9)]; end
for k=1:size(Q,2), %-- ... ao longo do tempo -----------------------------------------------
 DesenharRobo( P_LL(:,:,k),C_LL(:,:,:,k),F_LL(:,:,:,k),Rasto_LL(:,:,1:k),XYRef, COG_LL(:,k) )
% if NR_ciclos>=2, F = getframe; mov = addframe(mov,F); end
end
%-- Fase5: DS, LL2RL -----------------------------------------------------------------------
for k=1:size(Q,2), %-- ... ao longo do tempo -----------------------------------------------
 DesenharRobo( P_LL2RL(:,:,k),C_LL2RL(:,:,:,k),F_LL2RL(:,:,:,k),Rasto_LL2RL(:,:,1:k),XYRef, COG_LL2RL(:,k) )
% if NR_ciclos>=2, F = getframe; mov = addframe(mov,F); end
end

%-- Fora da area de trabalho --------------------------------------------------------------
if P_LL2RL(:,1,:) > 1.15, 
   XPos = -NR_ciclos*2*Sl; NovoCiclo
   XYRef = [-2*Sl 0]; 
   COG_RL(1,:) = COG_RL(1,:)-NR_ciclos*2*Sl; COG_RL2LL(1,:) = COG_RL2LL(1,:)-NR_ciclos*2*Sl;
   COG_LL(1,:) = COG_LL(1,:)-NR_ciclos*2*Sl; COG_LL2RL(1,:) = COG_LL2RL(1,:)-NR_ciclos*2*Sl;
   NR_ciclos = 0; 
   %movie2avi(FMOV,'HUMANIMATE.avi');
   %mov = close(mov);
   tecla = input('Prima uma tecla para continuar ou ESC para terminar ...: ','s');
   if strcmp(tecla,'')==1, return, end
else
    %-- Actualizar coordenada na direcçao do movimento ------------------------------------
     XPos = 2*Sl; NovoCiclo
    %-- Actualizar coordenadas de referencia ----------------------------------------------
     XYRef = [2*Sl*NR_ciclos 0]; 
     COG_RL(1,:) = COG_RL(1,:)+2*Sl; COG_RL2LL(1,:) = COG_RL2LL(1,:)+2*Sl;
     COG_LL(1,:) = COG_LL(1,:)+2*Sl; COG_LL2RL(1,:) = COG_LL2RL(1,:)+2*Sl;
     NR_ciclos = NR_ciclos+1;
end

end