modified_quadratic_shepard_method.f90

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

subroutine qshep3 ( n, x, y, z, f, nq, nw, nr, lcell, lnext, xyzmin, &
  xyzdel, rmax, rsq, a, ier )

!***********************************************************************
!
!! QSHEP3 defines a smooth trivariate interpolant of scattered 3D data.
!
!  Discussion:
!
!    This subroutine computes a set of parameters A and RSQ
!    defining a smooth (once continuously differentiable) trivariate 
!    function Q(X,Y,Z) which interpolates data values
!    F at scattered nodes (X,Y,Z).  The interpolant Q may be
!    evaluated at an arbitrary point by function QS3VAL, and
!    its first derivatives are computed by subroutine QS3GRD.
!
!    The interpolation scheme is a modified quadratic Shepard method:
!
!      Q = (W(1)*Q(1)+W(2)*Q(2)+..+W(N)*Q(N))/(W(1)+W(2)+..+W(N))
!
!    for trivariate functions W(K) and Q(K).  The nodal functions are 
!    given by
!
!      Q(K)(X,Y,Z) = 
!          A(1,K) * DX**2 
!        + A(2,K) * DX * DY 
!        + A(3,K) * DY**2
!        + A(4,K) * DX * DZ
!        + A(5,K) * DY * DZ 
!        + A(6,K) * DZ**2
!        + A(7,K) * DX 
!        + A(8,K) * DY 
!        + A(9,K) * DZ 
!        + F(K)
!
!    where DX = (X-X(K)), DY = (Y-Y(K)), and DZ = (Z-Z(K)).
!
!    Thus, Q(K) is a quadratic function which interpolates the
!    data value at node K.  Its coefficients A(*,K) are obtained
!    by a weighted least squares fit to the closest NQ data
!    points with weights similar to W(K).  Note that the radius
!    of influence for the least squares fit is fixed for each
!    K, but varies with K.
!
!    The weights are taken to be
!
!      W(K)(X,Y,Z) = ( (R(K)-D(K))+ / R(K)*D(K) )**2
!
!    where (R(K)-D(K))+ = 0 if R(K) <= D(K), and D(K)(X,Y,Z)
!    is the euclidean distance between (X,Y,Z) and node K.  The
!    radius of influence R(K) varies with K and is chosen so
!    that NW nodes are within the radius.  Note that W(K) is
!    not defined at node (X(K),Y(K),Z(K)), but Q(X,Y,Z) has
!    limit F(K) as (X,Y,Z) approaches (X(K),Y(K),Z(K)).
!
!  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, integer N, the number of nodes and associated data values.
!    10 <= N.
!
!    Input, real ( kind = 8 ) X(N), Y(N), Z(N), the coordinates of the nodes.
!
!    Input, real ( kind = 8 ) F(N), the data values at the nodes.
!
!    Input, integer NQ, the number of data points to be used in the least
!    squares fit for coefficients defining the nodal functions Q(K).  
!    A recommended value is NQ = 17.  9 <= NQ <= MIN ( 40, N-1 ).
!
!    Input, integer NW, the number of nodes within (and defining) the radii
!    of influence R(K) which enter into the weights W(K).  For N sufficiently
!    large, a recommended value is NW = 32.  1 <= NW <= min(40,N-1).
!
!    Input, integer NR, the number of rows, columns, and planes in the cell
!    grid defined in subroutine store3.  A box containing the nodes is
!    partitioned into cells in order to increase search efficiency.  
!    NR = (N/3)**(1/3) is recommended.  1 <= NR.
!
!    Output, integer LCELL(NR,NR,NR), nodal indices
!    associated with cells.  Refer to STORE3.
!
!    Output, integer LNEXT(N), next-node indices.  Refer to STORE3.
!
!    Output, real ( kind = 8 ) xyzmin(3), xyzdel(3), containing 
!    minimum nodal coordinates and cell dim-
!    ensions, respectively.  Refer to store3.
!
!    Output, real ( kind = 8 ) RMAX, square root of the largest element in 
!    RSQ, maximum radius R(K).
!
!    Output, real ( kind = 8 ) RSQ(N) = the squares of the radii r(k)
!    which enter into the weights W(K).
!
!    Output, real ( kind = 8 ) A(9,N), the coefficients for
!    quadratic nodal function Q(K) in column K.
!
!    Output, integer IER, error indicator.
!    0, if no errors were encountered.
!    1, if N, NQ, NW, or NR is out of range.
!    2, if duplicate nodes were encountered.
!    3, if all nodes are coplanar.
!
!  Local parameters:
!
! av =         root-mean-square distance between k and the
!     nodes in the least squares system (unless
!     additional nodes are introduced for stabil-
!     ity).  the first 6 columns of the matrix
!     are scaled by 1/avsq, the last 3 by 1/av
! avsq =       av*av
! b =         transpose of the augmented regression matrix
! c =         first component of the plane rotation used to
!     zero the lower triangle of b**t -- computed
!     by subroutine givens
! dmin =       minimum of the magnitudes of the diagonal
!     elements of the regression matrix after
!     zeros are introduced below the diagonal
! dtol =       tolerance for detecting an ill-conditioned
!     system.  the system is accepted when DTOL <= DMIN.
! fk =         data value at node k -- f(k)
! i =         index for a, b, npts, xyzmin, xyzmn, xyzdel,
!     and xyzdl
! ib =         do-loop index for back solve
! ierr =       error flag for the call to store3
! irm1 =       irow-1
! irow =       row index for b
! j =         index for a and b
! k =         nodal function index and column index for a
! lmax =       maximum number of npts elements (must be con-
!     sistent with the dimension statement above)
! lnp =        current length of npts
! neq =        number of equations in the least squares fit
! nn,nnq,nnr = local copies of n, nq, and nr
! nnw =        local copy of nw
! np =         npts element
! npts =       array containing the indices of a sequence of
!     nodes to be used in the least squares fit
!     or to compute rsq.  the nodes are ordered
!     by distance from k and the last element
!     (usually indexed by lnp) is used only to
!     determine rq, or rsq(k) if NQ < NW.
! nqwmax =     max(nq,nw)
! rq =         radius of influence which enters into the
!     weights for q(k) (see subroutine setup3)
! rs =         squared distance between k and npts(lnp) --
!     used to compute rq and rsq(k)
! rsmx =       maximum rsq element encountered
! rsold =      squared distance between k and npts(lnp-1) --
!     used to compute a relative change in rs
!     between succeeding npts elements
! rtol =       tolerance for detecting a sufficiently large
!     relative change in rs.  if the change is
!     not greater than rtol, the nodes are
!     treated as being the same distance from k
! rws =        current value of rsq(k)
! s =         second component of the plane givens rotation
! sf =         marquardt stabilization factor used to damp
!     out the first 6 solution components (second
!     partials of the quadratic) when the system
!     is ill-conditioned.  as sf increases, the
!     fitting function approaches a linear
! sum2 =        sum of squared euclidean distances between
!     node k and the nodes used in the least
!     squares fit (unless additional nodes are
!     added for stability)
! t =         temporary variable for accumulating a scalar
!     product in the back solve
! xk,yk,zk =   coordinates of node k -- x(k), y(k), z(k)
! xyzdl =      local variables for xyzdel
! xyzmn =      local variables for xyzmin
!
  implicit none

  integer n
  integer nr

  real ( kind = 8 ) a(9,n)
  real ( kind = 8 ) av
  real ( kind = 8 ) avsq
  real ( kind = 8 ) b(10,10)
  real ( kind = 8 ) c
  real ( kind = 8 ) dmin
  real ( kind = 8 ), parameter :: dtol = 0.01D+00
  real ( kind = 8 ) f(n)
  real ( kind = 8 ) fk
  integer i
  integer ib
  integer ier
  integer ierr
  integer irm1
  integer irow
  integer j
  integer k
  integer lcell(nr,nr,nr)
  integer lmax
  integer lnext(n)
  integer lnp
  integer neq
  integer nn
  integer nnq
  integer nnr
  integer nnw
  integer np
  integer npts(40)
  integer nq
  integer nqwmax
  integer nw
  real ( kind = 8 ) rmax
  real ( kind = 8 ) rq
  real ( kind = 8 ) rs
  real ( kind = 8 ) rsmx
  real ( kind = 8 ) rsold
  real ( kind = 8 ) rsq(n)
  real ( kind = 8 ), parameter :: rtol = 1.0D-05
  real ( kind = 8 ) rws
  real ( kind = 8 ) s
  real ( kind = 8 ), parameter :: sf = 1.0D+00
  real ( kind = 8 ) sum2
  real ( kind = 8 ) t
  real ( kind = 8 ) x(n)
  real ( kind = 8 ) xk
  real ( kind = 8 ) xyzdel(3)
  real ( kind = 8 ) xyzdl(3)
  real ( kind = 8 ) xyzmin(3)
  real ( kind = 8 ) xyzmn(3)
  real ( kind = 8 ) y(n)
  real ( kind = 8 ) yk
  real ( kind = 8 ) z(n)
  real ( kind = 8 ) zk

  nn = n
  nnq = nq
  nnw = nw
  nnr = nr
  nqwmax = max ( nnq, nnw )
  lmax = min ( 40, nn - 1 )

  if ( nnq < 9 ) then
    ier = 1
    return
  end if

  if ( nnw < 1 ) then
    ier = 1
    return
  end if

  if ( lmax < nqwmax ) then
    ier = 1
    return
  end if

  if ( nnr < 1 ) then
    ier = 1
    return
  end if
!
!  Create the cell data structure, and initialize RSMX.
!
  call store3 ( nn, x, y, z, nnr, lcell, lnext, xyzmn, xyzdl, ierr )

  if ( ierr /= 0 ) then
    xyzmin(1:3) = xyzmn(1:3)
    xyzdel(1:3) = xyzdl(1:3)
    ier = 3
    return
  end if

  rsmx = 0.0D+00
!
!  Outer loop on node K.
!
  do k = 1, nn

    xk = x(k)
    yk = y(k)
    zk = z(k)
    fk = f(k)
!
!  Mark node K to exclude it from the search for nearest neighbors.
!
    lnext(k) = -lnext(k)
!
!  Initialize for loop on NPTS.
!
    rs = 0.0D+00
    sum2 = 0.0D+00
    rws = 0.0D+00
    rq = 0.0D+00
    lnp = 0
!
!  Compute NPTS, LNP, rws, neq, rq, and avsq.
!
1   continue

    sum2 = sum2 + rs

    if ( lnp == lmax ) then
      go to 3
    end if

    lnp = lnp + 1
    rsold = rs
    call getnp3 ( xk, yk, zk, x, y, z, nnr, lcell, lnext, xyzmn, xyzdl, np, rs )

    if ( rs == 0.0D+00 ) then
      go to 21
    end if

    npts(lnp) = np

    if ( ( rs - rsold ) / rs < rtol ) then
      go to 1
    end if

    if ( rws == 0.0D+00 .and. nnw < lnp ) then
      rws = rs
    end if

    if ( rq /= 0.0D+00 .or. lnp <= nnq ) then
      go to 2
    end if
!
!  RQ = 0 (not yet computed) and NQ < LNP.  rq =
!  sqrt(rs) is sufficiently large to (strictly) include
!  nq nodes.  The least squares fit will include neq =
!  lnp-1 equations for 9 <= nq <= neq < lmax <= n-1.
!
    neq = lnp - 1
    rq = sqrt ( rs )
    avsq = sum2 / real ( neq, kind = 8 )
!
!  Bottom of loop -- test for termination.
!
2   continue

    if ( nqwmax < lnp ) then
      go to 4
    else
      go to 1
    end if
!
!  All LMAX nodes are included in npts.  rws and/or rq**2 is
!  (arbitrarily) taken to be 10 percent larger than the
!  distance RS to the last node included.
!
3   continue

    if ( rws == 0.0D+00 ) then
      rws = 1.1D+00 * rs
    end if

    if ( rq == 0.0D+00 ) then
      neq = lmax
      rq = sqrt ( 1.1D+00 * rs )
      avsq = sum2 / real ( neq,  kind = 8 )
    end if
!
!  Store RSQ(K), update RSMX if necessary, and compute av.
!
4   continue

    rsq(k) = rws
    rsmx = max ( rsmx, rs )
    av = sqrt ( avsq )
!
!  Set up the augmented regression matrix (transposed) as the
!  columns of B, and zero out the lower triangle (upper
!  triangle of b) with givens rotations -- qr decomposition
!  with orthogonal matrix q not stored.
!
    i = 0

5   continue

    i = i + 1
    np = npts(i)
    irow = min ( i, 10 )
    call setup3 ( xk, yk, zk, fk, x(np), y(np), z(np), f(np), av, avsq, &
      rq, b(1,irow) )

    if ( i == 1 ) then
      go to 5
    end if

    irm1 = irow-1

    do j = 1, irow-1
      call givens ( b(j,j), b(j,irow), c, s )
      call rotate ( 10-j, c, s, b(j+1,j), b(j+1,irow) )
    end do

    if ( i < neq ) then
      go to 5
    end if
!
!  Test the system for ill-conditioning.
!
    dmin = min ( abs(b(1,1)), abs(b(2,2)), abs(b(3,3)), &
               abs(b(4,4)), abs(b(5,5)), abs(b(6,6)), &
               abs(b(7,7)), abs(b(8,8)), abs(b(9,9)) )

    if ( dtol <= dmin * rq ) then
      go to 13
    end if

    if ( neq == lmax ) then
      go to 10
    end if
!
!  Increase RQ and add another equation to the system to
!  improve the conditioning.  The number of NPTS elements
!  is also increased if necessary.
!
7   continue

    rsold = rs
    neq = neq + 1
    if ( neq == lmax ) go to 9
    if ( neq == lnp ) go to 8
!
!  NEQ < LNP
!
    np = npts(neq+1)
    rs = (x(np)-xk)**2 + (y(np)-yk)**2 + (z(np)-zk)**2
    if ( ( rs - rsold ) / rs < rtol ) go to 7
    rq = sqrt ( rs )
    go to 5
!
!  Add an element to NPTS.
!
8   continue

    lnp = lnp + 1
    call getnp3 ( xk, yk, zk, x, y, z, nnr, lcell, lnext, xyzmn, xyzdl, np, rs )

    if ( np == 0 ) then
      go to 21
    end if

    npts(lnp) = np

    if ( ( rs - rsold ) / rs < rtol ) then
      go to 7
    end if

    rq = sqrt ( rs )
    go to 5

9   continue

    rq = sqrt ( 1.1D+00 * rs )
    go to 5
!
!  Stabilize the system by damping second partials.  Add
!  multiples of the first six unit vectors to the first
!  six equations.
!
10  continue

    do i = 1, 6

      b(i,10) = sf
      b(i+1:10,10) = 0.0D+00

      do j = i, 9
        call givens ( b(j,j), b(j,10), c, s )
        call rotate ( 10-j, c, s, b(j+1,j), b(j+1,10) )
      end do

    end do
!
!  Test the stabilized system for ill-conditioning.
!
    dmin = min ( abs(b(1,1)),abs(b(2,2)),abs(b(3,3)), &
               abs(b(4,4)),abs(b(5,5)),abs(b(6,6)), &
               abs(b(7,7)),abs(b(8,8)),abs(b(9,9)) )
!
!  No unique solution due to collinear nodes.
!
    if ( dmin * rq < dtol ) then
      xyzmin(1:3) = xyzmn(1:3)
      xyzdel(1:3) = xyzdl(1:3)
      ier = 3
      return
    end if
!
!  Solve the 9 by 9 triangular system for the coefficients
!
13  continue

    do ib = 1, 9