function pendA
%pendA.m

%Parameters
g=9.8;  %gravity
m1=2;   %mass of first particle
m2=2.2; %mass of second particle
r1=1.2; %constant 
r2=0.7; %constant

%                                       [th1 th2 th1d th2d]
[t,y]=ode45(@pendulum,linspace(0,5,300),[1 0.2 0.1 0.2],[],g,m1,m2,r1,r2);
th1=y(:,1);
th2=y(:,2);
th1d=y(:,3);
th2d=y(:,4);

%Constraint forces:
c1=zeros(size(t));  c2=c1;
for k=1:length(t)
    yp=pendulum(t(k),[th1(k) th2(k) th1d(k) th2d(k)],g,m1,m2,r1,r2);
    th1dd=yp(3);
    th2dd=yp(4);
    c1(k)=-(m1+m2)*g*cos(th1(k))...
          -m2*r2*(th1dd+th2dd)*sin(th2(k))...
          -m2*r2*(th1d(k)+th2d(k))^2*cos(th2(k))...
          -(m1+m2)*r1*th1d(k)^2;
    c2(k)=-m2*g*cos(th1(k)+th2(k))...
          +m2*r1*th1dd*sin(th2(k))...
          -m2*r2*(th1d(k)+th2d(k))^2 ...
          -m2*r1*th1d(k)^2*cos(th2(k));
end
fig=figure;
set(fig,'color',[1 1 1])
plot(t,c1,'r',t,c2,'b')

%Animate the motion!
fig1=figure;  
set(fig1,'color',[1 1 1],'backingstore','off','Doublebuffer','on');
for i=1:length(t)
    %construct a 2xN array of position vectors
    x=zeros(2);
    x(:,1)=r1*[sin(th1(i,1));-cos(th1(i,1))];
    x(:,2)=x(:,1)+r2*[sin(th1(i)+th2(i));-cos(th1(i)+th2(i))];
    plot([0 x(1,:)],[0 x(2,:)],'b',x(1,:),x(2,:),'ro',...
         0.5*[-r1 r1],[0 0],'k',[0 0],0.5*[-r1 r1],'k');
    axis equal
    xlim([-r1-r2,r1+r2])
    ylim([-r1-r2,r1+r2])
    drawnow
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function yp=pendulum(t,y,g,m1,m2,r1,r2);
%pendulum.m returns derivative information

th1=y(1);
th2=y(2);
th1d=y(3);
th2d=y(4);

D(2,2)=m2*r2^2;
D(1,2)=m2*(r2^2+r1*r2*cos(th2));
D(2,1)=D(1,2);
D(1,1)=(m1+m2)*r1^2+m2*r2^2+2*m2*r1*r2*cos(th2);

b(1,1)=m2*r1*r2*th2d*(2*th1d+th2d)*sin(th2)...
       -(m1+m2)*g*r1*sin(th1)...
       -m2*g*r2*sin(th1+th2);
b(2,1)=m2*r1*r2*th1d*th2d*sin(th2)...
       -m2*r1*r2*th1d*(th1d+th2d)*sin(th2)...
       -m2*g*r2*sin(th1+th2);

yp=[th1d;th2d;D\b];