clear; clc;
%% Interpolacja obrotu

P=[1 0 0; 
   0 1 0;
   0 0 1];

v=[1;1;1];         % wektor osi  obrotu
fi=120;             % kąt obrotu

% q=qrot(v,fi);      % kwtaernion  obrotu


N=100; % liczba węzłów interpolacji
u=v'/norm(v);
df=fi/N;
ds=cosd(df/2);
dv=u*sind(df/2);
dq=[ds dv];
dQ=Qmatrix(dq);      % macierz obrotu

%% Rysowanie
figure(1); clf; scale=2; hold on;
view(-38,24);
set(gca,'XDir','reverse','YDir','reverse');
grid on;
box on;


axis manual;
axis([-1 1 -1 1 -1 1]*scale);
axis equal
line([0 0],[-1 1]*scale, [0 0],'LineWidth',1,'Color','k'); hold on;
line([-1 1]*scale,[0 0], [0 0],'LineWidth',1,'Color','k');
line([0 0], [0 0],[-1 1]*scale,'LineWidth',1,'Color','k');
line([0 v(1)/norm(v)],[0 v(2)/norm(v)],[0,v(3)/norm(v)]);

for i=1:N
    P=dQ*P;
    h=fill3(P(1,:),P(2,:),P(3,:),'r','FaceAlpha',0.4); 
    p=plot3(P(1,:),P(2,:),P(3,:),'ro','MarkerFaceColor','r');
%     pause(0.001);
    drawnow();
    delete([h,p]);
end
   h=fill3(P(1,:),P(2,:),P(3,:),'r','FaceAlpha',0.4); 
   p=plot3(P(1,:),P(2,:),P(3,:),'ro','MarkerFaceColor','r');
  
hold off;