1      PROGRAM DRSJACG2
2c>> 2009-10-28 DRSJACG2 Krogh -- Remove implicit none.
3c>> 2009-10-26 DRSJACG2 Krogh -- *'s used for dims for NAG compiler.
4c>> 2008-11-01 DRSJACG2 Hanson -- Added row dim parameters to evaluators
5c>> 2006-12-19 DRSJACG2 Hanson -- Added tot. energy constraint in 3rd ex
6c>> 2006-04-11 DRDJACG1 Hanson -- Reduced lengths of sjacg work arrays.
7c>> 2006-04-09 DRSJACG2  Krogh Fixed dimension of YSCALE.
8c>> 2003-07-08 DRSJACG2  R. J. Hanson Fix accum. option with sjacg().
9c>> 2002-02-01 DRSJACG2  R. J. Hanson Example 2 Code, with Download
10
11C     Solve a planar pendulum problem in rectangular coordinates.
12C     The equation is transformed from "Index 3" to "Index 1"
13C     by differentiating the length constraint twice.  The system
14C     is integrated (using SDASSFC) using these two constraints.
15C     In the second integration the problem is transformed from
16C     "Index 3" to "Index 0" by differentiating the length constraint
17C     three times.  The routine SDASSFD uses these three constraints.
18C     A total energy constraint is added in routine SDASSFE.
19C     This example shows that the constraints remain satisfied,
20C     and the integration can succeed with either approach.
21C     It is more efficient to use the "Index 0" problem than
22C     the "Index 1" problem.
23
24C     (THIS VERSION USES NUMERICAL DIFFERENTIATION FOR THE PARTIALS
25C     NEEDED IN ?DASLX, EXAMPLE 5, AVAILABLE WITH DOWNLOAD.)
26c--S replaces "?": DR?JACG2,?DASLX,?DASSFC,?DASSFD,?DASSFE,?COPY,?JACG
27
28      EXTERNAL SDASSFC, SDASSFD, SDASSFE
29
30      integer NDIG, NEQ, MAXCON, LRW, LIW
31C     Set number of equations:
32      parameter (NEQ = 5)
33C     Set number of constraints.
34      parameter (MAXCON = 4)
35C     Work space sizes:
36      parameter (LIW = 30 + NEQ)
37      parameter (LRW =  45 + (5 + 2*MAXCON + 4) * NEQ + NEQ**2)
38      real             TOL
39c++S Default NDIG = 4
40c++  Default NDIG = 11
41c++ Substitute for NDIG below
42      parameter (NDIG = 4 )
43      parameter (TOL = 10.E0 **(-NDIG))
44
45      INTEGER I, INFO(16), IDID, IWORK(LIW)
46      REAL             T, Y(5), YPRIME(5), TOUT,
47     +  RTOL(5), ATOL(5), RWORK(LRW), LENGTH,
48     +  DRL, DRV
49      LOGICAL CONSTRAINT
50      INTEGER KR, KF, KC
51      common / COUNTS / KR, KF, KC
52
53  100 format(20x,
54     +'Example Results for a Constrained Pendulum Problem, Index 1')
55  105 format(20x,'(Numerical Partials)')
56  106 format(20x,'(Numerical Partials and Total Energy Constrained)')
57  110 format(/8x,'T',12x,'y_1',11x,'y_2',11x,
58     + 'y_3',11x,'y_4',11x,'y_5'/ 1P,6D14.4/)
59  130 format(6x,'At the time value', F10.2, 2x,
60     + 'the integration was stopped.')
61  140 format(
62     +'The pendulum length or variation has large weighted errors.'/
63     +'These should remain less than 1 is magnitude:',
64     + 10x,2F8.2/)
65  150 format(6x,
66     +'The pendulum length and its variation have small errors.'//)
67  160 format(20x,
68     +'Example Results for a Constrained Pendulum Problem, Index 0')
69  170 format(5x,'No. of Residual Evaluations',6x,
70     + 'Factorizations',6x,'No. of User Solves'/
71     + 'Constraint Partials-',1x,I8,12x,I8,17x,I8/)
72
73C     Tolerances:
74      DO 10 I=1,NEQ
75        ATOL(I)=TOL
76        RTOL(I)=TOL
77   10 CONTINUE
78C     Setup options:
79      DO 20 I=1,16
80        INFO(I)=0
81   20 CONTINUE
82C     Use partial derivatives provided in evaluation routine:
83      INFO(5)=2
84C     Constrain solution, with 2 constraints:
85      INFO(10)=2
86C     Allow for more 10 * steps (than default):
87      INFO(12)=50000
88C     This is the pendulum length.
89      LENGTH=1.1E0
90      DO 30 I=1,1000,10
91C     Integrate from T=I-1 to TOUT=T+1.  Final TOUT=10.
92C     When the solution first drifts away from the constraints, stop.
93        T=real(I-1)
94        TOUT=T+10.E0
95        CALL SDASLX (SDASSFC, NEQ, T, Y, YPRIME, TOUT, INFO, RTOL,
96     +    ATOL, IDID, RWORK, LRW, IWORK, LIW)
97C  Compute residuals on length and its variation.  They
98C  should be smaller than the tolerances used.
99        DRL=(Y(1)**2+Y(2)**2-LENGTH**2)/(RTOL(1)*LENGTH**2+ATOL(1))
100        DRV=(Y(1)*Y(3)+Y(2)*Y(4))/(RTOL(1)*(abs(Y(1)*Y(3))+
101     +   abs(Y(1)*Y(4)))+ATOL(1))
102        CONSTRAINT=ABS(DRL) .le. 1.E0 .and. ABS(DRV) .le. 1.E0
103        IF(.not. CONSTRAINT) GO TO 40
104   30 CONTINUE
105   40 CONTINUE
106
107      write(*,100)
108      write(*,105)
109      write(*,110) TOUT, Y
110      write(*,170) KR, KF, KC
111      write(*,130) TOUT
112      IF(.not. CONSTRAINT) THEN
113        write(*,140) DRL, DRV
114      ELSE
115        write(*,150)
116      END IF
117
118C     Start the integration over for the index 0 problem.
119      INFO(1)=0
120C     Set the number of constraints to 3.
121      INFO(10)=3
122      DO 50 I=1,1000,10
123C     Integrate from T=I-1 to TOUT=T+1.
124        T=real(I-1)
125        TOUT=T+10.E0
126        CALL SDASLX (SDASSFD, NEQ, T, Y, YPRIME, TOUT, INFO, RTOL,
127     +    ATOL, IDID, RWORK, LRW, IWORK, LIW)
128
129C  Compute residuals on length and its variation.  They
130C  should be smaller than the tolerances used.
131        DRL=(Y(1)**2+Y(2)**2-LENGTH**2)/(RTOL(1)*LENGTH**2+ATOL(1))
132        DRV=(Y(1)*Y(3)+Y(2)*Y(4))/(RTOL(1)*(abs(Y(1)*Y(3))+
133     +   abs(Y(1)*Y(4)))+ATOL(1))
134        CONSTRAINT=ABS(DRL) .le. 1.E0 .and. ABS(DRV) .le. 1.E0
135        IF(.not. CONSTRAINT) GO TO 60
136   50 CONTINUE
137   60 CONTINUE
138      write(*,160)
139      write(*,105)
140      write(*,110) TOUT, Y
141      write(*,170) KR, KF, KC
142      write(*,130) TOUT
143      IF(.not. CONSTRAINT) THEN
144        write(*,140) DRL, DRV
145      ELSE
146        write(*,150)
147      END IF
148C     Start the integration over for the index 0 problem.
149      INFO(1)=0
150C     Set the number of constraints to 4.
151C     (This includes constant total energy).
152      INFO(10)=4
153      DO 70 I=1,1000,10
154C     Integrate from T=I-1 to TOUT=T+1.
155        T=real(I-1)
156        TOUT=T+10.E0
157        CALL SDASLX (SDASSFE, NEQ, T, Y, YPRIME, TOUT, INFO, RTOL,
158     +    ATOL, IDID, RWORK, LRW, IWORK, LIW)
159
160C  Compute residuals on length and its variation.  They
161C  should be smaller than the tolerances used.
162        DRL=(Y(1)**2+Y(2)**2-LENGTH**2)/(RTOL(1)*LENGTH**2+ATOL(1))
163        DRV=(Y(1)*Y(3)+Y(2)*Y(4))/(RTOL(1)*(abs(Y(1)*Y(3))+
164     +   abs(Y(1)*Y(4)))+ATOL(1))
165        CONSTRAINT=ABS(DRL) .le. 1.E0 .and. ABS(DRV) .le. 1.E0
166        IF(.not. CONSTRAINT) GO TO 80
167   70 CONTINUE
168   80 CONTINUE
169      write(*,160)
170      write(*,106)
171      write(*,110) TOUT, Y
172      write(*,170) KR, KF, KC
173      write(*,130) TOUT
174      IF(.not. CONSTRAINT) THEN
175        write(*,140) DRL, DRV
176      ELSE
177        write(*,150)
178      END IF
179      END
180
181      SUBROUTINE SDASSFC(T,Y,YPRIME,DELTA,D,LDD,CJ,IRES,RWORK,IWORK)
182C     Routine for the swinging simple pendulum problem,
183C     with constraints on the index 2 and 3 equations.
184      REAL             T, Y(*), YPRIME(*), DELTA(*), D(LDD,*),
185     +  CJ, RWORK(*), LSQ, MG
186      INTEGER IRES, IWORK(*),LDD
187
188      REAL             ONE, TWO, ZERO, MASS, LENGTH, GRAVITY
189
190C     Use numerical derivatives but based on calls to SJACG.
191      INTEGER MODE, IOPT(5), IWK(21)
192      REAL             WK(3*5+18), F(5), FACC(5),
193     .  FACJ(5), YSCALE(5)
194
195      PARAMETER (ONE=1.E0, TWO=2.E0,
196     +  ZERO=0.E0, MASS=1.E0, LENGTH=1.1E0, GRAVITY=9.806650E0)
197      SAVE mg, LSQ, FACC, FACJ, IOPT, YSCALE
198      INTEGER KR, KF, KC
199      common / COUNTS / KR, KF, KC
200C This is the setup call.  YPRIME need not be solved for as we
201C are providing the correct initial values.
202      IF(IRES .EQ. 0) THEN
203        mg=mass*gravity
204        LSQ=length**2
205        Y(1)=LENGTH
206        Y(2)=ZERO
207        Y(3)=ZERO
208        Y(4)=ZERO
209        Y(5)=ZERO
210
211        YPRIME(1)=ZERO
212        YPRIME(2)=ZERO
213        YPRIME(3)=ZERO
214        YPRIME(4)=-GRAVITY
215        YPRIME(5)=ZERO
216
217        KR=0
218        KF=0
219        KC=0
220C Initialize things needed for numerical differentiation.
221        FACC(1)=ZERO
222        FACJ(1)=ZERO
223      END IF
224
225C The sytem residual value.
226
227      IF(IRES .EQ. 1) THEN
228        DELTA(1)= Y(3)-YPRIME(1)
229        DELTA(2)= Y(4)-YPRIME(2)
230        DELTA(3)=-Y(1)*Y(5)-mass*YPRIME(3)
231        DELTA(4)=-Y(2)*Y(5)-mass*YPRIME(4)-mg
232        DELTA(5)= mass*(Y(3)**2+Y(4)**2)-mg*Y(2)-LSQ*Y(5)
233C Count residual evaluations.
234        KR=KR+1
235      END IF
236
237C The partial derivative matrix.
238      IF(IRES .EQ. 2) THEN
239        MODE=0
240C Accumulate all but one partial derivative columns.
241
242C Set to accumulate variables 1-4:
243        IOPT(1)=-3
244        IOPT(2)= 4
245C Shift to using one-sided differences.
246        IOPT(3)=-1
247C Use on all remaining variables: number 5.
248        IOPT(4)= 0
249        YSCALE(1)=ZERO
250C Pre-compute partials wrt y'.  These values are accumulated.
251        D(1,1)=-CJ
252        D(2,2)=-CJ
253        D(3,3)=-mass*CJ
254        D(4,4)=D(3,3)
255   10 CONTINUE
256C The value of f(y) is normally returned in WK(1:5).
257C Note that all but one partial with respect to y' have been
258C analytically pre-computed and so the results are accumulated.
259        WK(1)=Y(3)
260        WK(2)=Y(4)
261        WK(3)=-Y(1)*Y(5)
262        WK(4)=-Y(2)*Y(5)
263        WK(5)= mass*(Y(3)**2+Y(4)**2)-mg*Y(2)-LSQ*Y(5)
264C Compute f(y) and make a special copy step for the
265C  evaluation point.
266        IF(MODE .eq. 0) THEN
267          F(1)=WK(1)
268          F(2)=WK(2)
269          F(3)=WK(3)
270          F(4)=WK(4)
271          F(5)=WK(5)
272          FACJ(1)=ZERO
273        END IF
274C This is a Math a la Carte code for numerical derivatives.
275        CALL SJACG (MODE, 5, 5, Y,
276     .              F, D, LDD, YSCALE,
277     .              FACJ, IOPT, WK, 33, IWK, 21)
278        IF(MODE .gt. 0) GO TO 10
279        IF(MODE .lt. 0) THEN
280            PRINT
281     1      '('' SDASSFC: Initial error in argument number '',I2)',-MODE
282            STOP '1'
283          END IF
284C All system partials are computed.  Count an evaluation.
285        KF=KF+1
286      END IF
287
288C The constraining equations after the corrector has converged.
289C Both partials and residuals are required.
290      IF(IRES .EQ. 5) THEN
291        MODE=0
292C In this use of numerical differentiation, no special treatment
293C is required for the variables.  The rest of IOPT(:) stays intact.
294        IOPT(1)=0
295        YSCALE(1)=ZERO
296   20 CONTINUE
297C The value of f(y) is normally returned in WK(1:2).
298        WK(1)=Y(1)**2+Y(2)**2-LSQ
299        WK(2)=Y(1)*Y(3)+Y(2)*Y(4)
300C Compute f(y) and make a special copy step for the point in question.
301        IF(MODE .eq. 0) THEN
302          DELTA(6)=WK(1)
303          DELTA(7)=WK(2)
304          FACC(1)=ZERO
305        END IF
306C This is a Math a la Carte code for numerical derivatives.
307        CALL SJACG (MODE, 2, 5, Y,
308     .              DELTA(6), D(6,1), LDD, YSCALE,
309     .              FACC, IOPT, WK, 33, IWK, 21)
310        IF(MODE .gt. 0) GO TO 20
311
312        IF(MODE .lt. 0) THEN
313            PRINT
314     1      '('' SDASSFC: Initial error in argument number '',I2)',-MODE
315            STOP '2'
316          END IF
317C All constraint partials are computed.  Count an evaluation.
318        KC=KC+1
319      END IF
320      END
321
322      SUBROUTINE SDASSFD(T,Y,YPRIME,DELTA,P,LDP,CJ,IRES,RWORK,IWORK)
323C
324C     Routine for the swinging simple pendulum problem,
325C     with constraints on the index 1,2 amd 3 equations.
326C     Use P(:,:) to distinguish from D(:,:) in routine SDASSFC.
327      INTEGER IRES, IWORK(*), LDP
328      REAL             T, Y(*), YPRIME(*), DELTA(*), P(LDP,*),
329     +  CJ, RWORK(*), LSQ, MG
330C     Use numerical derivatives but based on calls to SJACG.
331      INTEGER MODE, IOPT(2), IWK(21)
332      REAL             WK(3*5+18), F(5), FACC(5),
333     .  FACJ(5), YSCALE(5)
334
335      REAL             ONE, TWO, THREE, ZERO, MASS, LENGTH, GRAVITY
336      PARAMETER (ONE=1.E0, TWO=2.E0, THREE=3.E0,
337     +  ZERO=0.E0, MASS=1.E0, LENGTH=1.1E0, GRAVITY=9.806650E0)
338      SAVE mg, LSQ, FACC, FACJ, YSCALE, IOPT
339      INTEGER KR, KF, KC
340      common / COUNTS / KR, KF, KC
341C This is the setup call.
342      IF(IRES .EQ. 0) THEN
343        mg=mass*gravity
344        LSQ=length**2
345        Y(1)=LENGTH
346        Y(2)=ZERO
347        Y(3)=ZERO
348        Y(4)=ZERO
349        Y(5)=ZERO
350        YPRIME(1)=ZERO
351        CALL SCOPY(5,YPRIME,0,YPRIME,1)
352        yprime(4)=-gravity
353        KR=0
354        KF=0
355        KC=0
356        YSCALE(1)=ZERO
357      END IF
358
359C The sytem residual value.
360      IF(IRES .EQ. 1) THEN
361        DELTA(1)=Y(3)-YPRIME(1)
362        DELTA(2)=Y(4)-YPRIME(2)
363        DELTA(3)=-Y(1)*Y(5)-mass*YPRIME(3)
364        DELTA(4)=-Y(2)*Y(5)-mass*YPRIME(4)-mg
365        DELTA(5)= -THREE*mg*Y(4) -LSQ*YPRIME(5)
366        KR=KR+1
367      END IF
368
369C The partial derivative matrix.
370      IF(IRES .EQ. 2) THEN
371        MODE=0
372        IOPT(1)=0
373C This is the last column number to be accumulated when
374C doing numerical differentiation.
375        IOPT(2)= 5
376
377   10 CONTINUE
378C The value of f(y) is normally returned in WK(1:5).
379C Note that the partials with respect to y' have been
380C analytically pre-computed and so the results are accumulated.
381        WK(1)=Y(3)
382        WK(2)=Y(4)
383        WK(3)=-Y(1)*Y(5)
384        WK(4)=-Y(2)*Y(5)
385        WK(5)= -THREE*mg*Y(4)
386C Compute f(y) and make a special copy step for the point in question.
387        IF(MODE .eq. 0) THEN
388          F(1)=WK(1)
389          F(2)=WK(2)
390          F(3)=WK(3)
391          F(4)=WK(4)
392          F(5)=WK(5)
393          FACJ(1)=ZERO
394        END IF
395C This is a Math a la Carte code for numerical derivatives.
396        CALL SJACG (MODE, 5, 5, Y,
397     .              F, P, LDP, YSCALE,
398     .              FACJ, IOPT, WK, 33, IWK, 21)
399        IF(MODE .gt. 0) GO TO 10
400        IF(MODE .lt. 0) THEN
401            PRINT
402     1     '('' SDASSFD: Initial error in argument number '',I2)', -MODE
403            STOP '1'
404          END IF
405C All system partials are computed.  Count an evaluation.
406        KF=KF+1
407C This part of the derivative matrix is a diagonal, hence
408C easy to compute.
409        P(1,1)=-CJ+P(1,1)
410        P(2,2)=-CJ+P(2,2)
411        P(3,3)=-mass*CJ+P(3,3)
412        P(4,4)=P(3,3)
413        P(5,5)=-LSQ*CJ+P(5,5)
414
415      END IF
416
417C The constraining equations after the corrector has converged.
418C Both partials and residuals are required.
419      IF(IRES .EQ. 5) THEN
420        MODE=0
421        IOPT(1)=0
422        YSCALE(1)=ZERO
423   20 CONTINUE
424C The value of f(y) is normally returned in WK(1:3).
425        WK(1)=Y(1)**2+Y(2)**2-LSQ
426        WK(2)=Y(1)*Y(3)+Y(2)*Y(4)
427        WK(3)=MASS*(Y(3)**2+Y(4)**2)-MG*Y(2)-LSQ*Y(5)
428C Compute f(y) and make a special copy step for the
429C evaluation point.
430        IF(MODE .eq. 0) THEN
431          DELTA(6)=WK(1)
432          DELTA(7)=WK(2)
433          DELTA(8)=WK(3)
434          FACC(1)=ZERO
435        END IF
436C This is a Math a la Carte code for numerical derivatives.
437        CALL SJACG (MODE, 3, 5, Y,
438     .              DELTA(6), P(6,1), LDP, YSCALE,
439     .              FACC, IOPT, WK, 33, IWK, 21)
440        IF(MODE .gt. 0) GO TO 20
441        IF(MODE .lt. 0) THEN
442            PRINT
443     1      '('' SDASSFD: Initial error in argument number '',I2)',-MODE
444            STOP '2'
445          END IF
446C All constraint partials are computed.  Count an evaluation.
447        KC=KC+1
448      END IF
449      END
450
451      SUBROUTINE SDASSFE(T,Y,YPRIME,DELTA,P,LDP,CJ,IRES,RWORK,IWORK)
452C
453C     Routine for the swinging simple pendulum problem,
454C     with constraints on the index 1,2 amd 3 equations.
455C     Also the constant energy constraint is added.
456C     Use P(:,:) to distinguish from D(:,:) in routine SDASSFC.
457      INTEGER IRES, IWORK(*), LDP
458      REAL             T, Y(*), YPRIME(*), DELTA(*), P(LDP,*),
459     +  CJ, RWORK(*)
460C     Use numerical derivatives but based on calls to SJACG.
461      INTEGER MODE, IOPT(2), IWK(21)
462      REAL             WK(43), F(5), FACC(5),
463     .  FACJ(5), YSCALE(1)
464
465      REAL             HALF, ONE, TWO, THREE, ZERO, MASS, LENGTH,
466     +                 GRAVITY, LSQ, MG
467      PARAMETER (ONE=1.E0, TWO=2.E0, THREE=3.E0, HALF=0.5E0,
468     +  ZERO=0.E0, MASS=1.E0, LENGTH=1.1E0, GRAVITY=9.806650E0,
469     +          mg=mass*gravity, LSQ=length**2)
470c      SAVE FACC, FACJ, YSCALE, IOPT
471      INTEGER KR, KF, KC
472      common / COUNTS / KR, KF, KC
473C This is the setup call.
474      IF(IRES .EQ. 0) THEN
475        Y(1)=LENGTH
476        Y(2)=ZERO
477        Y(3)=ZERO
478        Y(4)=ZERO
479        Y(5)=ZERO
480        YPRIME(1)=ZERO
481        YPRIME(2)=ZERO
482        YPRIME(3)=ZERO
483        YPRIME(5)=ZERO
484        YPRIME(4)=-GRAVITY
485        KR=0
486        KF=0
487        KC=0
488        YSCALE(1)=ZERO
489      END IF
490
491C The sytem residual value.
492      IF(IRES .EQ. 1) THEN
493        DELTA(1)=Y(3)-YPRIME(1)
494        DELTA(2)=Y(4)-YPRIME(2)
495        DELTA(3)=-Y(1)*Y(5)-mass*YPRIME(3)
496        DELTA(4)=-Y(2)*Y(5)-mass*YPRIME(4)-mg
497        DELTA(5)= -THREE*mg*Y(4) -LSQ*YPRIME(5)
498        KR=KR+1
499      END IF
500
501C The partial derivative matrix.
502      IF(IRES .EQ. 2) THEN
503        MODE=0
504C Accumulate all partial derivative columns.
505C Pre-compute partials wrt y' and accumulate results.
506        IOPT(1)=-3
507C This is the last column number to be accumulated when
508C doing numerical differentiation.
509        IOPT(2)= 5
510
511C This part of the derivative matrix is a diagonal, hence
512C easy to compute.
513        P(1,1)=-CJ
514        P(2,2)=-CJ
515        P(3,3)=-mass*CJ
516        P(4,4)=P(3,3)
517        P(5,5)=-LSQ*CJ
518   10 CONTINUE
519C The value of f(y) is normally returned in WK(1:5).
520C Note that the partials with respect to y' have been
521C analytically pre-computed and the results are accumulated.
522        WK(1)=Y(3)
523        WK(2)=Y(4)
524        WK(3)=-Y(1)*Y(5)
525        WK(4)=-Y(2)*Y(5)
526        WK(5)=-THREE*mg*Y(4)
527C Compute f(y) and make a special copy step for the point in question.
528        IF(MODE .eq. 0) THEN
529          F(1)=WK(1)
530          F(2)=WK(2)
531          F(3)=WK(3)
532          F(4)=WK(4)
533          F(5)=WK(5)
534          FACJ(1)=ZERO
535          YSCALE(1)=ZERO
536        END IF
537C This is a Math a la Carte code for numerical derivatives.
538        CALL SJACG (MODE, 5, 5, Y,
539     .              F, P, LDP, YSCALE,
540     .              FACJ, IOPT, WK, 43, IWK, 21)
541        IF(MODE .gt. 0) GO TO 10
542        IF(MODE .lt. 0) THEN
543            PRINT
544     1     '('' SDASSFD: Initial error in argument number '',I2)', -MODE
545            STOP '1'
546          END IF
547C All system partials are computed.  Count an evaluation.
548        KF=KF+1
549      END IF
550
551C The constraining equations after the corrector has converged.
552C Both partials and residuals are required.
553      IF(IRES .EQ. 5) THEN
554        MODE=0
555        IOPT(1)=0
556        FACC(1)=ZERO
557        YSCALE(1)=ZERO
558   20 CONTINUE
559C The value of f(y) is normally returned in WK(1:3).
560        WK(1)=Y(1)**2+Y(2)**2-LSQ
561        WK(2)=Y(1)*Y(3)+Y(2)*Y(4)
562        WK(3)=MASS*(Y(3)**2+Y(4)**2)-MG*Y(2)-LSQ*Y(5)
563C This constraint is constant total energy.
564        WK(4)=HALF*MASS*(Y(3)**2+Y(4)**2)+MG*Y(2)
565
566C Compute f(y) and make a special copy step for the
567C evaluation point.
568        IF(MODE .eq. 0) THEN
569          DELTA(6)=WK(1)
570          DELTA(7)=WK(2)
571          DELTA(8)=WK(3)
572          DELTA(9)=WK(4)
573          F(1)=WK(1)
574          F(2)=WK(2)
575          F(3)=WK(3)
576          F(4)=WK(4)
577        END IF
578C This is a Math a la Carte code for numerical derivatives.
579        CALL SJACG (MODE, 4, 5, Y,
580     .              F, P(6,1), LDP, YSCALE,
581     .              FACC, IOPT, WK, 43, IWK, 21)
582        IF(MODE .gt. 0) GO TO 20
583        IF(MODE .lt. 0) THEN
584            PRINT
585     1      '('' SDASSFE: Initial error in argument number '',I2)',-MODE
586            STOP '2'
587          END IF
588C All constraint partials are computed.  Count an evaluation.
589         KC=KC+1
590      END IF
591      END
592