function [thit,xhit_minus,tani,xani] = cg_find_impact(tvals,xvals)

% Find the state just BEFORE the swing leg hits the ground
% Katie Byl (1/19/2010). ECE 5194D, Winter 2010 (UCSB)

qns = xvals(:,1);  % non-stance leg, wrt y axis (CCW)
qs = xvals(:,2);   % stance leg, wrt y axis (CCW)
q1 = qs+pi/2;  % stance leg, wrt x axis (CCW)
q2 = qns+pi/2-pi; % non-stance leg, wrt x axis (CCW)

% Constants defining geometry and mass distribution:
cg_params

xfoot = L*cos(q1) + L*cos(q2);
yfoot = L*sin(q1) + L*sin(q2);

% First, find when swing leg first goes past stance leg, in x:
fi = find(xfoot>0);

% Then, look for time AFTER this when swing leg is BELOW ground, in y:
fi2 = find(yfoot(fi(1):end) < -tan(gam_slope)*xfoot(fi(1):end));

fiu = fi(1)+fi2(1) + [-2 -1];
%fiu = fi(1)+fi2(1) + [-3:0];

% Find collision time:
thit = interp1(atan2(yfoot(fiu),xfoot(fiu))+gam_slope,tvals(fiu),0);
% Find state at ground collision (just BEFORE):
X1hit = interp1(atan2(yfoot(fiu),xfoot(fiu))+gam_slope,xvals(fiu,1),0);
X2hit = interp1(atan2(yfoot(fiu),xfoot(fiu))+gam_slope,xvals(fiu,2),0);
X3hit = interp1(atan2(yfoot(fiu),xfoot(fiu))+gam_slope,xvals(fiu,3),0);
X4hit = interp1(atan2(yfoot(fiu),xfoot(fiu))+gam_slope,xvals(fiu,4),0);
xhit_minus = [X1hit, X2hit, X3hit, X4hit];  % PRE-collision state
% (Above is the PRE-collision state! To get POST-collision state, 
% swap feet and balance angular momentum at impact.)

% define a continuous list of times and states (for animation later)
tani=tvals(1:fiu(1));
%tani=tvals(1:fiu(2));
tani(end+1) = thit;
xani=xvals(1:fiu(1),:);
%xani=xvals(1:fiu(2),:);
xani(end+1,:) = xhit_minus;
