modified_quadratic_shepard_method.f90

FORTRAN程序 共有8个插值程序 希望能帮到大家

!  and w(l) = ((r-d)+/(r*d))**2 for radius r(l) and dist-
!  ance d(l).  thus
!
!    qx = ( swqx * sw - swq * swx ) / sw**2
!    qy = ( swqy * sw - swq * swy ) / sw**2
!    qz = ( swqz * sw - swq * swz ) / sw**2
!
!  where swqx and swx are partial derivatives with respect
!  to X of swq and sw, respectively.  swqy, swy, swqz, and
!  swz are defined similarly.
!
  sw = 0.0D+00
  swx = 0.0D+00
  swy = 0.0D+00
  swz = 0.0D+00
  swq = 0.0D+00
  swqx = 0.0D+00
  swqy = 0.0D+00
  swqz = 0.0D+00
!
!  Outer loop on cells (i,j,k).
!
  do k = kmin, kmax
    do j = jmin, jmax
      do i = imin, imax

        l = lcell(i,j,k)
        if ( l == 0 ) then
          cycle
        end if
!
!  Inner loop on nodes L.
!
        do

          delx = xp - x(l)
          dely = yp - y(l)
          delz = zp - z(l)
          dxsq = delx * delx
          dysq = dely * dely
          dzsq = delz * delz
          ds = dxsq + dysq + dzsq
          rs = rsq(l)

          if ( ds < rs ) then

            if ( ds == 0.0D+00 ) then
              q = f(l)
              qx = a(7,l)
              qy = a(8,l)
              qz = a(9,l)
              ier = 0
              return
            end if

            rds = rs * ds
            rd = sqrt ( rds )
            w = ( rs + ds - rd - rd ) / rds
            t = 2.0D+00 * ( rd - rs ) / ( ds * rds )
            wx = delx * t
            wy = dely * t
            wz = delz * t

            qlx = 2.0D+00 *a(1,l)*delx + a(2,l)*dely + a(4,l)*delz
            qly = a(2,l)*delx + 2.0D+00 * a(3,l)*dely + a(5,l)*delz
            qlz = a(4,l)*delx + a(5,l)*dely + 2.0D+00 * a(6,l)*delz
            ql = (qlx*delx + qly*dely + qlz*delz) / 2.0D+00 + &
              a(7,l)*delx + a(8,l)*dely + a(9,l)*delz + f(l)
            qlx = qlx + a(7,l)
            qly = qly + a(8,l)
            qlz = qlz + a(9,l)

            sw = sw + w
            swx = swx + wx
            swy = swy + wy
            swz = swz + wz
            swq = swq + w*ql
            swqx = swqx + wx*ql + w*qlx
            swqy = swqy + wy*ql + w*qly
            swqz = swqz + wz*ql + w*qlz

          end if

          lp = l
          l = lnext(lp)

          if ( l == lp ) then
            exit
          end if

        end do

      end do
    end do
  end do
!
!  SW = 0 iff P is not within the radius R(L) for any node L.
!
  if ( sw /= 0.0D+00 ) then

    q = swq/sw
    sws = sw*sw
    qx = (swqx*sw - swq*swx)/sws
    qy = (swqy*sw - swq*swy)/sws
    qz = (swqz*sw - swq*swz)/sws
    ier = 0
!
!  No cells contain a point within RMAX of P, or
!  SW = 0 and thus RSQ(L) <= DS for all L.
!
  else

    q = 0.0D+00
    qx = 0.0D+00
    qy = 0.0D+00
    qz = 0.0D+00
    ier = 2

  end if

  return
end
subroutine getnp3 ( px, py, pz, x, y, z, nr, lcell, lnext, xyzmin, &
  xyzdel, np, dsq )

!***********************************************************************
!
!! GETNP3 finds the closest node to a given point.
!
!  Discussion:
!
!    Given a set of N nodes and the data structure defined in
!    subroutine STORE3, this subroutine uses the cell method to
!    find the closest unmarked node NP to a specified point P.
!
!    NP is then marked by setting LNEXT(NP) to -LNEXT(NP).  (A
!    node is marked if and only if the corresponding lnext element 
!    is negative.  The absolute values of LNEXT elements,
!    however, must be preserved.)  Thus, the closest M nodes to
!    P may be determined by a sequence of M calls to this routine.  
!    Note that if the nearest neighbor to node K is to
!    be determined (PX = X(K), PY = Y(K), and PZ = Z(K)), then
!    K should be marked before the call to this routine.
!
!    The search is begun in the cell containing (or closest
!    to) P and proceeds outward in box-shaped layers until all
!    cells which contain points within distance R of P have
!    been searched, where R is the distance from P to the first
!    unmarked node encountered (infinite if no unmarked nodes
!    are present).
!
!  Author:
!
!    Robert Renka,
!    University of North Texas,
!    [phone].
!
!  Reference:
!
!    Robert Renka,
!    Algorithm 661: QSHEP3D, Quadratic Shepard method for trivariate
!    interpolation of scattered data,
!    ACM Transactions on Mathematical Software,
!    Volume 14, 1988, pages 151-152.
!
!  Parameters:
!
!    Input, real ( kind = 8 ) PX, PY, PZ, the coordinates of the point P
!    whose nearest unmarked neighbor is to be found.
!
!    Input, real ( kind = 8 ) X(N), Y(N), Z(N), the coordinates of the nodes.
!
!    Input, integer NR, the number of rows, columns, and planes in the cell
!    grid.  1 <= NR.
!
!    Input, integer LCELL(NR,NR,NR), nodal indices associated with cells.
!
!    Input/output, integer LNEXT(N), next-node indices (or their negatives).
!
!    Input, real ( kind = 8 ) XYZMIN(3), XYZDEL(3), minimum nodal 
!    coordinates and cell dimensions, respectively.  XYZEL elements must 
!    be positive.
!
!    Output, integer NP, index of the nearest unmarked node to P, or 0 
!    if all nodes are marked or NR < 1 or an element of XYZDEL is not 
!    positive.  LNEXT(NP) < 0 if NP /= 0.
!
!    Output, real ( kind = 8 ) DSQ, squared euclidean distance between 
!    P and node NP, or 0 if NP = 0.
!
  implicit none

  integer nr

  real ( kind = 8 ) delx
  real ( kind = 8 ) dely
  real ( kind = 8 ) delz
  real ( kind = 8 ) dsq
  real ( kind = 8 ) dx
  real ( kind = 8 ) dy
  real ( kind = 8 ) dz
  logical first
  integer i
  integer i0
  integer i1
  integer i2
  integer imax
  integer imin
  integer j
  integer j0
  integer j1
  integer j2
  integer jmax
  integer jmin
  integer k
  integer k0
  integer k1
  integer k2
  integer kmax
  integer kmin
  integer l
  integer lcell(nr,nr,nr)
  integer lmin
  integer ln
  integer lnext(*)
  integer np
  real ( kind = 8 ) px
  real ( kind = 8 ) py
  real ( kind = 8 ) pz
  real ( kind = 8 ) r
  real ( kind = 8 ) rsmin
  real ( kind = 8 ) rsq
  real ( kind = 8 ) x(*)
  real ( kind = 8 ) xp
  real ( kind = 8 ) xyzdel(3)
  real ( kind = 8 ) xyzmin(3)
  real ( kind = 8 ) y(*)
  real ( kind = 8 ) yp
  real ( kind = 8 ) z(*)
  real ( kind = 8 ) zp

  xp = px
  yp = py
  zp = pz
  dx = xyzdel(1)
  dy = xyzdel(2)
  dz = xyzdel(3)
!
!  Test for invalid input parameters.
!
  if ( nr < 1 .or. dx <= 0.0D+00 .or. dy <= 0.0D+00 .or. dz <= 0.0D+00 ) then
    np = 0
    dsq = 0.0D+00
    return
  end if
!
!  Initialize parameters --
!
!  first = true iff the first unmarked node has yet to be encountered,
!  imin,...,kmax = cell indices defining the range of the search,
!  delx,dely,delz = px-xyzmin(1), py-xyzmin(2), and pz-xyzmin(3),
!  i0,j0,k0 = cell containing or closest to p,
!  i1,...,k2 = cell indices of the layer whose intersection
!  with the range defined by imin,...,kmax is
!  currently being searched.
!
  first = .true.
  imin = 1
  imax = nr
  jmin = 1
  jmax = nr
  kmin = 1
  kmax = nr
  delx = xp - xyzmin(1)
  dely = yp - xyzmin(2)
  delz = zp - xyzmin(3)
  i0 = int(delx/dx) + 1
  if ( i0 < 1 ) i0 = 1
  if ( nr < i0 ) then
    i0 = nr
  end if

  j0 = int ( dely / dy ) + 1
  if ( j0 < 1 ) j0 = 1
  if ( nr < j0 ) then
    j0 = nr
  end if

  k0 = int(delz/dz) + 1
  if ( k0 < 1 ) k0 = 1
  if ( nr < k0 ) then
    k0 = nr
  end if

  i1 = i0
  i2 = i0
  j1 = j0
  j2 = j0
  k1 = k0
  k2 = k0
!
!  Outer loop on layers, inner loop on layer cells, excluding
!  those outside the range (imin,imax) x (jmin,jmax) x (kmin,kmax).
!
1 continue

  do k = k1, k2

    if ( kmax < k ) then
      exit
    end if

    if ( k < kmin ) then
      cycle
    end if

    do j = j1, j2

      if ( jmax < j ) then
        exit
      end if

      if ( j < jmin ) then
        cycle
      end if

      do i = i1, i2

        if ( imax < i ) then
          exit
        end if

        if ( i < imin ) then
          cycle
        end if

        if ( k /= k1  .and.  k /= k2  .and. j /= j1  .and.  &
          j /= j2 .and. i /= i1 .and. i /= i2 ) then
          cycle
        end if
!
!  Search cell (i,j,k) for unmarked nodes L.
!
        l = lcell(i,j,k)

        if ( l == 0 ) then
          cycle
        end if
!
!  Loop on nodes in cell (i,j,k).
!
2       continue

        ln = lnext(l)

        if ( ln < 0 ) then
          go to 4
        end if
!
!  Node L is not marked.
!
        rsq = (x(l)-xp)**2 + (y(l)-yp)**2 + (z(l)-zp)**2

        if ( .not. first ) then
          go to 3
        end if
!
!  Node L is the first unmarked neighbor of P encountered.
!  initialize LMIN to the current candidate for NP, and
!  rsmin to the squared distance from P to lmin.  imin,
!  imax, jmin, jmax, kmin, and kmax are updated to define
!  the smallest rectangle containing a sphere of radius
!  r = sqrt(rsmin) centered at P, and contained in (1,nr)
!  x (1,nr) x (1,nr) (except that, if P is outside the
!  box defined by the nodes, it is possible that NR < imin
!  or imax < 1, etc.).  FIRST is reset to
!  false.
!
        lmin = l
        rsmin = rsq
        r = sqrt(rsmin)
        imin = int((delx-r)/dx) + 1
        if ( imin < 1 ) imin = 1
        imax = int((delx+r)/dx) + 1
        if ( nr < imax ) imax = nr
        jmin = int((dely-r)/dy) + 1
        if ( jmin < 1 ) jmin = 1
        jmax = int((dely+r)/dy) + 1
        if ( nr < jmax ) jmax = nr
        kmin = int((delz-r)/dz) + 1
        if ( kmin < 1 ) kmin = 1
        kmax = int((delz+r)/dz) + 1
        if ( nr < kmax ) kmax = nr
        first = .false.
        go to 4
!
!  Test for node L closer than LMIN to P.
!
3       continue
!
!  Update LMIN and RSMIN.
!
        if ( rsq < rsmin ) then
          lmin = l
          rsmin = rsq
        end if
!
!  Test for termination of loop on nodes in cell (i,j,k).
!
4       continue
 
        if ( abs(ln) == l ) then
          cycle
        end if

        l = abs ( ln )
        go to 2

      end do

    end do

  end do
!
! Test for termination of loop on cell layers.
!
  if ( i1 <= imin .and. imax <= i2 .and. &
       j1 <= jmin .and. jmax <= j2 .and. &
       k1 <= kmin .and. kmax <= k2 ) go to 9
  i1 = i1 - 1
  i2 = i2 + 1
  j1 = j1 - 1
  j2 = j2 + 1
  k1 = k1 - 1
  k2 = k2 + 1
  go to 1
!
!  Unless no unmarked nodes were encountered, LMIN is the
!  closest unmarked node to P.
!
9 continue

  if ( .not. first ) then
    np = lmin
    dsq = rsmin
    lnext(lmin) = -lnext(lmin)
  else
    np = 0
    dsq = 0.0D+00
  end if

  return
end
subroutine givens ( a, b, c, s )

!***********************************************************************
!
!! GIVENS constructs a Givens plane rotation.
!
!  Discussion:
!
!    This routine constructs the Givens plane rotation
!
!          ( c  s)
!      g = (     ) 
!          (-s  c)
!
!    where c*c + s*s = 1, which zeros the second entry of the 2-vector 
!    (a b)-transpose.  A call to GIVENS is normally followed by a call 
!    to ROTATE which applies the transformation to a 2 by N matrix.  
!    This routine was taken from LINPACK.
!
!  Author:
!
!    Robert Renka,
!    University of North Texas,
!    [phone].
!
!  Reference:
!
!    Robert Renka,
!    Algorithm 661: QSHEP3D, Quadratic Shepard method for trivariate
!    interpolation of scattered data,
!    ACM Transactions on Mathematical Software,
!    Volume 14, 1988, pages 151-152.
!
!  Parameters:
!
!    Input/output, real ( kind = 8 ) A.  On input, the first component of
!    the vector.  On output, overwritten by r = +/-sqrt(a*a + b*b)
!
!    Input/output, real ( kind = 8 ) B.  On input, the second component of 
!    the vector.  On output, overwritten by a value z which allows c
!    and s to be recovered as follows --
!    c = sqrt(1-z*z), s=z     if abs(z) <= 1.
!    c = 1/z, s = sqrt(1-c*c) if 1 < abs(z).
!
!    Output, real ( kind = 8 ) C, c = +/-(a/r)
!
!    Output, real ( kind = 8 ) S, s = +/-(b/r)
!
!  Local parameters:
!
!    aa,bb = local copies of a and b
!    r =    c*a + s*b = +/-sqrt(a*a+b*b)