|
| 1 | +/***************************************************************************** |
| 2 | +
|
| 3 | + This file is part of QSS Solver. |
| 4 | +
|
| 5 | + QSS Solver 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 |
| 8 | + (at your option) any later version. |
| 9 | +
|
| 10 | + QSS Solver is distributed in the hope that it will be useful, |
| 11 | + but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 12 | + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 13 | + GNU General Public License for more details. |
| 14 | +
|
| 15 | + You should have received a copy of the GNU General Public License |
| 16 | + along with QSS Solver. If not, see <http://www.gnu.org/licenses/>. |
| 17 | +
|
| 18 | + ******************************************************************************/ |
| 19 | + |
| 20 | +#include <gsl/gsl_multiroots.h> |
| 21 | + |
| 22 | +#include "solve_loop.h" |
| 23 | + |
| 24 | +// Eval and argument function callbacks. |
| 25 | +SVL_eval _eval_callback; |
| 26 | +double *_x = NULL; |
| 27 | +double *_d = NULL; |
| 28 | +double *_a = NULL; |
| 29 | +double *_t = NULL; |
| 30 | +double *_dx = NULL; |
| 31 | +int _tearing_vars; |
| 32 | +double *_tearing_var_values = NULL; |
| 33 | + |
| 34 | +int SVL_SolveEval(const gsl_vector *__x, void *__p, gsl_vector *__f) |
| 35 | +{ |
| 36 | + _eval_callback(_x, _d, _a, _t, _dx, _tearing_var_values); |
| 37 | + for (int i = 0; i < _tearing_vars; i++) { |
| 38 | + gsl_vector_set(__f, i, _tearing_var_values[i]); |
| 39 | + } |
| 40 | + return GSL_SUCCESS; |
| 41 | +} |
| 42 | + |
| 43 | +void SVL_AssignCallback(double *x, double *d, double *a, double *t, double *dx, int tearing_vars, SVL_eval eval_tearing_vars) |
| 44 | +{ |
| 45 | + if (_tearing_var_values == NULL) { |
| 46 | + _eval_callback = eval_tearing_vars; |
| 47 | + _x = x; |
| 48 | + _d = d; |
| 49 | + _a = a; |
| 50 | + _t = t; |
| 51 | + _dx = dx; |
| 52 | + _tearing_vars = tearing_vars; |
| 53 | + _tearing_var_values = (double *)malloc(sizeof(double) * _tearing_vars); |
| 54 | + } |
| 55 | +} |
| 56 | + |
| 57 | +void SVL_SolveLoop(double *x, double *d, double *a, double *t, double *dx, int tearing_vars, SVL_iter assign_loop_vars, |
| 58 | + SVL_eval eval_tearing_vars) |
| 59 | +{ |
| 60 | + size_t __iter = 0; |
| 61 | + int __status; |
| 62 | + |
| 63 | + SVL_AssignCallback(x, d, a, t, dx, tearing_vars, eval_tearing_vars); |
| 64 | + |
| 65 | + const gsl_multiroot_fsolver_type *__T = gsl_multiroot_fsolver_hybrid; |
| 66 | + gsl_multiroot_fsolver *__s = gsl_multiroot_fsolver_alloc(__T, tearing_vars); |
| 67 | + gsl_multiroot_function __F; |
| 68 | + static gsl_vector *__x = NULL; |
| 69 | + |
| 70 | + if (__x == NULL) { |
| 71 | + __x = gsl_vector_alloc(tearing_vars); |
| 72 | + for (int i = 0; i < tearing_vars; i++) { |
| 73 | + gsl_vector_set(__x, i, 0); |
| 74 | + } |
| 75 | + } |
| 76 | + __F.n = tearing_vars; |
| 77 | + __F.f = SVL_SolveEval; |
| 78 | + gsl_vector *__f = gsl_vector_alloc(tearing_vars); |
| 79 | + // Try if we are already in the solution from the start (useful for discrete dependendt loops) |
| 80 | + SVL_SolveEval(__x, NULL, __f); |
| 81 | + if (gsl_multiroot_test_residual(__f, 1e-7) == GSL_SUCCESS) { |
| 82 | + gsl_vector_free(__f); |
| 83 | + gsl_multiroot_fsolver_free(__s); |
| 84 | + return; |
| 85 | + } |
| 86 | + gsl_vector_free(__f); |
| 87 | + gsl_multiroot_fsolver_set(__s, &__F, __x); |
| 88 | + |
| 89 | + do { |
| 90 | + __iter++; |
| 91 | + __status = gsl_multiroot_fsolver_iterate(__s); |
| 92 | + if (__status) { /* check if solver is stuck */ |
| 93 | + break; |
| 94 | + } |
| 95 | + __status = gsl_multiroot_test_residual(__s->f, 1e-7); |
| 96 | + assign_loop_vars(x, d, a, t, dx); |
| 97 | + } while (__status == GSL_CONTINUE && __iter < 100); |
| 98 | + |
| 99 | + if (__iter == 100) { |
| 100 | + printf("Warning: GSL could not solve an algebraic loop after %d iterations\n", (int)__iter); |
| 101 | + } |
| 102 | + |
| 103 | + for (int i = 0; i < tearing_vars; i++) { |
| 104 | + double ret = gsl_vector_get(__s->x, i); |
| 105 | + gsl_vector_set(__x, i, ret); |
| 106 | + } |
| 107 | + |
| 108 | + gsl_multiroot_fsolver_free(__s); |
| 109 | +} |
0 commit comments