gravmod.f90

!-----*-----------------------------------------------------------------
!     Polygon Gravity Module
!     Based on gravmap, by:
!     Jim Richardson, Lunar and Planetary Laboratory, Univ. of Arizona
!     Version 1.0, January 9, 2003
!
!     Modernized and modularized by David Minton
!     Last change 5 June 2007
!
!     Adapted for Python using f2py by David Minton
!     Last change 5 June 2019
!
!-----*-----------------------------------------------------------------
module gravity
!$ USE omp_lib
implicit none
public

real*8,parameter :: pi = 3.14159265358979d0
real*8,parameter :: small = 1.0d-10
real*8,parameter :: third = 0.33333333333333333d0
real*8,parameter :: Gcon = 6.6726d-11

!$f2p real*8,parameter :: pi
!$f2p real*8,parameter :: small
!$f2p real*8,parameter :: third
!$f2p real*8,parameter :: Gcon

contains

!*******************************************************************************
!                          SUBROUTINE POLYGRAV_WRAP
!*******************************************************************************
! A Fortran 95 version of Jim Richardson's polygrav f77 subroutine
! Author: David Minton
!     program calculating polyhedron gravity after Werner,
!     Cel. Mech. Dyn. Astron. 1994, _59_, 253-278
! Last change: 5 Jun 2019
!*******************************************************************************
    subroutine polygrav(x0,m,v,mxn,rot_cen,rot_rate,d,a,pot)
    implicit none

    !     x0(1:3) are coords of a point at which the potential and
    !     attraction is evaluated
    real*8,dimension(3),intent(in) :: x0,rot_cen,rot_rate
    real*8,dimension(:,:),intent(in) :: m,mxn
    integer,dimension(:,:),intent(in) :: v
    real*8,intent(in) :: d
    real*8,dimension(3),intent(out) :: a
    real*8,intent(out) :: pot
    !f2py intent(in) x0
    !f2py intent(in) rot_cen
    !f2py intent(in) rot_rate
    !f2py intent(in) m
    !f2py intent(in) mxn
    !f2py intent(in) v
    !f2py intent(in) d
    !f2py intent(out) a
    !f2py intent(out) pot

    integer :: nface
    real*8,dimension(3) :: rvec,omegavec,tmp,ans
    real*8 :: omegmag2,rpar2
    integer :: i

    real*8 :: nl,dl
    !     ---- specific routine variables -----

    !     Setting up lists of points this way makes for a large amount of
    !     redundancy.
    real*8 :: x1 , y1, z1
    real*8 :: x2 , y2, z2
    real*8 :: x3 , y3, z3

    !     x12, x23 are 2 of the three edges (1-2) and (2-3)
    real*8 :: x12, y12, z12
    real*8 :: x23, y23, z23

    !     n is the cross-product normal vector; nmag the magnitude, nhat
    !     the unit vector
    real*8 nhatx,nhaty,nhatz

    !     ihat is a unit vector along the 1-2 edge of each face
    real*8 :: x12mag,ihatx,ihaty,ihatz

    !     jhat is a unit vector perp to ihat and nhat
    real*8 :: jmag,jhatx,jhaty,jhatz

    !     xi, eta, zeta are face-centered coordinates
    real*8:: xi1,eta1,zeta1
    real*8:: xi2,eta2,zeta2
    real*8:: xi3,eta3,zeta3

    !     r12, r23, r31 are lengths of edges of face
    real*8 :: r12,r23,r31

    !     bracketed factors cf p. 263 of the paper
    real*8 :: nnum,denom1,denom2,denom3

    !     x0p, y0p, z0p are x0,y0,z0 rotated into face coordinates
    real*8 :: x0p,y0p,z0p

    !     coord diffs from (x0,y0,z0) to face vertices
    real*8 :: dx1,dy1,dz1,dz
    real*8 :: dx2,dy2,dz2
    real*8 :: dx3,dy3,dz3
    real*8 :: r1,r2,r3,L12,L23,L31,S1,S2,S3

    !     more factors
    real*8 ::  det12, det23, det31, sfac, af
    real*8 :: snum,sdenom1,sdenom2,sdenom3

    !     attraction -- dUdx0 are in face coordinates, dUdx are in
    !     body coords
    real*8 :: dUdx0,dUdy0,dUdz0
    real*8 :: dUdx,dUdy,dUdz
    real*8 :: dU
    real*8 :: ax,ay,az


    !     ---------- begin routine ----------


    !     reset measured parameters for particle attraction
    nface = size(v,2)



    !     ----- begin surface face loop -----
    !$OMP PARALLEL DO DEFAULT(PRIVATE) SHARED(x0,m,v,mxn,nface) &
    !$OMP REDUCTION(+:ax,ay,az,pot)
    do i=1,nface
    !     retrieve polygon vertices coord
         x1 = m(1,v(1,i))
         y1 = m(2,v(1,i))
         z1 = m(3,v(1,i))
         x2 = m(1,v(2,i))
         y2 = m(2,v(2,i))
         z2 = m(3,v(2,i))
         x3 = m(1,v(3,i))
         y3 = m(2,v(3,i))
         z3 = m(3,v(3,i))

    !     vertex check
    !      if((x0.eq.x1).or.(x0.eq.x2).or.(x0.eq.x3)) x0 = x0 + 0.001d0
    !      if((y0.eq.y1).or.(y0.eq.y2).or.(y0.eq.y3)) y0 = y0 + 0.001d0
    !      if((z0.eq.z1).or.(z0.eq.z2).or.(z0.eq.z3)) z0 = z0 + 0.001d0

    !     retrieve polygon normal vector coord
         nhatx = mxn(1,i)
         nhaty = mxn(2,i)
         nhatz = mxn(3,i)

    !     calculate edge 1-2
         x12 = x2 - x1
         y12 = y2 - y1
         z12 = z2 - z1

    !     calculate edge 2-3
         x23 = x3 - x2
         y23 = y3 - y2
         z23 = z3 - z2

    !     choose ihat = x12/|x12|
         x12mag = sqrt((x12*x12)+(y12*y12)+(z12*z12))
         if (x12mag .eq. 0.d0) x12mag = small
         ihatx = x12 / x12mag
         ihaty = y12 / x12mag
         ihatz = z12 / x12mag

    !     jhat = (nhat x ihat)
         jhatx = (nhaty * ihatz) - (nhatz * ihaty)
         jhaty = (nhatz * ihatx) - (nhatx * ihatz)
         jhatz = (nhatx * ihaty) - (nhaty * ihatx)
         jmag = sqrt((jhatx*jhatx)+(jhaty*jhaty)+(jhatz*jhatz))
         if (jmag .eq. 0.d0) jmag = small
         jhatx = jhatx / jmag
         jhaty = jhaty / jmag
         jhatz = jhatz / jmag

    !     calculate face-plane coordinates
         xi1 = (ihatx*x1) + (ihaty*y1) + (ihatz*z1)
         eta1 = (jhatx*x1) + (jhaty*y1) + (jhatz*z1)
         zeta1 = (nhatx*x1) + (nhaty*y1) + (nhatz*z1)
         xi2 = (ihatx*x2) + (ihaty*y2) + (ihatz*z2)
         eta2 = (jhatx*x2) + (jhaty*y2) + (jhatz*z2)
         zeta2 = (nhatx*x2) + (nhaty*y2) + (nhatz*z2)
         xi3 = (ihatx*x3) + (ihaty*y3) + (ihatz*z3)
         eta3 = (jhatx*x3) + (jhaty*y3) + (jhatz*z3)
         zeta3 = (nhatx*x3) + (nhaty*y3) + (nhatz*z3)

    !     find face-plane polygon edge lengths
    !     side 1-2
         r12 = (xi2 - xi1)**2
         r12 = r12 + ((eta2 - eta1)**2)
         r12 = r12 + ((zeta2 - zeta1)**2)
         r12 = sqrt(r12)
         if (r12 .eq. 0.d0) r12 = small
    !     side 2-3
         r23 = (xi3 - xi2)**2
         r23 = r23 + ((eta3 - eta2)**2)
         r23 = r23 + ((zeta3 - zeta2)**2)
         r23 = sqrt(r23)
         if (r23 .eq. 0.d0) r23 = small
    !     side 3-1
         r31 = (xi1 - xi3)**2
         r31 = r31 + ((eta1 - eta3)**2)
         r31 = r31 + ((zeta1 - zeta3)**2)
         r31 = sqrt(r31)
         if (r31 .eq. 0.d0) r31 = small

    !     rotate test particle to face coordinates
         x0p = (ihatx*x0(1)) + (ihaty*x0(2)) + (ihatz*x0(3))
         y0p = (jhatx*x0(1)) + (jhaty*x0(2)) + (jhatz*x0(3))
         z0p = (nhatx*x0(1)) + (nhaty*x0(2)) + (nhatz*x0(3))

    !     coordinate diffs from test particle to face vertices
         dx1 = xi1 - x0p
         dy1 = eta1 - y0p
         dz1 = zeta1 - z0p
         dx2 = xi2 - x0p
         dy2 = eta2 - y0p
         dz2 = zeta2 - z0p
         dx3 = xi3 - x0p
         dy3 = eta3 - y0p
         dz3 = zeta3 - z0p
         dz = (dz1 + dz2 + dz3) * third
         if (dz .eq. 0.d0) dz = small

    !     dists from test point to vertices
         r1 = sqrt((dx1*dx1)+(dy1*dy1)+(dz1*dz1))
         if (r1 .eq. 0.d0) r1 = small
         r2 = sqrt((dx2*dx2)+(dy2*dy2)+(dz2*dz2))
         if (r2 .eq. 0.d0) r2 = small
         r3 = sqrt((dx3*dx3)+(dy3*dy3)+(dz3*dz3))
         if (r3 .eq. 0.d0) r3 = small

    !     p. 263 upper factors
         det12 = (dx1 * dy2) - (dx2 * dy1)
         det23 = (dx2 * dy3) - (dx3 * dy2)
         det31 = (dx3 * dy1) - (dx1 * dy3)
         dl = (r1 + r2 - r12)
         if (dl .eq. 0.d0) dl = small
         nl = (r1 + r2 + r12) / dl
         if (nl .le. 0.d0) nl = small
         L12 = log(nl)
         L12 = L12 / r12
         dl = (r2 + r3 - r23)
         if (dl .eq. 0.d0) dl = small
         nl = (r2 + r3 + r23) / dl
         if (nl .le. 0.d0) nl = small
         L23 = log(nl)
         L23 = L23 / r23
         dl = (r3 + r1 - r31)
         if (dl .eq. 0.d0) dl = small
         nl = (r3 + r1 + r31) / dl
         if (nl .le. 0.d0) nl = small
         L31 = log(nl)
         L31 = L31 / r31

    !     calculate the inner bracketed factors on p. 263
         nnum = (xi1*(eta2-eta3))+(xi2*(eta3-eta1))+(xi3*(eta1-eta2))
         denom1 = ((xi2-xi1)*(xi1-xi3))+((eta2-eta1)*(eta1-eta3))
         denom2 = ((xi3-xi2)*(xi2-xi1))+((eta3-eta2)*(eta2-eta1))
         denom3 = ((xi1-xi3)*(xi3-xi2))+((eta1-eta3)*(eta3-eta2))

    !     outer brackets and S terms (page 263)
         snum = dz * nnum
         sdenom1 = ((det31*det12) + ((dz*dz)*denom1)) / (-1.0d0*r1)
         if (sdenom1 .eq. 0.d0) sdenom1 = small
         sdenom2 = ((det12*det23) + ((dz*dz)*denom2)) / (-1.0d0*r2)
         if (sdenom2 .eq. 0.d0) sdenom2 = small
         sdenom3 = ((det23*det31) + ((dz*dz)*denom3)) / (-1.0d0*r3)
         if (sdenom3 .eq. 0.d0) sdenom3 = small
         S1 = atan(snum / sdenom1)
         if ((snum.gt.0.d0).and.(sdenom1.lt.0.d0)) S1 = pi + S1
         if ((snum.lt.0.d0).and.(sdenom1.lt.0.d0)) S1 = S1 - pi
         S2 = atan(snum / sdenom2)
         if ((snum.gt.0.d0).and.(sdenom2.lt.0.d0)) S2 = pi + S2
         if ((snum.lt.0.d0).and.(sdenom2.lt.0.d0)) S2 = S2 - pi
         S3 = atan(snum / sdenom3)
         if ((snum.gt.0.d0).and.(sdenom3.lt.0.d0)) S3 = pi + S3
         if ((snum.lt.0.d0).and.(sdenom3.lt.0.d0)) S3 = S3 - pi

    !     sum factor with sign dependent Pi
         if (dz.ge.0.d0) sfac = 1.0d0
         if (dz.lt.0.d0) sfac = -1.0d0
         af = (S1 + S2 + S3) - (sfac*pi)

    !     gravitational potential
         dU = (0.5d0 * dz) * ((det12*L12)+(det23*L23)+(det31*L31))
         dU = dU - (0.5d0 * dz * dz * af)

    !     attraction (in face coordinates, pp. 264-265)
         dUdx0 = ((eta2-eta1)*L12)+((eta3-eta2)*L23)+((eta1-eta3)*L31)
         dUdx0 = (-0.5d0*dz) * dUdx0
         dUdy0 = ((xi2-xi1)*L12)+((xi3-xi2)*L23)+((xi1-xi3)*L31)
         dUdy0 = (0.5d0*dz) * dUdy0
         dUdz0 = (-0.5d0) * ((det12*L12)+(det23*L23)+(det31*L31))
         dUdz0 = dudz0 + (dz * af)

    !     need to convert to body coords before summing attractions
    !     inverse rotation matrix is just transpose of rotation matrix
         dUdx = (ihatx*dUdx0) + (jhatx*dUdy0) + (nhatx*dUdz0)
         dUdy = (ihaty*dUdx0) + (jhaty*dUdy0) + (nhaty*dUdz0)
         dUdz = (ihatz*dUdx0) + (jhatz*dUdy0) + (nhatz*dUdz0)

    !     sum up total attraction and potential
         pot = pot + dU
         ax  = ax  + dUdx
         ay  = ay  + dUdy
         az  = az  + dUdz
    !     ----- end face summation loop ---
    end do
    !$OMP END PARALLEL DO
    a(1) = ax
    a(2) = ay
    a(3) = az
    pot = pot * Gcon * d
    a = a * Gcon * d
    ! Calculate rotational component
    omegmag2=dot_product(rot_rate,rot_rate)!rot_rate(4)**2+rot(5)**2+rot(6)**2
    if (omegmag2<small) return
    rvec(1:3)=x0(1:3)-rot_cen(1:3)
    tmp = cross_product(rot_rate,rvec)
    ans = cross_product(rot_rate,tmp)
    a = a - ans
    ! Now get the perp vector
    tmp =- ans / omegmag2
    rpar2 = tmp(1)**2 + tmp(2)**2 + tmp(3)**2
    pot = pot + 0.5d0 * omegmag2 * rpar2

    return

    end subroutine polygrav




    subroutine test_omp()
    implicit none
    integer :: nthreads = 1

    !$ write(*,*) 'Dynamic thread allocation: ',OMP_get_dynamic()
    !$ nthreads = OMP_get_max_threads() ! In the *parallel* case
    !$ write(*,'(a)')      ' OpenMP parameters:'
    !$ write(*,'(a)')      ' ------------------'
    !$ write(*,'(a,i3,/)') ' Number of threads  = ', nthreads

    end subroutine test_omp


   function cross_product(ar1,ar2) result(ans)
   implicit none

   real*8,dimension(3),intent(in) :: ar1,ar2
   !f2py intent(in) ar1
   !f2py intent(in) ar2
   real*8,dimension(3) :: ans

   ans(1) = ar1(2) * ar2(3) - ar1(3) * ar2(2)
   ans(2) = ar1(3) * ar2(1) - ar1(1) * ar2(3)
   ans(3) = ar1(1) * ar2(2) - ar1(2) * ar2(1)

   return
   end function cross_product

end module gravity