aboutsummaryrefslogtreecommitdiff
path: root/orrs/src/spline.f
diff options
context:
space:
mode:
Diffstat (limited to 'orrs/src/spline.f')
-rwxr-xr-xorrs/src/spline.f814
1 files changed, 814 insertions, 0 deletions
diff --git a/orrs/src/spline.f b/orrs/src/spline.f
new file mode 100755
index 0000000..e0ebcf5
--- /dev/null
+++ b/orrs/src/spline.f
@@ -0,0 +1,814 @@
+c
+c 1-D Cubic Spline Package.
+c Interpolates a function x(s) from discrete x(i) points.
+c
+c Mark Drela
+c 1985
+c
+c Usage:
+c
+cC---- fill S(i), X(i) arrays
+c S(i) = ...
+c X(i) = ...
+c
+cC---- or.. for a space curve X(i), Y(i), the spline parameter S(i)
+cC- can be computed by
+c CALL SCALC(X,Y,S,N)
+c
+cC---- calculate spline coefficients XS(i), YS(i)
+cC- (or can use SPLIND,SPLINA,SEGSPL,SEGSPD instead as needed)
+c CALL SPLINE(X,XS,S,N)
+c CALL SPLINE(Y,YS,S,N)
+c
+cC---- The above calls are done once, which then enables any number
+cC of calls to the spline interrogation routines. Examples are below.
+cC. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
+c
+c
+cC---- evaluate splined x(s) and/or its derivatives
+cC- at any number of s points SS
+c XX = SEVAL(SS,X,XS,S,N)
+c XXS = DEVAL(SS,X,XS,S,N)
+c XXSS = D2VAL(SS,X,XS,S,N)
+c
+cC---- also evaluate y(s), etc
+c YY = SEVAL(SS,Y,YS,S,N)
+c YYS = DEVAL(SS,Y,YS,S,N)
+c
+cC---- evaluate curvature k(s) of x,y curve
+c CV = CURV(SS,X,XS,Y,YS,S,N)
+c
+cC---- alternative to calling SEVAL,DEVAL,D2VAL separately
+cC- (slightly more efficient if all three quantities are needed)
+c CALL SEVALL(SS,X,XS,S,N, XX,XXS,XXSS)
+c
+c
+
+
+ SUBROUTINE SPLINE(X,XS,S,N)
+ DIMENSION X(N),XS(N),S(N)
+ PARAMETER (NMAX=5001)
+ DIMENSION A(NMAX),B(NMAX),C(NMAX)
+C-------------------------------------------------------
+C Calculates spline coefficients for X(S). |
+C Natural end conditions are used (zero 3rd |
+C derivative over first, last intervals). |
+C |
+C To evaluate the spline at some value of S, |
+C use SEVAL and/or DEVAL. |
+C |
+C S independent variable array (input) |
+C X dependent variable array (input) |
+C XS dX/dS array (calculated) |
+C N number of points (input) |
+C |
+C-------------------------------------------------------
+ IF(N.GT.NMAX) STOP 'SPLINE: array overflow, increase NMAX'
+C
+ DO 1 I=2, N-1
+ DSM = S(I) - S(I-1)
+ DSP = S(I+1) - S(I)
+ B(I) = DSP
+ A(I) = 2.0*(DSM+DSP)
+ C(I) = DSM
+ XS(I) = 3.0*((X(I+1)-X(I))*DSM/DSP + (X(I)-X(I-1))*DSP/DSM)
+ 1 CONTINUE
+C
+C---- set zero 3rd derivative end conditions
+ A(1) = 1.0
+ C(1) = 1.0
+ XS(1) = 2.0*(X(2)-X(1)) / (S(2)-S(1))
+C
+ B(N) = 1.0
+ A(N) = 1.0
+ XS(N) = 2.0*(X(N)-X(N-1)) / (S(N)-S(N-1))
+C
+ IF(N.EQ.2) THEN
+C----- if only two points are present, specify zero 2nd derivative instead
+C- (straight line interpolation will result)
+ B(N) = 1.0
+ A(N) = 2.0
+ XS(N) = 3.0*(X(N)-X(N-1)) / (S(N)-S(N-1))
+ ENDIF
+C
+C---- solve for derivative array XS
+ CALL TRISOL(A,B,C,XS,N)
+C
+ RETURN
+ END ! SPLINE
+
+
+
+ SUBROUTINE SPLIND(X,XS,S,N,XS1,XS2)
+ DIMENSION X(N),XS(N),S(N)
+ PARAMETER (NMAX=5001)
+ DIMENSION A(NMAX),B(NMAX),C(NMAX)
+C-------------------------------------------------------
+C Calculates spline coefficients for X(S). |
+C Same as SPLINE, but also allows specified-slope |
+C or zero-curvature end conditions to be imposed. |
+C |
+C To evaluate the spline at some value of S, |
+C use SEVAL and/or DEVAL. |
+C |
+C S independent variable array (input) |
+C X dependent variable array (input) |
+C XS dX/dS array (calculated) |
+C N number of points (input) |
+C XS1,XS2 endpoint derivatives (input) |
+C If = 999.0, then usual zero second |
+C derivative end condition(s) are used |
+C If = -999.0, then zero third |
+C derivative end condition(s) are used |
+C |
+C Note: specifying both XS1,XS2 = -999.0 |
+C is equivalent to using SPLINE. |
+C |
+C-------------------------------------------------------
+ IF(N.GT.NMAX) STOP 'SPLIND: array overflow, increase NMAX'
+C
+ DO 1 I=2, N-1
+ DSM = S(I) - S(I-1)
+ DSP = S(I+1) - S(I)
+ B(I) = DSP
+ A(I) = 2.0*(DSM+DSP)
+ C(I) = DSM
+ XS(I) = 3.0*((X(I+1)-X(I))*DSM/DSP + (X(I)-X(I-1))*DSP/DSM)
+ 1 CONTINUE
+C
+ IF(XS1.EQ.999.0) THEN
+C----- set zero second derivative end condition
+ A(1) = 2.0
+ C(1) = 1.0
+ XS(1) = 3.0*(X(2)-X(1)) / (S(2)-S(1))
+ ELSE IF(XS1.EQ.-999.0) THEN
+C----- set zero third derivative end condition
+ A(1) = 1.0
+ C(1) = 1.0
+ XS(1) = 2.0*(X(2)-X(1)) / (S(2)-S(1))
+ ELSE
+C----- set specified first derivative end condition
+ A(1) = 1.0
+ C(1) = 0.
+ XS(1) = XS1
+ ENDIF
+C
+ IF(XS2.EQ.999.0) THEN
+ B(N) = 1.0
+ A(N) = 2.0
+ XS(N) = 3.0*(X(N)-X(N-1)) / (S(N)-S(N-1))
+ ELSE IF(XS2.EQ.-999.0) THEN
+ B(N) = 1.0
+ A(N) = 1.0
+ XS(N) = 2.0*(X(N)-X(N-1)) / (S(N)-S(N-1))
+ ELSE
+ A(N) = 1.0
+ B(N) = 0.
+ XS(N) = XS2
+ ENDIF
+C
+ IF(N.EQ.2 .AND. XS1.EQ.-999.0 .AND. XS2.EQ.-999.0) THEN
+ B(N) = 1.0
+ A(N) = 2.0
+ XS(N) = 3.0*(X(N)-X(N-1)) / (S(N)-S(N-1))
+ ENDIF
+C
+C---- solve for derivative array XS
+ CALL TRISOL(A,B,C,XS,N)
+C
+ RETURN
+ END ! SPLIND
+
+
+ SUBROUTINE SPLINA(X,XS,S,N)
+ DIMENSION X(N),XS(N),S(N)
+ LOGICAL LEND
+C-------------------------------------------------------
+C Calculates spline coefficients for X(S) by a |
+C simple averaging of adjacent segment slopes. |
+C |
+C Interpolated X(S) is less likely to oscillate |
+C than with SPLINE, but does not have continuity |
+C in curvature. |
+C |
+C To evaluate the spline at some value of S, |
+C use SEVAL and/or DEVAL. |
+C |
+C S independent variable array (input) |
+C X dependent variable array (input) |
+C XS dX/dS array (calculated) |
+C N number of points (input) |
+C |
+C-------------------------------------------------------
+C
+ LEND = .TRUE.
+ DO 1 I=1, N-1
+ DS = S(I+1)-S(I)
+ IF (DS.EQ.0.) THEN
+ XS(I) = XS1
+ LEND = .TRUE.
+ ELSE
+ DX = X(I+1)-X(I)
+ XS2 = DX / DS
+ IF (LEND) THEN
+ XS(I) = XS2
+ LEND = .FALSE.
+ ELSE
+ XS(I) = 0.5*(XS1 + XS2)
+ ENDIF
+ ENDIF
+ XS1 = XS2
+ 1 CONTINUE
+ XS(N) = XS1
+C
+ RETURN
+ END ! SPLINA
+
+
+ SUBROUTINE TRISOL(A,B,C,D,KK)
+ DIMENSION A(KK),B(KK),C(KK),D(KK)
+C-----------------------------------------
+C Solves KK long, tri-diagonal system |
+C |
+C A C D |
+C B A C D |
+C B A . . |
+C . . C . |
+C B A D |
+C |
+C The righthand side D is replaced by |
+C the solution. A, C are destroyed. |
+C-----------------------------------------
+C
+ DO 1 K=2, KK
+ KM = K-1
+ C(KM) = C(KM) / A(KM)
+ D(KM) = D(KM) / A(KM)
+ A(K) = A(K) - B(K)*C(KM)
+ D(K) = D(K) - B(K)*D(KM)
+ 1 CONTINUE
+C
+ D(KK) = D(KK)/A(KK)
+C
+ DO 2 K=KK-1, 1, -1
+ D(K) = D(K) - C(K)*D(K+1)
+ 2 CONTINUE
+C
+ RETURN
+ END ! TRISOL
+
+
+ FUNCTION GEVAL(SS,X,XS,S,N)
+ DIMENSION X(N),XS(N),S(N)
+C--------------------------------------------------
+C Calculates int( X(SS) ) dS |
+C XS array must have been calculated by SPLINE |
+C--------------------------------------------------
+ ILOW = 1
+ I = N
+C
+ 10 IF(I-ILOW .LE. 1) GO TO 11
+C
+ IMID = (I+ILOW)/2
+ IF(SS .LT. S(IMID)) THEN
+ I = IMID
+ ELSE
+ ILOW = IMID
+ ENDIF
+ GO TO 10
+C
+ 11 CONTINUE
+C
+C---- first integrate up to I-1 point
+ GEVAL = 0.
+ DO K = 2, I-1
+ DS = S(K) - S(K-1)
+C
+C------ Int X(t) dt for t = 0..1
+ DGEV = 0.5*(X(K) + X(K-1)) + (XS(K-1) - XS(K))*DS/12.0
+C
+ GEVAL = GEVAL + DGEV*DS
+ ENDDO
+C
+C---- now integrate up to SS value in I-1..I interval
+ DS = S(I) - S(I-1)
+ T = (SS - S(I-1)) / DS
+ CX1 = DS*XS(I-1) - X(I) + X(I-1)
+ CX2 = DS*XS(I) - X(I) + X(I-1)
+C
+ DGEV = 0.5*T*T *X(I)
+ & + (T - 0.5*T*T)*X(I-1)
+ & + (6.0 - 8.0*T + 3.0*T*T)*T*T*CX1/12.0
+ & + ( - 4.0*T + 3.0*T*T)*T*T*CX2/12.0
+C
+ GEVAL = GEVAL + DGEV*DS
+C
+ RETURN
+ END ! GEVAL
+
+
+ FUNCTION SEVAL(SS,X,XS,S,N)
+ DIMENSION X(N),XS(N),S(N)
+C--------------------------------------------------
+C Calculates X(SS) |
+C XS array must have been calculated by SPLINE |
+C--------------------------------------------------
+ ILOW = 1
+ I = N
+C
+ 10 IF(I-ILOW .LE. 1) GO TO 11
+C
+ IMID = (I+ILOW)/2
+ IF(SS .LT. S(IMID)) THEN
+ I = IMID
+ ELSE
+ ILOW = IMID
+ ENDIF
+ GO TO 10
+C
+ 11 DS = S(I) - S(I-1)
+ T = (SS - S(I-1)) / DS
+ CX1 = DS*XS(I-1) - X(I) + X(I-1)
+ CX2 = DS*XS(I) - X(I) + X(I-1)
+ SEVAL = T*X(I) + (1.0-T)*X(I-1) + (T-T*T)*((1.0-T)*CX1 - T*CX2)
+ RETURN
+ END ! SEVAL
+
+
+ FUNCTION DEVAL(SS,X,XS,S,N)
+ DIMENSION X(N),XS(N),S(N)
+C--------------------------------------------------
+C Calculates dX/dS(SS) |
+C XS array must have been calculated by SPLINE |
+C--------------------------------------------------
+ ILOW = 1
+ I = N
+C
+ 10 IF(I-ILOW .LE. 1) GO TO 11
+C
+ IMID = (I+ILOW)/2
+ IF(SS .LT. S(IMID)) THEN
+ I = IMID
+ ELSE
+ ILOW = IMID
+ ENDIF
+ GO TO 10
+C
+ 11 DS = S(I) - S(I-1)
+ T = (SS - S(I-1)) / DS
+ CX1 = DS*XS(I-1) - X(I) + X(I-1)
+ CX2 = DS*XS(I) - X(I) + X(I-1)
+ DEVAL = X(I) - X(I-1) + (1.-4.0*T+3.0*T*T)*CX1 + T*(3.0*T-2.)*CX2
+ DEVAL = DEVAL/DS
+ RETURN
+ END ! DEVAL
+
+ FUNCTION D2VAL(SS,X,XS,S,N)
+ DIMENSION X(N),XS(N),S(N)
+C--------------------------------------------------
+C Calculates d2X/dS2(SS) |
+C XS array must have been calculated by SPLINE |
+C--------------------------------------------------
+ ILOW = 1
+ I = N
+C
+ 10 IF(I-ILOW .LE. 1) GO TO 11
+C
+ IMID = (I+ILOW)/2
+ IF(SS .LT. S(IMID)) THEN
+ I = IMID
+ ELSE
+ ILOW = IMID
+ ENDIF
+ GO TO 10
+C
+ 11 DS = S(I) - S(I-1)
+ T = (SS - S(I-1)) / DS
+ CX1 = DS*XS(I-1) - X(I) + X(I-1)
+ CX2 = DS*XS(I) - X(I) + X(I-1)
+ D2VAL = (6.*T-4.)*CX1 + (6.*T-2.0)*CX2
+ D2VAL = D2VAL/DS**2
+ RETURN
+ END ! D2VAL
+
+
+ SUBROUTINE SEVALL(SS,X,XS,S,N,
+ & XX, XXS, XXSS )
+ DIMENSION X(N),XS(N),S(N)
+C--------------------------------------------------
+C Calculates all spline derivatives. |
+C (Combines SEVAL, DEVAL, D2VAL) |
+C XS array must have been calculated by SPLINE |
+C--------------------------------------------------
+ ILOW = 1
+ I = N
+C
+ 10 IF(I-ILOW .LE. 1) GO TO 11
+C
+ IMID = (I+ILOW)/2
+ IF(SS .LT. S(IMID)) THEN
+ I = IMID
+ ELSE
+ ILOW = IMID
+ ENDIF
+ GO TO 10
+C
+ 11 DS = S(I) - S(I-1)
+ T = (SS - S(I-1)) / DS
+C
+ F0 = X(I-1)
+ F1 = DS*XS(I-1)
+ F2 = -DS*(2.0*XS(I-1) + XS(I)) + 3.0*(X(I) - X(I-1))
+ F3 = DS*( XS(I-1) + XS(I)) - 2.0*(X(I) - X(I-1))
+C
+ XX = F0 + T*(F1 + T*( F2 + T* F3))
+ XXS = F1 + T*(2.0*F2 + T*3.0*F3)
+ XXSS = 2.0*F2 + T*6.0*F3
+C
+ XXS = XXS/DS
+ XXSS = XXSS/DS**2
+C
+ RETURN
+ END ! SEVALL
+
+
+
+ SUBROUTINE SEVLIN(SS,X,S,N, XX,XXS)
+ DIMENSION X(N),S(N)
+C------------------------------------------------------------
+C Calculates X(SS) and dX/ds(SS) using piecewise-linear |
+C interpolation. This is intended for intepolating very |
+C noisy data for which a cubic spline is inappropriate. |
+C------------------------------------------------------------
+ ILOW = 1
+ I = N
+C
+ 10 IF(I-ILOW .LE. 1) GO TO 11
+C
+ IMID = (I+ILOW)/2
+ IF(SS .LT. S(IMID)) THEN
+ I = IMID
+ ELSE
+ ILOW = IMID
+ ENDIF
+ GO TO 10
+C
+ 11 DS = S(I) - S(I-1)
+ T = (SS - S(I-1)) / DS
+ XX = T*X(I) + (1.0-T)*X(I-1)
+ XXS = (X(I) - X(I-1))/DS
+C
+ RETURN
+ END ! SEVLIN
+
+
+
+ FUNCTION CURV(SS,X,XS,Y,YS,S,N)
+ DIMENSION X(N), XS(N), Y(N), YS(N), S(N)
+C-----------------------------------------------
+C Calculates curvature of splined 2-D curve |
+C at S = SS |
+C |
+C S arc length array of curve |
+C X, Y coordinate arrays of curve |
+C XS,YS derivative arrays |
+C (calculated earlier by SPLINE) |
+C-----------------------------------------------
+C
+ ILOW = 1
+ I = N
+C
+ 10 IF(I-ILOW .LE. 1) GO TO 11
+C
+ IMID = (I+ILOW)/2
+ IF(SS .LT. S(IMID)) THEN
+ I = IMID
+ ELSE
+ ILOW = IMID
+ ENDIF
+ GO TO 10
+C
+ 11 DS = S(I) - S(I-1)
+ T = (SS - S(I-1)) / DS
+C
+ F1 = DS*XS(I-1)
+ F2 = -DS*(2.0*XS(I-1) + XS(I)) + 3.0*(X(I) - X(I-1))
+ F3 = DS*( XS(I-1) + XS(I)) - 2.0*(X(I) - X(I-1))
+C
+ XD = F1 + T*(2.0*F2 + T*3.0*F3)
+ XDD = 2.0*F2 + T*6.0*F3
+C
+C
+ G1 = DS*YS(I-1)
+ G2 = -DS*(2.0*YS(I-1) + YS(I)) + 3.0*(Y(I) - Y(I-1))
+ G3 = DS*( YS(I-1) + YS(I)) - 2.0*(Y(I) - Y(I-1))
+C
+ YD = G1 + T*(2.0*G2 + T*3.0*G3)
+ YDD = 2.0*G2 + T*6.0*G3
+C
+C
+ CURV = (XD*YDD - YD*XDD) / SQRT((XD*XD + YD*YD)**3)
+C
+ RETURN
+ END ! CURV
+
+
+ FUNCTION CURVS(SS,X,XS,Y,YS,S,N)
+ DIMENSION X(N), XS(N), Y(N), YS(N), S(N)
+C-----------------------------------------------
+C Calculates curvature derivative of |
+C splined 2-D curve at S = SS |
+C |
+C S arc length array of curve |
+C X, Y coordinate arrays of curve |
+C XS,YS derivative arrays |
+C (calculated earlier by SPLINE) |
+C-----------------------------------------------
+C
+ ILOW = 1
+ I = N
+C
+ 10 IF(I-ILOW .LE. 1) GO TO 11
+C
+ IMID = (I+ILOW)/2
+ IF(SS .LT. S(IMID)) THEN
+ I = IMID
+ ELSE
+ ILOW = IMID
+ ENDIF
+ GO TO 10
+C
+ 11 DS = S(I) - S(I-1)
+ T = (SS - S(I-1)) / DS
+C
+ CX1 = DS*XS(I-1) - X(I) + X(I-1)
+ CX2 = DS*XS(I) - X(I) + X(I-1)
+ XD = X(I) - X(I-1) + (1.0-4.0*T+3.0*T*T)*CX1 + T*(3.0*T-2.0)*CX2
+ XDD = (6.0*T-4.0)*CX1 + (6.0*T-2.0)*CX2
+ XDDD = 6.0*CX1 + 6.0*CX2
+C
+ CY1 = DS*YS(I-1) - Y(I) + Y(I-1)
+ CY2 = DS*YS(I) - Y(I) + Y(I-1)
+ YD = Y(I) - Y(I-1) + (1.0-4.0*T+3.0*T*T)*CY1 + T*(3.0*T-2.0)*CY2
+ YDD = (6.0*T-4.0)*CY1 + (6.0*T-2.0)*CY2
+ YDDD = 6.0*CY1 + 6.0*CY2
+C
+
+
+ F1 = DS*XS(I-1)
+ F2 = -DS*(2.0*XS(I-1) + XS(I)) + 3.0*(X(I) - X(I-1))
+ F3 = DS*( XS(I-1) + XS(I)) - 2.0*(X(I) - X(I-1))
+C
+ XD = F1 + T*(2.0*F2 + T*3.0*F3)
+ XDD = 2.0*F2 + T*6.0*F3
+ XDDD = 6.0*F3
+C
+C
+ G1 = DS*YS(I-1)
+ G2 = -DS*(2.0*YS(I-1) + YS(I)) + 3.0*(Y(I) - Y(I-1))
+ G3 = DS*( YS(I-1) + YS(I)) - 2.0*(Y(I) - Y(I-1))
+C
+ YD = G1 + T*(2.0*G2 + T*3.0*G3)
+ YDD = 2.0*G2 + T*6.0*G3
+ YDDD = 6.0*G3
+C
+ SQRTB = SQRT(XD*XD + YD*YD)
+ BOT = SQRTB**3
+ DBOTDT = 3.0*SQRTB*(XD*XDD + YD*YDD)
+C
+ TOP = XD*YDD - YD*XDD
+ DTOPDT = XD*YDDD - YD*XDDD
+C
+ CURVS = (DTOPDT*BOT - DBOTDT*TOP) / BOT**2 / DS
+C
+ RETURN
+ END ! CURVS
+
+
+ SUBROUTINE SINVRT(SI,XI,X,XS,S,N)
+ DIMENSION X(N),XS(N),S(N)
+C----------------------------------------------------
+C Calculates the "inverse" spline function S(X). |
+C Since S(X) can be multi-valued or not defined, |
+C this is not a "black-box" routine. The call- |
+C ing program must pass via SI a sufficiently |
+C good initial guess for S(XI). |
+C |
+C XI specified X value (input) |
+C SI calculated S(XI) value (input,output) |
+C X,XS,S usual spline arrays (input) |
+C |
+C----------------------------------------------------
+C
+ DO 10 ITER=1, 10
+ CALL SEVALL(SI,X,XS,S,N, XX,XXS,XXSS)
+ DS = (XI-XX)/XXS
+ SI = SI + DS
+ IF(ABS(DS/(S(N)-S(1))) .LT. 1.0E-5) RETURN
+ 10 CONTINUE
+ WRITE(6,*) 'SINVRT: spline inversion failed. Continuing...'
+ RETURN
+C
+ END ! SINVRT
+
+
+ SUBROUTINE SCALC(X,Y,S,N)
+ DIMENSION X(N),Y(N),S(N)
+C----------------------------------------
+C Calculates the arc length array S |
+C for a 2-D array of points (X,Y). |
+C----------------------------------------
+C
+ S(1) = 0.
+ DO 10 I=2, N
+ S(I) = S(I-1) + SQRT((X(I)-X(I-1))**2 + (Y(I)-Y(I-1))**2)
+ 10 CONTINUE
+C
+ RETURN
+ END ! SCALC
+
+
+ SUBROUTINE SEGSPL(X,XS,S,N)
+ DIMENSION X(N),XS(N),S(N)
+C-----------------------------------------------
+C Splines X(S) array just like SPLINE, |
+C but allows derivative discontinuities |
+C at segment joints. Segment joints are |
+C defined by identical successive S values. |
+C-----------------------------------------------
+C
+ IF(S(1).EQ.S(2) ) STOP 'SEGSPL: First input point duplicated'
+ IF(S(N).EQ.S(N-1)) STOP 'SEGSPL: Last input point duplicated'
+C
+ ISEG0 = 1
+ DO 10 ISEG=2, N-2
+ IF(S(ISEG).EQ.S(ISEG+1)) THEN
+ NSEG = ISEG - ISEG0 + 1
+ CALL SPLINE(X(ISEG0),XS(ISEG0),S(ISEG0),NSEG)
+ ISEG0 = ISEG+1
+ ENDIF
+ 10 CONTINUE
+C
+ NSEG = N - ISEG0 + 1
+ CALL SPLINE(X(ISEG0),XS(ISEG0),S(ISEG0),NSEG)
+C
+ RETURN
+ END ! SEGSPL
+
+
+ SUBROUTINE SEGSPD(X,XS,S,N,XS1,XS2)
+ DIMENSION X(N),XS(N),S(N)
+C-----------------------------------------------
+C Splines X(S) array just like SPLIND, |
+C but allows derivative discontinuities |
+C at segment joints. Segment joints are |
+C defined by identical successive S values. |
+C-----------------------------------------------
+C
+ IF(S(1).EQ.S(2) ) STOP 'SEGSPD: First input point duplicated'
+ IF(S(N).EQ.S(N-1)) STOP 'SEGSPD: Last input point duplicated'
+C
+ ISEG0 = 1
+ DO 10 ISEG=2, N-2
+ IF(S(ISEG).EQ.S(ISEG+1)) THEN
+ NSEG = ISEG - ISEG0 + 1
+ CALL SPLIND(X(ISEG0),XS(ISEG0),S(ISEG0),NSEG,XS1,XS2)
+ ISEG0 = ISEG+1
+ ENDIF
+ 10 CONTINUE
+C
+ NSEG = N - ISEG0 + 1
+ CALL SPLIND(X(ISEG0),XS(ISEG0),S(ISEG0),NSEG,XS1,XS2)
+C
+ RETURN
+ END ! SEGSPD
+
+
+
+ SUBROUTINE INTERS(OK,SS1,SS2,
+ & X1,XS1,Y1,YS1,S1,N1,
+ & X2,XS2,Y2,YS2,S2,N2 )
+ LOGICAL OK
+ DIMENSION X1(N1),XS1(N1),Y1(N1),YS1(N1),S1(N1)
+ DIMENSION X2(N2),XS2(N2),Y2(N2),YS2(N2),S2(N2)
+C-------------------------------------------------------
+C Finds spline coordinate values SS1, SS2 at the
+C intersection of two space curves (X1,Y1), (X2,Y2).
+C-------------------------------------------------------
+ LOGICAL CLIP1, CLIP2
+ DATA EPS / 1.0E-5 /
+C
+ OK = .TRUE.
+ccc SS1 = S1(1)
+ccc SS2 = S2(1)
+ RS1 = 1.0E12
+ RS2 = 1.0E12
+ DS1 = 0.0
+ DS2 = 0.0
+C
+ DO 1000 ITER=1, 12
+C
+ RLX = 1.0
+ SS1OLD = SS1
+ SS2OLD = SS2
+ RS1OLD = ABS(RS1)
+ RS2OLD = ABS(RS2)
+C
+ DO 10 IRLX=1, 16
+C
+ CLIP1 = .FALSE.
+ CLIP2 = .FALSE.
+ SS1 = SS1OLD + RLX*DS1
+ SS2 = SS2OLD + RLX*DS2
+C
+ IF(SS1.LT.S1(1) .OR. SS1.GT.S1(N1)) THEN
+ CLIP1 = .TRUE.
+ SS1 = MAX(SS1,S1(1 ))
+ SS1 = MIN(SS1,S1(N1))
+ ENDIF
+ IF(SS2.LT.S2(1) .OR. SS2.GT.S2(N2)) THEN
+ CLIP2 = .TRUE.
+ SS2 = MAX(SS2,S2(1 ))
+ SS2 = MIN(SS2,S2(N2))
+ ENDIF
+C
+ XX1 = SEVAL(SS1,X1,XS1,S1,N1)
+ XX2 = SEVAL(SS2,X2,XS2,S2,N2)
+ YY1 = SEVAL(SS1,Y1,YS1,S1,N1)
+ YY2 = SEVAL(SS2,Y2,YS2,S2,N2)
+C
+ RS1 = XX1 - XX2
+ RS2 = YY1 - YY2
+C
+ IF(ABS(RS1).LT.RS1OLD .AND.
+ & ABS(RS2).LT.RS2OLD ) GO TO 11
+C
+ RLX = 0.5*RLX
+C
+ 10 CONTINUE
+ WRITE(*,*) 'INTERS: Under-relaxation loop failed.'
+ 11 CONTINUE
+C
+ A11 = DEVAL(SS1,X1,XS1,S1,N1)
+ A12 = -DEVAL(SS2,X2,XS2,S2,N2)
+ A21 = DEVAL(SS1,Y1,YS1,S1,N1)
+ A22 = -DEVAL(SS2,Y2,YS2,S2,N2)
+C
+ DET = A11*A22 - A12*A21
+ DS1 = -(RS1*A22 - A12*RS2)/DET
+ DS2 = -(A11*RS2 - RS1*A21)/DET
+C
+ IF(ABS(DS1) .LT. EPS*(S1(N1)-S1(1)) .AND.
+ & ABS(DS2) .LT. EPS*(S2(N2)-S2(1)) ) RETURN
+C
+ 1000 CONTINUE
+ WRITE(*,*) 'INTERS: Convergence failed. Res =', RS1, RS2
+ IF(CLIP1)
+ & WRITE(*,*)' S1 clip:', S1(1), S1(N1), SS1, DS1
+ IF(CLIP2)
+ & WRITE(*,*)' S2 clip:', S2(1), S2(N2), SS2, DS2
+ OK = .FALSE.
+C
+ RETURN
+ END ! INTERS
+
+
+
+
+
+ SUBROUTINE NEARPT(XPNT,YPNT,SNEAR,X,XP,Y,YP,S,N)
+ IMPLICIT REAL (A-H,M,O-Z)
+ DIMENSION X(N),XP(N),Y(N),YP(N),S(N)
+C========================================================
+C Finds arc length position S=SNEAR of a point
+C on a 2-D splined curve X(S),Y(S) nearest the
+C specified point XPNT,YPNT.
+C
+C Assumes the value passed in via SNEAR is a good
+C initial guess.
+C========================================================
+C
+C---- convergence tolerance
+ EPS = 1.0E-4 * (S(N) - S(1))
+C
+C---- Newton iteration loop
+ DO 215 IPASS=1, 10
+ CALL SEVALL(SNEAR,X,XP,S,N,XXI,XPI,X2I)
+ CALL SEVALL(SNEAR,Y,YP,S,N,YYI,YPI,Y2I)
+C
+C------ residual is dot product with curve tangent vector
+ RES = (XXI-XPNT)*XPI + (YYI-YPNT)*YPI
+C
+ RES_S = (XPI )*XPI + (YPI )*YPI
+ & + (XXI-XPNT)*X2I + (YYI-YPNT)*Y2I
+C
+ DSN = -RES/RES_S
+ SNEAR = SNEAR + DSN
+ IF(ABS(DSN) .LT. EPS) GO TO 216
+C
+ 215 CONTINUE
+ WRITE(*,*) 'NEARPT: Convergence failed. Continuing...'
+ 216 CONTINUE
+C
+ RETURN
+ END ! NEARPT