% Jerry Pratt
% April 4, 2000
% m file for printing walking animation across bottom of screen for thesis.



dir = -1.0;

L = 0.406;
W = L/12;
V = L/4;

FOOT_H = 0.064;
FOOT_BACK = 0.134;
FOOT_FRONT = .082;


foot = [
-W/2 W/2    1
W/2 W/2     1
W/2 -FOOT_H+W  1
FOOT_BACK -FOOT_H+W 1
FOOT_BACK -FOOT_H    1
-FOOT_FRONT -FOOT_H    1
-FOOT_FRONT -FOOT_H+W    1
-W/2 -FOOT_H+W  1
-W/2 W/2 1
];

leg = [
-W/2 W/2    1
W/2 W/2     1
W/2 -L-W/2  1
-W/2 -L-W/2 1
-W/2 W/2    1
];

W = W*1.5;

body = [
3*W -W/2  1
3*W W   1
W W     1
W 2.5*W   1
-W 2.5*W  1
-W W    1
-3*W W  1
-3*W -W/2 1
3*W -W/2  1
];


body = [
V/2 -V/2  1
3*V 2*V   1
V 2.5*V   1
-V 2.5*V  1
-3*V 2*V  1
-V/2 -V/2 1
V/2 -V/2  1
];

skip = 3;  %0.02 seconds per image

figure(1);clg;

LEN = 250
for n = 1:LEN/skip;
i = (n-1)*skip + 1;
n

T_O_B = [
cos(q_theta(i))   -sin(q_theta(i))   -q_x(i)
sin(q_theta(i))  cos(q_theta(i))   q_z(i)
     0                     0                  1
];

T_B_LH = [
cos(q_lh(i))   -sin(q_lh(i))       0
sin(q_lh(i))  cos(q_lh(i))       0
     0                     0              1
];

T_B_RH = [
cos(q_rh(i))   -sin(q_rh(i))       0
sin(q_rh(i))  cos(q_rh(i))       0
     0                     0              1
];


T_LH_LK = [
cos(q_lk(i))   -sin(q_lk(i))       0
sin(q_lk(i))  cos(q_lk(i))       -L
     0                     0              1];

T_RH_RK = [
cos(q_rk(i))   -sin(q_rk(i))   0
sin(q_rk(i))  cos(q_rk(i))   -L
     0                     0          1];


T_LK_LA = [
cos(q_la(i))   -sin(q_la(i))       0
sin(q_la(i))  cos(q_la(i))       -L
     0                     0              1];

T_RK_RA = [
cos(q_ra(i))   -sin(q_ra(i))   0
sin(q_ra(i))  cos(q_ra(i))   -L
     0                     0          1];


body_2 = (T_O_B * body')';

left_upper_leg = (T_O_B * T_B_LH * leg')';
left_lower_leg = (T_O_B * T_B_LH * T_LH_LK * leg')';
left_foot = (T_O_B * T_B_LH * T_LH_LK * T_LK_LA * foot')';

right_upper_leg = (T_O_B * T_B_RH * leg')';
right_lower_leg = (T_O_B * T_B_RH * T_RH_RK * leg')';
right_foot = (T_O_B * T_B_RH * T_RH_RK * T_RK_RA * foot')';

figure(1);clf;

h=subplot(1,1,1);

%pos=get(h,'position');
%pos(1)=.64;
%pos(2)=0.005;  % less than .07
%set(h,'position',pos);


% left leg first.

gray1 = [0.0 0.0 0.0];
gray2 = [1.0 1.0 1.0];
gray3 = [0.7 0.7 0.7];

fill(-dir*left_upper_leg(:,1),left_upper_leg(:,2),gray1);hold on;
fill(-dir*left_lower_leg(:,1),left_lower_leg(:,2),gray1);
fill(-dir*left_foot(:,1),left_foot(:,2),gray1);

% then fill in body over it.

fill(-dir*body_2(:,1),body_2(:,2),gray2);

% then fill in right leg over everything

fill(-dir*right_upper_leg(:,1),right_upper_leg(:,2),gray3);
fill(-dir*right_lower_leg(:,1),right_lower_leg(:,2),gray3);
fill(-dir*right_foot(:,1),right_foot(:,2),gray3);

%axis('equal');

axis([-.6,5.0,-0.1,1.15]);
%axis([-.6,3.2,0,1.60]);
axis off;
%pause;


SF_cart_print

end;
