      program DoublePend
*     Program to integrate the double pendulum equations
*     Ross Bannister, June 2001

      implicit none
      integer Nt,t,Nout
      real l1,l2,m1,m2,g,dt,y(4),pi

*     Parameters (SI units)
      l1=0.5
      l2=0.3
      m1=0.1
      m2=0.05
      g=10.
      dt=0.002

      pi=4.*atan(1.)

*     Initial conditions
*     Angle 1
      y(1)=170.*pi/180.
*     Angle 2
      y(2)=-120.*pi/180.
*     Rate of change angle 1
      y(3)=20.*pi/180.
*     Rate of change angle 2
      y(4)=30.*pi/180.

*     Number of time steps
      Nt=6000

*     Output every how many timesteps?
      Nout=1

      open (12,file='Dpend1')
      write (12,100)'# Output from double pendulum version 1'
      write (12,102)'# Lengths : ',l1,l2
      write (12,102)'# Masses  : ',m1,m2


      do t=1,Nt
        print*,t,Nt
        call RungeKutt4 (y,m1,m2,l1,l2,g,dt,pi)
        if (mod(t,Nout).eq.0)
     :    write (12,101) real(t)*dt,180.*y(1)/pi, 180.*y(2)/pi,
     :                   180.*y(3)/pi, 180.*y(4)/pi
      enddo

      close (12)
100   format ((A))
101   format (5F12.2)
102   format ((A),2F8.4)
      end


      subroutine matrixA (y,m1,m2,l1,l2,A)
*     To determine the elements of matrix A
      implicit none
      real y(4),m1,m2,l1,l2,A(2,2)
      real th1,th2,al1,al2,term1,term2,term3
*     Angle 1
      th1=y(1)
*     Angle 2
      th2=y(2)
*     Rate of change angle 1
      al1=y(3)
*     Rate of change angle 2
      al2=y(4)

      term1=l1*l2*m2*sin(th1-th2)
      term2=al2*term1
      term3=al1*term1

      A(1,1)=-1.*term2
      A(1,2)=term2
      A(2,1)=-1.*term3
      A(2,2)=term3

      return
      end


      subroutine matrixBinv (y,m1,m2,l1,l2,Binv)
*     To determine the elements of matrix B-inverse
      implicit none
      real y(4),m1,m2,l1,l2,Binv(2,2)
      real B(2,2),th1,th2,al1,al2,term1,det
*     Angle 1
      th1=y(1)
*     Angle 2
      th2=y(2)
*     Rate of change angle 1
      al1=y(3)
*     Rate of change angle 2
      al2=y(4)

      term1=m2*l1*l2*cos(th1-th2)

*     Construct the actual matrix B ...
      B(1,1)=(m1+m2)*l1*l1
      B(1,2)=term1
      B(2,1)=term1
      B(2,2)=m2*l2*l2

*     ... but we need its inverse
      det=B(1,1)*B(2,2)-B(1,2)*B(2,1)
      Binv(1,1)=B(2,2)/det
      Binv(1,2)=-1.*B(1,2)/det
      Binv(2,1)=-1.*B(2,1)/det
      Binv(2,2)=B(1,1)/det

      return
      end



      subroutine vectorf (y,m1,m2,l1,l2,g,f)
*     To determine the elements of vector f
      implicit none
      real y(4),m1,m2,l1,l2,g,f(2)
      real th1,th2,al1,al2,term1,term2
*     Angle 1
      th1=y(1)
*     Angle 2
      th2=y(2)
*     Rate of change angle 1
      al1=y(3)
*     Rate of change angle 2
      al2=y(4)

      term1=m2*l1*l2*al1*al2*sin(th1-th2)
      term2=sin(th1)

      f(1)=-1.*term1-g*(m1+m2)*l1*sin(th1)
      f(2)=term1-g*m2*l2*sin(th2)

      return
      end


      subroutine operate (A,Binv,f,dt,y,kk)
*     To perform operation k=Ty+g where T(A,Binv) and g(f)
*     (see notes)
      implicit none
      integer i,j,k
      real A(2,2),Binv(2,2),f(2),dt,y(4),kk(4)
      real Prod(2,2),total,vec(2)

*     Calculate -Binv A
      do i=1,2
        do j=1,2
          total=0.
          do k=1,2
            total=total-Binv(i,k)*A(k,j)
          enddo
          Prod(i,j)=total
        enddo
      enddo

*     Calculate Binv f
      do i=1,2
        total=0.
        do j=1,2
          total=total+Binv(i,j)*f(j)
        enddo
        vec(i)=total
      enddo

*     Now operate
      kk(1)=dt*y(3)
      kk(2)=dt*y(4)
      kk(3)=dt*(Prod(1,1)*y(3)+Prod(1,2)*y(4)+vec(1))
      kk(4)=dt*(Prod(2,1)*y(3)+Prod(2,2)*y(4)+vec(2))

      return
      end


      subroutine sum (a,b,c,size,flag)
*     Vector sum c=a+b   (flag=1)
*     Vector sum c=a+b/2 (flag=2)
      implicit none
      integer size,i,flag
      real a(size),b(size),c(size)
      if (flag.eq.1) then
        do i=1,size
          c(i)=a(i)+b(i)
        enddo
        else
        do i=1,size
          c(i)=a(i)+b(i)/2.
        enddo
      endif
      return
      end


      subroutine RungeKutt4 (y,m1,m2,l1,l2,g,dt,pi)
*     Fourth order Runge-Kutta for double pendulum
      implicit none
      integer i
      real y(4),m1,m2,l1,l2,g,dt,pi
      real k1(4),k2(4),k3(4),k4(4),A(2,2),Binv(2,2),f(2),ytemp(4),mymod

*     Stage 1
*     -------
      call matrixA (y,m1,m2,l1,l2,A)
      call matrixBinv (y,m1,m2,l1,l2,Binv)
      call vectorf (y,m1,m2,l1,l2,g,f)
      call operate (A,Binv,f,dt,y,k1)

*     Stage 2
*     -------
      call sum (y,k1,ytemp,4,2)
      call matrixA (ytemp,m1,m2,l1,l2,A)
      call matrixBinv (ytemp,m1,m2,l1,l2,Binv)
      call vectorf (ytemp,m1,m2,l1,l2,g,f)
      call operate (A,Binv,f,dt,ytemp,k2)

*     Stage 3
*     -------
      call sum (y,k2,ytemp,4,2)
      call matrixA (ytemp,m1,m2,l1,l2,A)
      call matrixBinv (ytemp,m1,m2,l1,l2,Binv)
      call vectorf (ytemp,m1,m2,l1,l2,g,f)
      call operate (A,Binv,f,dt,ytemp,k3)

*     Stage 4
*     -------
      call sum (y,k3,ytemp,4,1)
      call matrixA (ytemp,m1,m2,l1,l2,A)
      call matrixBinv (ytemp,m1,m2,l1,l2,Binv)
      call vectorf (ytemp,m1,m2,l1,l2,g,f)
      call operate (A,Binv,f,dt,ytemp,k4)

*     Add-up for incremented y-vector
      do i=1,4
        y(i)=y(i) + k1(i)/6. + k2(i)/3. + k3(i)/3. + k4(i)/6.
      enddo

*     Put angles back into range (-pi to pi)
      do i=1,2
        y(i)=mymod(y(i),2.*pi)
        if (y(i).gt.pi) y(i)=y(i)-2.*pi
      enddo

      return
      end


      real function mymod (x,interval)
*     This function puts x into the interval 0<=x<interval
      implicit none
      real x,interval,sgn
      sgn=sign(1.0,x)
      x=abs(x)
      mymod=sgn*(x-interval*real(int(x/interval)))
      return
      end

