function project

close all
ts=[0,20];


g=9.81;
L1=15;
L2=1.5;
k1=70;
k2=70;
R1=0;
R2=0;
m1=2;
m2=3;

% speed is the real-time speed of the animation
speed=1;

% one determins what mass to trace
one=2;

smoothness=0.05;

% axes is the axis of the animate box
axes=[-20,20,-25,5];

% initial conditions
inits=[10,0,0,0,0,0, 15.5,0,0,0,0,0];

sets=odeset('refine',8,'AbsTol',1e-6,'RelTol',.5e-4);
[t,x]=ode45(@func,ts,inits,[sets],g,L1,L2,k1,k2,R1,R2,m1,m2);


Length_1=(x(:,1).^2+x(:,3).^2+x(:,5).^2).^(1/2);
Length_2=((x(:,1)-x(:,7)).^2+(x(:,3)-x(:,9)).^2+(x(:,5)-x(:,11)).^2).^(1/2);

Kinetic_1=1/2*m1*(x(:,2).^2+x(:,4).^2+x(:,6).^2);
Kinetic_2=1/2*m2*(x(:,8).^2+x(:,10).^2+x(:,12).^2);
Kinetic=Kinetic_1+Kinetic_2;
Potential_s1=1/2*k1*(Length_1-L1).^2;
Potential_s2=1/2*k2*(Length_2-L2).^2;
Potential_g1=m1*g*x(:,5);
Potential_g2=m2*g*x(:,11);
Potential_s=Potential_s1+Potential_s2;
Potential_g=Potential_g1+Potential_g2;
Potential=Potential_s+Potential_g;
Energy=Kinetic+Potential;
theEnd=size(Energy);
theEnd=theEnd(1);

ErrorSize=size(Energy)
ErrorSize=ErrorSize(1);
Percent_Error=(sum(Energy)/ErrorSize-Energy(1))/Energy(1)
plot(t,Energy)
legend('Potential+Kinetic')
xlabel(Percent_Error)

figure

animate([x(:,1),x(:,7)],'X',[x(:,5),x(:,11)],'Z',t,speed,axes,[x(:,2),x(:,8)],one,ts,smoothness);


function xprime=func(t,x,g,L1,L2,k1,k2,R1,R2,m1,m2)
xprime=zeros(12,1);
xprime(1)=x(2);
xprime(2)=k1/m1*x(1)*(L1/sqrt(x(1)^2+x(3)^2+x(5)^2)-1)...
    +k2/m1*(x(1)-x(7))*(L2/sqrt((x(1)-x(7))^2+(x(3)-x(9))^2+(x(5)-x(11))^2)-1)...
    -R1/m1*x(2);

xprime(3)=x(4);
xprime(4)=k1/m1*x(3)*(L1/sqrt(x(1)^2+x(3)^2+x(5)^2)-1)...
    +k2/m1*(x(3)-x(9))*(L2/sqrt((x(1)-x(7))^2+(x(3)-x(9))^2+(x(5)-x(11))^2)-1)...
    -R1/m1*x(4);

xprime(5)=x(6);
xprime(6)=k1/m1*x(5)*(L1/sqrt(x(1)^2+x(3)^2+x(5)^2)-1)...
    +k2/m1*(x(5)-x(11))*(L2/sqrt((x(1)-x(7))^2+(x(3)-x(9))^2+(x(5)-x(11))^2)-1)...
    -R1/m1*x(6)-g;

xprime(7)=x(8);
xprime(8)=k2/m2*(x(7)-x(1))*(L2/sqrt((x(7)-x(1))^2+(x(9)-x(3))^2+(x(11)-x(5))^2)-1)...
    -R2/m2*x(8);

xprime(9)=x(10);
xprime(10)=k2/m2*(x(9)-x(3))*(L2/sqrt((x(7)-x(1))^2+(x(9)-x(3))^2+(x(11)-x(5))^2)-1)...
    -R2/m2*x(10);

xprime(11)=x(12);
xprime(12)=k2/m2*(x(11)-x(5))*(L2/sqrt((x(7)-x(1))^2+(x(9)-x(3))^2+(x(11)-x(5))^2)-1)...
    -R2/m2*x(12)-g;