%     function pendulum

function zp = pendulum(t, z) 

%  The purpose of this function is to calculate the derivative of:
%    z = [ alpha, beta, alphadot, betadot ]
%
%  which we will call zp.  So,
%    zp = [ alphadot, betadot, alphadotdot, betadotdot ]
%
%  The values of alphadot and betadot are given in z, so we really
%  only need to find alphadotdot and betadotdot.
%
%  We have to find these using the given differential equations.
%  To put the equations in standard form, we'd solve the two equations
%  simultaneously for these two variables (alphadotdot and betadotdot).
%
%  Instead of doing it "by hand" (or with Maple), we'll instead look
%  at it as if we have two equations with two unknowns, which we
%  want to solve numerically, using Gaussian elimination.  In matrix
%  form, the given equations are:
%
%    [                                        ] [             ]
%    [   L0                B cos(alpha-beta)  ] [ alphadotdot ]
%    [                                        ] [             ]  =
%    [ B cos(alpha-beta)       Lbar           ] [  betadotdot ]
%    [                                        ] [             ]
%
%
%            [                                                   ]
%            [  -G sin(alpha) - B sin(alpha-beta) betadot^2      ]
%         =  [                                                   ]
%            [  -GB/L0 sin(beta) + B sin(alpha-beta) alphadot^2  ]
%            [                                                   ]
%
%  Everything is known in these equations, except alphadotdot and
%  betadotdot.  So, we can just set up the matrices and solve using
%  Gaussian elimination for alphadotdot and betadotdot.
%

% These are the global variables.

global B GG Lbar L0 L1

% Convert from standard form to meaningful names.

alpha    = z(1) ;
beta     = z(2) ;
alphadot = z(3) ;
betadot  = z(4) ;

% Compute the right-hand sides of the equations for the dot-dot variables.

rhs(1) = -B*sin(alpha - beta)*betadot*betadot   -GG*sin(alpha) ;
rhs(2) =  B*sin(alpha - beta)*alphadot*alphadot -GG*sin(beta )*B/L0 ;

% Set up the matrix.

C(1,1) =  L0 ;
C(1,2) =  B * cos( alpha - beta ) ;
C(2,1) =  B * cos( alpha - beta ) ;
C(2,2) =  Lbar ;

% Solve for the two dot-dot variables.

tmp  =  C \ rhs' ;
alphadotdot = tmp(1) ;
betadotdot  = tmp(2) ;

% Convert back to standard form.
% Note that the variables in zp correspond to the dots of the
% variables in z.

zp = [ alphadot; betadot; alphadotdot; betadotdot ] ;

