path: root/modules/interpolation/src/fortran/dspfit.f

*
*   This code comes from the NSWC fortran library with slight
*   modifications from Bruno Pincon 
*
      SUBROUTINE SPFIT (X, Y, WGT, M, BREAK, L, Z, A, WK, IERR)
      implicit none
      integer M, L, IERR
      DOUBLE PRECISION X(M), Y(M), WGT(M), BREAK(L)
      DOUBLE PRECISION Z(*), A(*), WK(*)
C-----------------------------------------------------------------------
C            WEIGHTED LEAST SQUARES CUBIC SPLINE FITTING
C-----------------------------------------------------------------------
      integer N, J, K, LA, LW, LQ, LM1
      DOUBLE PRECISION TEMP(20), DX, B, C
C---------------------
      N = L + 2
C
C                DEFINE THE KNOTS FOR THE B-SPLINES
C
      WK(1) = BREAK(1)
      WK(2) = BREAK(1)
      WK(3) = BREAK(1)
      WK(4) = BREAK(1)
      do J = 2,L               ! the conditions break(k) < break(k+1) are
         WK(J + 3) = BREAK(J)  ! verified in the interface
      enddo
      WK(L + 4) = BREAK(L)
      WK(L + 5) = BREAK(L)
      WK(L + 6) = BREAK(L)
C
C     OBTAIN THE B-SPLINE COEFFICIENTS OF THE LEAST SQUARES FIT
C
      LA = N + 5     ! start indices (in wk) for the others working area
      LW = LA + N    !
      LQ = LW + N    !
      CALL BSLSQ (X, Y, WGT, M, WK(1), N, 4, WK(LA),
     *            WK(LW), WK(LQ), IERR)
*
*     pour BSLSQ : IERR =-1  not enough points for the fit
*                  IERR = 0  OK 
*                  IERR = 1  non uniqness of the solution (but a solution is computed)
*
      IF (IERR .GE. 0) then
C         OBTAIN THE COEFFICIENTS OF THE FIT IN TAYLOR SERIES FORM
         CALL BSPP (WK(1), WK(LA), N, 4, BREAK,
     *              WK(LQ), LM1, TEMP)
         K = LQ
         DO J = 1,LM1
            Z(J) = WK(K)
            A(J) = WK(K + 1)
            K = K + 4
         enddo

         ! a trick to get the spline value (Z(L)) and first derivative
         ! (A(L)) on the last breakpoint : the last polynomial piece
         ! has the form  Z(LM1) + A(LM1)(x- break(l-1)) + B(LM1)(...)
         DX = BREAK(L) - BREAK(L-1)
         B = WK(LQ + 4*(L-2) + 2)
         C = WK(LQ + 4*(L-2) + 3)
         Z(L) = Z(LM1) + DX*(A(LM1) + DX*(B + DX*C))
         A(L) = A(LM1) + DX*( 2.d0*B + DX*3.d0*C)
      endif
      END


      SUBROUTINE BSLSQ (TAU, GTAU, WGT, NTAU, T, N, K, A, WK, Q, IERR)
      implicit none
      integer NTAU, K, N, IERR
      DOUBLE PRECISION TAU(NTAU), GTAU(NTAU), WGT(NTAU)
      DOUBLE PRECISION T(*), A(N), WK(N), Q(K,N)
C-----------------------------------------------------------------------
C
C        BSLSQ PRODUCES THE B-SPLINE COEFFICIENTS OF A PIECEWISE
C              POLYNOMIAL P(X) OF ORDER K WHICH MINIMIZES
C
C                SUM (WGT(J)*(P(TAU(J)) - GTAU(J))**2).
C
C
C     INPUT ...
C
C       TAU   ARRAY OF LENGTH NTAU CONTAINING DATA POINT ABSCISSAE.
C       GTAU  ARRAY OF LENGTH NTAU CONTAINING DATA POINT ORDINATES.
C       WGT   ARRAY OF LENGTH NTAU CONTAINING THE WEIGHTS.
C       NTAU  NUMBER OF DATA POINTS TO BE FITTED.
C       T     KNOT SEQUENCE OF LENGTH N + K.
C       N     DIMENSION OF THE PIECEWISE POLYNOMIAL SPACE.
C       K     ORDER OF THE B-SPLINES.
C
C     OUTPUT ...
C
C       A     ARRAY OF LENGTH N CONTAINING THE B-SPLINE COEFFICIENTS
C             OF THE L2 APPROXIMATION.
C
C       IERR  INTEGER REPORTING THE STATUS OF THE RESULTS ...
C
C             0  THE COEFFICIENT MATRIX IS NONSIGULAR. THE
C                UNIQUE LEAST SQUARES SOLUTION WAS OBTAINED.
C             1  THE COEFFICIENT MATRIX IS SINGULAR. A
C                LEAST SQUARES SOLUTION WAS OBTAINED.
C            -1  INPUT ERRORS WERE DETECTED.
C
C-----------------------------------------------------------------------
C
      integer I, J, JJ, L, LEFT, LEFTMK, MM, ntau_count
      double precision dw
      external isearch
      integer  isearch

*  some modifs :
*    1/ to avoid the sort on the data points use a dicho search to get
*       the interval LEFT
*    2/ all the data points outside the interval definition of the spline 
*       ([T(K),T(N+1)]) or with a non positive weight are not taken into acount
*       in the fit
*
*
*   Note: the breakpoints goes to T(K) until T(N+1)  (N+1-K+1 points)
*       T(K) is the first break point  (T(K) = X(1), ..., T(I) = X(I-K+1)
*       T(N+1) = T(L+K-1) = X(L)  is the last break point 
C
      do J = 1,N
         A(J) = 0.D0
         do  I = 1,K
            Q(I,J) = 0.d0
         enddo
      enddo
C
      ntau_count = 0
      LEFT = K
      DO  L = 1,NTAU
         if (      TAU(L).ge.T(K) .and. TAU(L).le.T(N+1)    ! added by Bruno 
     $       .and. WGT(L) .gt. 0.d0 ) then
            ntau_count = ntau_count + 1
*           find the index left such that  T(LEFT) <= TAU(L) <= T(LEFT+1) (modified by Bruno)
            LEFT = isearch(TAU(L), T(K), N-K+2) + 3
            JJ = 0
            CALL BSPVB (T, K, K, JJ, TAU(L), LEFT, WK)

            LEFTMK = LEFT - K
            DO MM = 1,K
               DW = WK(MM)*WGT(L)
               J = LEFTMK + MM
               A(J) = DW*GTAU(L) + A(J)
               I = 1
               DO JJ = MM,K
                  Q(I,J) = WK(JJ)*DW + Q(I,J)
                  I = I + 1
               enddo
            enddo
         endif
      enddo

      IF (ntau_count .ge. MAX(2,K)) then
C        SOLVE THE NORMAL EQUATIONS
         CALL BCHFAC (Q, K, N, WK, IERR)
         CALL BCHSLV (Q, K, N, A)
      else
         ierr = -1
      endif
      end


      SUBROUTINE BCHFAC (W, NB, N, DIAG, IFLAG)
      implicit none
      integer NB, N, IFLAG
      DOUBLE PRECISION W(NB,N), DIAG(N)

C-----------------------------------------------------------------------
C  FROM  * A PRACTICAL GUIDE TO SPLINES *  BY C. DE BOOR
C  CONSTRUCTS CHOLESKY FACTORIZATION
C                     C  =  L * D * L-TRANSPOSE
C  WITH L UNIT LOWER TRIANGULAR AND D DIAGONAL, FOR GIVEN MATRIX C OF
C  ORDER  N , IN CASE  C  IS (SYMMETRIC) POSITIVE SEMIDEFINITE
C  AND BANDED, HAVING NB DIAGONALS AT AND BELOW THE MAIN DIAGONAL.
C
C******  INPUT  ******
C
C     N      THE ORDER OF THE MATRIX C.
C
C     NB     THE BANDWIDTH OF C, I.E.,
C               C(I,J) = 0 FOR ABS(I-J) .GT. NB .
C
C     W      WORK ARRAY OF SIZE NB BY N CONTAINING THE NB DIAGONALS
C            IN ITS ROWS, WITH THE MAIN DIAGONAL IN ROW 1. PRECISELY,
C            W(I,J)  CONTAINS  C(I+J-1,J), I=1,...,NB, J=1,...,N.
C            FOR EXAMPLE, THE INTERESTING ENTRIES OF A SEVEN DIAGONAL
C            SYMMETRIC MATRIX C OF ORDER 9 WOULD BE STORED IN W AS
C
C                       11 22 33 44 55 66 77 88 99
C                       21 32 43 54 65 76 87 98
C                       31 42 53 64 75 86 97
C                       41 52 63 74 85 96
C
C            ALL OTHER ENTRIES OF W NOT IDENTIFIED WITH AN ENTRY OF C
C            ARE NEVER REFERENCED.
C
C     DIAG   WORK ARRAY OF LENGTH N.
C
C******  O U T P U T  ******
C                                                         T
C     W      CONTAINS THE CHOLESKY FACTORIZATION C = L*D*L   WHERE
C            W(1,I) = 1/D(I,I) AND W(I,J) = L(I-1+J,J) (I=2,...,NB).
C
C     IFLAG  0 IF C IS NONSINGULAR AND 1 IF C IS SINGULAR.
C
C******  M E T H O D  ******
C
C   GAUSS ELIMINATION, ADAPTED TO THE SYMMETRY AND BANDEDNESS OF  C , IS
C   USED .
C     NEAR ZERO PIVOTS ARE HANDLED IN A SPECIAL WAY. THE DIAGONAL ELE-
C  MENT C(K,K) = W(1,K) IS SAVED INITIALLY IN  DIAG(K), ALL K. AT THE K-
C  TH ELIMINATION STEP, THE CURRENT PIVOT ELEMENT, VIZ.  W(1,K), IS COM-
C  PARED WITH ITS ORIGINAL VALUE, DIAG(K). IF, AS THE RESULT OF PRIOR
C  ELIMINATION STEPS, THIS ELEMENT HAS BEEN REDUCED BY ABOUT A WORD
C  LENGTH, (I.E., IF W(1,K)+DIAG(K) .LE. DIAG(K)), THEN THE PIVOT IS DE-
C  CLARED TO BE ZERO, AND THE ENTIRE K-TH ROW IS DECLARED TO BE LINEARLY
C  DEPENDENT ON THE PRECEDING ROWS. THIS HAS THE EFFECT OF PRODUCING
C   X(K) = 0  WHEN SOLVING  C*X = B  FOR  X, REGARDLESS OF  B. JUSTIFIC-
C  ATION FOR THIS IS AS FOLLOWS. IN CONTEMPLATED APPLICATIONS OF THIS
C  PROGRAM, THE GIVEN EQUATIONS ARE THE NORMAL EQUATIONS FOR SOME LEAST-
C  SQUARES APPROXIMATION PROBLEM, DIAG(K) = C(K,K) GIVES THE NORM-SQUARE
C  OF THE K-TH BASIS FUNCTION, AND, AT THIS POINT,  W(1,K)  CONTAINS THE
C  NORM-SQUARE OF THE ERROR IN THE LEAST-SQUARES APPROXIMATION TO THE K-
C  TH BASIS FUNCTION BY LINEAR COMBINATIONS OF THE FIRST K-1 . HAVING
C  W(1,K)+DIAG(K) .LE. DIAG(K) SIGNIFIES THAT THE K-TH FUNCTION IS LIN-
C  EARLY DEPENDENT TO MACHINE ACCURACY ON THE FIRST K-1 FUNCTIONS, THERE
C  FORE CAN SAFELY BE LEFT OUT FROM THE BASIS OF APPROXIMATING FUNCTIONS
C     THE SOLUTION OF A LINEAR SYSTEM
C                       C*X = B
C   IS EFFECTED BY THE SUCCESSION OF THE FOLLOWING  T W O  CALLS ...
C     CALL BCHFAC (W, NB, N, DIAG, IFLAG)   , TO GET FACTORIZATION
C     CALL BCHSLV (W, NB, N, B, X )            , TO SOLVE FOR X.
C-----------------------------------------------------------------------
C
      integer I, J, K, IMAX, JMAX, KPI, IPJ
      double precision T, RATIO

      IF (N .GT. 1) GO TO 10
         IFLAG = 1
         IF (W(1,1) .EQ. 0.D0) RETURN
         IFLAG = 0
         W(1,1) = 1.D0/W(1,1)
         RETURN
C
C     STORE THE DIAGONAL OF C IN DIAG
C
   10 DO 11 K = 1,N
         DIAG(K) = W(1,K)
   11 CONTINUE
C
C     FACTORIZATION
C
      IFLAG = 0
      DO 60 K = 1,N
         T = W(1,K) + DIAG(K)
         IF (T .NE. DIAG(K)) GO TO 30
            IFLAG = 1
            DO 20 J = 1,NB
               W(J,K) = 0.D0
   20       CONTINUE
            GO TO 60
C
   30    T = 1.D0/W(1,K)
         W(1,K) = T
         IMAX = MIN(NB - 1,N - K)
         IF (IMAX .LT. 1) GO TO 60
         JMAX = IMAX
         DO 50 I = 1,IMAX
            RATIO = T*W(I+1,K)
            KPI = K + I
            DO 40 J = 1,JMAX
               IPJ = I + J
               W(J,KPI) = W(J,KPI) - W(IPJ,K)*RATIO
   40       CONTINUE
            JMAX = JMAX - 1
            W(I+1,K) = RATIO
   50    CONTINUE
   60 CONTINUE
      RETURN
      END

      SUBROUTINE BCHSLV (W, NB, N, B)
      implicit none
      integer NB, N
      DOUBLE PRECISION W(NB,N), B(N)
C-----------------------------------------------------------------------
C
C     BCHSLV SOLVES THE LINEAR SYSTEM C*X = B FOR X WHEN W CONTAINS
C     THE CHOLESKY FACTORIZATION OBTAINED BY THE SUBROUTINE BCHFAC
C     FOR THE BANDED SYMMETRIC POSITIVE DEFINITE MATRIX C.
C
C     INPUT ...
C
C        N   THE ORDER OF THE MATRIX C
C        NB  THE BANDWIDTH OF C
C        W   THE CHOLESKY FACTORIZATION OF C
C        B   VECTOR OF LENGTH N CONTAINING THE RIGHT SIDE
C
C     OUTPUT ...
C
C        B   SOLUTION X OF THE LINEAR SYSTEM C*X = B
C
C                                       T
C     NOTE.  THE FACTORIZATION C = L*D*L  IS USED, WHERE L IS A
C     UNIT LOWER TRIANGULAR MATRIX AND D A DIAGONAL MATRIX.
C
C-----------------------------------------------------------------------
C
      integer J, NBM1, K, JMAX, JPK

      IF (N .GT. 1) GO TO 10
      B(1) = B(1)*W(1,1)
      RETURN
C
C     FORWARD SUBSTITUTION. SOLVE L*Y = B FOR Y AND STORE Y IN B.
C
   10 NBM1 = NB - 1
      DO 30 K = 1,N
         JMAX = MIN(NBM1,N - K)
         IF (JMAX .LT. 1) GO TO 30
         DO 20 J = 1,JMAX
            JPK = J + K
            B(JPK) = B(JPK) - W(J + 1,K)*B(K)
   20    CONTINUE
   30 CONTINUE
C                              T     -1
C     BACKSUBSTITUTION. SOLVE L X = D  Y  FOR X AND STORE X IN B.
C
      K = N
   40    B(K) = B(K)*W(1,K)
         JMAX = MIN(NBM1,N - K)
         IF (JMAX .LT. 1) GO TO 60
         DO 50 J = 1,JMAX
            JPK = J + K
            B(K) = B(K) - W(J + 1,K)*B(JPK)
   50    CONTINUE
   60    K = K - 1
         IF (K .GT. 0) GO TO 40
      RETURN
      END

      SUBROUTINE BSPVB (T, K, JHIGH, J, X, LEFT, BLIST)
      implicit none
      integer K, JHIGH, J, LEFT
      DOUBLE PRECISION T(*), X, BLIST(K)
C-----------------------------------------------------------------------
C
C     BSPVB CALCULATES THE VALUE OF ALL POSSIBLY NONZERO B-SPLINES
C     AT X OF ORDER MAX(JHIGH,J + 1) WHERE T(K) .LE. X .LT. T(N+1).
C
C     DESCRIPTION OF ARGUMENTS
C
C         INPUT
C
C          T       - KNOT VECTOR OF LENGTH N + K.
C          K       - HIGHEST POSSIBLE ORDER OF THE B-SPLINES.
C          JHIGH   - ORDER OF B-SPLINES (1 .LE. JHIGH .LE. K).
C          J       - J .LE. 0  GIVES B-SPLINES OF ORDER JHIGH.
C                    J .GE. 1  ON A PREVIOUS CALL TO BSPVB THE
C                              B-SPLINES OF ORDER J WERE COM-
C                              PUTED AND STORED IN BLIST. IT IS
C                              ASSUMED THAT WORK HAS NOT BEEN
C                              MODIFIED AND THAT J .LT. K.
C          X       - ARGUMENT OF THE B-SPLINES.
C          LEFT    - LARGEST INTEGER SUCH THAT
C                    T(LEFT) .LE. X .LT. T(LEFT+1)
C
C         OUTPUT
C
C          BLIST   - VECTOR OF LENGTH K FOR SPLINE VALUES.
C          J       - B-SPLINES OF ORDER J HAVE BEEN COMPUTED
C                    AND STORED IN BLIST.
C
C-----------------------------------------------------------------------
C     WRITTEN BY CARL DE BOOR (UNIVERSITY OF WISCONSIN) AND MODIFIED
C         BY A.H. MORRIS (NSWC).
C-----------------------------------------------------------------------
C
      integer I, IMJ, L
      double precision S, TIMJ, TI, TERM

      IF (J .GT. 0) GO TO 10
         J = 1
         BLIST(1) = 1.D0
         IF (J .GE. JHIGH) RETURN
C
   10 S = 0.D0
      DO 20 L = 1,J
         I = LEFT + L
         IMJ = I - J
         TIMJ = T(IMJ)
         TI = T(I)
         TERM = BLIST(L)/(TI - TIMJ)
         BLIST(L) = S + (TI - X)*TERM
         S = (X - TIMJ)*TERM
   20 CONTINUE
      J = J + 1
      BLIST(J) = S
      IF (J .LT. JHIGH) GO TO 10
C
      RETURN
      END

      SUBROUTINE BSPP (T, A, N, K, BREAK, C, L, WK)
      implicit none
      integer N, K, L
      DOUBLE PRECISION T(*), A(N), BREAK(*), C(K,*), WK(K,*)
C-----------------------------------------------------------------------
C
C              CONVERSION FROM B-SPLINE REPRESENTATION
C              TO PIECEWISE POLYNOMIAL REPRESENTATION
C
C
C     INPUT ...
C
C       T     KNOT SEQUENCE OF LENGTH N+K
C       A     B-SPLINE COEFFICIENT SEQUENCE OF LENGTH N
C       N     LENGTH OF A
C       K     ORDER OF THE B-SPLINES
C
C     OUTPUT ...
C
C       BREAK BREAKPOINT SEQUENCE, OF LENGTH L+1, CONTAINING
C             (IN INCREASING ORDER) THE DISTINCT POINTS OF THE
C             SEQUENCE T(K),...,T(N+1).
C       C     KXL MATRIX WHERE C(I,J) = (I-1)ST RIGHT DERIVATIVE
C             OF THE PP AT BREAK(J) DIVIDED BY FACTORIAL(I-1).
C       L     NUMBER OF POLYNOMIALS WHICH FORM THE PP
C
C     WORK AREA ...
C
C       WK    2-DIMENSIONAL ARRAY OF DIMENSION (K,K+1)
C
C-----------------------------------------------------------------------
C
      integer I, J, KM1, KP1, LEFT, JJ, JP1, KMJ, IL, ILJ, ILKJ
      double precision TERM, DIFF, R, S, X

      L = 0
      BREAK(1) = T(K)
      IF (K .EQ. 1) GO TO 100
      KM1 = K - 1
      KP1 = K + 1
C
C          GENERAL K-TH ORDER CASE
C
      DO 60 LEFT = K,N
         IF (T(LEFT) .EQ. T(LEFT + 1)) GO TO 60
         L = L + 1
         BREAK(L + 1) = T(LEFT + 1)
         DO 10 J = 1,K
            JJ = LEFT - K + J
            WK(J,1) = A(JJ)
   10    CONTINUE
C
         DO 21 J = 1,KM1
            JP1 = J + 1
            KMJ = K - J
            DO 20 I = 1,KMJ
               IL = I + LEFT
               ILKJ = IL - KMJ
               DIFF = T(IL) - T(ILKJ)
               WK(I,JP1) = (WK(I+1,J) - WK(I,J))/DIFF
   20       CONTINUE
   21    CONTINUE
C
         WK(1,KP1) = 1.D0
         X = T(LEFT)
         C(K,L) = WK(1,K)
         R = 1.D0
         DO 50 J = 1,KM1
            JP1 = J + 1
            S = 0.D0
            DO 30 I = 1,J
               IL = I + LEFT
               ILJ = IL - J
               TERM = WK(I,KP1)/(T(IL) - T(ILJ))
               WK(I,KP1) = S + (T(IL) - X)*TERM
               S = (X - T(ILJ))*TERM
   30       CONTINUE
            WK(JP1,KP1) = S
C
            S = 0.D0
            KMJ = K - J
            DO 40 I = 1,JP1
               S = S + WK(I,KMJ)*WK(I,KP1)
   40       CONTINUE
            R = (R*DBLE(KMJ))/DBLE(J)
            C(KMJ,L) = R*S
   50    CONTINUE
   60 CONTINUE
      RETURN
C
C          PIECEWISE CONSTANT CASE
C
  100 DO 110 LEFT = K,N
         IF (T(LEFT) .EQ. T(LEFT + 1)) GO TO 110
         L = L + 1
         BREAK(L + 1) = T(LEFT + 1)
         C(1,L) = A(LEFT)
  110 CONTINUE
      RETURN
      END