1 /* multiroots/hybrid.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 <config.h>
21
22 #include <stddef.h>
23 #include <stdlib.h>
24 #include <stdio.h>
25 #include <math.h>
26 #include <float.h>
27
28 #include <gsl/gsl_math.h>
29 #include <gsl/gsl_errno.h>
30 #include <gsl/gsl_multiroots.h>
31 #include <gsl/gsl_linalg.h>
32
33 #include "dogleg.c"
34
35 typedef struct
36 {
37 size_t iter;
38 size_t ncfail;
39 size_t ncsuc;
40 size_t nslow1;
41 size_t nslow2;
42 double fnorm;
43 double delta;
44 gsl_matrix *J;
45 gsl_matrix *q;
46 gsl_matrix *r;
47 gsl_vector *tau;
48 gsl_vector *diag;
49 gsl_vector *qtf;
50 gsl_vector *newton;
51 gsl_vector *gradient;
52 gsl_vector *x_trial;
53 gsl_vector *f_trial;
54 gsl_vector *df;
55 gsl_vector *qtdf;
56 gsl_vector *rdx;
57 gsl_vector *w;
58 gsl_vector *v;
59 }
60 hybrid_state_t;
61
62 static int hybrid_alloc (void *vstate, size_t n);
63 static int hybrid_set (void *vstate, gsl_multiroot_function * func,
64 gsl_vector * x, gsl_vector * f, gsl_vector * dx);
65 static int hybrids_set (void *vstate, gsl_multiroot_function * func,
66 gsl_vector * x, gsl_vector * f, gsl_vector * dx);
67 static int hybrid_set_impl (void *vstate, gsl_multiroot_function * func, gsl_vector * x,
68 gsl_vector * f, gsl_vector * dx, int scale);
69 static int hybrid_iterate (void *vstate, gsl_multiroot_function * func,
70 gsl_vector * x, gsl_vector * f, gsl_vector * dx);
71 static void hybrid_free (void *vstate);
72 static int hybrid_iterate_impl (void *vstate, gsl_multiroot_function * func,
73 gsl_vector * x, gsl_vector * f, gsl_vector * dx,
74 int scale);
75
76 static int
hybrid_alloc(void * vstate,size_t n)77 hybrid_alloc (void *vstate, size_t n)
78 {
79 hybrid_state_t *state = (hybrid_state_t *) vstate;
80 gsl_matrix *J, *q, *r;
81 gsl_vector *tau, *diag, *qtf, *newton, *gradient, *x_trial, *f_trial,
82 *df, *qtdf, *rdx, *w, *v;
83
84 J = gsl_matrix_calloc (n, n);
85
86 if (J == 0)
87 {
88 GSL_ERROR ("failed to allocate space for J", GSL_ENOMEM);
89 }
90
91 state->J = J;
92
93 q = gsl_matrix_calloc (n, n);
94
95 if (q == 0)
96 {
97 gsl_matrix_free (J);
98
99 GSL_ERROR ("failed to allocate space for q", GSL_ENOMEM);
100 }
101
102 state->q = q;
103
104 r = gsl_matrix_calloc (n, n);
105
106 if (r == 0)
107 {
108 gsl_matrix_free (J);
109 gsl_matrix_free (q);
110
111 GSL_ERROR ("failed to allocate space for r", GSL_ENOMEM);
112 }
113
114 state->r = r;
115
116 tau = gsl_vector_calloc (n);
117
118 if (tau == 0)
119 {
120 gsl_matrix_free (J);
121 gsl_matrix_free (q);
122 gsl_matrix_free (r);
123
124 GSL_ERROR ("failed to allocate space for tau", GSL_ENOMEM);
125 }
126
127 state->tau = tau;
128
129 diag = gsl_vector_calloc (n);
130
131 if (diag == 0)
132 {
133 gsl_matrix_free (J);
134 gsl_matrix_free (q);
135 gsl_matrix_free (r);
136 gsl_vector_free (tau);
137
138 GSL_ERROR ("failed to allocate space for diag", GSL_ENOMEM);
139 }
140
141 state->diag = diag;
142
143 qtf = gsl_vector_calloc (n);
144
145 if (qtf == 0)
146 {
147 gsl_matrix_free (J);
148 gsl_matrix_free (q);
149 gsl_matrix_free (r);
150 gsl_vector_free (tau);
151 gsl_vector_free (diag);
152
153 GSL_ERROR ("failed to allocate space for qtf", GSL_ENOMEM);
154 }
155
156 state->qtf = qtf;
157
158 newton = gsl_vector_calloc (n);
159
160 if (newton == 0)
161 {
162 gsl_matrix_free (J);
163 gsl_matrix_free (q);
164 gsl_matrix_free (r);
165 gsl_vector_free (tau);
166 gsl_vector_free (diag);
167 gsl_vector_free (qtf);
168
169 GSL_ERROR ("failed to allocate space for newton", GSL_ENOMEM);
170 }
171
172 state->newton = newton;
173
174 gradient = gsl_vector_calloc (n);
175
176 if (gradient == 0)
177 {
178 gsl_matrix_free (J);
179 gsl_matrix_free (q);
180 gsl_matrix_free (r);
181 gsl_vector_free (tau);
182 gsl_vector_free (diag);
183 gsl_vector_free (qtf);
184 gsl_vector_free (newton);
185
186 GSL_ERROR ("failed to allocate space for gradient", GSL_ENOMEM);
187 }
188
189 state->gradient = gradient;
190
191 x_trial = gsl_vector_calloc (n);
192
193 if (x_trial == 0)
194 {
195 gsl_matrix_free (J);
196 gsl_matrix_free (q);
197 gsl_matrix_free (r);
198 gsl_vector_free (tau);
199 gsl_vector_free (diag);
200 gsl_vector_free (qtf);
201 gsl_vector_free (newton);
202 gsl_vector_free (gradient);
203
204 GSL_ERROR ("failed to allocate space for x_trial", GSL_ENOMEM);
205 }
206
207 state->x_trial = x_trial;
208
209 f_trial = gsl_vector_calloc (n);
210
211 if (f_trial == 0)
212 {
213 gsl_matrix_free (J);
214 gsl_matrix_free (q);
215 gsl_matrix_free (r);
216 gsl_vector_free (tau);
217 gsl_vector_free (diag);
218 gsl_vector_free (qtf);
219 gsl_vector_free (newton);
220 gsl_vector_free (gradient);
221 gsl_vector_free (x_trial);
222
223 GSL_ERROR ("failed to allocate space for f_trial", GSL_ENOMEM);
224 }
225
226 state->f_trial = f_trial;
227
228 df = gsl_vector_calloc (n);
229
230 if (df == 0)
231 {
232 gsl_matrix_free (J);
233 gsl_matrix_free (q);
234 gsl_matrix_free (r);
235 gsl_vector_free (tau);
236 gsl_vector_free (diag);
237 gsl_vector_free (qtf);
238 gsl_vector_free (newton);
239 gsl_vector_free (gradient);
240 gsl_vector_free (x_trial);
241 gsl_vector_free (f_trial);
242
243 GSL_ERROR ("failed to allocate space for df", GSL_ENOMEM);
244 }
245
246 state->df = df;
247
248 qtdf = gsl_vector_calloc (n);
249
250 if (qtdf == 0)
251 {
252 gsl_matrix_free (J);
253 gsl_matrix_free (q);
254 gsl_matrix_free (r);
255 gsl_vector_free (tau);
256 gsl_vector_free (diag);
257 gsl_vector_free (qtf);
258 gsl_vector_free (newton);
259 gsl_vector_free (gradient);
260 gsl_vector_free (x_trial);
261 gsl_vector_free (f_trial);
262 gsl_vector_free (df);
263
264 GSL_ERROR ("failed to allocate space for qtdf", GSL_ENOMEM);
265 }
266
267 state->qtdf = qtdf;
268
269
270 rdx = gsl_vector_calloc (n);
271
272 if (rdx == 0)
273 {
274 gsl_matrix_free (J);
275 gsl_matrix_free (q);
276 gsl_matrix_free (r);
277 gsl_vector_free (tau);
278 gsl_vector_free (diag);
279 gsl_vector_free (qtf);
280 gsl_vector_free (newton);
281 gsl_vector_free (gradient);
282 gsl_vector_free (x_trial);
283 gsl_vector_free (f_trial);
284 gsl_vector_free (df);
285 gsl_vector_free (qtdf);
286
287 GSL_ERROR ("failed to allocate space for rdx", GSL_ENOMEM);
288 }
289
290 state->rdx = rdx;
291
292 w = gsl_vector_calloc (n);
293
294 if (w == 0)
295 {
296 gsl_matrix_free (J);
297 gsl_matrix_free (q);
298 gsl_matrix_free (r);
299 gsl_vector_free (tau);
300 gsl_vector_free (diag);
301 gsl_vector_free (qtf);
302 gsl_vector_free (newton);
303 gsl_vector_free (gradient);
304 gsl_vector_free (x_trial);
305 gsl_vector_free (f_trial);
306 gsl_vector_free (df);
307 gsl_vector_free (qtdf);
308 gsl_vector_free (rdx);
309
310 GSL_ERROR ("failed to allocate space for w", GSL_ENOMEM);
311 }
312
313 state->w = w;
314
315 v = gsl_vector_calloc (n);
316
317 if (v == 0)
318 {
319 gsl_matrix_free (J);
320 gsl_matrix_free (q);
321 gsl_matrix_free (r);
322 gsl_vector_free (tau);
323 gsl_vector_free (diag);
324 gsl_vector_free (qtf);
325 gsl_vector_free (newton);
326 gsl_vector_free (gradient);
327 gsl_vector_free (x_trial);
328 gsl_vector_free (f_trial);
329 gsl_vector_free (df);
330 gsl_vector_free (qtdf);
331 gsl_vector_free (rdx);
332 gsl_vector_free (w);
333
334 GSL_ERROR ("failed to allocate space for v", GSL_ENOMEM);
335 }
336
337 state->v = v;
338
339 return GSL_SUCCESS;
340 }
341
342 static int
hybrid_set(void * vstate,gsl_multiroot_function * func,gsl_vector * x,gsl_vector * f,gsl_vector * dx)343 hybrid_set (void *vstate, gsl_multiroot_function * func, gsl_vector * x,
344 gsl_vector * f, gsl_vector * dx)
345 {
346 int status = hybrid_set_impl (vstate, func, x, f, dx, 0);
347 return status;
348 }
349
350 static int
hybrids_set(void * vstate,gsl_multiroot_function * func,gsl_vector * x,gsl_vector * f,gsl_vector * dx)351 hybrids_set (void *vstate, gsl_multiroot_function * func, gsl_vector * x,
352 gsl_vector * f, gsl_vector * dx)
353 {
354 int status = hybrid_set_impl (vstate, func, x, f, dx, 1);
355 return status;
356 }
357
358 static int
hybrid_set_impl(void * vstate,gsl_multiroot_function * func,gsl_vector * x,gsl_vector * f,gsl_vector * dx,int scale)359 hybrid_set_impl (void *vstate, gsl_multiroot_function * func, gsl_vector * x,
360 gsl_vector * f, gsl_vector * dx, int scale)
361 {
362 hybrid_state_t *state = (hybrid_state_t *) vstate;
363
364 gsl_matrix *J = state->J;
365 gsl_matrix *q = state->q;
366 gsl_matrix *r = state->r;
367 gsl_vector *tau = state->tau;
368 gsl_vector *diag = state->diag;
369
370 int status;
371
372 status = GSL_MULTIROOT_FN_EVAL (func, x, f);
373
374 if (status)
375 {
376 return status;
377 }
378
379 status = gsl_multiroot_fdjacobian (func, x, f, GSL_SQRT_DBL_EPSILON, J);
380
381 if (status)
382 {
383 return status;
384 }
385
386 state->iter = 1;
387 state->fnorm = enorm (f);
388 state->ncfail = 0;
389 state->ncsuc = 0;
390 state->nslow1 = 0;
391 state->nslow2 = 0;
392
393 gsl_vector_set_all (dx, 0.0);
394
395 /* Store column norms in diag */
396
397 if (scale)
398 compute_diag (J, diag);
399 else
400 gsl_vector_set_all (diag, 1.0);
401
402 /* Set delta to factor |D x| or to factor if |D x| is zero */
403
404 state->delta = compute_delta (diag, x);
405
406 /* Factorize J into QR decomposition */
407
408 status = gsl_linalg_QR_decomp (J, tau);
409
410 if (status)
411 {
412 return status;
413 }
414
415 status = gsl_linalg_QR_unpack (J, tau, q, r);
416
417 return status;
418 }
419
420 static int
hybrid_iterate(void * vstate,gsl_multiroot_function * func,gsl_vector * x,gsl_vector * f,gsl_vector * dx)421 hybrid_iterate (void *vstate, gsl_multiroot_function * func, gsl_vector * x,
422 gsl_vector * f, gsl_vector * dx)
423 {
424 int status = hybrid_iterate_impl (vstate, func, x, f, dx, 0);
425 return status;
426 }
427
428 static int
hybrids_iterate(void * vstate,gsl_multiroot_function * func,gsl_vector * x,gsl_vector * f,gsl_vector * dx)429 hybrids_iterate (void *vstate, gsl_multiroot_function * func, gsl_vector * x,
430 gsl_vector * f, gsl_vector * dx)
431 {
432 int status = hybrid_iterate_impl (vstate, func, x, f, dx, 1);
433 return status;
434 }
435
436 static int
hybrid_iterate_impl(void * vstate,gsl_multiroot_function * func,gsl_vector * x,gsl_vector * f,gsl_vector * dx,int scale)437 hybrid_iterate_impl (void *vstate, gsl_multiroot_function * func,
438 gsl_vector * x,
439 gsl_vector * f, gsl_vector * dx, int scale)
440 {
441 hybrid_state_t *state = (hybrid_state_t *) vstate;
442
443 const double fnorm = state->fnorm;
444
445 gsl_matrix *J = state->J;
446 gsl_matrix *q = state->q;
447 gsl_matrix *r = state->r;
448 gsl_vector *tau = state->tau;
449 gsl_vector *diag = state->diag;
450 gsl_vector *qtf = state->qtf;
451 gsl_vector *x_trial = state->x_trial;
452 gsl_vector *f_trial = state->f_trial;
453 gsl_vector *df = state->df;
454 gsl_vector *qtdf = state->qtdf;
455 gsl_vector *rdx = state->rdx;
456 gsl_vector *w = state->w;
457 gsl_vector *v = state->v;
458
459 double prered, actred;
460 double pnorm, fnorm1, fnorm1p;
461 double ratio;
462 double p1 = 0.1, p5 = 0.5, p001 = 0.001, p0001 = 0.0001;
463
464 /* Compute qtf = Q^T f */
465
466 compute_qtf (q, f, qtf);
467
468 /* Compute dogleg step */
469
470 dogleg (r, qtf, diag, state->delta, state->newton, state->gradient, dx);
471
472 /* Take a trial step */
473
474 compute_trial_step (x, dx, state->x_trial);
475
476 pnorm = scaled_enorm (diag, dx);
477
478 if (state->iter == 1)
479 {
480 if (pnorm < state->delta)
481 {
482 state->delta = pnorm;
483 }
484 }
485
486 /* Evaluate function at x + p */
487
488 {
489 int status = GSL_MULTIROOT_FN_EVAL (func, x_trial, f_trial);
490
491 if (status != GSL_SUCCESS)
492 {
493 return GSL_EBADFUNC;
494 }
495 }
496
497 /* Set df = f_trial - f */
498
499 compute_df (f_trial, f, df);
500
501 /* Compute the scaled actual reduction */
502
503 fnorm1 = enorm (f_trial);
504
505 actred = compute_actual_reduction (fnorm, fnorm1);
506
507 /* Compute rdx = R dx */
508
509 compute_rdx (r, dx, rdx);
510
511 /* Compute the scaled predicted reduction phi1p = |Q^T f + R dx| */
512
513 fnorm1p = enorm_sum (qtf, rdx);
514
515 prered = compute_predicted_reduction (fnorm, fnorm1p);
516
517 /* Compute the ratio of the actual to predicted reduction */
518
519 if (prered > 0)
520 {
521 ratio = actred / prered;
522 }
523 else
524 {
525 ratio = 0;
526 }
527
528 /* Update the step bound */
529
530 if (ratio < p1)
531 {
532 state->ncsuc = 0;
533 state->ncfail++;
534 state->delta *= p5;
535 }
536 else
537 {
538 state->ncfail = 0;
539 state->ncsuc++;
540
541 if (ratio >= p5 || state->ncsuc > 1)
542 state->delta = GSL_MAX (state->delta, pnorm / p5);
543 if (fabs (ratio - 1) <= p1)
544 state->delta = pnorm / p5;
545 }
546
547 /* Test for successful iteration */
548
549 if (ratio >= p0001)
550 {
551 gsl_vector_memcpy (x, x_trial);
552 gsl_vector_memcpy (f, f_trial);
553 state->fnorm = fnorm1;
554 state->iter++;
555 }
556
557 /* Determine the progress of the iteration */
558
559 state->nslow1++;
560 if (actred >= p001)
561 state->nslow1 = 0;
562
563 if (actred >= p1)
564 state->nslow2 = 0;
565
566 if (state->ncfail == 2)
567 {
568 gsl_multiroot_fdjacobian (func, x, f, GSL_SQRT_DBL_EPSILON, J);
569
570 state->nslow2++;
571
572 if (state->iter == 1)
573 {
574 if (scale)
575 compute_diag (J, diag);
576 state->delta = compute_delta (diag, x);
577 }
578 else
579 {
580 if (scale)
581 update_diag (J, diag);
582 }
583
584 /* Factorize J into QR decomposition */
585
586 gsl_linalg_QR_decomp (J, tau);
587 gsl_linalg_QR_unpack (J, tau, q, r);
588
589 return GSL_SUCCESS;
590 }
591
592 /* Compute qtdf = Q^T df, w = (Q^T df - R dx)/|dx|, v = D^2 dx/|dx| */
593
594 compute_qtf (q, df, qtdf);
595
596 compute_wv (qtdf, rdx, dx, diag, pnorm, w, v);
597
598 /* Rank-1 update of the jacobian Q'R' = Q(R + w v^T) */
599
600 gsl_linalg_QR_update (q, r, w, v);
601
602 /* No progress as measured by jacobian evaluations */
603
604 if (state->nslow2 == 5)
605 {
606 return GSL_ENOPROGJ;
607 }
608
609 /* No progress as measured by function evaluations */
610
611 if (state->nslow1 == 10)
612 {
613 return GSL_ENOPROG;
614 }
615
616 return GSL_SUCCESS;
617 }
618
619
620 static void
hybrid_free(void * vstate)621 hybrid_free (void *vstate)
622 {
623 hybrid_state_t *state = (hybrid_state_t *) vstate;
624
625 gsl_vector_free (state->v);
626 gsl_vector_free (state->w);
627 gsl_vector_free (state->rdx);
628 gsl_vector_free (state->qtdf);
629 gsl_vector_free (state->df);
630 gsl_vector_free (state->f_trial);
631 gsl_vector_free (state->x_trial);
632 gsl_vector_free (state->gradient);
633 gsl_vector_free (state->newton);
634 gsl_vector_free (state->qtf);
635 gsl_vector_free (state->diag);
636 gsl_vector_free (state->tau);
637 gsl_matrix_free (state->r);
638 gsl_matrix_free (state->q);
639 gsl_matrix_free (state->J);
640 }
641
642 static const gsl_multiroot_fsolver_type hybrid_type = {
643 "hybrid", /* name */
644 sizeof (hybrid_state_t),
645 &hybrid_alloc,
646 &hybrid_set,
647 &hybrid_iterate,
648 &hybrid_free
649 };
650
651 static const gsl_multiroot_fsolver_type hybrids_type = {
652 "hybrids", /* name */
653 sizeof (hybrid_state_t),
654 &hybrid_alloc,
655 &hybrids_set,
656 &hybrids_iterate,
657 &hybrid_free
658 };
659
660 const gsl_multiroot_fsolver_type *gsl_multiroot_fsolver_hybrid = &hybrid_type;
661 const gsl_multiroot_fsolver_type *gsl_multiroot_fsolver_hybrids =
662 &hybrids_type;
663