% Double pendulum equations of motion. 
% CCK 2016-Sep-17
%
function state_t = dp(state, t)
% The state vector is:
%     state = [theta1, theta2, p1, p2]
% I use the canonical momenta p1 and p2 conjugate to angles theta1 and theta2, which
% are the angles of the inner and outer arms with respect to the gravity vector.
% Note that there is no explicit time dependence. 
%
% Time t is an unused input parameter, required for compatibility with LSODE. 

% System parameters (cgs):
global L M g
% Typical values might be:
%  L = 1.0;   % (cm) Length of both pendulum segments
%  M = 0.1;   % (g) Mass of both pendulum bobs
%  g = 981;   % (cm/s^2) Acceleration of gravity
ML2 = M*L*L; % This combination is used repeatedly
MgL = M*g*L; %         "             " 

% Decode state variable
theta1 = state(1);
theta2 = state(2);
p1     = state(3);
p2     = state(4);
cos21  = cos(theta2-theta1); % This expression is used repeatedly
sin21  = sin(theta2-theta1); %         "            "

% Equations of motion for each state variable
theta1_t = (p1  -  p2*cos21) / (ML2*(2 - cos21^2));
theta2_t = (p1*cos21 - 2*p2) / (ML2*(cos21^2 - 2));
p1_t =  ML2*theta1_t*theta2_t*sin21 - 2*MgL*sin(theta1);
p2_t = -ML2*theta1_t*theta2_t*sin21 -   MgL*sin(theta2);

% Return time derivative of state
state_t = [theta1_t, theta2_t, p1_t, p2_t];