pendulum

!==============================================================================
! This program use Runge-Kutta method to calculate  dynamics system
!                         of double pendulum
!==============================================================================
subroutine runge_kutt
  implicit none
  real*16, external :: F1, F2, F3, F4
  integer :: i, j, m = 4, n = 5*10e3
  real*16, allocatable :: k1(:), k2(:), k3(:), k4(:)
  real*16 :: h = 0.0005d0, x1, y1, x2, y2
! matrix (n * m) with solution
  real*16, allocatable :: y(:,:)
  write(*,101)
101 format(1x,'System of differential equation of first order is solved'/&
              ' due runge-kutta method of 4-th order')
!  write(*,"(A)", advance = 'no') ' due runge-kutta method of 4-th order'
  write(*,'(a36, i5)') 'Number of rows (sampling rate) is  ', n

  allocate(k1(m), k2(m), k3(m), k4(m))
  if (m==0) stop 'Allocation error, m = 0'

  allocate(y(n,m))
  if (n==0) stop 'Allocation error, n = 0'

  write(*,100) h
100 format(1x,'Step:  ', e10.4)
  write(*,10)
10 format(1x,'Solution:  '/&
         ' +----------------+-----------------+'/&
         ' |    analitic    |  interpolation  |'/&
         ' +----------------+-----------------+'/&
         ' |    not exist   |  xypendulum.txt |'/&
         ' +----------------+-----------------+')
!----------------------------------------------------------------------------
! assignment decart coordinates
  x1 = 0.0d0
  y1 = 0.0d0
  x2 = 0.0d0
  y2 = 0.0d0
! assignment  matrix solution
  y = 0.0d0
  open(unit = 1, file = 'xypendulum.txt')
! Algorithm Runge-Kutta
  do i = 1, n-1, 1          ! inception condition for:
     y(1,1) = 1.7d0         !  alpha1 (0) = angle1 in rad
     y(1,2) = 1.15d0         !  alpha2 (0) = angle2 in rad
     y(1,3) = 1.0d0         !  p1 (0) = 0
     y(1,4) = 1.0d0         !  p2 (0) = 0
     ! K_{ij} = (k11 & k12 & k13 & k14\\ ...)
     k1(1) = F1(y(i,1), y(i,2), y(i,3), y(i,4))*h
     k1(2) = F2(y(i,1), y(i,2), y(i,3), y(i,4))*h
     k1(3) = F3(y(i,1), y(i,2), y(i,3), y(i,4))*h
     k1(4) = F4(y(i,1), y(i,2), y(i,3), y(i,4))*h
     k2(1) = F1(y(i,1) + k1(1)/2, y(i,2) + k1(2)/2, y(i,3) + k1(3)/2, y(i,4) + k1(4)/2)*h
     k2(2) = F2(y(i,1) + k1(1)/2, y(i,2) + k1(2)/2, y(i,3) + k1(3)/2, y(i,4) + k1(4)/2)*h
     k2(3) = F3(y(i,1) + k1(1)/2, y(i,2) + k1(2)/2, y(i,3) + k1(3)/2, y(i,4) + k1(4)/2)*h
     k2(4) = F4(y(i,1) + k1(1)/2, y(i,2) + k1(2)/2, y(i,3) + k1(3)/2, y(i,4) + k1(4)/2)*h
     k3(1) = F1(y(i,1) + k1(1)/2, y(i,2) + k1(2)/2, y(i,3) + k1(3)/2, y(i,4) + k1(4)/2)*h
     k3(2) = F2(y(i,1) + k1(1)/2, y(i,2) + k1(2)/2, y(i,3) + k1(3)/2, y(i,4) + k1(4)/2)*h
     k3(3) = F3(y(i,1) + k1(1)/2, y(i,2) + k1(2)/2, y(i,3) + k1(3)/2, y(i,4) + k1(4)/2)*h
     k3(4) = F4(y(i,1) + k1(1)/2, y(i,2) + k1(2)/2, y(i,3) + k1(3)/2, y(i,4) + k1(4)/2)*h
     k4(1) = F1(y(i,1) + k3(1), y(i,2) + k3(2), y(i,3) + k3(3), y(i,4) + k3(4))*h
     k4(2) = F2(y(i,1) + k3(1), y(i,2) + k3(2), y(i,3) + k3(3), y(i,4) + k3(4))*h
     k4(3) = F3(y(i,1) + k3(1), y(i,2) + k3(2), y(i,3) + k3(3), y(i,4) + k3(4))*h
     k4(4) = F4(y(i,1) + k3(1), y(i,2) + k3(2), y(i,3) + k3(3), y(i,4) + k3(4))*h
     do j = 1, m
        y(i + 1,j) = y(i,j) + (k1(j) + 2*k2(j) + 2*k3(j) + k4(j))/6
     end do
  end do
  do i = 1, n
     x1 = sin(y(i,1))
     y1 = -cos(y(i,1))
     x2 = x1 + sin(y(i,2))
     y2 = y1 - cos(y(i,2))
     write(1,'(1x, 4e20.12)') x2, y2, x1, y1 ! output
  end do
  close(1)
end subroutine runge_kutt
!-----------------------------------------------------------------------------
! Function's
function F1(alpha1, alpha2, p1, p2)
  implicit none
  real*16 :: m1, l, p1, p2, alpha1, alpha2, mu, F1
  m1 = 1.0d0
  l = 1.0d0
  mu = 1.0d0
  F1 = (p1 - p2) * cos(alpha1 - alpha2) / ( m1 * l*l * (1 + mu * sin(alpha1 - alpha2)*sin(alpha1 - alpha2)))
  return
end function F1

function F2(alpha1, alpha2, p1, p2)
  implicit none
  real*16 :: m1, l, p1, p2, alpha1, alpha2, F2, mu
  m1 = 1.0d0
  mu = 1.0d0
  l = 1.0d0
  F2 = (p2 * (1 + mu) - p1 * mu * cos(alpha1 - alpha2)) / (m1 * l*l * (1 + mu * sin(alpha1 - alpha2)*sin(alpha1 - alpha2)))
  return
end function F2

function F3(alpha1, alpha2, p1, p2)
  implicit none
  real*16 :: m1, mu, g, l, alpha1, alpha2, p1, p2, A1, A2, F3
  m1 = 1.0d0
  mu = 1.0d0
  g = 9.8d0
  l = 1.0d0
  A1 = (p1 * p2 * sin(alpha1 - alpha2)) / (m1 * l*l * (1 + mu * sin(alpha1 - alpha2)*sin(alpha1 - alpha2)))
  A2 = ((p1*p1 * mu - 2 * p1 * p2 * mu * cos(alpha1 - alpha2) +&
       p2*p2 * (1 + mu)) * sin(2*(alpha1 -alpha2)))/(2 * m1 * l*l * (1 +  mu * sin(alpha1 - alpha2)*sin(alpha1 - alpha2))**2)
  F3 = -m1 * (1 + mu) * g * l * sin(alpha1) - A1 + A2
  return
end function F3

function F4(alpha1, alpha2, p1, p2)
  implicit none
  real*16 :: m1, mu, g, l, alpha1, alpha2, p2, A1, A2, F4, p1
  m1 = 1.0d0
  mu = 1.0d0
  g = 9.8d0
  l = 1.0d0
  A1 = (p1 * p2 * sin(alpha1 - alpha2)) / (m1 * l*l * (1 + mu * sin(alpha1 - alpha2)*sin(alpha1 - alpha2)))
  A2 = ((p1*p1 * mu - 2 * p1 * p2 * mu * cos(alpha1 - alpha2) +&
       p2*p2 * (1 + mu)) * sin(2*(alpha1 -alpha2)))/(2 * m1 * l*l * (1 +  mu * sin(alpha1 - alpha2)*sin(alpha1 - alpha2))**2)
  F4 = -m1 * mu * g * l * sin(alpha2) + A1 - A2
  return
end function F4

program pendulum
  implicit none
  call runge_kutt
end program pendulum