function [xnew,energies] = s2stransition(x, walk, parms)
% xnew = s2stransition(x, walk) returns the new state given the state
% prior to heel contact and a toe-off impulse P. The effect of the
% toe-off impulse and the heel impact are computed as perfectly inelastic
% impulses, and impulse-momentum and conservation of angular momentum
% principles are used to find the velocities after impact. There is no
% change in configuration except that after heel strike the legs are
% switched. For heel strike, a component of angular momentum conserved
% about the point of contact, and for the trailing leg about the hip.
% walk is a walksw2 object containing parameters.
% [xnew,energies] = s2stransition(x, walk) also returns the energies
% before and after the step-to-step transition
% x = [qstance, qsswing ustance uswing]'
% where qsswing is defined relative to the stance leg
% Arthur D. Kuo, see:
% Kuo, A. D. (2002) Energetics of actively powered locomotion using the
% simplest walking model, Journal of Biomechanical Engineering, 124: 113-120.
% Kuo, A. D. (2001) A simple model predicts the step length-speed
% relationship in human walking, Journal of Biomechanical Engineering,
% 123: 264-269.
% CHANGES
% Added parms to input list to speed up simulations
if nargin < 3
parms = get(walk, 'parms');
end
P = parms.P; % the toe-off impulse
q1 = x(1); q2 = x(2); u1 = x(3); u2 = x(4);
c2t = cos(2*q1); s2t = sin(2*q1);
xnew(1,1) = -q1;
xnew(1,2) = -2*q1;
xnew(1,3) = c2t*u1 + s2t*P;
xnew(1,4) = c2t*(1-c2t)*u1 + (1-c2t)*s2t*P;
%mvcombefore = [-u1*cos(q1) -u1*sin(q1)];
%mvcomintermediate = mvcombefore + P*[-sin(q1) cos(q1)];
%mvcomafter = [-xnew(3)*cos(q1) -xnew(3)*sin(q1)];
energies.before = energy(walk, x); % energies before impulse
energies.intermediate = energies.before; energies.intermediate.KE = energies.intermediate.KE + 0.5*P*P;
energies.intermediate.total = energies.intermediate.total + 0.5*P*P;
energies.after = energy(walk, xnew);
energies.pushoffwork = 0.5*P*P;
energies.heelstrikework = energies.after.KE - energies.intermediate.KE;