1 /*
2 This source file based on Minpack: initially converted from
3 fortran using f2c, then rendered into relatively idiomatic
4 C with zero-based indexing throughout and pass-by-value for
5 parameters that do not function as pointers. We also rely
6 on <float.h> for the machine precision rather than Minpack's
7 dpmpar().
8
9 See README in this directory for the Minpack Copyright.
10
11 Allin Cottrell, Wake Forest University, April 2012
12
13 Modified in March 2014 by Jack Lucchetti to add a "quality"
14 switch (see below).
15
16 */
17
18 #include "minpack.h"
19 #include <math.h>
20 #include <float.h>
21
22 /*
23 c fdjac2:
24 c
25 c This subroutine computes a forward-difference approximation
26 c to the m by n jacobian matrix associated with a specified
27 c problem of m functions in n variables.
28 c
29 c The subroutine statement is
30 c
31 c subroutine fdjac2(fcn,m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa)
32 c
33 c where
34 c
35 c fcn is the name of the user-supplied subroutine which
36 c calculates the functions. fcn must be declared
37 c in an external statement in the user calling
38 c program, and should be written as follows.
39 c
40 c subroutine fcn(m,n,x,fvec,iflag)
41 c integer m,n,iflag
42 c double precision x(n),fvec(m)
43 c ----------
44 c calculate the functions at x and
45 c return this vector in fvec.
46 c ----------
47 c return
48 c end
49 c
50 c the value of iflag should not be changed by fcn unless
51 c the user wants to terminate execution of fdjac2.
52 c in this case set iflag to a negative integer.
53 c
54 c m is a positive integer input variable set to the number
55 c of functions.
56 c
57 c n is a positive integer input variable set to the number
58 c of variables. n must not exceed m.
59 c
60 c quality is a [0-2] int: 0 gives old-style forward-difference,
61 c 1 bilateral difference and 2 gives 6th order Richardson
62 c extrapolation; higher quality means lower speed.
63 c
64 c x is an input array of length n.
65 c
66 c fvec is an input array of length m which must contain the
67 c functions evaluated at x.
68 c
69 c fjac is an output m by n array which contains the
70 c approximation to the jacobian matrix evaluated at x.
71 c
72 c ldfjac is a positive integer input variable not less than m
73 c which specifies the leading dimension of the array fjac.
74 c
75 c iflag is an integer variable which can be used to terminate
76 c the execution of fdjac2. see description of fcn.
77 c
78 c epsfcn is an input variable used in determining a suitable
79 c step length for the forward-difference approximation. this
80 c approximation assumes that the relative errors in the
81 c functions are of the order of epsfcn. if epsfcn is less
82 c than the machine precision, it is assumed that the relative
83 c errors in the functions are of the order of the machine
84 c precision.
85 c
86 c wa is a work array of length m.
87 c
88 c Subprograms called:
89 c
90 c user-supplied ...... fcn
91 c
92 c Argonne National Laboratory. MINPACK Project. March, 1980.
93 c Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More
94 */
95
96 #define BIGGER_H 1
97
98 #if BIGGER_H
99 # define XREF 128
100 # define DEN sqrt(6.0)
101 #endif
102
fdjac2_(S_fp fcn,int m,int n,int quality,double * x,double * fvec,double * fjac,int ldfjac,int * iflag,double epsfcn,double * wa,void * p)103 int fdjac2_(S_fp fcn, int m, int n, int quality,
104 double *x, double *fvec,
105 double *fjac, int ldfjac, int *iflag,
106 double epsfcn, double *wa, void *p)
107 {
108 double eps = sqrt(DBL_EPSILON);
109 double h, temp;
110 int i, j, user_eps = 0;
111
112 if (epsfcn >= DBL_EPSILON) {
113 user_eps = 1;
114 eps = sqrt(epsfcn);
115 }
116
117 if (quality == 0) {
118 /* plain old minpack fdjac2 */
119 for (j = 0; j < n; j++) {
120 temp = x[j];
121 h = eps * fabs(temp);
122 if (h == 0.0) {
123 h = eps;
124 }
125 x[j] = temp + h;
126 (*fcn)(m, n, x, wa, iflag, p);
127 if (*iflag < 0) {
128 return 0;
129 }
130 x[j] = temp;
131 for (i = 0; i < m; i++) {
132 fjac[i + j * ldfjac] = (wa[i] - fvec[i]) / h;
133 }
134 }
135 } else {
136 int k, dim, d;
137 int a[6], b[6];
138 double y[m];
139
140 if (quality == 1) {
141 if (!user_eps) {
142 eps *= 2;
143 }
144 dim = 2;
145 d = 2;
146 a[0] = b[0] = -1;
147 a[1] = b[1] = 1;
148 } else if (quality == 2) {
149 dim = 4;
150 d = 12;
151 a[0] = -2; a[1] = -1; a[2] = 1; a[3] = 2;
152 b[0] = 1; b[1] = -8; b[2] = 8; b[3] = -1;
153 } else if (quality == 3) {
154 dim = 6;
155 d = 60;
156 if (!user_eps) {
157 eps *= 10;
158 }
159 a[0] = -(a[5] = 3); a[1] = -(a[4] = 2); a[2] = -(a[3] = 1);
160 b[0] = -(b[5] = 1); b[1] = -(b[4] = -9); b[2] = -(b[3] = 45);
161 } else {
162 *iflag = -1;
163 return 0;
164 }
165
166 for (j = 0; j < n; j++) {
167 temp = x[j];
168 #if BIGGER_H
169 if (quality > 1) {
170 if (fabs(temp) > XREF) {
171 h = XREF * sqrt(fabs(temp/XREF)) * eps / DEN;
172 } else {
173 h = XREF * eps / DEN;
174 }
175 } else {
176 h = eps * fabs(temp);
177 }
178 #else
179 h = eps * fabs(temp);
180 #endif
181 if (h == 0.0) {
182 h = eps;
183 }
184
185 for (i = 0; i < m; i++) {
186 y[i] = 0.0;
187 }
188
189 for (k = 0; k < dim; k++) {
190 x[j] = temp + a[k]*h;
191 (*fcn)(m, n, x, wa, iflag, p);
192 if (*iflag < 0) {
193 return 0;
194 }
195 for (i = 0; i < m; i++) {
196 y[i] += b[k]*wa[i];
197 }
198 }
199
200 for (i = 0; i < m; i++) {
201 fjac[i + j * ldfjac] = y[i] / (d*h);
202 }
203
204 x[j] = temp;
205 }
206 }
207
208 return 0;
209 }
210
211