Diff of /lmdif.c [000000] .. [2a97a8] Maximize Restore

  Switch to unified view

a b/lmdif.c
1
/************************lmdif*************************/
2
3
/*
4
 * Solves or minimizes the sum of squares of m nonlinear
5
 * functions of n variables.
6
 *
7
 * From public domain Fortran version
8
 * of Argonne National Laboratories MINPACK
9
 *
10
 * C translation by Steve Moshier
11
 */
12
#include "filter.h"
13
#include <float.h>
14
extern lmfunc fcn;
15
16
// These globals are needed by MINPACK
17
18
/* resolution of arithmetic */
19
double MACHEP = 1.2e-16;      
20
21
/* smallest nonzero number */ 
22
double DWARF = 1.0e-38; 
23
24
25
26
/*********************** lmdif.c ****************************/
27
#define BUG 0
28
29
30
extern double MACHEP;
31
32
lmdif(m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn,
33
 diag,mode,factor,nprint,info,nfev,fjac,
34
 ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4)
35
int m,n,maxfev,mode,nprint,ldfjac;
36
int *info, *nfev;
37
double ftol, xtol, gtol, epsfcn, factor;
38
double x[], fvec[], diag[], fjac[], qtf[];
39
double wa1[], wa2[], wa3[], wa4[];
40
int ipvt[];
41
{
42
/*
43
*     **********
44
*
45
*     subroutine lmdif
46
*
47
*     the purpose of lmdif is to minimize the sum of the squares of
48
*     m nonlinear functions in n variables by a modification of
49
*     the levenberg-marquardt algorithm. the user must provide a
50
*     subroutine which calculates the functions. the jacobian is
51
*     then calculated by a forward-difference approximation.
52
*
53
*     the subroutine statement is
54
*
55
* subroutine lmdif(fcn,m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn,
56
*          diag,mode,factor,nprint,info,nfev,fjac,
57
*          ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4)
58
*
59
*     where
60
*
61
* fcn is the name of the user-supplied subroutine which
62
*   calculates the functions. fcn must be declared
63
*   in an external statement in the user calling
64
*   program, and should be written as follows.
65
*
66
*   subroutine fcn(m,n,x,fvec,iflag)
67
*   integer m,n,iflag
68
*   double precision x(n),fvec(m)
69
*   ----------
70
*   calculate the functions at x and
71
*   return this vector in fvec.
72
*   ----------
73
*   return
74
*   end
75
*
76
*   the value of iflag should not be changed by fcn unless
77
*   the user wants to terminate execution of lmdif.
78
*   in this case set iflag to a negative integer.
79
*
80
* m is a positive integer input variable set to the number
81
*   of functions.
82
*
83
* n is a positive integer input variable set to the number
84
*   of variables. n must not exceed m.
85
*
86
* x is an array of length n. on input x must contain
87
*   an initial estimate of the solution vector. on output x
88
*   contains the final estimate of the solution vector.
89
*
90
* fvec is an output array of length m which contains
91
*   the functions evaluated at the output x.
92
*
93
* ftol is a nonnegative input variable. termination
94
*   occurs when both the actual and predicted relative
95
*   reductions in the sum of squares are at most ftol.
96
*   therefore, ftol measures the relative error desired
97
*   in the sum of squares.
98
*
99
* xtol is a nonnegative input variable. termination
100
*   occurs when the relative error between two consecutive
101
*   iterates is at most xtol. therefore, xtol measures the
102
*   relative error desired in the approximate solution.
103
*
104
* gtol is a nonnegative input variable. termination
105
*   occurs when the cosine of the angle between fvec and
106
*   any column of the jacobian is at most gtol in absolute
107
*   value. therefore, gtol measures the orthogonality
108
*   desired between the function vector and the columns
109
*   of the jacobian.
110
*
111
* maxfev is a positive integer input variable. termination
112
*   occurs when the number of calls to fcn is at least
113
*   maxfev by the end of an iteration.
114
*
115
* epsfcn is an input variable used in determining a suitable
116
*   step length for the forward-difference approximation. this
117
*   approximation assumes that the relative errors in the
118
*   functions are of the order of epsfcn. if epsfcn is less
119
*   than the machine precision, it is assumed that the relative
120
*   errors in the functions are of the order of the machine
121
*   precision.
122
*
123
* diag is an array of length n. if mode = 1 (see
124
*   below), diag is internally set. if mode = 2, diag
125
*   must contain positive entries that serve as
126
*   multiplicative scale factors for the variables.
127
*
128
* mode is an integer input variable. if mode = 1, the
129
*   variables will be scaled internally. if mode = 2,
130
*   the scaling is specified by the input diag. other
131
*   values of mode are equivalent to mode = 1.
132
*
133
* factor is a positive input variable used in determining the
134
*   initial step bound. this bound is set to the product of
135
*   factor and the euclidean norm of diag*x if nonzero, or else
136
*   to factor itself. in most cases factor should lie in the
137
*   interval (.1,100.). 100. is a generally recommended value.
138
*
139
* nprint is an integer input variable that enables controlled
140
*   printing of iterates if it is positive. in this case,
141
*   fcn is called with iflag = 0 at the beginning of the first
142
*   iteration and every nprint iterations thereafter and
143
*   immediately prior to return, with x and fvec available
144
*   for printing. if nprint is not positive, no special calls
145
*   of fcn with iflag = 0 are made.
146
*
147
* info is an integer output variable. if the user has
148
*   terminated execution, info is set to the (negative)
149
*   value of iflag. see description of fcn. otherwise,
150
*   info is set as follows.
151
*
152
*   info = 0  improper input parameters.
153
*
154
*   info = 1  both actual and predicted relative reductions
155
*         in the sum of squares are at most ftol.
156
*
157
*   info = 2  relative error between two consecutive iterates
158
*         is at most xtol.
159
*
160
*   info = 3  conditions for info = 1 and info = 2 both hold.
161
*
162
*   info = 4  the cosine of the angle between fvec and any
163
*         column of the jacobian is at most gtol in
164
*         absolute value.
165
*
166
*   info = 5  number of calls to fcn has reached or
167
*         exceeded maxfev.
168
*
169
*   info = 6  ftol is too small. no further reduction in
170
*         the sum of squares is possible.
171
*
172
*   info = 7  xtol is too small. no further improvement in
173
*         the approximate solution x is possible.
174
*
175
*   info = 8  gtol is too small. fvec is orthogonal to the
176
*         columns of the jacobian to machine precision.
177
*
178
* nfev is an integer output variable set to the number of
179
*   calls to fcn.
180
*
181
* fjac is an output m by n array. the upper n by n submatrix
182
*   of fjac contains an upper triangular matrix r with
183
*   diagonal elements of nonincreasing magnitude such that
184
*
185
*      t     t       t
186
*     p *(jac *jac)*p = r *r,
187
*
188
*   where p is a permutation matrix and jac is the final
189
*   calculated jacobian. column j of p is column ipvt(j)
190
*   (see below) of the identity matrix. the lower trapezoidal
191
*   part of fjac contains information generated during
192
*   the computation of r.
193
*
194
* ldfjac is a positive integer input variable not less than m
195
*   which specifies the leading dimension of the array fjac.
196
*
197
* ipvt is an integer output array of length n. ipvt
198
*   defines a permutation matrix p such that jac*p = q*r,
199
*   where jac is the final calculated jacobian, q is
200
*   orthogonal (not stored), and r is upper triangular
201
*   with diagonal elements of nonincreasing magnitude.
202
*   column j of p is column ipvt(j) of the identity matrix.
203
*
204
* qtf is an output array of length n which contains
205
*   the first n elements of the vector (q transpose)*fvec.
206
*
207
* wa1, wa2, and wa3 are work arrays of length n.
208
*
209
* wa4 is a work array of length m.
210
*
211
*     subprograms called
212
*
213
* user-supplied ...... fcn
214
*
215
* minpack-supplied ... dpmpar,enorm,fdjac2,lmpar,qrfac
216
*
217
* fortran-supplied ... dabs,dmax1,dmin1,dsqrt,mod
218
*
219
*     argonne national laboratory. minpack project. march 1980.
220
*     burton s. garbow, kenneth e. hillstrom, jorge j. more
221
*
222
*     **********
223
*/
224
int i,iflag,ij,jj,iter,j,l;
225
double actred,delta,dirder,fnorm,fnorm1,gnorm;
226
double par,pnorm,prered,ratio;
227
double sum,temp,temp1,temp2,temp3,xnorm;
228
double enorm(), fabs(), dmax1(), dmin1(), sqrt();
229
// int fcn(); /* user supplied function */
230
static double one = 1.0;
231
static double p1 = 0.1;
232
static double p5 = 0.5;
233
static double p25 = 0.25;
234
static double p75 = 0.75;
235
static double p0001 = 1.0e-4;
236
static double zero = 0.0;
237
static double p05 = 0.05;
238
239
240
MACHEP    = DBL_EPSILON;      // machine precision, was 1.2e-16;
241
/* smallest nonzero number */ 
242
DWARF     = DBL_MIN; // was 1.0e-38;
243
244
245
246
*info = 0;
247
iflag = 0;
248
*nfev = 0;
249
/*
250
*     check the input parameters for errors.
251
*/
252
if( (n <= 0) || (m < n) || (ldfjac < m) || (ftol < zero)
253
  || (xtol < zero) || (gtol < zero) || (maxfev <= 0)
254
  || (factor <= zero) )
255
  goto L300;
256
257
if( mode == 2 )
258
  { /* scaling by diag[] */
259
  for( j=0; j<n; j++ )
260
      {
261
      if( diag[j] <= 0.0 )
262
          goto L300;
263
      }   
264
  }
265
#if BUG
266
// printf( "lmdif\n" );
267
#endif
268
/*
269
*     evaluate the function at the starting point
270
*     and calculate its norm.
271
*/
272
iflag = 1;
273
fcn(m,n,x,fvec,&iflag);
274
*nfev = 1;
275
if(iflag < 0)
276
  goto L300;
277
fnorm = enorm(m,fvec);
278
/*
279
*     initialize levenberg-marquardt parameter and iteration counter.
280
*/
281
par = zero;
282
iter = 1;
283
/*
284
*     beginning of the outer loop.
285
*/
286
287
L30:
288
289
/*
290
*  calculate the jacobian matrix.
291
*/
292
iflag = 2;
293
fdjac2(m,n,x,fvec,fjac,ldfjac,&iflag,epsfcn,wa4);
294
*nfev += n;
295
if(iflag < 0)
296
  goto L300;
297
/*
298
*  if requested, call fcn to enable printing of iterates.
299
*/
300
if( nprint > 0 )
301
  {
302
  iflag = 0;
303
  if(mod(iter-1,nprint) == 0)
304
      {
305
      fcn(m,n,x,fvec,&iflag);
306
      if(iflag < 0)
307
          goto L300;
308
      // printf( "fnorm %.15e\n", enorm(m,fvec) );
309
      }
310
  }
311
/*
312
*  compute the qr factorization of the jacobian.
313
*/
314
qrfac(m,n,fjac,ldfjac,1,ipvt,n,wa1,wa2,wa3);
315
/*
316
*  on the first iteration and if mode is 1, scale according
317
*  to the norms of the columns of the initial jacobian.
318
*/
319
if(iter == 1)
320
  {
321
  if(mode != 2)
322
      {
323
      for( j=0; j<n; j++ )
324
          {
325
          diag[j] = wa2[j];
326
          if( wa2[j] == zero )
327
              diag[j] = one;
328
          }
329
      }
330
331
/*
332
*  on the first iteration, calculate the norm of the scaled x
333
*  and initialize the step bound delta.
334
*/
335
  for( j=0; j<n; j++ )
336
      wa3[j] = diag[j] * x[j];
337
338
  xnorm = enorm(n,wa3);
339
  delta = factor*xnorm;
340
  if(delta == zero)
341
      delta = factor;
342
  }
343
344
/*
345
*  form (q transpose)*fvec and store the first n components in
346
*  qtf.
347
*/
348
for( i=0; i<m; i++ )
349
  wa4[i] = fvec[i];
350
jj = 0;
351
for( j=0; j<n; j++ )
352
  {
353
  temp3 = fjac[jj];
354
  if(temp3 != zero)
355
      {
356
      sum = zero;
357
      ij = jj;
358
      for( i=j; i<m; i++ )
359
          {
360
          sum += fjac[ij] * wa4[i];
361
          ij += 1;    /* fjac[i+m*j] */
362
          }
363
      temp = -sum / temp3;
364
      ij = jj;
365
      for( i=j; i<m; i++ )
366
          {
367
          wa4[i] += fjac[ij] * temp;
368
          ij += 1;    /* fjac[i+m*j] */
369
          }
370
      }
371
  fjac[jj] = wa1[j];
372
  jj += m+1;  /* fjac[j+m*j] */
373
  qtf[j] = wa4[j];
374
  }
375
376
/*
377
*  compute the norm of the scaled gradient.
378
*/
379
 gnorm = zero;
380
 if(fnorm != zero)
381
  {
382
  jj = 0;
383
  for( j=0; j<n; j++ )
384
      {
385
      l = ipvt[j];
386
      if(wa2[l] != zero)
387
              {
388
          sum = zero;
389
          ij = jj;
390
          for( i=0; i<=j; i++ )
391
              {
392
              sum += fjac[ij]*(qtf[i]/fnorm);
393
              ij += 1; /* fjac[i+m*j] */
394
              }
395
          gnorm = dmax1(gnorm,fabs(sum/wa2[l]));
396
          }
397
      jj += m;
398
      }
399
  }
400
401
/*
402
*  test for convergence of the gradient norm.
403
*/
404
 if(gnorm <= gtol)
405
  *info = 4;
406
 if( *info != 0)
407
  goto L300;
408
/*
409
*  rescale if necessary.
410
*/
411
 if(mode != 2)
412
  {
413
  for( j=0; j<n; j++ )
414
      diag[j] = dmax1(diag[j],wa2[j]);
415
  }
416
417
/*
418
*  beginning of the inner loop.
419
*/
420
L200:
421
/*
422
*     determine the levenberg-marquardt parameter.
423
*/
424
lmpar(n,fjac,ldfjac,ipvt,diag,qtf,delta,&par,wa1,wa2,wa3,wa4);
425
/*
426
*     store the direction p and x + p. calculate the norm of p.
427
*/
428
for( j=0; j<n; j++ )
429
  {
430
       wa1[j] = -wa1[j];
431
       wa2[j] = x[j] + wa1[j];
432
       wa3[j] = diag[j]*wa1[j];
433
  }
434
pnorm = enorm(n,wa3);
435
/*
436
*     on the first iteration, adjust the initial step bound.
437
*/
438
if(iter == 1)
439
  delta = dmin1(delta,pnorm);
440
/*
441
*     evaluate the function at x + p and calculate its norm.
442
*/
443
iflag = 1;
444
fcn(m,n,wa2,wa4,&iflag);
445
*nfev += 1;
446
if(iflag < 0)
447
  goto L300;
448
fnorm1 = enorm(m,wa4);
449
#if BUG 
450
// printf( "pnorm %.10e  fnorm1 %.10e\n", pnorm, fnorm1 );
451
#endif
452
/*
453
*     compute the scaled actual reduction.
454
*/
455
actred = -one;
456
if( (p1*fnorm1) < fnorm)
457
  {
458
  temp = fnorm1/fnorm;
459
  actred = one - temp * temp;
460
  }
461
/*
462
*     compute the scaled predicted reduction and
463
*     the scaled directional derivative.
464
*/
465
jj = 0;
466
for( j=0; j<n; j++ )
467
  {
468
  wa3[j] = zero;
469
  l = ipvt[j];
470
  temp = wa1[l];
471
  ij = jj;
472
  for( i=0; i<=j; i++ )
473
      {
474
      wa3[i] += fjac[ij]*temp;
475
      ij += 1; /* fjac[i+m*j] */
476
      }
477
  jj += m;
478
  }
479
temp1 = enorm(n,wa3)/fnorm;
480
temp2 = (sqrt(par)*pnorm)/fnorm;
481
prered = temp1*temp1 + (temp2*temp2)/p5;
482
dirder = -(temp1*temp1 + temp2*temp2);
483
/*
484
*     compute the ratio of the actual to the predicted
485
*     reduction.
486
*/
487
ratio = zero;
488
if(prered != zero)
489
  ratio = actred/prered;
490
/*
491
*     update the step bound.
492
*/
493
if(ratio <= p25)
494
  {
495
  if(actred >= zero)
496
      temp = p5;
497
  else
498
      temp = p5*dirder/(dirder + p5*actred);
499
  if( ((p1*fnorm1) >= fnorm)
500
  || (temp < p1) )
501
      temp = p1;
502
       delta = temp*dmin1(delta,pnorm/p1);
503
       par = par/temp;
504
  }
505
else
506
  {
507
  if( (par == zero) || (ratio >= p75) )
508
      {
509
      delta = pnorm/p5;
510
      par = p5*par;
511
      }
512
  }
513
/*
514
*     test for successful iteration.
515
*/
516
if(ratio >= p0001)
517
  {
518
/*
519
*     successful iteration. update x, fvec, and their norms.
520
*/
521
  for( j=0; j<n; j++ )
522
      {
523
      x[j] = wa2[j];
524
      wa2[j] = diag[j]*x[j];
525
      }
526
  for( i=0; i<m; i++ )
527
      fvec[i] = wa4[i];
528
  xnorm = enorm(n,wa2);
529
  fnorm = fnorm1;
530
  iter += 1;
531
  }
532
/*
533
*     tests for convergence.
534
*/
535
if( (fabs(actred) <= ftol)
536
  && (prered <= ftol)
537
  && (p5*ratio <= one) )
538
  *info = 1;
539
if(delta <= xtol*xnorm)
540
  *info = 2;
541
if( (fabs(actred) <= ftol)
542
  && (prered <= ftol)
543
  && (p5*ratio <= one)
544
  && ( *info == 2) )
545
  *info = 3;
546
if( *info != 0)
547
  goto L300;
548
/*
549
*     tests for termination and stringent tolerances.
550
*/
551
if( *nfev >= maxfev)
552
  *info = 5;
553
if( (fabs(actred) <= MACHEP)
554
  && (prered <= MACHEP)
555
  && (p5*ratio <= one) )
556
  *info = 6;
557
if(delta <= MACHEP*xnorm)
558
  *info = 7;
559
if(gnorm <= MACHEP)
560
  *info = 8;
561
if( *info != 0)
562
  goto L300;
563
/*
564
*     end of the inner loop. repeat if iteration unsuccessful.
565
*/
566
if(ratio < p0001)
567
  goto L200;
568
/*
569
*  end of the outer loop.
570
*/
571
goto L30;
572
  
573
L300:
574
/*
575
*     termination, either normal or user imposed.
576
*/
577
if(iflag < 0)
578
  *info = iflag;
579
iflag = 0;
580
if(nprint > 0)
581
  fcn(m,n,x,fvec,&iflag);
582
/*
583
      last card of subroutine lmdif.
584
*/
585
return 0; 
586
}
587
/************************lmpar.c*************************/
588
589
#define BUG 0
590
591
lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag,wa1,wa2)
592
int n,ldr;
593
int ipvt[];
594
double delta;
595
double *par;
596
double r[],diag[],qtb[],x[],sdiag[],wa1[],wa2[];
597
{
598
/*     **********
599
*
600
*     subroutine lmpar
601
*
602
*     given an m by n matrix a, an n by n nonsingular diagonal
603
*     matrix d, an m-vector b, and a positive number delta,
604
*     the problem is to determine a value for the parameter
605
*     par such that if x solves the system
606
*
607
*     a*x = b ,     sqrt(par)*d*x = 0 ,
608
*
609
*     in the least squares sense, and dxnorm is the euclidean
610
*     norm of d*x, then either par is zero and
611
*
612
*     (dxnorm-delta) .le. 0.1*delta ,
613
*
614
*     or par is positive and
615
*
616
*     abs(dxnorm-delta) .le. 0.1*delta .
617
*
618
*     this subroutine completes the solution of the problem
619
*     if it is provided with the necessary information from the
620
*     qr factorization, with column pivoting, of a. that is, if
621
*     a*p = q*r, where p is a permutation matrix, q has orthogonal
622
*     columns, and r is an upper triangular matrix with diagonal
623
*     elements of nonincreasing magnitude, then lmpar expects
624
*     the full upper triangle of r, the permutation matrix p,
625
*     and the first n components of (q transpose)*b. on output
626
*     lmpar also provides an upper triangular matrix s such that
627
*
628
*      t   t           t
629
*     p *(a *a + par*d*d)*p = s *s .
630
*
631
*     s is employed within lmpar and may be of separate interest.
632
*
633
*     only a few iterations are generally needed for convergence
634
*     of the algorithm. if, however, the limit of 10 iterations
635
*     is reached, then the output par will contain the best
636
*     value obtained so far.
637
*
638
*     the subroutine statement is
639
*
640
* subroutine lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag,
641
*          wa1,wa2)
642
*
643
*     where
644
*
645
* n is a positive integer input variable set to the order of r.
646
*
647
* r is an n by n array. on input the full upper triangle
648
*   must contain the full upper triangle of the matrix r.
649
*   on output the full upper triangle is unaltered, and the
650
*   strict lower triangle contains the strict upper triangle
651
*   (transposed) of the upper triangular matrix s.
652
*
653
* ldr is a positive integer input variable not less than n
654
*   which specifies the leading dimension of the array r.
655
*
656
* ipvt is an integer input array of length n which defines the
657
*   permutation matrix p such that a*p = q*r. column j of p
658
*   is column ipvt(j) of the identity matrix.
659
*
660
* diag is an input array of length n which must contain the
661
*   diagonal elements of the matrix d.
662
*
663
* qtb is an input array of length n which must contain the first
664
*   n elements of the vector (q transpose)*b.
665
*
666
* delta is a positive input variable which specifies an upper
667
*   bound on the euclidean norm of d*x.
668
*
669
* par is a nonnegative variable. on input par contains an
670
*   initial estimate of the levenberg-marquardt parameter.
671
*   on output par contains the final estimate.
672
*
673
* x is an output array of length n which contains the least
674
*   squares solution of the system a*x = b, sqrt(par)*d*x = 0,
675
*   for the output par.
676
*
677
* sdiag is an output array of length n which contains the
678
*   diagonal elements of the upper triangular matrix s.
679
*
680
* wa1 and wa2 are work arrays of length n.
681
*
682
*     subprograms called
683
*
684
* minpack-supplied ... dpmpar,enorm,qrsolv
685
*
686
* fortran-supplied ... dabs,dmax1,dmin1,dsqrt
687
*
688
*     argonne national laboratory. minpack project. march 1980.
689
*     burton s. garbow, kenneth e. hillstrom, jorge j. more
690
*
691
*     **********
692
*/
693
int i,iter,ij,jj,j,jm1,jp1,k,l,nsing;
694
double dxnorm,fp,gnorm,parc,parl,paru;
695
double sum,temp;
696
double enorm(), fabs(), dmax1(), dmin1(), sqrt();
697
static double zero = 0.0;
698
static double one = 1.0;
699
static double p1 = 0.1;
700
static double p001 = 0.001;
701
extern double MACHEP;
702
extern double DWARF;
703
704
#if BUG
705
// printf( "lmpar\n" );
706
#endif
707
/*
708
*     compute and store in x the gauss-newton direction. if the
709
*     jacobian is rank-deficient, obtain a least squares solution.
710
*/
711
nsing = n;
712
jj = 0;
713
for( j=0; j<n; j++ )
714
  {
715
  wa1[j] = qtb[j];
716
  if( (r[jj] == zero) && (nsing == n) )
717
      nsing = j;
718
  if(nsing < n)
719
      wa1[j] = zero;
720
  jj += ldr+1; /* [j+ldr*j] */
721
  }
722
#if BUG
723
// printf( "nsing %d ", nsing );
724
#endif
725
if(nsing >= 1)
726
  {
727
  for( k=0; k<nsing; k++ )
728
      {
729
      j = nsing - k - 1;
730
      wa1[j] = wa1[j]/r[j+ldr*j];
731
      temp = wa1[j];
732
      jm1 = j - 1;
733
      if(jm1 >= 0)
734
          {
735
          ij = ldr * j;
736
          for( i=0; i<=jm1; i++ )
737
              {
738
              wa1[i] -= r[ij]*temp;
739
              ij += 1;
740
              }
741
          }
742
      }
743
  }
744
745
for( j=0; j<n; j++ )
746
  {
747
  l = ipvt[j];
748
  x[l] = wa1[j];
749
  }
750
/*
751
*     initialize the iteration counter.
752
*     evaluate the function at the origin, and test
753
*     for acceptance of the gauss-newton direction.
754
*/
755
iter = 0;
756
for( j=0; j<n; j++ )
757
  wa2[j] = diag[j]*x[j];
758
dxnorm = enorm(n,wa2);
759
fp = dxnorm - delta;
760
if(fp <= p1*delta)
761
  {
762
#if BUG
763
  // printf( "going to L220\n" );
764
#endif
765
  goto L220;
766
  }
767
/*
768
*     if the jacobian is not rank deficient, the newton
769
*     step provides a lower bound, parl, for the zero of
770
*     the function. otherwise set this bound to zero.
771
*/
772
parl = zero;
773
if(nsing >= n)
774
  {
775
  for( j=0; j<n; j++ )
776
      {
777
      l = ipvt[j];
778
      wa1[j] = diag[l]*(wa2[l]/dxnorm);
779
      }
780
  jj = 0;
781
  for( j=0; j<n; j++ )
782
      {
783
      sum = zero;
784
      jm1 = j - 1;
785
      if(jm1 >= 0)
786
          {
787
          ij = jj;
788
          for( i=0; i<=jm1; i++ )
789
              {
790
              sum += r[ij]*wa1[i];
791
              ij += 1;
792
              }
793
          }
794
      wa1[j] = (wa1[j] - sum)/r[j+ldr*j];
795
      jj += ldr; /* [i+ldr*j] */
796
      }
797
  temp = enorm(n,wa1);
798
  parl = ((fp/delta)/temp)/temp;
799
  }
800
/*
801
*     calculate an upper bound, paru, for the zero of the function.
802
*/
803
jj = 0;
804
for( j=0; j<n; j++ )
805
  {
806
  sum = zero;
807
  ij = jj;
808
  for( i=0; i<=j; i++ )
809
      {
810
      sum += r[ij]*qtb[i];
811
      ij += 1;
812
      }
813
  l = ipvt[j];
814
  wa1[j] = sum/diag[l];
815
  jj += ldr; /* [i+ldr*j] */
816
  }
817
gnorm = enorm(n,wa1);
818
paru = gnorm/delta;
819
if(paru == zero)
820
  paru = DWARF/dmin1(delta,p1);
821
/*
822
*     if the input par lies outside of the interval (parl,paru),
823
*     set par to the closer endpoint.
824
*/
825
*par = dmax1( *par,parl);
826
*par = dmin1( *par,paru);
827
if( *par == zero)
828
  *par = gnorm/dxnorm;
829
#if BUG
830
// printf( "parl %.4e  par %.4e  paru %.4e\n", parl, *par, paru );
831
#endif
832
/*
833
*     beginning of an iteration.
834
*/
835
L150:
836
iter += 1;
837
/*
838
*  evaluate the function at the current value of par.
839
*/
840
if( *par == zero)
841
  *par = dmax1(DWARF,p001*paru);
842
temp = sqrt( *par );
843
for( j=0; j<n; j++ )
844
  wa1[j] = temp*diag[j];
845
qrsolv(n,r,ldr,ipvt,wa1,qtb,x,sdiag,wa2);
846
for( j=0; j<n; j++ )
847
  wa2[j] = diag[j]*x[j];
848
dxnorm = enorm(n,wa2);
849
temp = fp;
850
fp = dxnorm - delta;
851
/*
852
*  if the function is small enough, accept the current value
853
*  of par. also test for the exceptional cases where parl
854
*  is zero or the number of iterations has reached 10.
855
*/
856
if( (fabs(fp) <= p1*delta)
857
 || ((parl == zero) && (fp <= temp) && (temp < zero))
858
 || (iter == 10) )
859
  goto L220;
860
/*
861
*  compute the newton correction.
862
*/
863
for( j=0; j<n; j++ )
864
  {
865
  l = ipvt[j];
866
  wa1[j] = diag[l]*(wa2[l]/dxnorm);
867
  }
868
jj = 0;
869
for( j=0; j<n; j++ )
870
  {
871
  wa1[j] = wa1[j]/sdiag[j];
872
  temp = wa1[j];
873
  jp1 = j + 1;
874
  if(jp1 < n)
875
      {
876
      ij = jp1 + jj;
877
      for( i=jp1; i<n; i++ )
878
          {
879
          wa1[i] -= r[ij]*temp;
880
          ij += 1; /* [i+ldr*j] */
881
          }
882
      }
883
  jj += ldr; /* ldr*j */
884
  }
885
temp = enorm(n,wa1);
886
parc = ((fp/delta)/temp)/temp;
887
/*
888
*  depending on the sign of the function, update parl or paru.
889
*/
890
if(fp > zero)
891
  parl = dmax1(parl, *par);
892
if(fp < zero)
893
  paru = dmin1(paru, *par);
894
/*
895
*  compute an improved estimate for par.
896
*/
897
*par = dmax1(parl, *par + parc);
898
/*
899
*  end of an iteration.
900
*/
901
goto L150;
902
903
L220:
904
/*
905
*     termination.
906
*/
907
if(iter == 0)
908
  *par = zero;
909
/*
910
*     last card of subroutine lmpar.
911
*/
912
return 0;
913
}
914
/************************qrfac.c*************************/
915
916
#define BUG 0
917
918
qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa)
919
int m,n,lda,lipvt;
920
int ipvt[];
921
int pivot;
922
double a[],rdiag[],acnorm[],wa[];
923
{
924
#pragma unused(lipvt)
925
#pragma unused(lda)
926
/*
927
*     **********
928
*
929
*     subroutine qrfac
930
*
931
*     this subroutine uses householder transformations with column
932
*     pivoting (optional) to compute a qr factorization of the
933
*     m by n matrix a. that is, qrfac determines an orthogonal
934
*     matrix q, a permutation matrix p, and an upper trapezoidal
935
*     matrix r with diagonal elements of nonincreasing magnitude,
936
*     such that a*p = q*r. the householder transformation for
937
*     column k, k = 1,2,...,min(m,n), is of the form
938
*
939
*             t
940
*     i - (1/u(k))*u*u
941
*
942
*     where u has zeros in the first k-1 positions. the form of
943
*     this transformation and the method of pivoting first
944
*     appeared in the corresponding linpack subroutine.
945
*
946
*     the subroutine statement is
947
*
948
* subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa)
949
*
950
*     where
951
*
952
* m is a positive integer input variable set to the number
953
*   of rows of a.
954
*
955
* n is a positive integer input variable set to the number
956
*   of columns of a.
957
*
958
* a is an m by n array. on input a contains the matrix for
959
*   which the qr factorization is to be computed. on output
960
*   the strict upper trapezoidal part of a contains the strict
961
*   upper trapezoidal part of r, and the lower trapezoidal
962
*   part of a contains a factored form of q (the non-trivial
963
*   elements of the u vectors described above).
964
*
965
* lda is a positive integer input variable not less than m
966
*   which specifies the leading dimension of the array a.
967
*
968
* pivot is a logical input variable. if pivot is set true,
969
*   then column pivoting is enforced. if pivot is set false,
970
*   then no column pivoting is done.
971
*
972
* ipvt is an integer output array of length lipvt. ipvt
973
*   defines the permutation matrix p such that a*p = q*r.
974
*   column j of p is column ipvt(j) of the identity matrix.
975
*   if pivot is false, ipvt is not referenced.
976
*
977
* lipvt is a positive integer input variable. if pivot is false,
978
*   then lipvt may be as small as 1. if pivot is true, then
979
*   lipvt must be at least n.
980
*
981
* rdiag is an output array of length n which contains the
982
*   diagonal elements of r.
983
*
984
* acnorm is an output array of length n which contains the
985
*   norms of the corresponding columns of the input matrix a.
986
*   if this information is not needed, then acnorm can coincide
987
*   with rdiag.
988
*
989
* wa is a work array of length n. if pivot is false, then wa
990
*   can coincide with rdiag.
991
*
992
*     subprograms called
993
*
994
* minpack-supplied ... dpmpar,enorm
995
*
996
* fortran-supplied ... dmax1,dsqrt,min0
997
*
998
*     argonne national laboratory. minpack project. march 1980.
999
*     burton s. garbow, kenneth e. hillstrom, jorge j. more
1000
*
1001
*     **********
1002
*/
1003
int i,ij,jj,j,jp1,k,kmax,minmn;
1004
double ajnorm,sum,temp;
1005
static double zero = 0.0;
1006
static double one = 1.0;
1007
static double p05 = 0.05;
1008
extern double MACHEP;
1009
double enorm(), dmax1(), sqrt();
1010
/*
1011
*     compute the initial column norms and initialize several arrays.
1012
*/
1013
ij = 0;
1014
for( j=0; j<n; j++ )
1015
  {
1016
  acnorm[j] = enorm(m,&a[ij]);
1017
  rdiag[j] = acnorm[j];
1018
  wa[j] = rdiag[j];
1019
  if(pivot != 0)
1020
      ipvt[j] = j;
1021
  ij += m; /* m*j */
1022
  }
1023
#if BUG
1024
// printf( "qrfac\n" );
1025
#endif
1026
/*
1027
*     reduce a to r with householder transformations.
1028
*/
1029
minmn = min0(m,n);
1030
for( j=0; j<minmn; j++ )
1031
{
1032
if(pivot == 0)
1033
  goto L40;
1034
/*
1035
*  bring the column of largest norm into the pivot position.
1036
*/
1037
kmax = j;
1038
for( k=j; k<n; k++ )
1039
  {
1040
  if(rdiag[k] > rdiag[kmax])
1041
      kmax = k;
1042
  }
1043
if(kmax == j)
1044
  goto L40;
1045
1046
ij = m * j;
1047
jj = m * kmax;
1048
for( i=0; i<m; i++ )
1049
  {
1050
  temp = a[ij]; /* [i+m*j] */
1051
  a[ij] = a[jj]; /* [i+m*kmax] */
1052
  a[jj] = temp;
1053
  ij += 1;
1054
  jj += 1;
1055
  }
1056
rdiag[kmax] = rdiag[j];
1057
wa[kmax] = wa[j];
1058
k = ipvt[j];
1059
ipvt[j] = ipvt[kmax];
1060
ipvt[kmax] = k;
1061
1062
L40:
1063
/*
1064
*  compute the householder transformation to reduce the
1065
*  j-th column of a to a multiple of the j-th unit vector.
1066
*/
1067
jj = j + m*j;
1068
ajnorm = enorm(m-j,&a[jj]);
1069
if(ajnorm == zero)
1070
  goto L100;
1071
if(a[jj] < zero)
1072
  ajnorm = -ajnorm;
1073
ij = jj;
1074
for( i=j; i<m; i++ )
1075
  {
1076
  a[ij] /= ajnorm;
1077
  ij += 1; /* [i+m*j] */
1078
  }
1079
a[jj] += one;
1080
/*
1081
*  apply the transformation to the remaining columns
1082
*  and update the norms.
1083
*/
1084
jp1 = j + 1;
1085
if(jp1 < n )
1086
{
1087
for( k=jp1; k<n; k++ )
1088
  {
1089
  sum = zero;
1090
  ij = j + m*k;
1091
  jj = j + m*j;
1092
  for( i=j; i<m; i++ )
1093
      {
1094
      sum += a[jj]*a[ij];
1095
      ij += 1; /* [i+m*k] */
1096
      jj += 1; /* [i+m*j] */
1097
      }
1098
  temp = sum/a[j+m*j];
1099
  ij = j + m*k;
1100
  jj = j + m*j;
1101
  for( i=j; i<m; i++ )
1102
      {
1103
      a[ij] -= temp*a[jj];
1104
      ij += 1; /* [i+m*k] */
1105
      jj += 1; /* [i+m*j] */
1106
      }
1107
  if( (pivot != 0) && (rdiag[k] != zero) )
1108
      {
1109
      temp = a[j+m*k]/rdiag[k];
1110
      temp = dmax1( zero, one-temp*temp );
1111
      rdiag[k] *= sqrt(temp);
1112
      temp = rdiag[k]/wa[k];
1113
      if( (p05*temp*temp) <= MACHEP)
1114
          {
1115
          rdiag[k] = enorm(m-j-1,&a[jp1+m*k]);
1116
          wa[k] = rdiag[k];
1117
          }
1118
      }
1119
  }
1120
}
1121
1122
L100:
1123
  rdiag[j] = -ajnorm;
1124
}
1125
/*
1126
*     last card of subroutine qrfac.
1127
*/
1128
return 0;
1129
}
1130
/************************qrsolv.c*************************/
1131
1132
#define BUG 0
1133
1134
qrsolv(n,r,ldr,ipvt,diag,qtb,x,sdiag,wa)
1135
int n,ldr;
1136
int ipvt[];
1137
double r[],diag[],qtb[],x[],sdiag[],wa[];
1138
{
1139
/*
1140
*     **********
1141
*
1142
*     subroutine qrsolv
1143
*
1144
*     given an m by n matrix a, an n by n diagonal matrix d,
1145
*     and an m-vector b, the problem is to determine an x which
1146
*     solves the system
1147
*
1148
*     a*x = b ,     d*x = 0 ,
1149
*
1150
*     in the least squares sense.
1151
*
1152
*     this subroutine completes the solution of the problem
1153
*     if it is provided with the necessary information from the
1154
*     qr factorization, with column pivoting, of a. that is, if
1155
*     a*p = q*r, where p is a permutation matrix, q has orthogonal
1156
*     columns, and r is an upper triangular matrix with diagonal
1157
*     elements of nonincreasing magnitude, then qrsolv expects
1158
*     the full upper triangle of r, the permutation matrix p,
1159
*     and the first n components of (q transpose)*b. the system
1160
*     a*x = b, d*x = 0, is then equivalent to
1161
*
1162
*        t       t
1163
*     r*z = q *b ,  p *d*p*z = 0 ,
1164
*
1165
*     where x = p*z. if this system does not have full rank,
1166
*     then a least squares solution is obtained. on output qrsolv
1167
*     also provides an upper triangular matrix s such that
1168
*
1169
*      t   t       t
1170
*     p *(a *a + d*d)*p = s *s .
1171
*
1172
*     s is computed within qrsolv and may be of separate interest.
1173
*
1174
*     the subroutine statement is
1175
*
1176
* subroutine qrsolv(n,r,ldr,ipvt,diag,qtb,x,sdiag,wa)
1177
*
1178
*     where
1179
*
1180
* n is a positive integer input variable set to the order of r.
1181
*
1182
* r is an n by n array. on input the full upper triangle
1183
*   must contain the full upper triangle of the matrix r.
1184
*   on output the full upper triangle is unaltered, and the
1185
*   strict lower triangle contains the strict upper triangle
1186
*   (transposed) of the upper triangular matrix s.
1187
*
1188
* ldr is a positive integer input variable not less than n
1189
*   which specifies the leading dimension of the array r.
1190
*
1191
* ipvt is an integer input array of length n which defines the
1192
*   permutation matrix p such that a*p = q*r. column j of p
1193
*   is column ipvt(j) of the identity matrix.
1194
*
1195
* diag is an input array of length n which must contain the
1196
*   diagonal elements of the matrix d.
1197
*
1198
* qtb is an input array of length n which must contain the first
1199
*   n elements of the vector (q transpose)*b.
1200
*
1201
* x is an output array of length n which contains the least
1202
*   squares solution of the system a*x = b, d*x = 0.
1203
*
1204
* sdiag is an output array of length n which contains the
1205
*   diagonal elements of the upper triangular matrix s.
1206
*
1207
* wa is a work array of length n.
1208
*
1209
*     subprograms called
1210
*
1211
* fortran-supplied ... dabs,dsqrt
1212
*
1213
*     argonne national laboratory. minpack project. march 1980.
1214
*     burton s. garbow, kenneth e. hillstrom, jorge j. more
1215
*
1216
*     **********
1217
*/
1218
int i,ij,ik,kk,j,jp1,k,kp1,l,nsing;
1219
double cos,cotan,qtbpj,sin,sum,tan,temp;
1220
static double zero = 0.0;
1221
static double p25 = 0.25;
1222
static double p5 = 0.5;
1223
double fabs(), sqrt();
1224
1225
/*
1226
*     copy r and (q transpose)*b to preserve input and initialize s.
1227
*     in particular, save the diagonal elements of r in x.
1228
*/
1229
kk = 0;
1230
for( j=0; j<n; j++ )
1231
  {
1232
  ij = kk;
1233
  ik = kk;
1234
  for( i=j; i<n; i++ )
1235
      {
1236
      r[ij] = r[ik];
1237
      ij += 1;   /* [i+ldr*j] */
1238
      ik += ldr; /* [j+ldr*i] */
1239
      }
1240
  x[j] = r[kk];
1241
  wa[j] = qtb[j];
1242
  kk += ldr+1; /* j+ldr*j */
1243
  }
1244
#if BUG
1245
// printf( "qrsolv\n" );
1246
#endif
1247
/*
1248
*     eliminate the diagonal matrix d using a givens rotation.
1249
*/
1250
for( j=0; j<n; j++ )
1251
{
1252
/*
1253
*  prepare the row of d to be eliminated, locating the
1254
*  diagonal element using p from the qr factorization.
1255
*/
1256
l = ipvt[j];
1257
if(diag[l] == zero)
1258
  goto L90;
1259
for( k=j; k<n; k++ )
1260
  sdiag[k] = zero;
1261
sdiag[j] = diag[l];
1262
/*
1263
*  the transformations to eliminate the row of d
1264
*  modify only a single element of (q transpose)*b
1265
*  beyond the first n, which is initially zero.
1266
*/
1267
qtbpj = zero;
1268
for( k=j; k<n; k++ )
1269
  {
1270
/*
1271
*     determine a givens rotation which eliminates the
1272
*     appropriate element in the current row of d.
1273
*/
1274
  if(sdiag[k] == zero)
1275
      continue;
1276
  kk = k + ldr * k;
1277
  if(fabs(r[kk]) < fabs(sdiag[k]))
1278
      {
1279
      cotan = r[kk]/sdiag[k];
1280
      sin = p5/sqrt(p25+p25*cotan*cotan);
1281
      cos = sin*cotan;
1282
      }
1283
  else
1284
      {
1285
      tan = sdiag[k]/r[kk];
1286
      cos = p5/sqrt(p25+p25*tan*tan);
1287
      sin = cos*tan;
1288
      }
1289
/*
1290
*     compute the modified diagonal element of r and
1291
*     the modified element of ((q transpose)*b,0).
1292
*/
1293
  r[kk] = cos*r[kk] + sin*sdiag[k];
1294
  temp = cos*wa[k] + sin*qtbpj;
1295
  qtbpj = -sin*wa[k] + cos*qtbpj;
1296
  wa[k] = temp;
1297
/*
1298
*     accumulate the tranformation in the row of s.
1299
*/
1300
  kp1 = k + 1;
1301
  if( n > kp1 )
1302
      {
1303
      ik = kk + 1;
1304
      for( i=kp1; i<n; i++ )
1305
          {
1306
          temp = cos*r[ik] + sin*sdiag[i];
1307
          sdiag[i] = -sin*r[ik] + cos*sdiag[i];
1308
          r[ik] = temp;
1309
          ik += 1; /* [i+ldr*k] */
1310
          }
1311
      }
1312
  }
1313
L90:
1314
/*
1315
*  store the diagonal element of s and restore
1316
*  the corresponding diagonal element of r.
1317
*/
1318
  kk = j + ldr*j;
1319
  sdiag[j] = r[kk];
1320
  r[kk] = x[j];
1321
}
1322
/*
1323
*     solve the triangular system for z. if the system is
1324
*     singular, then obtain a least squares solution.
1325
*/
1326
nsing = n;
1327
for( j=0; j<n; j++ )
1328
  {
1329
  if( (sdiag[j] == zero) && (nsing == n) )
1330
      nsing = j;
1331
  if(nsing < n)
1332
      wa[j] = zero;
1333
  }
1334
if(nsing < 1)
1335
  goto L150;
1336
1337
for( k=0; k<nsing; k++ )
1338
  {
1339
  j = nsing - k - 1;
1340
  sum = zero;
1341
  jp1 = j + 1;
1342
  if(nsing > jp1)
1343
      {
1344
      ij = jp1 + ldr * j;
1345
      for( i=jp1; i<nsing; i++ )
1346
          {
1347
          sum += r[ij]*wa[i];
1348
          ij += 1; /* [i+ldr*j] */
1349
          }
1350
      }
1351
  wa[j] = (wa[j] - sum)/sdiag[j];
1352
  }
1353
L150:
1354
/*
1355
*     permute the components of z back to components of x.
1356
*/
1357
for( j=0; j<n; j++ )
1358
  {
1359
  l = ipvt[j];
1360
  x[l] = wa[j];
1361
  }
1362
/*
1363
*     last card of subroutine qrsolv.
1364
*/
1365
return 0;
1366
}
1367
/************************enorm.c*************************/
1368
 
1369
double enorm(n,x)
1370
int n;
1371
double x[];
1372
{
1373
/*
1374
*     **********
1375
*
1376
*     function enorm
1377
*
1378
*     given an n-vector x, this function calculates the
1379
*     euclidean norm of x.
1380
*
1381
*     the euclidean norm is computed by accumulating the sum of
1382
*     squares in three different sums. the sums of squares for the
1383
*     small and large components are scaled so that no overflows
1384
*     occur. non-destructive underflows are permitted. underflows
1385
*     and overflows do not occur in the computation of the unscaled
1386
*     sum of squares for the intermediate components.
1387
*     the definitions of small, intermediate and large components
1388
*     depend on two constants, rdwarf and rgiant. the main
1389
*     restrictions on these constants are that rdwarf**2 not
1390
*     underflow and rgiant**2 not overflow. the constants
1391
*     given here are suitable for every known computer.
1392
*
1393
*     the function statement is
1394
*
1395
* double precision function enorm(n,x)
1396
*
1397
*     where
1398
*
1399
* n is a positive integer input variable.
1400
*
1401
* x is an input array of length n.
1402
*
1403
*     subprograms called
1404
*
1405
* fortran-supplied ... dabs,dsqrt
1406
*
1407
*     argonne national laboratory. minpack project. march 1980.
1408
*     burton s. garbow, kenneth e. hillstrom, jorge j. more
1409
*
1410
*     **********
1411
*/
1412
int i;
1413
double agiant,floatn,s1,s2,s3,xabs,x1max,x3max;
1414
double ans, temp;
1415
static double rdwarf = 3.834e-20;
1416
static double rgiant = 1.304e19;
1417
static double zero = 0.0;
1418
static double one = 1.0;
1419
double fabs(), sqrt();
1420
1421
s1 = zero;
1422
s2 = zero;
1423
s3 = zero;
1424
x1max = zero;
1425
x3max = zero;
1426
floatn = n;
1427
agiant = rgiant/floatn;
1428
1429
for( i=0; i<n; i++ )
1430
{
1431
xabs = fabs(x[i]);
1432
if( (xabs > rdwarf) && (xabs < agiant) )
1433
  {
1434
/*
1435
*     sum for intermediate components.
1436
*/
1437
  s2 += xabs*xabs;
1438
  continue;
1439
  }
1440
1441
if(xabs > rdwarf)
1442
  {
1443
/*
1444
*        sum for large components.
1445
*/
1446
  if(xabs > x1max)
1447
      {
1448
      temp = x1max/xabs;
1449
      s1 = one + s1*temp*temp;
1450
      x1max = xabs;
1451
      }
1452
  else
1453
      {
1454
      temp = xabs/x1max;
1455
      s1 += temp*temp;
1456
      }
1457
  continue;
1458
  }
1459
/*
1460
*        sum for small components.
1461
*/
1462
if(xabs > x3max)
1463
  {
1464
  temp = x3max/xabs;
1465
  s3 = one + s3*temp*temp;
1466
  x3max = xabs;
1467
  }
1468
else  
1469
  {
1470
  if(xabs != zero)
1471
      {
1472
      temp = xabs/x3max;
1473
      s3 += temp*temp;
1474
      }
1475
  }
1476
}
1477
/*
1478
*     calculation of norm.
1479
*/
1480
if(s1 != zero)
1481
  {
1482
  temp = s1 + (s2/x1max)/x1max;
1483
  ans = x1max*sqrt(temp);
1484
  return(ans);
1485
  }
1486
if(s2 != zero)
1487
  {
1488
  if(s2 >= x3max)
1489
      temp = s2*(one+(x3max/s2)*(x3max*s3));
1490
  else
1491
      temp = x3max*((s2/x3max)+(x3max*s3));
1492
  ans = sqrt(temp);
1493
  }
1494
else
1495
  {
1496
  ans = x3max*sqrt(s3);
1497
  }
1498
return(ans);
1499
/*
1500
*     last card of function enorm.
1501
*/
1502
}
1503
1504
/************************fdjac2.c*************************/
1505
1506
#define BUG 0
1507
1508
fdjac2(m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa)
1509
int m,n,ldfjac;
1510
int *iflag;
1511
double epsfcn;
1512
double x[],fvec[],fjac[],wa[];
1513
{
1514
#pragma unused(ldfjac)
1515
/*
1516
*     **********
1517
*
1518
*     subroutine fdjac2
1519
*
1520
*     this subroutine computes a forward-difference approximation
1521
*     to the m by n jacobian matrix associated with a specified
1522
*     problem of m functions in n variables.
1523
*
1524
*     the subroutine statement is
1525
*
1526
* subroutine fdjac2(fcn,m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa)
1527
*
1528
*     where
1529
*
1530
* fcn is the name of the user-supplied subroutine which
1531
*   calculates the functions. fcn must be declared
1532
*   in an external statement in the user calling
1533
*   program, and should be written as follows.
1534
*
1535
*   subroutine fcn(m,n,x,fvec,iflag)
1536
*   integer m,n,iflag
1537
*   double precision x(n),fvec(m)
1538
*   ----------
1539
*   calculate the functions at x and
1540
*   return this vector in fvec.
1541
*   ----------
1542
*   return
1543
*   end
1544
*
1545
*   the value of iflag should not be changed by fcn unless
1546
*   the user wants to terminate execution of fdjac2.
1547
*   in this case set iflag to a negative integer.
1548
*
1549
* m is a positive integer input variable set to the number
1550
*   of functions.
1551
*
1552
* n is a positive integer input variable set to the number
1553
*   of variables. n must not exceed m.
1554
*
1555
* x is an input array of length n.
1556
*
1557
* fvec is an input array of length m which must contain the
1558
*   functions evaluated at x.
1559
*
1560
* fjac is an output m by n array which contains the
1561
*   approximation to the jacobian matrix evaluated at x.
1562
*
1563
* ldfjac is a positive integer input variable not less than m
1564
*   which specifies the leading dimension of the array fjac.
1565
*
1566
* iflag is an integer variable which can be used to terminate
1567
*   the execution of fdjac2. see description of fcn.
1568
*
1569
* epsfcn is an input variable used in determining a suitable
1570
*   step length for the forward-difference approximation. this
1571
*   approximation assumes that the relative errors in the
1572
*   functions are of the order of epsfcn. if epsfcn is less
1573
*   than the machine precision, it is assumed that the relative
1574
*   errors in the functions are of the order of the machine
1575
*   precision.
1576
*
1577
* wa is a work array of length m.
1578
*
1579
*     subprograms called
1580
*
1581
* user-supplied ...... fcn
1582
*
1583
* minpack-supplied ... dpmpar
1584
*
1585
* fortran-supplied ... dabs,dmax1,dsqrt
1586
*
1587
*     argonne national laboratory. minpack project. march 1980.
1588
*     burton s. garbow, kenneth e. hillstrom, jorge j. more
1589
*
1590
      **********
1591
*/
1592
int i,j,ij;
1593
double eps,h,temp;
1594
double fabs(), dmax1(), sqrt();
1595
static double zero = 0.0;
1596
extern double MACHEP;
1597
1598
temp = dmax1(epsfcn,MACHEP);
1599
eps = sqrt(temp);
1600
#if BUG
1601
// printf( "fdjac2\n" );
1602
#endif
1603
ij = 0;
1604
for( j=0; j<n; j++ )
1605
  {
1606
  temp = x[j];
1607
  h = eps * fabs(temp);
1608
  if(h == zero)
1609
      h = eps;
1610
  x[j] = temp + h;
1611
  fcn(m,n,x,wa,iflag);
1612
  if( *iflag < 0)
1613
      return 0;
1614
  x[j] = temp;
1615
  for( i=0; i<m; i++ )
1616
      {
1617
      fjac[ij] = (wa[i] - fvec[i])/h;
1618
      ij += 1;    /* fjac[i+m*j] */
1619
      }
1620
  }
1621
#if BUG
1622
pmat( m, n, fjac );
1623
#endif
1624
/*
1625
*     last card of subroutine fdjac2.
1626
*/
1627
return 0;
1628
}
1629
/************************lmmisc.c*************************/
1630
1631
double dmax1(a,b)
1632
double a,b;
1633
{
1634
if( a >= b )
1635
  return(a);
1636
else
1637
  return(b);
1638
}
1639
1640
double dmin1(a,b)
1641
double a,b;
1642
{
1643
if( a <= b )
1644
  return(a);
1645
else
1646
  return(b);
1647
}
1648
1649
int min0(a,b)
1650
int a,b;
1651
{
1652
if( a <= b )
1653
  return(a);
1654
else
1655
  return(b);
1656
}
1657
1658
int mod( k, m )
1659
int k, m;
1660
{
1661
return( k % m );
1662
}
1663
1664
1665
pmat( m, n, y  )
1666
int m, n;
1667
double y[];
1668
{
1669
int i, j, k;
1670
1671
k = 0;
1672
for( i=0; i<m; i++ )
1673
  {
1674
  for( j=0; j<n; j++ )
1675
      {
1676
      // printf( "%.5e ", y[k] );
1677
      k += 1;
1678
      }
1679
  // printf( "\n" );
1680
  }
1681
return 0;
1682
} 
1683
1684