% This files contains the ordinary differential equations
% for a double pendulum.
%
% The first order kinematic transform function Fij
% and the convective accelerations gk are cut-and-pasted
% from the symbolic derivations as done by: hwa6sym.m
% 
% Homework Assigment 6, double pendulum.
% wb1413, Multibody Dynamics B, Spring 2005.

%     Arend L. Schwab 26-Feb-2003
%     Copyright (c) 2003 by TU-Delft, the Netherlands.

function dy = odehwa6(t,y)
global m l J g

% copy the input to meaningfull variables
ud = y(1);
vd = y(2);
 u = y(3);
 v = y(4);

% copy-and-pasted result from symbolic derivations
Fij = [      -1/2*l*sin(u),                         0
              1/2*l*cos(u),                         0
                         1,                         0
 -l*sin(u)+1/2*l*sin(-u+v),          -1/2*l*sin(-u+v)
  l*cos(u)+1/2*l*cos(-u+v),          -1/2*l*cos(-u+v)
                         1,                        -1];
gk = [                                               -1/2*l*cos(u)*ud^2
                                                     -1/2*l*sin(u)*ud^2
                                                                      0
 -1/2*l*(2*cos(u)*ud^2+cos(-u+v)*ud^2-2*ud*cos(-u+v)*vd+cos(-u+v)*vd^2)
 -1/2*l*(2*sin(u)*ud^2-sin(-u+v)*ud^2+2*ud*sin(-u+v)*vd-sin(-u+v)*vd^2)
                                                                      0];
% mass matrix
Mik = diag([m, m, J, m, m, J]);

% reduced massmatrix
Mr=Fij.'*Mik*Fij;

% applied forces
fi = [m*g; 0; 0; m*g; 0; 0];
% applied generalized forces
Qj = [0; 0];

% reduced generalized forces vector
Qr=Qj+Fij.'*(fi-Mik*gk);

% solve for qdd
qdd = Mr\Qr;

% copy the qd and qdd to the output dy
dy=[qdd; ud; vd];