1 /* multiroots/dogleg.c
2 *
3 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Brian Gough
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 3 of the License, or (at
8 * your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful, but
11 * WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 * General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18 */
19
20 #include "gsl_multiroots__enorm.c"
21
22 static void compute_diag (const gsl_matrix * J, gsl_vector * diag);
23 static void update_diag (const gsl_matrix * J, gsl_vector * diag);
24 static double compute_delta (gsl_vector * diag, gsl_vector * x);
25 static void compute_df (const gsl_vector * f_trial, const gsl_vector * f, gsl_vector * df);
26 static void compute_wv (const gsl_vector * qtdf, const gsl_vector *rdx, const gsl_vector *dx, const gsl_vector *diag, double pnorm, gsl_vector * w, gsl_vector * v);
27
28 static double scaled_enorm (const gsl_vector * d, const gsl_vector * f);
29
scaled_enorm(const gsl_vector * d,const gsl_vector * f)30 static double scaled_enorm (const gsl_vector * d, const gsl_vector * f) {
31 double e2 = 0 ;
32 size_t i, n = f->size ;
33 for (i = 0; i < n ; i++) {
34 double fi= gsl_vector_get(f, i);
35 double di= gsl_vector_get(d, i);
36 double u = di * fi;
37 e2 += u * u ;
38 }
39 return sqrt(e2);
40 }
41
42 static double enorm_sum (const gsl_vector * a, const gsl_vector * b);
43
enorm_sum(const gsl_vector * a,const gsl_vector * b)44 static double enorm_sum (const gsl_vector * a, const gsl_vector * b) {
45 double e2 = 0 ;
46 size_t i, n = a->size ;
47 for (i = 0; i < n ; i++) {
48 double ai= gsl_vector_get(a, i);
49 double bi= gsl_vector_get(b, i);
50 double u = ai + bi;
51 e2 += u * u ;
52 }
53 return sqrt(e2);
54 }
55
56 static void
compute_wv(const gsl_vector * qtdf,const gsl_vector * rdx,const gsl_vector * dx,const gsl_vector * diag,double pnorm,gsl_vector * w,gsl_vector * v)57 compute_wv (const gsl_vector * qtdf, const gsl_vector *rdx, const gsl_vector *dx, const gsl_vector *diag, double pnorm, gsl_vector * w, gsl_vector * v)
58 {
59 size_t i, n = qtdf->size;
60
61 for (i = 0; i < n; i++)
62 {
63 double qtdfi = gsl_vector_get (qtdf, i);
64 double rdxi = gsl_vector_get (rdx, i);
65 double dxi = gsl_vector_get (dx, i);
66 double diagi = gsl_vector_get (diag, i);
67
68 gsl_vector_set (w, i, (qtdfi - rdxi) / pnorm);
69 gsl_vector_set (v, i, diagi * diagi * dxi / pnorm);
70 }
71 }
72
73
74 static void
compute_df(const gsl_vector * f_trial,const gsl_vector * f,gsl_vector * df)75 compute_df (const gsl_vector * f_trial, const gsl_vector * f, gsl_vector * df)
76 {
77 size_t i, n = f->size;
78
79 for (i = 0; i < n; i++)
80 {
81 double dfi = gsl_vector_get (f_trial, i) - gsl_vector_get (f, i);
82 gsl_vector_set (df, i, dfi);
83 }
84 }
85
86 static void
compute_diag(const gsl_matrix * J,gsl_vector * diag)87 compute_diag (const gsl_matrix * J, gsl_vector * diag)
88 {
89 size_t i, j, n = diag->size;
90
91 for (j = 0; j < n; j++)
92 {
93 double sum = 0;
94 for (i = 0; i < n; i++)
95 {
96 double Jij = gsl_matrix_get (J, i, j);
97 sum += Jij * Jij;
98 }
99 if (sum == 0)
100 sum = 1.0;
101
102 gsl_vector_set (diag, j, sqrt (sum));
103 }
104 }
105
106 static void
update_diag(const gsl_matrix * J,gsl_vector * diag)107 update_diag (const gsl_matrix * J, gsl_vector * diag)
108 {
109 size_t i, j, n = diag->size;
110
111 for (j = 0; j < n; j++)
112 {
113 double cnorm, diagj, sum = 0;
114 for (i = 0; i < n; i++)
115 {
116 double Jij = gsl_matrix_get (J, i, j);
117 sum += Jij * Jij;
118 }
119 if (sum == 0)
120 sum = 1.0;
121
122 cnorm = sqrt (sum);
123 diagj = gsl_vector_get (diag, j);
124
125 if (cnorm > diagj)
126 gsl_vector_set (diag, j, cnorm);
127 }
128 }
129
130 static double
compute_delta(gsl_vector * diag,gsl_vector * x)131 compute_delta (gsl_vector * diag, gsl_vector * x)
132 {
133 double Dx = scaled_enorm (diag, x);
134 double factor = 100;
135
136 return (Dx > 0) ? factor * Dx : factor;
137 }
138
139 static double
compute_actual_reduction(double fnorm,double fnorm1)140 compute_actual_reduction (double fnorm, double fnorm1)
141 {
142 double actred;
143
144 if (fnorm1 < fnorm)
145 {
146 double u = fnorm1 / fnorm;
147 actred = 1 - u * u;
148 }
149 else
150 {
151 actred = -1;
152 }
153
154 return actred;
155 }
156
157 static double
compute_predicted_reduction(double fnorm,double fnorm1)158 compute_predicted_reduction (double fnorm, double fnorm1)
159 {
160 double prered;
161
162 if (fnorm1 < fnorm)
163 {
164 double u = fnorm1 / fnorm;
165 prered = 1 - u * u;
166 }
167 else
168 {
169 prered = 0;
170 }
171
172 return prered;
173 }
174
175 static void
compute_qtf(const gsl_matrix * q,const gsl_vector * f,gsl_vector * qtf)176 compute_qtf (const gsl_matrix * q, const gsl_vector * f, gsl_vector * qtf)
177 {
178 size_t i, j, N = f->size ;
179
180 for (j = 0; j < N; j++)
181 {
182 double sum = 0;
183 for (i = 0; i < N; i++)
184 sum += gsl_matrix_get (q, i, j) * gsl_vector_get (f, i);
185
186 gsl_vector_set (qtf, j, sum);
187 }
188 }
189
190 static void
compute_rdx(const gsl_matrix * r,const gsl_vector * dx,gsl_vector * rdx)191 compute_rdx (const gsl_matrix * r, const gsl_vector * dx, gsl_vector * rdx)
192 {
193 size_t i, j, N = dx->size ;
194
195 for (i = 0; i < N; i++)
196 {
197 double sum = 0;
198
199 for (j = i; j < N; j++)
200 {
201 sum += gsl_matrix_get (r, i, j) * gsl_vector_get (dx, j);
202 }
203
204 gsl_vector_set (rdx, i, sum);
205 }
206 }
207
208
209 static void
compute_trial_step(gsl_vector * x,gsl_vector * dx,gsl_vector * x_trial)210 compute_trial_step (gsl_vector *x, gsl_vector * dx, gsl_vector * x_trial)
211 {
212 size_t i, N = x->size;
213
214 for (i = 0; i < N; i++)
215 {
216 double pi = gsl_vector_get (dx, i);
217 double xi = gsl_vector_get (x, i);
218 gsl_vector_set (x_trial, i, xi + pi);
219 }
220 }
221
222 static int
newton_direction(const gsl_matrix * r,const gsl_vector * qtf,gsl_vector * p)223 newton_direction (const gsl_matrix * r, const gsl_vector * qtf, gsl_vector * p)
224 {
225 const size_t N = r->size2;
226 size_t i;
227 int status;
228
229 status = gsl_linalg_R_solve (r, qtf, p);
230
231 #ifdef DEBUG
232 printf("rsolve status = %d\n", status);
233 #endif
234
235 for (i = 0; i < N; i++)
236 {
237 double pi = gsl_vector_get (p, i);
238 gsl_vector_set (p, i, -pi);
239 }
240
241 return status;
242 }
243
244 static void
gradient_direction(const gsl_matrix * r,const gsl_vector * qtf,const gsl_vector * diag,gsl_vector * g)245 gradient_direction (const gsl_matrix * r, const gsl_vector * qtf,
246 const gsl_vector * diag, gsl_vector * g)
247 {
248 const size_t M = r->size1;
249 const size_t N = r->size2;
250
251 size_t i, j;
252
253 for (j = 0; j < M; j++)
254 {
255 double sum = 0;
256 double dj;
257
258 for (i = 0; i < N; i++)
259 {
260 sum += gsl_matrix_get (r, i, j) * gsl_vector_get (qtf, i);
261 }
262
263 dj = gsl_vector_get (diag, j);
264 gsl_vector_set (g, j, -sum / dj);
265 }
266 }
267
268 static void
minimum_step(double gnorm,const gsl_vector * diag,gsl_vector * g)269 minimum_step (double gnorm, const gsl_vector * diag, gsl_vector * g)
270 {
271 const size_t N = g->size;
272 size_t i;
273
274 for (i = 0; i < N; i++)
275 {
276 double gi = gsl_vector_get (g, i);
277 double di = gsl_vector_get (diag, i);
278 gsl_vector_set (g, i, (gi / gnorm) / di);
279 }
280 }
281
282 static void
compute_Rg(const gsl_matrix * r,const gsl_vector * gradient,gsl_vector * Rg)283 compute_Rg (const gsl_matrix * r, const gsl_vector * gradient, gsl_vector * Rg)
284 {
285 const size_t N = r->size2;
286 size_t i, j;
287
288 for (i = 0; i < N; i++)
289 {
290 double sum = 0;
291
292 for (j = i; j < N; j++)
293 {
294 double gj = gsl_vector_get (gradient, j);
295 double rij = gsl_matrix_get (r, i, j);
296 sum += rij * gj;
297 }
298
299 gsl_vector_set (Rg, i, sum);
300 }
301 }
302
303 static void
scaled_addition(double alpha,gsl_vector * newton,double beta,gsl_vector * gradient,gsl_vector * p)304 scaled_addition (double alpha, gsl_vector * newton, double beta, gsl_vector * gradient, gsl_vector * p)
305 {
306 const size_t N = p->size;
307 size_t i;
308
309 for (i = 0; i < N; i++)
310 {
311 double ni = gsl_vector_get (newton, i);
312 double gi = gsl_vector_get (gradient, i);
313 gsl_vector_set (p, i, alpha * ni + beta * gi);
314 }
315 }
316
317 static int
dogleg(const gsl_matrix * r,const gsl_vector * qtf,const gsl_vector * diag,double delta,gsl_vector * newton,gsl_vector * gradient,gsl_vector * p)318 dogleg (const gsl_matrix * r, const gsl_vector * qtf,
319 const gsl_vector * diag, double delta,
320 gsl_vector * newton, gsl_vector * gradient, gsl_vector * p)
321 {
322 double qnorm, gnorm, sgnorm, bnorm, temp;
323
324 newton_direction (r, qtf, newton);
325
326 #ifdef DEBUG
327 printf("newton = "); gsl_vector_fprintf(stdout, newton, "%g"); printf("\n");
328 #endif
329
330 qnorm = scaled_enorm (diag, newton);
331
332 if (qnorm <= delta)
333 {
334 gsl_vector_memcpy (p, newton);
335 #ifdef DEBUG
336 printf("took newton (qnorm = %g <= delta = %g)\n", qnorm, delta);
337 #endif
338 return GSL_SUCCESS;
339 }
340
341 gradient_direction (r, qtf, diag, gradient);
342
343 #ifdef DEBUG
344 printf("grad = "); gsl_vector_fprintf(stdout, gradient, "%g"); printf("\n");
345 #endif
346
347 gnorm = enorm (gradient);
348
349 if (gnorm == 0)
350 {
351 double alpha = delta / qnorm;
352 double beta = 0;
353 scaled_addition (alpha, newton, beta, gradient, p);
354 #ifdef DEBUG
355 printf("took scaled newton because gnorm = 0\n");
356 #endif
357 return GSL_SUCCESS;
358 }
359
360 minimum_step (gnorm, diag, gradient);
361
362 compute_Rg (r, gradient, p); /* Use p as temporary space to compute Rg */
363
364 #ifdef DEBUG
365 printf("mingrad = "); gsl_vector_fprintf(stdout, gradient, "%g"); printf("\n");
366 printf("Rg = "); gsl_vector_fprintf(stdout, p, "%g"); printf("\n");
367 #endif
368
369 temp = enorm (p);
370 sgnorm = (gnorm / temp) / temp;
371
372 if (sgnorm > delta)
373 {
374 double alpha = 0;
375 double beta = delta;
376 scaled_addition (alpha, newton, beta, gradient, p);
377 #ifdef DEBUG
378 printf("took gradient\n");
379 #endif
380 return GSL_SUCCESS;
381 }
382
383 bnorm = enorm (qtf);
384
385 {
386 double bg = bnorm / gnorm;
387 double bq = bnorm / qnorm;
388 double dq = delta / qnorm;
389 double dq2 = dq * dq;
390 double sd = sgnorm / delta;
391 double sd2 = sd * sd;
392
393 double t1 = bg * bq * sd;
394 double u = t1 - dq;
395 double t2 = t1 - dq * sd2 + sqrt (u * u + (1-dq2) * (1 - sd2));
396
397 double alpha = dq * (1 - sd2) / t2;
398 double beta = (1 - alpha) * sgnorm;
399
400 #ifdef DEBUG
401 printf("bnorm = %g\n", bnorm);
402 printf("gnorm = %g\n", gnorm);
403 printf("qnorm = %g\n", qnorm);
404 printf("delta = %g\n", delta);
405 printf("alpha = %g beta = %g\n", alpha, beta);
406 printf("took scaled combination of newton and gradient\n");
407 #endif
408
409 scaled_addition (alpha, newton, beta, gradient, p);
410 }
411
412 return GSL_SUCCESS;
413 }
414