function qdot=get_qdot_from_u(t,q,flag,u)
%
% sliding/rolling disk simulation: compute qdot with given u 
%
% Synopsis:  get_qdot_from_u(t,q,flag,u)
%
%
% Input:     
%			t = time, the independent variable
%           q = 5x1 position vector coordinates of the disk
%           flag   = dummy argument for compatibility with ode45
%           u =  speed vector in the B frame; 
%                3x1 if rolling, 5x1 if sliding
% Output:    
%            qdot = 5x1 vector of the derivatives of q
% 
% 
% Global variable: 
%            radius   = radius of the disk (global variable specified in disk_simulation.m) 
%
%
% MEAM 535
%
%
global radius

% get positionis
q1 = q(1); q2 = q(2); q3 = q(3); q4 = q(4); q5 = q(5);

% check rolling or sliding
if (length(u) == 3) % rolling
    u1 = u(1); u2 = u(2); u3 = u(3);
    
    q1dot = u2/cos(q2);
    q2dot = -u1;
    q3dot = -u2*sin(q2)/cos(q2)+u3;
    q4dot = radius*cos(q1)*u2*tan(q2)-radius*cos(q1)*u3; % from rolling copnstraint
    q5dot = q4dot*tan(q1); % from rolling copnstraint
    
else % sliding
    u1 = u(1); u2 = u(2); u3 = u(3); u4 = u(4); u5 = u(5);
    
    q1dot = u2/cos(q2);
    q2dot = -u1;
    q3dot = -u2*sin(q2)/cos(q2)+u3;
    q4dot = u4;
    q5dot = u5;
           
end

qdot = [q1dot; q2dot; q3dot; q4dot; q5dot];