out=init_graphics_3D(y,y3incr);

function out=init_graphics_3D(y,y3incr);

global g gam m R p01 p23 p34 pc3 pc4 p4f Icm3 Icm4 p4f drawing

% Code by Mariano Garcia ([e-mail]), 1998
% does graphics update for 3D walker integration

% set sines and cosines and define rotation matrices.

cO1 = cos(y(1));
sO1 = sin(y(1));

cO2 = cos(y(2));
sO2 = sin(y(2));

cO3 = cos(y(3));
sO3 = sin(y(3));

sO4 = sin(y(4));
cO4 = cos(y(4));

R01=[cO1 -sO1 0 ;sO1 cO1 0; 0 0 1];
R12=[cO2 -sO2 0 ;0 0 -1;sO2 cO2 0];
R23=[cO3 -sO3 0 ;0 0 -1;sO3 cO3 0];
R34=[cO4 -sO4 0 ;sO4 cO4 0; 0 0 1];

R10=R01';
R21=R12';
R32=R23';
R43=R34';

p01=[y(9) y(10) 0]'; % vector from the fixed basis to the contact point
                     % needed for rolling contact.

% define some points for plotting purposes

stfoot = p01+R01*R12*p23; % center of stance foot
sthip = R01*R12*R23*[p34(1) p34(2) 0]' + stfoot; % stance leg hip (not the
                                                 % "real" hip hinge)
swhip = R01*R12*R23*[0    0   p34(3)]' + sthip;  % swing leg hip (where the
                                                 % hinge is)
swfoot = R01*R12*R23*R34*p4f + swhip; % swing foot center
swend = -R01*R12*p23 + swfoot; % swing foot bottom

% reset the stick figure on the screen.
graphics = [p01';stfoot';sthip';swhip';swfoot';swend';];
set(drawing,'xdata',graphics(:,1),'ydata',graphics(:,2),'zdata',graphics(:,3), ...
'erasemode','xor');
drawnow;