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