solver

clear all;
clc;

global epsSqr l m g RKG xOld tau eps

dim    = 4;
stage  = 3;
l      = 1;
m      = 1;
g      = 1;
eps    = 1e-2;
epsSqr = eps^2;
tau    = 0.01;
T      = 10;
nSteps = T/tau;

b   = [5/18 4/9 5/18];
RKG = [5/36 2/9-sqrt(15)/15 5/36-sqrt(15)/30;...
       5/36+sqrt(15)/24 2/9 5/36-sqrt(15)/24;...
       5/36+sqrt(15)/30 2/9+sqrt(15)/15 5/36];
d   = b/RKG;
%x0 = [1; 0; 0; 0];
x0 = [l+eps; 0; 0; 0];
z0 = zeros(dim*stage,1);
xOld = x0;
x_soln = zeros(dim,nSteps+1);
x_soln(:,1) = x0;


options = optimset('Display','none','TolFun',1e-8,'TolX',1e-8);


for step = 1:nSteps

    time = step*tau;
    %display(time);

    zNew = fsolve(@f_RKG,z0,options);
    zNew = reshape(zNew,dim,stage);
    xNew = xOld + zNew*d';
    x_soln(:,step+1) = xNew;
    xOld = xNew;

end

x1 = x_soln(1,:);
x2 = x_soln(2,:);
v1 = x_soln(3,:);
v2 = x_soln(4,:);
time = 0:tau:T;

subplot(2,1,1)
plot(x1,x2); axis equal;

Kinetic   = m/2*(v1.^2 + v2.^2);
Potential = m*g*x2 + 1/(2*epsSqr)*(sqrt(x1.^2 + x2.^2) - l).^2;
Energy  = Kinetic + Potential;
subplot(2,1,2)
hold on;
plot(time,Kinetic,'y',time,Potential,'g');
plot(time,Energy,'b','linewidth',3)
plot(time,x1,'r', time, x2, 'k');
legend('kinetic energy','potential energy','total energy','x1','x2');
hold off;