GET_PNT_POS_PGM.f90

Go to the documentation of this file.

!    Copyright (C) 2012 The SPEED FOUNDATION
!    Author: Ilario Mazzieri
!
!    This file is part of SPEED.
!
!    SPEED is free software; you can redistribute it and/or modify it
!    under the terms of the GNU Affero General Public License as
!    published by the Free Software Foundation, either version 3 of the
!    License, or (at your option) any later version.
!
!    SPEED is distributed in the hope that it will be useful, but
!    WITHOUT ANY WARRANTY; without even the implied warranty of
!    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
!    Affero General Public License for more details.
!
!    You should have received a copy of the GNU Affero General Public License
!    along with SPEED.  If not, see <http://www.gnu.org/licenses/>.
 
 
      subroutine get_pnt_pos_pgm(nb_el,&
                                 AA11,AA12,AA13,AA21,AA22,AA23,&
                                 AA31,AA32,AA33,BB11,BB12,BB13,&
                                 BB21,BB22,BB23,BB31,BB32,BB33,&
                                 GG1,GG2,GG3,DD1,DD2,DD3,&
                                 x_true,y_true,z_true,el,xref,yref,zref,&
                                 highest,elevation)
      
      implicit none
      
      integer*4 :: nb_el,el
      integer*4 :: inr,ie
 
      real*8 :: x_true,y_true,z_true,xref,yref,zref
      real*8 :: csi,eta,zeta,delta_csi,delta_eta,delta_zeta,dist,dist_min
      real*8 :: termx,termy,termz,det_j
      real*8 :: dxdx,dxdy,dxdz
      real*8 :: dydx,dydy,dydz
      real*8 :: dzdx,dzdy,dzdz
      real*8 :: a11,a12,a13
      real*8 :: a21,a22,a23
      real*8 :: a31,a32,a33
      real*8 :: elevation
 
      real*8, dimension(nb_el) :: aa11,aa12,aa13
      real*8, dimension(nb_el) :: aa21,aa22,aa23
      real*8, dimension(nb_el) :: aa31,aa32,aa33
      real*8, dimension(nb_el) :: bb11,bb12,bb13
      real*8, dimension(nb_el) :: bb21,bb22,bb23
      real*8, dimension(nb_el) :: bb31,bb32,bb33
      real*8, dimension(nb_el) :: gg1,gg2,gg3
      real*8, dimension(nb_el) :: dd1,dd2,dd3
      real*8, dimension(nb_el) :: highest
      
      dist_min = 1.0e20
      xref = 0.0;      yref = 0.0;      zref = 0.0
      el = 1
      
      do ie = 1,nb_el
         if (highest(ie) .gt. elevation) then
            csi = 0.0;             eta = 0.0;     zeta = 0.0
            
            do inr = 1, 100
                dxdx = aa11(ie) + bb12(ie)*zeta + bb13(ie)*eta + gg1(ie)*eta*zeta
                dydx = aa21(ie) + bb22(ie)*zeta + bb23(ie)*eta + gg2(ie)*eta*zeta
                dzdx = aa31(ie) + bb32(ie)*zeta + bb33(ie)*eta + gg3(ie)*eta*zeta
          
                dxdy = aa12(ie) + bb11(ie)*zeta + bb13(ie)*csi + gg1(ie)*zeta*csi
                dydy = aa22(ie) + bb21(ie)*zeta + bb23(ie)*csi + gg2(ie)*zeta*csi
                dzdy = aa32(ie) + bb31(ie)*zeta + bb33(ie)*csi + gg3(ie)*zeta*csi
          
                dxdz = aa13(ie) + bb11(ie)*eta + bb12(ie)*csi  + gg1(ie)*csi*eta
                dydz = aa23(ie) + bb21(ie)*eta + bb22(ie)*csi  + gg2(ie)*csi*eta
                dzdz = aa33(ie) + bb31(ie)*eta + bb32(ie)*csi  + gg3(ie)*csi*eta
          
                det_j = dxdz * (dydx*dzdy - dzdx*dydy) - dydz * (dxdx*dzdy - dzdx*dxdy) &
                          + dzdz * (dxdx*dydy - dydx*dxdy)
          
          
                a11 = +dydy*dzdz -dydz*dzdy
                a12 = -dydx*dzdz +dydz*dzdx
                a13 = +dydx*dzdy -dydy*dzdx
          
                a21 = -dxdy*dzdz +dxdz*dzdy
                a22 = +dxdx*dzdz -dxdz*dzdx
                a23 = -dxdx*dzdy +dxdy*dzdx
          
                a31 = +dxdy*dydz -dxdz*dydy
                a32 = -dxdx*dydz +dxdz*dydx
                a33 = +dxdx*dydy -dxdy*dydx
          
                termx = aa11(ie)*csi + aa12(ie)*eta &
                   + aa13(ie)*zeta + bb11(ie)*eta*zeta &
                   + bb12(ie)*csi*zeta + bb13(ie)*csi*eta &
                   + gg1(ie)*csi*eta*zeta + dd1(ie) -x_true
          
                termy = aa21(ie)*csi + aa22(ie)*eta &
                   + aa23(ie)*zeta + bb21(ie)*eta*zeta &
                   + bb22(ie)*csi*zeta + bb23(ie)*csi*eta &
                   + gg2(ie)*csi*eta*zeta + dd2(ie) -y_true
            
                termz = aa31(ie)*csi + aa32(ie)*eta &
                   + aa33(ie)*zeta + bb31(ie)*eta*zeta &
                   + bb32(ie)*csi*zeta + bb33(ie)*csi*eta &
                   + gg3(ie)*csi*eta*zeta + dd3(ie) -z_true
            
                    delta_csi  = -(a11*termx +a21*termy +a31*termz) / det_j
                delta_eta  = -(a12*termx +a22*termy +a32*termz) / det_j
                delta_zeta = -(a13*termx +a23*termy +a33*termz) / det_j
            
                csi = csi + delta_csi
                eta = eta + delta_eta
                zeta = zeta + delta_zeta
                    
                    if (csi.lt.-1.0) csi = -1.0
                if (csi.gt. 1.0) csi =  1.0
                if (eta.lt.-1.0) eta = -1.0
                if (eta.gt. 1.0) eta =  1.0
                if (zeta.lt.-1.0) zeta = -1.0
                if (zeta.gt. 1.0) zeta =  1.0
            
            enddo
         
                 dist = termx*termx +termy*termy +termz*termz
         
                 if (dist.lt.dist_min) then
                    el = ie
                    xref = csi
                    yref = eta
                    zref = zeta
                    dist_min = dist
                 endif
 
        endif !if (highest(ie).gt.elevation) then
         
      enddo !do ie = 1,nb_el
      
      return
      
      end subroutine get_pnt_pos_pgm