function [du] = StateSpace_new(t, u, IDDATA, DYDATA)
%{
Function for generating the state space.
--------------------------------------------------------------------------
Syntax :
[du] = StateSpace(t, u, TimeID, IDDATA)
--------------------------------------------------------------------------
Function Description :
This function consturcts the state space form of the equations of motion in
order to let the numerical solver of matlab handle the problem.
--------------------------------------------------------------------------
%}
TimeID = floor((t/0.01)+1);
 
q1 = u(1);      q2 = u(2);      q3 = u(3);
dq1 = u(4);     dq2 = u(5);     dq3 = u(6);

% define the muscle force direction vector, magnitude and lever arms at
% each time step of the integrator (solver)
for muscleID = 1:6
Dir_inT(:, muscleID) = interp1(IDDATA.t(TimeID:TimeID+1),[IDDATA.ForceDirection{TimeID}(:,muscleID)';IDDATA.ForceDirection{TimeID+1}(:,muscleID)'],t)';
Rho_inT(:, muscleID) = interp1(IDDATA.t(TimeID:TimeID+1),[IDDATA.LeverArm{TimeID}(:,muscleID)';IDDATA.LeverArm{TimeID+1}(:,muscleID)'],t)';                               ;
F_final(muscleID) = interp1(IDDATA.t(TimeID:TimeID+1),IDDATA.MuscleForces(muscleID,TimeID:TimeID+1),t);
end

GeneralizedForces

Q1 = Q(1,1);  Q2 = Q(2,1); Q3 = Q(3,1);

GeneralizedAccelerations;

du(1) = u(4);
du(4) = DDq(1);

du(2) = u(5);
du(5) = DDq(2);

du(3) = u(6);
du(6) = DDq(3);


du=du';
return;