SUBROUTINE STEPQS(N,NFE,IFLAG,LENQR,START,CRASH,HOLD,H, $ WK,RELERR,ABSERR,S,Y,YP,YOLD,YPOLD,A,QR,PIVOT,PP, $ RHOVEC,Z0,DZ,T,WORK,SSPAR,PAR,IPAR) C C SUBROUTINE STEPQS TAKES ONE STEP ALONG THE ZERO CURVE OF THE C HOMOTOPY MAP RHO(X,LAMBDA) USING A PREDICTOR-CORRECTOR ALGORITHM. C THE PREDICTOR USES A HERMITE CUBIC INTERPOLANT, AND THE CORRECTOR C RETURNS TO THE ZERO CURVE USING A NEWTON ITERATION, REMAINING C IN A HYPERPLANE PERPENDICULAR TO THE MOST RECENT TANGENT VECTOR. C STEPQS ALSO ESTIMATES A STEP SIZE H FOR THE NEXT STEP ALONG THE C ZERO CURVE. C C C ON INPUT: C C N = DIMENSION OF X. C C NFE = NUMBER OF JACOBIAN MATRIX EVALUATIONS. C C IFLAG = -2, -1, OR 0, INDICATING THE PROBLEM TYPE. C C LENQR = THE LENGTH OF THE ONE DIMENSIONAL ARRAY QR. C C START = .TRUE. ON FIRST CALL TO STEPQS, .FALSE. OTHERWISE. C SHOULD NOT BE MODIFIED BY THE USER AFTER THE FIRST CALL. C C HOLD = ||Y - YOLD|| ; SHOULD NOT BE MODIFIED BY THE USER. C C H = UPPER LIMIT ON LENGTH OF STEP THAT WILL BE ATTEMPTED. H MUST C BE SET TO A POSITIVE NUMBER ON THE FIRST CALL TO STEPQS. C THEREAFTER, STEPQS CALCULATES AN OPTIMAL VALUE FOR H, AND H C SHOULD NOT BE MODIFIED BY THE USER. C C WK = APPROXIMATE CURVATURE FOR THE LAST STEP (COMPUTED BY PREVIOUS C CALL TO STEPQS). UNDEFINED ON FIRST CALL. SHOULD NOT BE C MODIFIED BY THE USER. C C RELERR, ABSERR = RELATIVE AND ABSOLUTE ERROR VALUES. THE ITERATION C IS CONSIDERED TO HAVE CONVERGED WHEN A POINT Z=(X,LAMBDA) IS C FOUND SUCH THAT C ||DZ|| .LE. RELERR*||Z|| + ABSERR, C WHERE DZ IS THE LAST NEWTON STEP. C C S = (APPROXIMATE) ARC LENGTH ALONG THE HOMOTOPY ZERO CURVE UP TO C Y(S) = (X(S),LAMBDA(S)). C C Y(1:N+1) = PREVIOUS POINT (X(S),LAMBDA(S)) FOUND ON THE ZERO CURVE C OF THE HOMOTOPY MAP. C C YP(1:N+1) = UNIT TANGENT VECTOR TO THE ZERO CURVE OF THE HOMOTOPY C MAP AT Y. INPUT IN THIS VECTOR IS NOT USED ON THE FIRST CALL C TO STEPQS. C C YOLD(1:N+1) = A POINT BEFORE Y ON THE ZERO CURVE OF THE HOMOTOPY C MAP. INPUT IN THIS VECTOR IS NOT USED ON THE FIRST CALL TO C STEPQS. C C YPOLD(1:N+1) = UNIT TANGENT VECTOR TO THE ZERO CURVE OF THE C HOMOTOPY MAP AT YOLD. C C A(1:N) = PARAMETER VECTOR IN THE HOMOTOPY MAP. C C QR(1:LENQR) IS A WORK ARRAY CONTAINING THE N X N SYMMETRIC C JACOBIAN MATRIX WITH RESPECT TO X STORED IN PACKED SKYLINE C STORAGE FORMAT. LENQR AND PIVOT DESCRIBE THE DATA C STRUCTURE IN QR. (SEE SUBROUTINE PCGQS FOR A DESCRIPTION C OF THIS DATA STRUCTURE). C C PIVOT(1:N+2) IS A WORK ARRAY WHOSE FIRST N+1 COMPONENTS CONTAIN C THE INDICES OF THE DIAGONAL ELEMENTS OF THE N X N SYMMETRIC C JACOBIAN MATRIX (WITH RESPECT TO X) WITHIN QR. C C PP(1:N) IS A WORK ARRAY CONTAINING THE NEGATIVE OF THE LAST COLUMN C OF THE JACOBIAN MATRIX -[D RHO/D LAMBDA]. C C RHOVEC(1:N+1), Z0(1:N+1), DZ(1:N+1), T(1:N+1) ARE ALL WORK ARRAYS C USED BY STEPQS, TANGQS, AND ROOTQS TO CALCULATE THE TANGENT C VECTORS AND NEWTON STEPS. C C WORK(1:8*(N+1)+LENQR) IS A WORK ARRAY USED BY THE CONJUGATE GRADIENT C ALGORITHM TO SOLVE LINEAR SYSTEMS. C C SSPAR(1:4) = PARAMETERS USED FOR COMPUTATION OF THE OPTIMAL STEP SIZE. C SSPAR(1) = HMIN, SSPAR(2) = HMAX, SSPAR(3) = BMIN, SSPAR(4) = BMAX. C THE OPTIMAL STEP H IS RESTRICTED SUCH THAT C HMIN .LE. H .LE. HMAX, AND BMIN*HOLD .LE. H .LE. BMAX*HOLD. C C PAR(1:*) AND IPAR(1:*) ARE ARRAYS FOR (OPTIONAL) USER PARAMETERS, C WHICH ARE SIMPLY PASSED THROUGH TO THE USER WRITTEN SUBROUTINES C RHO, RHOJS. C C C ON OUTPUT: C C N, LENQR, A ARE UNCHANGED. C C NFE HAS BEEN UPDATED. C C IFLAG C C = -2, -1, OR 0 (UNCHANGED) ON A NORMAL RETURN. C C = 4 IF A JACOBIAN MATRIX WITH RANK < N HAS OCCURRED. THE C ITERATION WAS NOT COMPLETED. C C = 6 IF THE ITERATION FAILED TO CONVERGE. C C START = .FALSE. ON A NORMAL RETURN. C C CRASH C C = .FALSE. ON A NORMAL RETURN. C C = .TRUE. IF THE STEP SIZE H WAS TOO SMALL. H HAS BEEN C INCREASED TO AN ACCEPTABLE VALUE, WITH WHICH STEPQS MAY BE C CALLED AGAIN. C C = .TRUE. IF RELERR AND/OR ABSERR WERE TOO SMALL. THEY HAVE C BEEN INCREASED TO ACCEPTABLE VALUES, WITH WHICH STEPQS MAY C BE CALLED AGAIN. C C HOLD = ||Y-YOLD||. C C H = OPTIMAL VALUE FOR NEXT STEP TO BE ATTEMPTED. NORMALLY H SHOULD C NOT BE MODIFIED BY THE USER. C C WK = APPROXIMATE CURVATURE FOR THE STEP TAKEN BY STEPQS. C C S = (APPROXIMATE) ARC LENGTH ALONG THE ZERO CURVE OF THE HOMOTOPY C MAP UP TO THE LATEST POINT FOUND, WHICH IS RETURNED IN Y. C C RELERR, ABSERR ARE UNCHANGED ON A NORMAL RETURN. THEY ARE POSSIBLY C CHANGED IF CRASH = .TRUE. (SEE DESCRIPTION OF CRASH ABOVE). C C Y, YP, YOLD, YPOLD CONTAIN THE TWO MOST RECENT POINTS AND TANGENT C VECTORS FOUND ON THE ZERO CURVE OF THE HOMOTOPY MAP. C C C CALLS D1MACH, DAXPY, DCOPY, DDOT, DNRM2, DSCAL, F (OR RHO), FJACS C (OR RHOJS), PCGQS, TANGQS. C C ***** DECLARATIONS ***** C C FUNCTION DECLARATIONS C DOUBLE PRECISION D1MACH, DDOT, DNRM2, QOFS C C LOCAL VARIABLES C DOUBLE PRECISION ALPHA, CORDIS, DD001, DD0011,DD01,DD011,DELS, $ FOURU, GAMMA, HFAIL, HTEMP, IDLERR, LAMBDA, OMEGA, ONE, P0, $ P1, PP0, PP1, SIGMA, TEMP, THETA, TWOU, WKOLD, XSTEP INTEGER I, ITCNT, LITFH, J, LK, LST, NP1, PCGWK, ZU LOGICAL FAILED C C SCALAR ARGUMENTS C INTEGER N, NFE, IFLAG, LENQR LOGICAL START, CRASH DOUBLE PRECISION HOLD, H, WK, RELERR, ABSERR, S C C ARRAY DECLARATIONS C DOUBLE PRECISION Y(N+1), YP(N+1), YOLD(N+1), YPOLD(N+1), $ A(N), QR(LENQR), PP(N), RHOVEC(N+1), Z0(N+1), DZ(N+1), $ T(N+1), WORK(8*(N+1)+LENQR), SSPAR(4), PAR(1) INTEGER PIVOT(N+2), IPAR(1) REAL WRGE(8),ACOF(12) C SAVE C DATA WRGE / $ .8735115E+00, .1531947E+00, .3191815E-01, .3339946E-10, $ .4677788E+00, .6970123E-03, .1980863E-05, .1122789E-08/ DATA ACOF / $ .9043128E+00,-.7075675E+00,-.4667383E+01,-.3677482E+01, $ .8516099E+00,-.1953119E+00,-.4830636E+01,-.9770528E+00, $ .1040061E+01, .3793395E-01, .1042177E+01, .4450706E-01/ C C ***** END OF DECLARATIONS ***** C C DEFINITION OF HERMITE CUBIC INTERPOLANT VIA DIVIDED DIFFERENCES. C DD01(P0,P1,DELS) = (P1-P0)/DELS DD001(P0,PP0,P1,DELS) = (DD01(P0,P1,DELS)-PP0)/DELS DD011(P0,P1,PP1,DELS) = (PP1-DD01(P0,P1,DELS))/DELS DD0011(P0,PP0,P1,PP1,DELS) = (DD011(P0,P1,PP1,DELS) - $ DD001(P0,PP0,P1,DELS))/DELS QOFS(P0,PP0,P1,PP1,DELS,S) = ((DD0011(P0,PP0,P1,PP1,DELS)* $ (S-DELS) + DD001(P0,PP0,P1,DELS))*S + PP0)*S + P0 C C ***** FIRST EXECUTABLE STATEMENT ***** C C C ***** INITIALIZATION ***** C C LITFH = MAXIMUM NUMBER OF NEWTON ITERATIONS ALLOWED. C ONE = 1.0 TWOU = 2.0*D1MACH(4) FOURU = TWOU + TWOU NP1 = N+1 FAILED = .FALSE. CRASH = .TRUE. LITFH = 10 PCGWK = 2*N+3 ZU = 3*N+4 C C CHECK THAT ALL INPUT PARAMETERS ARE CORRECT. C C THE ARCLENGTH S MUST BE NONNEGATIVE. C IF (S .LT. 0.0) RETURN C C IF STEP SIZE IS TOO SMALL, DETERMINE AN ACCEPTABLE ONE. C IF (H .LT. FOURU*(1.0+S)) THEN H=FOURU*(1.0 + S) RETURN END IF C C IF ERROR TOLERANCES ARE TOO SMALL, INCREASE THEM TO ACCEPTABLE C VALUES. C TEMP=DNRM2(NP1,Y,1) + 1.0 IF (.5*(RELERR*TEMP+ABSERR) .LT. TWOU*TEMP) THEN IF (RELERR .NE. 0.0) THEN RELERR = FOURU*(1.0+FOURU) TEMP = 0.0 ABSERR = MAX(ABSERR,TEMP) ELSE ABSERR=FOURU*TEMP END IF RETURN END IF C C INPUT PARAMETERS WERE ALL ACCEPTABLE. C CRASH = .FALSE. C C COMPUTE YP ON FIRST CALL. C IF (START) THEN C C INITIALIZE THE IDEAL ERROR USED FOR STEP SIZE ESTIMATION. C IDLERR=SQRT(SQRT(ABSERR)) C C INITIALIZE STARTING POINTS FOR THE CONJUGATE GRADIENT C ALGORITHM TO ZERO. C DO 10 J=1,2*N+2 WORK(J)=0.0 10 CONTINUE CALL TANGQS(Y,YP,YPOLD,A,QR,PIVOT,PP,RHOVEC,WORK, $ N,LENQR,IFLAG,NFE,PAR,IPAR) IF (IFLAG .GT. 0) RETURN END IF C C ***** COMPUTE PREDICTOR POINT Z0 ***** C 20 IF (START) THEN C C COMPUTE Z0 WITH LINEAR PREDICTOR USING Y, YP -- C Z0 = Y+H*YP. C CALL DCOPY(NP1,Y,1,Z0,1) CALL DAXPY(NP1,H,YP,1,Z0,1) C ELSE C C COMPUTE Z0 WITH CUBIC PREDICTOR. C DO 30 I=1,NP1 Z0(I) = QOFS(YOLD(I),YPOLD(I),Y(I),YP(I),HOLD,HOLD+H) 30 CONTINUE C END IF C C ***** END OF PREDICTOR SECTION ***** C C ***** NEWTON ITERATION ***** C DO 140 ITCNT = 1,LITFH C C SET STARTING POINTS FOR CONJUGATE GRADIENT ALGORITHM. C DO 40 J=ZU,ZU+2*N+1 WORK(J) = 0.0 40 CONTINUE C C COMPUTE QR = [D RHO/DX], RHOVEC=RHO, -PP= (D RHO/D LAMBDA). C LAMBDA = Z0(NP1) IF (IFLAG .EQ. -2) THEN C C CURVE TRACKING PROBLEM. C CALL RHOJS(A,LAMBDA,Z0,QR,LENQR,PIVOT,PP,PAR,IPAR) CALL RHO(A,LAMBDA,Z0,RHOVEC,PAR,IPAR) ELSE IF (IFLAG .EQ. -1) THEN C C ZERO FINDING PROBLEM. C CALL FJACS(Z0,QR,LENQR,PIVOT) CALL DSCAL(LENQR,LAMBDA,QR,1) SIGMA=1.0-LAMBDA DO 50 J=1,N QR(PIVOT(J))=QR(PIVOT(J))+SIGMA 50 CONTINUE CALL DCOPY(N,Z0,1,RHOVEC,1) CALL DAXPY(N,-ONE,A,1,RHOVEC,1) CALL F(Z0,PP) CALL DSCAL(N,-ONE,PP,1) CALL DAXPY(N,ONE,RHOVEC,1,PP,1) CALL DAXPY(N,-LAMBDA,PP,1,RHOVEC,1) ELSE C C FIXED POINT PROBLEM. C CALL FJACS(Z0,QR,LENQR,PIVOT) CALL DSCAL(LENQR,-LAMBDA,QR,1) DO 60 J=1,N QR(PIVOT(J)) = QR(PIVOT(J))+1.0 60 CONTINUE CALL DCOPY(N,Z0,1,RHOVEC,1) CALL DAXPY(N,-ONE,A,1,RHOVEC,1) CALL F(Z0,PP) CALL DAXPY(N,-ONE,A,1,PP,1) CALL DAXPY(N,-LAMBDA,PP,1,RHOVEC,1) END IF RHOVEC(NP1) = 0.0 NFE = NFE+1 C C SOLVE SYSTEM TO FIND NEWTON STEP DZ. C CALL PCGQS(N,QR,LENQR,PIVOT,PP,YP,RHOVEC,DZ, $ WORK(PCGWK),IFLAG) IF (IFLAG .GT. 0) RETURN C C TAKE STEP. C CALL DAXPY(NP1, ONE, DZ, 1, Z0, 1) C C CHECK FOR CONVERGENCE. C XSTEP=DNRM2(NP1,DZ,1) IF (XSTEP .LE. RELERR*DNRM2(NP1,Z0,1)+ABSERR) THEN GO TO 160 END IF C 140 CONTINUE C C ***** END OF NEWTON LOOP ***** C C ***** DIDN'T CONVERGE OR TANGENT AT NEW POINT DID NOT MAKE ***** C AN ANGLE SMALLER THAN 60 DEGREES WITH YPOLD -- C TRY AGAIN WITH A SMALLER H C 150 FAILED = .TRUE. HFAIL = H IF (H .LE. FOURU*(1.0 + S)) THEN IFLAG = 6 RETURN ELSE H = .5 * H END IF GO TO 20 C C ***** END OF CONVERGENCE FAILURE SECTION ***** C C ***** CONVERGED -- MOP UP AND RETURN ***** C C COMPUTE TANGENT AT Z0. C 160 CALL TANGQS(Z0,T,YP,A,QR,PIVOT,PP,RHOVEC,WORK,N, $ LENQR,IFLAG,NFE,PAR,IPAR) IF (IFLAG .GT. 0) RETURN C C CHECK THAT COMPUTED TANGENT T MAKES AN ANGLE NO LARGER THAN C 60 DEGREES WITH CURRENT TANGENT YP. (I.E., COS OF ANGLE < .5) C IF NOT, STEP SIZE WAS TOO LARGE, SO THROW AWAY Z0, AND TRY C AGAIN WITH A SMALLER STEP. C ALPHA = DDOT(NP1,T,1,YP,1) IF (ALPHA .LT. 0.5) GOTO 150 ALPHA = ACOS(ALPHA) C C COMPUTE CORRECTOR DISTANCE. C IF (START) THEN CALL DCOPY(NP1,Y,1,WORK(PCGWK),1) CALL DAXPY(NP1,H,YP,1,WORK(PCGWK),1) ELSE DO 170 I=1,NP1 WORK(PCGWK+I-1)=QOFS(YOLD(I),YPOLD(I),Y(I),YP(I),HOLD,HOLD+H) 170 CONTINUE ENDIF CALL DAXPY(NP1,-ONE,Z0,1,WORK(PCGWK),1) CORDIS=DNRM2(NP1,WORK(PCGWK),1) C C SET UP VARIABLES FOR NEXT CALL. C CALL DCOPY(NP1,Y,1,YOLD,1) CALL DCOPY(NP1,Z0,1,Y,1) CALL DCOPY(NP1,YP,1,YPOLD,1) CALL DCOPY(NP1,T,1,YP,1) C C UPDATE ARCLENGTH S = S + ||Y-YOLD||. C HTEMP = HOLD CALL DAXPY(NP1,-ONE,YOLD,1,Z0,1) HOLD = DNRM2(NP1,Z0,1) S = S+HOLD C C COMPUTE IDEAL ERROR FOR STEP SIZE ESTIMATION. C IF (ITCNT .LE. 1) THEN THETA = 8.0 ELSE IF (ITCNT .EQ. 4) THEN THETA = 1.0 ELSE OMEGA=XSTEP/CORDIS IF (ITCNT .LT. 4) THEN LK = 4*ITCNT-7 IF (OMEGA .GE. WRGE(LK)) THEN THETA = 1.0 ELSE IF (OMEGA .GE. WRGE(LK+1)) THEN THETA = ACOF(LK) + ACOF(LK+1)*LOG(OMEGA) ELSE IF (OMEGA .GE. WRGE(LK+2)) THEN THETA = ACOF(LK+2) + ACOF(LK+3)*LOG(OMEGA) ELSE THETA = 8.0 END IF ELSE IF (ITCNT .GE. 7) THEN THETA = 0.125 ELSE LK = 4*ITCNT - 16 IF (OMEGA .GT. WRGE(LK)) THEN LST = 2*ITCNT - 1 THETA = ACOF(LST) + ACOF(LST+1)*LOG(OMEGA) ELSE THETA = 0.125 END IF END IF END IF IDLERR=THETA*IDLERR C C IDLERR SHOULD BE NO BIGGER THAN 1/2 PREVIOUS STEP. C IDLERR = MIN(.5*HOLD,IDLERR) C C COMPUTE OPTIMAL STEP SIZE. C WK = APPROXIMATE CURVATURE = 2*SIN(ALPHA/2)/HOLD WHERE C ALPHA = ARCCOS(YP*YPOLD). C GAMMA = EXPECTED CURVATURE FOR NEXT STEP, COMPUTED BY C EXTRAPOLATING FROM CURRENT CURVATURE WK, AND LAST C CURVATURE WKOLD. GAMMA IS FURTHER REQUIRED TO BE C POSITIVE. C WKOLD = WK WK = 2.0*ABS(SIN(.5*ALPHA))/HOLD IF (START) THEN GAMMA = WK ELSE GAMMA = WK + HOLD/(HOLD+HTEMP)*(WK-WKOLD) END IF GAMMA = MAX(GAMMA, 0.01*ONE) H = SQRT(2.0*IDLERR/GAMMA) C C ENFORCE RESTRICTIONS ON STEP SIZE SO AS TO ENSURE STABILITY. C HMIN <= H <= HMAX, BMIN*HOLD <= H <= BMAX*HOLD. C H = MIN(MAX(SSPAR(1),SSPAR(3)*HOLD,H),SSPAR(4)*HOLD,SSPAR(2)) IF (FAILED) H = MIN(HFAIL,H) START = .FALSE. C C ***** END OF MOP UP SECTION ***** C RETURN C C ***** END OF SUBROUTINE STEPQS ***** END