Branch data Line data Source code
1 : : /*
2 : : * eqnsys.cpp - equation system solver class implementation
3 : : *
4 : : * Copyright (C) 2004, 2005, 2006, 2007, 2008 Stefan Jahn <stefan@lkcc.org>
5 : : *
6 : : * This is free software; you can redistribute it and/or modify
7 : : * it under the terms of the GNU General Public License as published by
8 : : * the Free Software Foundation; either version 2, or (at your option)
9 : : * any later version.
10 : : *
11 : : * This software is distributed in the hope that it will be useful,
12 : : * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 : : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 : : * GNU General Public License for more details.
15 : : *
16 : : * You should have received a copy of the GNU General Public License
17 : : * along with this package; see the file COPYING. If not, write to
18 : : * the Free Software Foundation, Inc., 51 Franklin Street - Fifth Floor,
19 : : * Boston, MA 02110-1301, USA.
20 : : *
21 : : * $Id$
22 : : *
23 : : */
24 : :
25 : : // the types required for qucs library files are defined
26 : : // in qucs_typedefs.h, created by configure from
27 : : // qucs_typedefs.h.in
28 : : #include "qucs_typedefs.h"
29 : :
30 : : #include <assert.h>
31 : : #include <time.h>
32 : : #include <cmath>
33 : : #include <float.h>
34 : :
35 : : #include <limits>
36 : :
37 : : #include "compat.h"
38 : : #include "logging.h"
39 : : #include "precision.h"
40 : : #include "complex.h"
41 : : #include "tmatrix.h"
42 : : #include "eqnsys.h"
43 : : #include "exception.h"
44 : : #include "exceptionstack.h"
45 : :
46 : : //! Little helper macro.
47 : : #define Swap(type,a,b) { type t; t = a; a = b; b = t; }
48 : :
49 : : namespace qucs {
50 : :
51 : : //! Constructor creates an unnamed instance of the eqnsys class.
52 : : template <class nr_type_t>
53 : 429216 : eqnsys<nr_type_t>::eqnsys () {
54 : 429216 : A = V = NULL;
55 : 429216 : B = X = NULL;
56 : 429216 : S = E = NULL;
57 : 429216 : T = R = NULL;
58 : 429216 : nPvt = NULL;
59 : 429216 : cMap = rMap = NULL;
60 : 429216 : update = 1;
61 : 429216 : pivoting = PIVOT_PARTIAL;
62 : 429216 : N = 0;
63 : 429216 : }
64 : :
65 : : //! Destructor deletes the eqnsys class object.
66 : : template <class nr_type_t>
67 : 429216 : eqnsys<nr_type_t>::~eqnsys () {
68 [ - + ][ # # ]: 429216 : if (R != NULL) delete R;
69 [ - + ][ # # ]: 429216 : if (T != NULL) delete T;
70 [ + + ][ + - ]: 429216 : if (B != NULL) delete B;
71 [ - + ][ # # ]: 429216 : if (S != NULL) delete S;
72 [ - + ][ # # ]: 429216 : if (E != NULL) delete E;
73 [ - + ][ # # ]: 429216 : if (V != NULL) delete V;
74 [ + + ][ + - ]: 429216 : if (rMap != NULL) delete[] rMap;
75 [ + + ][ + - ]: 429216 : if (cMap != NULL) delete[] cMap;
76 [ + + ][ + - ]: 429216 : if (nPvt != NULL) delete[] nPvt;
77 : 429216 : }
78 : :
79 : : /*! The copy constructor creates a new instance of the eqnsys class
80 : : based on the given eqnsys object. */
81 : : template <class nr_type_t>
82 : 0 : eqnsys<nr_type_t>::eqnsys (eqnsys & e) {
83 : 0 : A = e.A;
84 : 0 : V = NULL;
85 : 0 : S = E = NULL;
86 : 0 : T = R = NULL;
87 [ # # ][ # # ]: 0 : B = e.B ? new tvector<nr_type_t> (*(e.B)) : NULL;
88 : 0 : cMap = rMap = NULL;
89 : 0 : nPvt = NULL;
90 : 0 : update = 1;
91 : 0 : X = e.X;
92 : 0 : N = 0;
93 : 0 : }
94 : :
95 : : /*! With this function the describing matrices for the equation system
96 : : is passed to the equation system solver class. Matrix A is the
97 : : left hand side of the equation system and B the right hand side
98 : : vector. The reference pointer to the X vector is going to be the
99 : : solution vector. The B vector will be locally copied. */
100 : : template <class nr_type_t>
101 : 903249 : void eqnsys<nr_type_t>::passEquationSys (tmatrix<nr_type_t> * nA,
102 : : tvector<nr_type_t> * refX,
103 : : tvector<nr_type_t> * nB) {
104 [ + + ]: 903249 : if (nA != NULL) {
105 : 877587 : A = nA;
106 : 877587 : update = 1;
107 [ + + ]: 877587 : if (N != A->getCols ()) {
108 : 216152 : N = A->getCols ();
109 [ + + ][ + - ]: 216152 : if (cMap) delete[] cMap; cMap = new int[N];
110 [ + + ][ + - ]: 216152 : if (rMap) delete[] rMap; rMap = new int[N];
111 [ + + ][ + - ]: 216152 : if (nPvt) delete[] nPvt; nPvt = new nr_double_t[N];
112 : : }
113 : : }
114 : : else {
115 : 25662 : update = 0;
116 : : }
117 [ + + ][ + - ]: 903249 : if (B != NULL) delete B;
118 [ + - ]: 903249 : B = new tvector<nr_type_t> (*nB);
119 : 903249 : X = refX;
120 : 903249 : }
121 : :
122 : : /*! Depending on the algorithm applied to the equation system solver
123 : : the function stores the solution of the system into the matrix
124 : : pointed to by the X matrix reference. */
125 : : template <class nr_type_t>
126 : 903249 : void eqnsys<nr_type_t>::solve (void) {
127 : : #if DEBUG && 0
128 : : time_t t = time (NULL);
129 : : #endif
130 [ - - - + : 903249 : switch (algo) {
- + - + -
- - - - -
- - ]
131 : : case ALGO_INVERSE:
132 : 0 : solve_inverse ();
133 : 0 : break;
134 : : case ALGO_GAUSS:
135 : 0 : solve_gauss ();
136 : 0 : break;
137 : : case ALGO_GAUSS_JORDAN:
138 : 0 : solve_gauss_jordan ();
139 : 0 : break;
140 : : case ALGO_LU_DECOMPOSITION_CROUT:
141 : 874546 : solve_lu_crout ();
142 : 874546 : break;
143 : : case ALGO_LU_DECOMPOSITION_DOOLITTLE:
144 : 0 : solve_lu_doolittle ();
145 : 0 : break;
146 : : case ALGO_LU_FACTORIZATION_CROUT:
147 : 3005 : factorize_lu_crout ();
148 : 3005 : break;
149 : : case ALGO_LU_FACTORIZATION_DOOLITTLE:
150 : 0 : factorize_lu_doolittle ();
151 : 0 : break;
152 : : case ALGO_LU_SUBSTITUTION_CROUT:
153 : 25698 : substitute_lu_crout ();
154 : 25698 : break;
155 : : case ALGO_LU_SUBSTITUTION_DOOLITTLE:
156 : 0 : substitute_lu_doolittle ();
157 : 0 : break;
158 : : case ALGO_JACOBI: case ALGO_GAUSS_SEIDEL:
159 : 0 : solve_iterative ();
160 : 0 : break;
161 : : case ALGO_SOR:
162 : 0 : solve_sor ();
163 : 0 : break;
164 : : case ALGO_QR_DECOMPOSITION:
165 : 0 : solve_qr ();
166 : 0 : break;
167 : : case ALGO_QR_DECOMPOSITION_LS:
168 : 0 : solve_qr_ls ();
169 : 0 : break;
170 : : case ALGO_SV_DECOMPOSITION:
171 : 0 : solve_svd ();
172 : 0 : break;
173 : : case ALGO_QR_DECOMPOSITION_2:
174 : 0 : solve_qrh ();
175 : 0 : break;
176 : : }
177 : : #if DEBUG && 0
178 : : logprint (LOG_STATUS, "NOTIFY: %dx%d eqnsys solved in %ld seconds\n",
179 : : A->getRows (), A->getCols (), time (NULL) - t);
180 : : #endif
181 : 903249 : }
182 : :
183 : : /*! Simple matrix inversion is used to solve the equation system. */
184 : : template <class nr_type_t>
185 : 0 : void eqnsys<nr_type_t>::solve_inverse (void) {
186 [ # # ][ # # ]: 0 : *X = inverse (*A) * *B;
[ # # ][ # # ]
187 : 0 : }
188 : :
189 : : #define A_ (*A)
190 : : #define X_ (*X)
191 : : #define B_ (*B)
192 : :
193 : : /*! The function solves the equation system applying Gaussian
194 : : elimination with full column pivoting only (no row pivoting). */
195 : : template <class nr_type_t>
196 : 0 : void eqnsys<nr_type_t>::solve_gauss (void) {
197 : : nr_double_t MaxPivot;
198 : 0 : nr_type_t f;
199 : : int i, c, r, pivot;
200 : :
201 : : // triangulate the matrix
202 [ # # ]: 0 : for (i = 0; i < N; i++) {
203 : : // find maximum column value for pivoting
204 [ # # ]: 0 : for (MaxPivot = 0, pivot = r = i; r < N; r++) {
205 [ # # ][ # # ]: 0 : if (abs (A_(r, i)) > MaxPivot) {
[ # # ]
206 [ # # ]: 0 : MaxPivot = abs (A_(r, i));
207 : 0 : pivot = r;
208 : : }
209 : : }
210 : : // exchange rows if necessary
211 [ # # ]: 0 : assert (MaxPivot != 0);
212 [ # # ]: 0 : if (i != pivot) {
213 [ # # ]: 0 : A->exchangeRows (i, pivot);
214 [ # # ]: 0 : B->exchangeRows (i, pivot);
215 : : }
216 : : // compute new rows and columns
217 [ # # ]: 0 : for (r = i + 1; r < N; r++) {
218 [ # # ][ # # ]: 0 : f = A_(r, i) / A_(i, i);
[ # # ]
219 [ # # ][ # # ]: 0 : for (c = i + 1; c < N; c++) A_(r, c) -= f * A_(i, c);
[ # # ][ # # ]
[ # # ]
220 [ # # ][ # # ]: 0 : B_(r) -= f * B_(i);
[ # # ]
221 : : }
222 : : }
223 : :
224 : : // backward substitution
225 [ # # ][ # # ]: 0 : for (i = N - 1; i >= 0; i--) {
226 [ # # ]: 0 : f = B_(i);
227 [ # # ][ # # ]: 0 : for (c = i + 1; c < N; c++) f -= A_(i, c) * X_(c);
[ # # ][ # # ]
[ # # ]
228 [ # # ][ # # ]: 0 : X_(i) = f / A_(i, i);
[ # # ]
229 : : }
230 : 0 : }
231 : :
232 : : /*! The function solves the equation system applying a modified
233 : : Gaussian elimination with full column pivoting only (no row
234 : : pivoting). */
235 : : template <class nr_type_t>
236 : 0 : void eqnsys<nr_type_t>::solve_gauss_jordan (void) {
237 : : nr_double_t MaxPivot;
238 : 0 : nr_type_t f;
239 : : int i, c, r, pivot;
240 : :
241 : : // create the eye matrix
242 [ # # ]: 0 : for (i = 0; i < N; i++) {
243 : : // find maximum column value for pivoting
244 [ # # ]: 0 : for (MaxPivot = 0, pivot = r = i; r < N; r++) {
245 [ # # ][ # # ]: 0 : if (abs (A_(r, i)) > MaxPivot) {
[ # # ]
246 [ # # ]: 0 : MaxPivot = abs (A_(r, i));
247 : 0 : pivot = r;
248 : : }
249 : : }
250 : : // exchange rows if necessary
251 [ # # ]: 0 : assert (MaxPivot != 0);
252 [ # # ]: 0 : if (i != pivot) {
253 [ # # ]: 0 : A->exchangeRows (i, pivot);
254 [ # # ]: 0 : B->exchangeRows (i, pivot);
255 : : }
256 : :
257 : : // compute current row
258 [ # # ]: 0 : f = A_(i, i);
259 [ # # ][ # # ]: 0 : for (c = i + 1; c < N; c++) A_(i, c) /= f;
[ # ]
260 [ # # ]: 0 : B_(i) /= f;
261 : :
262 : : // compute new rows and columns
263 [ # # ]: 0 : for (r = 0; r < N; r++) {
264 [ # # ]: 0 : if (r != i) {
265 [ # # ]: 0 : f = A_(r, i);
266 [ # # ][ # # ]: 0 : for (c = i + 1; c < N; c++) A_(r, c) -= f * A_(i, c);
[ # # ][ # # ]
[ # # ]
267 [ # # ][ # # ]: 0 : B_(r) -= f * B_(i);
[ # # ]
268 : : }
269 : : }
270 : : }
271 : :
272 : : // right hand side is now the solution
273 [ # # ]: 0 : *X = *B;
274 : 0 : }
275 : :
276 : : #define LU_FAILURE 0
277 : : #define VIRTUAL_RES(txt,i) { \
278 : : qucs::exception * e = new qucs::exception (EXCEPTION_SINGULAR); \
279 : : e->setText (txt); \
280 : : e->setData (rMap[i]); \
281 : : A_(i, i) = NR_TINY; /* virtual resistance to ground */ \
282 : : throw_exception (e); }
283 : :
284 : : /*! The function uses LU decomposition and the appropriate forward and
285 : : backward substitutions in order to solve the linear equation
286 : : system. It is very useful when dealing with equation systems where
287 : : the left hand side (the A matrix) does not change but the right
288 : : hand side (the B vector) only. */
289 : : template <class nr_type_t>
290 : 874546 : void eqnsys<nr_type_t>::solve_lu_crout (void) {
291 : :
292 : : // skip decomposition if requested
293 [ + - ]: 874546 : if (update) {
294 : : // perform LU composition
295 : 874546 : factorize_lu_crout ();
296 : : }
297 : :
298 : : // finally solve the equation system
299 : 874546 : substitute_lu_crout ();
300 : 874546 : }
301 : :
302 : : /*! The other LU decomposition. */
303 : : template <class nr_type_t>
304 : 0 : void eqnsys<nr_type_t>::solve_lu_doolittle (void) {
305 : :
306 : : // skip decomposition if requested
307 [ # # ]: 0 : if (update) {
308 : : // perform LU composition
309 : 0 : factorize_lu_doolittle ();
310 : : }
311 : :
312 : : // finally solve the equation system
313 : 0 : substitute_lu_doolittle ();
314 : 0 : }
315 : :
316 : : /*! This function decomposes the left hand matrix into an upper U and
317 : : lower L matrix. The algorithm is called LU decomposition (Crout's
318 : : definition). The function performs the actual LU decomposition of
319 : : the matrix A using (implicit) partial row pivoting. */
320 : : template <class nr_type_t>
321 : 877551 : void eqnsys<nr_type_t>::factorize_lu_crout (void) {
322 : : nr_double_t d, MaxPivot;
323 : 9678 : nr_type_t f;
324 : : int k, c, r, pivot;
325 : :
326 : : // initialize pivot exchange table
327 [ + + ]: 6729518 : for (r = 0; r < N; r++) {
328 [ + + ]: 65252782 : for (MaxPivot = 0, c = 0; c < N; c++)
329 [ + - ][ + + ]: 59400815 : if ((d = abs (A_(r, c))) > MaxPivot)
[ + + ]
330 : 8969988 : MaxPivot = d;
331 [ - + ]: 5851967 : if (MaxPivot <= 0) MaxPivot = NR_TINY;
332 : 5851967 : nPvt[r] = 1 / MaxPivot;
333 : 5851967 : rMap[r] = r;
334 : : }
335 : :
336 : : // decompose the matrix into L (lower) and U (upper) matrix
337 [ + + ]: 6729518 : for (c = 0; c < N; c++) {
338 : : // upper matrix entries
339 [ + + ][ + + ]: 32626391 : for (r = 0; r < c; r++) {
340 [ + - ]: 26774424 : f = A_(r, c);
341 [ + - ][ + - ]: 155872495 : for (k = 0; k < r; k++) f -= A_(r, k) * A_(k, c);
[ + - ][ + + ]
[ + + ]
342 [ + - ][ + - ]: 26774424 : A_(r, c) = f / A_(r, r);
[ + - ]
343 : : }
344 : : // lower matrix entries
345 [ + + ]: 38478358 : for (MaxPivot = 0, pivot = r; r < N; r++) {
346 [ + - ]: 32626391 : f = A_(r, c);
347 [ + - ][ + - ]: 188498886 : for (k = 0; k < c; k++) f -= A_(r, k) * A_(k, c);
[ + - ][ + + ]
[ + + ]
348 [ + - ]: 32626391 : A_(r, c) = f;
349 : : // larger pivot ?
350 [ + + ][ + + ]: 32626391 : if ((d = nPvt[r] * abs (f)) > MaxPivot) {
351 : 8205638 : MaxPivot = d;
352 : 8205638 : pivot = r;
353 : : }
354 : : }
355 : :
356 : : // check pivot element and throw appropriate exception
357 [ + + ]: 5851967 : if (MaxPivot <= 0) {
358 : : #if LU_FAILURE
359 : : qucs::exception * e = new qucs::exception (EXCEPTION_PIVOT);
360 : : e->setText ("no pivot != 0 found during Crout LU decomposition");
361 : : e->setData (c);
362 : : throw_exception (e);
363 : : goto fail;
364 : : #else /* insert virtual resistance */
365 [ # + ][ # # ]: 17 : VIRTUAL_RES ("no pivot != 0 found during Crout LU decomposition", c);
[ # # ][ # # ]
[ # # ][ - ]
366 : : #endif
367 : : }
368 : :
369 : : // swap matrix rows if necessary and remember that step in the
370 : : // exchange table
371 [ + + ]: 5851967 : if (c != pivot) {
372 [ + - ]: 2224451 : A->exchangeRows (c, pivot);
373 : 2224451 : Swap (int, rMap[c], rMap[pivot]);
374 : 2224451 : Swap (nr_double_t, nPvt[c], nPvt[pivot]);
375 : : }
376 : : }
377 : : #if LU_FAILURE
378 : : fail:
379 : : #endif
380 : 877551 : }
381 : :
382 : : /*! This function decomposes the left hand matrix into an upper U and
383 : : lower L matrix. The algorithm is called LU decomposition
384 : : (Doolittle's definition). The function performs the actual LU
385 : : decomposition of the matrix A using (implicit) partial row pivoting. */
386 : : template <class nr_type_t>
387 : 0 : void eqnsys<nr_type_t>::factorize_lu_doolittle (void) {
388 : : nr_double_t d, MaxPivot;
389 : 0 : nr_type_t f;
390 : : int k, c, r, pivot;
391 : :
392 : : // initialize pivot exchange table
393 [ # # ]: 0 : for (r = 0; r < N; r++) {
394 [ # # ]: 0 : for (MaxPivot = 0, c = 0; c < N; c++)
395 [ # # ][ # # ]: 0 : if ((d = abs (A_(r, c))) > MaxPivot)
[ # # ]
396 : 0 : MaxPivot = d;
397 [ # # ]: 0 : if (MaxPivot <= 0) MaxPivot = NR_TINY;
398 : 0 : nPvt[r] = 1 / MaxPivot;
399 : 0 : rMap[r] = r;
400 : : }
401 : :
402 : : // decompose the matrix into L (lower) and U (upper) matrix
403 [ # # ]: 0 : for (c = 0; c < N; c++) {
404 : : // upper matrix entries
405 [ # # ][ # # ]: 0 : for (r = 0; r < c; r++) {
406 [ # # ]: 0 : f = A_(r, c);
407 [ # # ][ # # ]: 0 : for (k = 0; k < r; k++) f -= A_(r, k) * A_(k, c);
[ # # ][ # # ]
[ # # ]
408 [ # # ]: 0 : A_(r, c) = f;
409 : : }
410 : : // lower matrix entries
411 [ # # ]: 0 : for (MaxPivot = 0, pivot = r; r < N; r++) {
412 [ # # ]: 0 : f = A_(r, c);
413 [ # # ][ # # ]: 0 : for (k = 0; k < c; k++) f -= A_(r, k) * A_(k, c);
[ # # ][ # # ]
[ # # ]
414 [ # # ]: 0 : A_(r, c) = f;
415 : : // larger pivot ?
416 [ # # ][ # # ]: 0 : if ((d = nPvt[r] * abs (f)) > MaxPivot) {
417 : 0 : MaxPivot = d;
418 : 0 : pivot = r;
419 : : }
420 : : }
421 : :
422 : : // check pivot element and throw appropriate exception
423 [ # # ]: 0 : if (MaxPivot <= 0) {
424 : : #if LU_FAILURE
425 : : qucs::exception * e = new qucs::exception (EXCEPTION_PIVOT);
426 : : e->setText ("no pivot != 0 found during Doolittle LU decomposition");
427 : : e->setData (c);
428 : : throw_exception (e);
429 : : goto fail;
430 : : #else /* insert virtual resistance */
431 [ # # ][ # # ]: 0 : VIRTUAL_RES ("no pivot != 0 found during Doolittle LU decomposition", c);
[ # # ][ # # ]
[ # # ][ # ]
432 : : #endif
433 : : }
434 : :
435 : : // swap matrix rows if necessary and remember that step in the
436 : : // exchange table
437 [ # # ]: 0 : if (c != pivot) {
438 [ # # ]: 0 : A->exchangeRows (c, pivot);
439 : 0 : Swap (int, rMap[c], rMap[pivot]);
440 : 0 : Swap (nr_double_t, nPvt[c], nPvt[pivot]);
441 : : }
442 : :
443 : : // finally divide by the pivot element
444 [ # # ]: 0 : if (c < N - 1) {
445 [ # # ]: 0 : f = 1.0 / A_(c, c);
446 [ # # ][ # # ]: 0 : for (r = c + 1; r < N; r++) A_(r, c) *= f;
[ # # ]
447 : : }
448 : : }
449 : : #if LU_FAILURE
450 : : fail:
451 : : #endif
452 : 0 : }
453 : :
454 : : /*! The function is used in order to run the forward and backward
455 : : substitutions using the LU decomposed matrix (Crout's definition -
456 : : Uii are ones). */
457 : : template <class nr_type_t>
458 : 900244 : void eqnsys<nr_type_t>::substitute_lu_crout (void) {
459 : 32371 : nr_type_t f;
460 : : int i, c;
461 : :
462 : : // forward substitution in order to solve LY = B
463 [ + + ][ + + ]: 6953118 : for (i = 0; i < N; i++) {
464 [ + - ]: 6052874 : f = B_(rMap[i]);
465 [ + - ][ + - ]: 33637883 : for (c = 0; c < i; c++) f -= A_(i, c) * X_(c);
[ + - ][ + + ]
[ + + ]
466 [ + - ][ + - ]: 6052874 : X_(i) = f / A_(i, i);
[ + - ]
467 : : }
468 : :
469 : : // backward substitution in order to solve UX = Y
470 [ + + ][ + + ]: 6953118 : for (i = N - 1; i >= 0; i--) {
471 [ + - ]: 6052874 : f = X_(i);
472 [ + - ][ + - ]: 33637883 : for (c = i + 1; c < N; c++) f -= A_(i, c) * X_(c);
[ + - ][ + + ]
[ + + ]
473 : : // remember that the Uii diagonal are ones only in Crout's definition
474 [ + - ]: 6052874 : X_(i) = f;
475 : : }
476 : 900244 : }
477 : :
478 : : /*! The function is used in order to run the forward and backward
479 : : substitutions using the LU decomposed matrix (Doolittle's
480 : : definition - Lii are ones). This function is here because of
481 : : transposed LU matrices as used in the AC noise analysis. */
482 : : template <class nr_type_t>
483 : 0 : void eqnsys<nr_type_t>::substitute_lu_doolittle (void) {
484 : 0 : nr_type_t f;
485 : : int i, c;
486 : :
487 : : // forward substitution in order to solve LY = B
488 [ # # ][ # # ]: 0 : for (i = 0; i < N; i++) {
489 [ # # ]: 0 : f = B_(rMap[i]);
490 [ # # ][ # # ]: 0 : for (c = 0; c < i; c++) f -= A_(i, c) * X_(c);
[ # # ][ # # ]
[ # # ]
491 : : // remember that the Lii diagonal are ones only in Doolittle's definition
492 [ # # ]: 0 : X_(i) = f;
493 : : }
494 : :
495 : : // backward substitution in order to solve UX = Y
496 [ # # ][ # # ]: 0 : for (i = N - 1; i >= 0; i--) {
497 [ # # ]: 0 : f = X_(i);
498 [ # # ][ # # ]: 0 : for (c = i + 1; c < N; c++) f -= A_(i, c) * X_(c);
[ # # ][ # # ]
[ # # ]
499 [ # # ][ # # ]: 0 : X_(i) = f / A_(i, i);
[ # # ]
500 : : }
501 : 0 : }
502 : :
503 : : /*! The function solves the equation system using a full-step iterative
504 : : method (called Jacobi's method) or a single-step method (called
505 : : Gauss-Seidel) depending on the given algorithm. If the current X
506 : : vector (the solution vector) is the solution within a
507 : : Newton-Raphson iteration it is a good initial guess and the method
508 : : is very likely to converge. On divergence the method falls back to
509 : : LU decomposition. */
510 : : template <class nr_type_t>
511 : 0 : void eqnsys<nr_type_t>::solve_iterative (void) {
512 : 0 : nr_type_t f;
513 : : int error, conv, i, c, r;
514 : 0 : int MaxIter = N; // -> less than N^3 operations
515 : 0 : nr_double_t reltol = 1e-4;
516 : 0 : nr_double_t abstol = NR_TINY;
517 : : nr_double_t diff, crit;
518 : :
519 : : // ensure that all diagonal values are non-zero
520 [ # # ]: 0 : ensure_diagonal ();
521 : :
522 : : // try to raise diagonal dominance
523 [ # # ]: 0 : preconditioner ();
524 : :
525 : : // decide here about possible convergence
526 [ # # ]: 0 : if ((crit = convergence_criteria ()) >= 1) {
527 : : #if DEBUG && 0
528 : : logprint (LOG_STATUS, "NOTIFY: convergence criteria: %g >= 1 (%dx%d)\n",
529 : : crit, N, N);
530 : : #endif
531 : : //solve_lu ();
532 : : //return;
533 : : }
534 : :
535 : : // normalize the equation system to have ones on its diagonal
536 [ # # ][ # # ]: 0 : for (r = 0; r < N; r++) {
537 [ # # ]: 0 : f = A_(r, r);
538 [ # # ][ # # ]: 0 : assert (f != 0); // singular matrix
[ # # ]
539 [ # # ][ # # ]: 0 : for (c = 0; c < N; c++) A_(r, c) /= f;
[ # # ]
540 [ # # ]: 0 : B_(r) /= f;
541 : : }
542 : :
543 : : // the current X vector is a good initial guess for the iteration
544 [ # # ][ # # ]: 0 : tvector<nr_type_t> * Xprev = new tvector<nr_type_t> (*X);
[ # ]
545 : :
546 : : // start iterating here
547 : 0 : i = 0; error = 0;
548 [ # # ][ # # ]: 0 : do {
[ # # ][ # # ]
[ # # ][ # # ]
549 : : // compute new solution vector
550 [ # # ]: 0 : for (r = 0; r < N; r++) {
551 [ # # # ]: 0 : for (f = 0, c = 0; c < N; c++) {
552 [ # # ]: 0 : if (algo == ALGO_GAUSS_SEIDEL) {
553 : : // Gauss-Seidel
554 [ # # ][ # # ]: 0 : if (c < r) f += A_(r, c) * X_(c);
[ # # ][ # # ]
555 [ # # ][ # # ]: 0 : else if (c > r) f += A_(r, c) * Xprev->get (c);
[ # # ][ # # ]
556 : : }
557 : : else {
558 : : // Jacobi
559 [ # # ][ # # ]: 0 : if (c != r) f += A_(r, c) * Xprev->get (c);
[ # # ][ # # ]
560 : : }
561 : : }
562 [ # # ][ # # ]: 0 : X_(r) = B_(r) - f;
563 : : }
564 : : // check for convergence
565 [ # # ]: 0 : for (conv = 1, r = 0; r < N; r++) {
566 [ # # ][ # # ]: 0 : diff = abs (X_(r) - Xprev->get (r));
567 [ # # ][ # # ]: 0 : if (diff >= abstol + reltol * abs (X_(r))) {
[ # # ]
568 : 0 : conv = 0;
569 : 0 : break;
570 : : }
571 [ # # ]: 0 : if (!std::isfinite (diff)) { error++; break; }
572 : : }
573 : : // save last values
574 [ # # ]: 0 : *Xprev = *X;
575 : : }
576 : : while (++i < MaxIter && !conv);
577 : :
578 [ # # ]: 0 : delete Xprev;
579 : :
580 [ # # ][ # # ]: 0 : if (!conv || error) {
581 : : logprint (LOG_ERROR,
582 : : "WARNING: no convergence after %d %s iterations\n",
583 [ # # ][ # # ]: 0 : i, algo == ALGO_JACOBI ? "jacobi" : "gauss-seidel");
584 [ # # ]: 0 : solve_lu_crout ();
585 : : }
586 : : #if DEBUG && 0
587 : : else {
588 : : logprint (LOG_STATUS,
589 : : "NOTIFY: %s convergence after %d iterations\n",
590 : : algo == ALGO_JACOBI ? "jacobi" : "gauss-seidel", i);
591 : : }
592 : : #endif
593 : 0 : }
594 : :
595 : : /*! The function solves the linear equation system using a single-step
596 : : iterative algorithm. It is a modification of the Gauss-Seidel
597 : : method and is called successive over relaxation. The function uses
598 : : an adaptive scheme to adjust the relaxation parameter. */
599 : : template <class nr_type_t>
600 : 0 : void eqnsys<nr_type_t>::solve_sor (void) {
601 : 0 : nr_type_t f;
602 : : int error, conv, i, c, r;
603 : 0 : int MaxIter = N; // -> less than N^3 operations
604 : 0 : nr_double_t reltol = 1e-4;
605 : 0 : nr_double_t abstol = NR_TINY;
606 : 0 : nr_double_t diff, crit, l = 1, d, s;
607 : :
608 : : // ensure that all diagonal values are non-zero
609 [ # # ]: 0 : ensure_diagonal ();
610 : :
611 : : // try to raise diagonal dominance
612 [ # # ]: 0 : preconditioner ();
613 : :
614 : : // decide here about possible convergence
615 [ # # ]: 0 : if ((crit = convergence_criteria ()) >= 1) {
616 : : #if DEBUG && 0
617 : : logprint (LOG_STATUS, "NOTIFY: convergence criteria: %g >= 1 (%dx%d)\n",
618 : : crit, N, N);
619 : : #endif
620 : : //solve_lu ();
621 : : //return;
622 : : }
623 : :
624 : : // normalize the equation system to have ones on its diagonal
625 [ # # ][ # # ]: 0 : for (r = 0; r < N; r++) {
626 [ # # ]: 0 : f = A_(r, r);
627 [ # # ][ # # ]: 0 : assert (f != 0); // singular matrix
[ # # ]
628 [ # # ][ # # ]: 0 : for (c = 0; c < N; c++) A_(r, c) /= f;
[ # # ]
629 [ # # ]: 0 : B_(r) /= f;
630 : : }
631 : :
632 : : // the current X vector is a good initial guess for the iteration
633 [ # # ][ # # ]: 0 : tvector<nr_type_t> * Xprev = new tvector<nr_type_t> (*X);
[ # ]
634 : :
635 : : // start iterating here
636 : 0 : i = 0; error = 0;
637 [ # # ][ # # ]: 0 : do {
[ # # ][ # # ]
[ # # ][ # # ]
638 : : // compute new solution vector
639 [ # # ]: 0 : for (r = 0; r < N; r++) {
640 [ # # # ]: 0 : for (f = 0, c = 0; c < N; c++) {
641 [ # # ][ # # ]: 0 : if (c < r) f += A_(r, c) * X_(c);
[ # # ][ # # ]
642 [ # # ][ # # ]: 0 : else if (c > r) f += A_(r, c) * Xprev->get (c);
[ # # ][ # # ]
643 : : }
644 [ # # ][ # # ]: 0 : X_(r) = (1 - l) * Xprev->get (r) + l * (B_(r) - f);
[ # # ]
645 : : }
646 : : // check for convergence
647 [ # # ]: 0 : for (s = 0, d = 0, conv = 1, r = 0; r < N; r++) {
648 [ # # ][ # # ]: 0 : diff = abs (X_(r) - Xprev->get (r));
649 [ # # ][ # # ]: 0 : if (diff >= abstol + reltol * abs (X_(r))) {
[ # # ]
650 : 0 : conv = 0;
651 : 0 : break;
652 : : }
653 [ # # ]: 0 : d += diff; s += abs (X_(r));
654 [ # # ]: 0 : if (!std::isfinite (diff)) { error++; break; }
655 : : }
656 [ # # ]: 0 : if (!error) {
657 : : // adjust relaxation based on average errors
658 [ # # ][ # # ]: 0 : if ((s == 0 && d == 0) || d >= abstol * N + reltol * s) {
[ # # ]
659 : : // values <= 1 -> non-convergence to convergence
660 [ # # ]: 0 : if (l >= 0.6) l -= 0.1;
661 [ # # ]: 0 : if (l >= 1.0) l = 1.0;
662 : : }
663 : : else {
664 : : // values >= 1 -> faster convergence
665 [ # # ]: 0 : if (l < 1.5) l += 0.01;
666 [ # # ]: 0 : if (l < 1.0) l = 1.0;
667 : : }
668 : : }
669 : : // save last values
670 [ # # ]: 0 : *Xprev = *X;
671 : : }
672 : : while (++i < MaxIter && !conv);
673 : :
674 [ # # ]: 0 : delete Xprev;
675 : :
676 [ # # ][ # # ]: 0 : if (!conv || error) {
677 : : logprint (LOG_ERROR,
678 : : "WARNING: no convergence after %d sor iterations (l = %g)\n",
679 [ # # ]: 0 : i, l);
680 [ # # ]: 0 : solve_lu_crout ();
681 : : }
682 : : #if DEBUG && 0
683 : : else {
684 : : logprint (LOG_STATUS,
685 : : "NOTIFY: sor convergence after %d iterations\n", i);
686 : : }
687 : : #endif
688 : 0 : }
689 : :
690 : : /*! The function computes the convergence criteria for iterative
691 : : methods like Jacobi or Gauss-Seidel as defined by Schmidt and
692 : : v.Mises. */
693 : : template <class nr_type_t>
694 : 0 : nr_double_t eqnsys<nr_type_t>::convergence_criteria (void) {
695 : 0 : nr_double_t f = 0;
696 [ # # ]: 0 : for (int r = 0; r < A->getCols (); r++) {
697 [ # # ]: 0 : for (int c = 0; c < A->getCols (); c++) {
698 [ # # ]: 0 : if (r != c) f += norm (A_(r, c) / A_(r, r));
699 : : }
700 : : }
701 : 0 : return sqrt (f);
702 : : }
703 : :
704 : : /*! The function tries to ensure that there are non-zero diagonal
705 : : elements in the equation system matrix A. This is required for
706 : : iterative solution methods. */
707 : : template <class nr_type_t>
708 : 0 : void eqnsys<nr_type_t>::ensure_diagonal (void) {
709 : 0 : ensure_diagonal_MNA ();
710 : 0 : }
711 : :
712 : : /*! The function ensures that there are non-zero diagonal elements in
713 : : the equation system matrix A. It achieves this condition for
714 : : non-singular matrices which have been produced by the modified
715 : : nodal analysis. It takes advantage of the fact that the zero
716 : : diagonal elements have pairs of 1 and -1 on the same column and
717 : : row. */
718 : : template <class nr_type_t>
719 : 0 : void eqnsys<nr_type_t>::ensure_diagonal_MNA (void) {
720 : 0 : int restart, exchanged, begin = 0, pairs;
721 : : int pivot1, pivot2, i;
722 [ # # ]: 0 : do {
723 : 0 : restart = exchanged = 0;
724 : : /* search for zero diagonals with lone pairs */
725 [ # # ]: 0 : for (i = begin; i < N; i++) {
726 [ # # ][ # # ]: 0 : if (A_(i, i) == 0) {
[ # # ][ # ]
[ # ]
727 [ # # ]: 0 : pairs = countPairs (i, pivot1, pivot2);
728 [ # # ]: 0 : if (pairs == 1) { /* lone pair found, substitute rows */
729 [ # # ]: 0 : A->exchangeRows (i, pivot1);
730 [ # # ]: 0 : B->exchangeRows (i, pivot1);
731 : 0 : exchanged = 1;
732 : : }
733 [ # # ][ # # ]: 0 : else if ((pairs > 1) && !restart) {
734 : 0 : restart = 1;
735 : 0 : begin = i;
736 : : }
737 : : }
738 : : }
739 : :
740 : : /* all lone pairs are gone, look for zero diagonals with multiple pairs */
741 [ # # ]: 0 : if (restart) {
742 [ # # ][ # # ]: 0 : for (i = begin; !exchanged && i < N; i++) {
[ # # ]
743 [ # # ][ # # ]: 0 : if (A_(i, i) == 0) {
[ # # ][ # ]
[ # ]
744 [ # # ]: 0 : pairs = countPairs (i, pivot1, pivot2);
745 [ # # ]: 0 : A->exchangeRows (i, pivot1);
746 [ # # ]: 0 : B->exchangeRows (i, pivot1);
747 : 0 : exchanged = 1;
748 : : }
749 : : }
750 : : }
751 : : }
752 : : while (restart);
753 : 0 : }
754 : :
755 : : /*! Helper function for the above ensure_diagonal_MNA() function. It
756 : : looks for the pairs of 1 and -1 on the given row and column index. */
757 : : template <class nr_type_t>
758 : 0 : int eqnsys<nr_type_t>::countPairs (int i, int& r1, int& r2) {
759 : 0 : int pairs = 0;
760 [ # # ]: 0 : for (int r = 0; r < N; r++) {
761 [ # # ]: 0 : if (fabs (real (A_(r, i))) == 1.0) {
762 : 0 : r1 = r;
763 : 0 : pairs++;
764 [ # # ]: 0 : for (r++; r < N; r++) {
765 [ # # ]: 0 : if (fabs (real (A_(r, i))) == 1.0) {
766 : 0 : r2 = r;
767 [ # # ]: 0 : if (++pairs >= 2) return pairs;
768 : : }
769 : : }
770 : : }
771 : : }
772 : 0 : return pairs;
773 : : }
774 : :
775 : : /*! The function tries to raise the absolute value of diagonal elements
776 : : by swapping rows and thereby make the A matrix diagonally
777 : : dominant. */
778 : : template <class nr_type_t>
779 : 0 : void eqnsys<nr_type_t>::preconditioner (void) {
780 : : int pivot, r;
781 : : nr_double_t MaxPivot;
782 [ # # ]: 0 : for (int i = 0; i < N; i++) {
783 : : // find maximum column value for pivoting
784 [ # # ]: 0 : for (MaxPivot = 0, pivot = i, r = 0; r < N; r++) {
785 [ # # ][ # # ]: 0 : if (abs (A_(r, i)) > MaxPivot &&
[ # # ]
786 : : abs (A_(i, r)) >= abs (A_(r, r))) {
787 : 0 : MaxPivot = abs (A_(r, i));
788 : 0 : pivot = r;
789 : : }
790 : : }
791 : : // swap matrix rows if possible
792 [ # # ]: 0 : if (i != pivot) {
793 : 0 : A->exchangeRows (i, pivot);
794 : 0 : B->exchangeRows (i, pivot);
795 : : }
796 : : }
797 : 0 : }
798 : :
799 : : #define R_ (*R)
800 : : #define T_ (*T)
801 : :
802 : : /*! This function uses the QR decomposition using householder
803 : : transformations in order to solve the given equation system. */
804 : : template <class nr_type_t>
805 : 0 : void eqnsys<nr_type_t>::solve_qrh (void) {
806 : 0 : factorize_qrh ();
807 : 0 : substitute_qrh ();
808 : 0 : }
809 : :
810 : : /*! This function uses the QR decomposition using householder
811 : : transformations in order to solve the given equation system. */
812 : : template <class nr_type_t>
813 : 0 : void eqnsys<nr_type_t>::solve_qr (void) {
814 : 0 : factorize_qr_householder ();
815 : 0 : substitute_qr_householder ();
816 : 0 : }
817 : :
818 : : /*! The function uses the QR decomposition using householder
819 : : transformations in order to solve the given under-determined
820 : : equation system in its minimum norm (least square) sense. */
821 : : template <class nr_type_t>
822 : 0 : void eqnsys<nr_type_t>::solve_qr_ls (void) {
823 : 0 : A->transpose ();
824 : 0 : factorize_qr_householder ();
825 : 0 : substitute_qr_householder_ls ();
826 : 0 : }
827 : :
828 : : /*! Helper function for the euclidian norm calculators. */
829 : : static void
830 : 0 : euclidian_update (nr_double_t a, nr_double_t& n, nr_double_t& scale) {
831 : : nr_double_t x, ax;
832 [ # # ]: 0 : if ((x = a) != 0) {
833 : 0 : ax = fabs (x);
834 [ # # ]: 0 : if (scale < ax) {
835 : 0 : x = scale / ax;
836 : 0 : n = 1 + n * x * x;
837 : 0 : scale = ax;
838 : : }
839 : : else {
840 : 0 : x = ax / scale;
841 : 0 : n += x * x;
842 : : }
843 : : }
844 : 0 : }
845 : :
846 : : /*! The following function computes the euclidian norm of the given
847 : : column vector of the matrix A starting from the given row. */
848 : : template <class nr_type_t>
849 : 0 : nr_double_t eqnsys<nr_type_t>::euclidian_c (int c, int r) {
850 : 0 : nr_double_t scale = 0, n = 1;
851 [ # # ]: 0 : for (int i = r; i < N; i++) {
852 [ # # ][ # # ]: 0 : euclidian_update (real (A_(i, c)), n, scale);
853 [ # # ][ # # ]: 0 : euclidian_update (imag (A_(i, c)), n, scale);
854 : : }
855 [ # # ]: 0 : return scale * sqrt (n);
856 : : }
857 : :
858 : : /*! The following function computes the euclidian norm of the given
859 : : row vector of the matrix A starting from the given column. */
860 : : template <class nr_type_t>
861 : 0 : nr_double_t eqnsys<nr_type_t>::euclidian_r (int r, int c) {
862 : 0 : nr_double_t scale = 0, n = 1;
863 [ # # ]: 0 : for (int i = c; i < N; i++) {
864 [ # # ][ # # ]: 0 : euclidian_update (real (A_(r, i)), n, scale);
865 [ # # ][ # # ]: 0 : euclidian_update (imag (A_(r, i)), n, scale);
866 : : }
867 [ # # ]: 0 : return scale * sqrt (n);
868 : : }
869 : :
870 : : template <typename nr_type_t>
871 : 0 : inline nr_type_t cond_conj (nr_type_t t) {
872 : 0 : return std::conj(t);
873 : : }
874 : :
875 : : template <>
876 : 0 : inline double cond_conj (double t) {
877 : 0 : return t;
878 : : }
879 : :
880 : : /*! The function decomposes the matrix A into two matrices, the
881 : : orthonormal matrix Q and the upper triangular matrix R. The
882 : : original matrix is replaced by the householder vectors in the lower
883 : : triangular (including the diagonal) and the upper triangular R
884 : : matrix with its diagonal elements stored in the R vector. */
885 : : template <class nr_type_t>
886 : 0 : void eqnsys<nr_type_t>::factorize_qrh (void) {
887 : : int c, r, k, pivot;
888 : 0 : nr_type_t f, t;
889 : : nr_double_t s, MaxPivot;
890 : :
891 [ # # ][ # # ]: 0 : if (R) delete R; R = new tvector<nr_type_t> (N);
[ # # ][ # # ]
[ # ]
892 : :
893 [ # # ][ # # ]: 0 : for (c = 0; c < N; c++) {
894 : : // compute column norms and save in work array
895 [ # # ]: 0 : nPvt[c] = euclidian_c (c);
896 : 0 : cMap[c] = c; // initialize permutation vector
897 : : }
898 : :
899 [ # # ]: 0 : for (c = 0; c < N; c++) {
900 : :
901 : : // put column with largest norm into pivot position
902 : 0 : MaxPivot = nPvt[c]; pivot = c;
903 [ # # ]: 0 : for (r = c + 1; r < N; r++) {
904 [ # # ]: 0 : if ((s = nPvt[r]) > MaxPivot) {
905 : 0 : pivot = r;
906 : 0 : MaxPivot = s;
907 : : }
908 : : }
909 [ # # ]: 0 : if (pivot != c) {
910 [ # # ]: 0 : A->exchangeCols (pivot, c);
911 : 0 : Swap (int, cMap[pivot], cMap[c]);
912 : 0 : Swap (nr_double_t, nPvt[pivot], nPvt[c]);
913 : : }
914 : :
915 : : // compute householder vector
916 [ # # ]: 0 : if (c < N) {
917 : 0 : nr_type_t a, b;
918 [ # # ]: 0 : s = euclidian_c (c, c + 1);
919 [ # # ]: 0 : a = A_(c, c);
920 [ # # ][ # # ]: 0 : b = -sign (a) * xhypot (a, s); // Wj
921 [ # # ]: 0 : t = xhypot (s, a - b); // || Vi - Wi ||
922 [ # # ]: 0 : R_(c) = b;
923 : : // householder vector entries Ui
924 [ # # ][ # # ]: 0 : A_(c, c) = (a - b) / t;
925 [ # # ][ # # ]: 0 : for (r = c + 1; r < N; r++) A_(r, c) /= t;
[ # ]
926 : : }
927 : : else {
928 [ # # ][ # # ]: 0 : R_(c) = A_(c, c);
929 : : }
930 : :
931 : : // apply householder transformation to remaining columns
932 [ # # ]: 0 : for (r = c + 1; r < N; r++) {
933 [ # # ][ # # ]: 0 : for (f = 0, k = c; k < N; k++) f += cond_conj (A_(k, c)) * A_(k, r);
[ # # ][ # # ]
[ # ]
934 [ # # ][ # # ]: 0 : for (k = c; k < N; k++) A_(k, r) -= 2.0 * f * A_(k, c);
[ # # ][ # # ]
[ # # ]
935 : : }
936 : :
937 : : // update norms of remaining columns too
938 [ # # ][ # # ]: 0 : for (r = c + 1; r < N; r++) {
939 [ # # ]: 0 : nPvt[r] = euclidian_c (r, c + 1);
940 : : }
941 : : }
942 : 0 : }
943 : :
944 : : /*! The function decomposes the matrix A into two matrices, the
945 : : orthonormal matrix Q and the upper triangular matrix R. The
946 : : original matrix is replaced by the householder vectors in the lower
947 : : triangular and the upper triangular R matrix (including the
948 : : diagonal). The householder vectors are normalized to have one in
949 : : their first position. */
950 : : template <class nr_type_t>
951 : 0 : void eqnsys<nr_type_t>::factorize_qr_householder (void) {
952 : : int c, r, pivot;
953 : : nr_double_t s, MaxPivot;
954 : :
955 [ # # ][ # # ]: 0 : if (T) delete T; T = new tvector<nr_type_t> (N);
[ # # ][ # # ]
[ # ]
956 : :
957 [ # # ][ # # ]: 0 : for (c = 0; c < N; c++) {
958 : : // compute column norms and save in work array
959 [ # # ]: 0 : nPvt[c] = euclidian_c (c);
960 : 0 : cMap[c] = c; // initialize permutation vector
961 : : }
962 : :
963 [ # # ]: 0 : for (c = 0; c < N; c++) {
964 : :
965 : : // put column with largest norm into pivot position
966 : 0 : MaxPivot = nPvt[c]; pivot = c;
967 [ # # ]: 0 : for (r = c + 1; r < N; r++)
968 [ # # ]: 0 : if ((s = nPvt[r]) > MaxPivot) {
969 : 0 : pivot = r;
970 : 0 : MaxPivot = s;
971 : : }
972 [ # # ]: 0 : if (pivot != c) {
973 [ # # ]: 0 : A->exchangeCols (pivot, c);
974 : 0 : Swap (int, cMap[pivot], cMap[c]);
975 : 0 : Swap (nr_double_t, nPvt[pivot], nPvt[c]);
976 : : }
977 : :
978 : : // compute and apply householder vector
979 [ # # ][ # # ]: 0 : T_(c) = householder_left (c);
980 : :
981 : : // update norms of remaining columns too
982 [ # # ][ # # ]: 0 : for (r = c + 1; r < N; r++) {
983 [ # # ]: 0 : if ((s = nPvt[r]) > 0) {
984 : 0 : nr_double_t y = 0;
985 [ # # ][ # # ]: 0 : nr_double_t t = norm (A_(c, r) / s);
986 [ # # ][ # # ]: 0 : if (t < 1)
987 [ # # ]: 0 : y = s * sqrt (1 - t);
988 [ # # ]: 0 : if (fabs (y / s) < NR_TINY)
989 [ # # ]: 0 : nPvt[r] = euclidian_c (r, c + 1);
990 : : else
991 : 0 : nPvt[r] = y;
992 : : }
993 : : }
994 : : }
995 : 0 : }
996 : :
997 : : /*! The function uses the householder vectors in order to compute Q'B,
998 : : then the equation system RX = B is solved by backward substitution. */
999 : : template <class nr_type_t>
1000 : 0 : void eqnsys<nr_type_t>::substitute_qrh (void) {
1001 : : int c, r;
1002 : 0 : nr_type_t f;
1003 : :
1004 : : // form the new right hand side Q'B
1005 [ # # ]: 0 : for (c = 0; c < N - 1; c++) {
1006 : : // scalar product u_k^T * B
1007 [ # # ][ # # ]: 0 : for (f = 0, r = c; r < N; r++) f += cond_conj (A_(r, c)) * B_(r);
[ # # ][ # # ]
[ # ]
1008 : : // z - 2 * f * u_k
1009 [ # # ][ # # ]: 0 : for (r = c; r < N; r++) B_(r) -= 2.0 * f * A_(r, c);
[ # # ][ # # ]
[ # # ]
1010 : : }
1011 : :
1012 : : // backward substitution in order to solve RX = Q'B
1013 [ # # ]: 0 : for (r = N - 1; r >= 0; r--) {
1014 [ # # ]: 0 : f = B_(r);
1015 [ # # ][ # # ]: 0 : for (c = r + 1; c < N; c++) f -= A_(r, c) * X_(cMap[c]);
[ # # ][ # # ]
[ # # ]
1016 [ # # ][ # # ]: 0 : if (abs (R_(r)) > std::numeric_limits<nr_double_t>::epsilon())
[ # # ]
1017 [ # # ][ # # ]: 0 : X_(cMap[r]) = f / R_(r);
[ # # ]
1018 : : else
1019 [ # # ]: 0 : X_(cMap[r]) = 0;
1020 : : }
1021 : 0 : }
1022 : :
1023 : : /*! The function uses the householder vectors in order to compute Q'B,
1024 : : then the equation system RX = B is solved by backward substitution. */
1025 : : template <class nr_type_t>
1026 : 0 : void eqnsys<nr_type_t>::substitute_qr_householder (void) {
1027 : : int c, r;
1028 : 0 : nr_type_t f;
1029 : :
1030 : : // form the new right hand side Q'B
1031 [ # # ]: 0 : for (c = 0; c < N; c++) {
1032 [ # # ][ # # ]: 0 : if (T_(c) != 0) {
[ # # ]
1033 : : // scalar product u' * B
1034 [ # # ][ # # ]: 0 : for (f = B_(c), r = c + 1; r < N; r++) f += cond_conj (A_(r, c)) * B_(r);
[ # # ][ # # ]
[ # # ][ # ]
1035 : : // z - T * f * u
1036 [ # # ][ # # ]: 0 : f *= cond_conj (T_(c)); B_(c) -= f;
1037 [ # # ][ # # ]: 0 : for (r = c + 1; r < N; r++) B_(r) -= f * A_(r, c);
[ # # ][ # # ]
[ # ]
1038 : : }
1039 : : }
1040 : :
1041 : : // backward substitution in order to solve RX = Q'B
1042 [ # # ]: 0 : for (r = N - 1; r >= 0; r--) {
1043 [ # # ][ # # ]: 0 : for (f = B_(r), c = r + 1; c < N; c++) f -= A_(r, c) * X_(cMap[c]);
[ # # ][ # # ]
[ # # ]
1044 [ # # ][ # # ]: 0 : if (abs (A_(r, r)) > std::numeric_limits<nr_double_t>::epsilon())
[ # # ]
1045 [ # # ][ # # ]: 0 : X_(cMap[r]) = f / A_(r, r);
[ # # ]
1046 : : else
1047 [ # # ]: 0 : X_(cMap[r]) = 0;
1048 : : }
1049 : 0 : }
1050 : :
1051 : : /*! The function uses the householder vectors in order to the solve the
1052 : : equation system R'X = B by forward substitution, then QX is computed
1053 : : to obtain the least square solution of the under-determined equation
1054 : : system AX = B. */
1055 : : template <class nr_type_t>
1056 : 0 : void eqnsys<nr_type_t>::substitute_qr_householder_ls (void) {
1057 : : int c, r;
1058 : 0 : nr_type_t f;
1059 : :
1060 : : // forward substitution in order to solve R'X = B
1061 [ # # ]: 0 : for (r = 0; r < N; r++) {
1062 [ # # ][ # # ]: 0 : for (f = B_(r), c = 0; c < r; c++) f -= A_(c, r) * B_(c);
[ # # ][ # # ]
[ # # ]
1063 [ # # ][ # # ]: 0 : if (abs (A_(r, r)) > std::numeric_limits<nr_double_t>::epsilon())
[ # # ]
1064 [ # # ][ # # ]: 0 : B_(r) = f / A_(r, r);
[ # # ]
1065 : : else
1066 [ # # ]: 0 : B_(r) = 0;
1067 : : }
1068 : :
1069 : : // compute the least square solution QX
1070 [ # # ]: 0 : for (c = N - 1; c >= 0; c--) {
1071 [ # # ][ # # ]: 0 : if (T_(c) != 0) {
[ # # ]
1072 : : // scalar product u' * B
1073 [ # # ][ # # ]: 0 : for (f = B_(c), r = c + 1; r < N; r++) f += cond_conj (A_(r, c)) * B_(r);
[ # # ][ # # ]
[ # # ][ # ]
1074 : : // z - T * f * u_k
1075 [ # # ][ # # ]: 0 : f *= T_(c); B_(c) -= f;
1076 [ # # ][ # # ]: 0 : for (r = c + 1; r < N; r++) B_(r) -= f * A_(r, c);
[ # # ][ # # ]
[ # ]
1077 : : }
1078 : : }
1079 : :
1080 : : // permute solution vector
1081 [ # # ][ # # ]: 0 : for (r = 0; r < N; r++) X_(cMap[r]) = B_(r);
[ # # ][ # ]
1082 : 0 : }
1083 : :
1084 : : #define sign_(a) (real (a) < 0 ? -1 : 1)
1085 : :
1086 : : /*! The function creates the left-hand householder vector for a given
1087 : : column which eliminates the column except the first element. The
1088 : : householder vector is normalized to have one in the first position.
1089 : : The diagonal element is replaced by the applied householder vector
1090 : : and the vector itself is located in the free matrix positions. The
1091 : : function returns the normalization factor. */
1092 : : template <class nr_type_t>
1093 : 0 : nr_type_t eqnsys<nr_type_t>::householder_create_left (int c) {
1094 : 0 : nr_type_t a, b, t;
1095 : : nr_double_t s, g;
1096 [ # # ]: 0 : s = euclidian_c (c, c + 1);
1097 [ # # ][ # # ]: 0 : if (s == 0 && imag (A_(c, c)) == 0) {
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
1098 : : // no reflection necessary
1099 : 0 : t = 0;
1100 : : }
1101 : : else {
1102 : : // calculate householder reflection
1103 [ # # ]: 0 : a = A_(c, c);
1104 [ # # ][ # # ]: 0 : g = sign_(a) * xhypot (a, s);
[ # # ]
1105 : 0 : b = a + g;
1106 : 0 : t = b / g;
1107 : : // store householder vector
1108 [ # # ][ # # ]: 0 : for (int r = c + 1; r < N; r++) A_(r, c) /= b;
[ # # ]
1109 [ # # ]: 0 : A_(c, c) = -g;
1110 : : }
1111 : 0 : return t;
1112 : : }
1113 : :
1114 : : /*! The function computes a Householder vector to zero-out the matrix
1115 : : entries in the given column, stores it in the annihilated vector
1116 : : space (in a packed form) and applies the transformation to the
1117 : : remaining right-hand columns. */
1118 : : template <class nr_type_t>
1119 : 0 : nr_type_t eqnsys<nr_type_t>::householder_left (int c) {
1120 : : // compute householder vector
1121 [ # # ]: 0 : nr_type_t t = householder_create_left (c);
1122 : : // apply householder transformation to remaining columns if necessary
1123 [ # # ][ # # ]: 0 : if (t != 0) {
[ # # ]
1124 [ # # ]: 0 : householder_apply_left (c, t);
1125 : : }
1126 : 0 : return t;
1127 : : }
1128 : :
1129 : : /*! The function computes a Householder vector to zero-out the matrix
1130 : : entries in the given row, stores it in the annihilated vector space
1131 : : (in a packed form) and applies the transformation to the remaining
1132 : : downward rows. */
1133 : : template <class nr_type_t>
1134 : 0 : nr_type_t eqnsys<nr_type_t>::householder_right (int r) {
1135 : : // compute householder vector
1136 [ # # ]: 0 : nr_type_t t = householder_create_right (r);
1137 : : // apply householder transformation to remaining rows if necessary
1138 [ # # ][ # # ]: 0 : if (t != 0) {
[ # # ]
1139 [ # # ]: 0 : householder_apply_right (r, t);
1140 : : }
1141 : 0 : return t;
1142 : : }
1143 : :
1144 : : /*! The function creates the right-hand householder vector for a given
1145 : : row which eliminates the row except the first element. The
1146 : : householder vector is normalized to have one in the first position.
1147 : : The super-diagonal element is replaced by the applied householder
1148 : : vector and the vector itself is located in the free matrix
1149 : : positions. The function returns the normalization factor. */
1150 : : template <class nr_type_t>
1151 : 0 : nr_type_t eqnsys<nr_type_t>::householder_create_right (int r) {
1152 : 0 : nr_type_t a, b, t;
1153 : : nr_double_t s, g;
1154 [ # # ]: 0 : s = euclidian_r (r, r + 2);
1155 [ # # ][ # # ]: 0 : if (s == 0 && imag (A_(r, r + 1)) == 0) {
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
1156 : : // no reflection necessary
1157 : 0 : t = 0;
1158 : : }
1159 : : else {
1160 : : // calculate householder reflection
1161 [ # # ]: 0 : a = A_(r, r + 1);
1162 [ # # ][ # # ]: 0 : g = sign_(a) * xhypot (a, s);
[ # # ]
1163 : 0 : b = a + g;
1164 : 0 : t = b / g;
1165 : : // store householder vector
1166 [ # # ][ # # ]: 0 : for (int c = r + 2; c < N; c++) A_(r, c) /= b;
[ # # ]
1167 [ # # ]: 0 : A_(r, r + 1) = -g;
1168 : : }
1169 : 0 : return t;
1170 : : }
1171 : :
1172 : : /*! Applies the householder vector stored in the given column to the
1173 : : right-hand columns using the given normalization factor. */
1174 : : template <class nr_type_t>
1175 : 0 : void eqnsys<nr_type_t>::householder_apply_left (int c, nr_type_t t) {
1176 : 0 : nr_type_t f;
1177 : : int k, r;
1178 : : // apply the householder vector to each right-hand column
1179 [ # # ]: 0 : for (r = c + 1; r < N; r++) {
1180 : : // calculate f = u' * A (a scalar product)
1181 [ # # ]: 0 : f = A_(c, r);
1182 [ # # ][ # # ]: 0 : for (k = c + 1; k < N; k++) f += cond_conj (A_(k, c)) * A_(k, r);
[ # # ][ # # ]
[ # # ]
1183 : : // calculate A -= T * f * u
1184 [ # # ]: 0 : f *= cond_conj (t); A_(c, r) -= f;
1185 [ # # ][ # # ]: 0 : for (k = c + 1; k < N; k++) A_(k, r) -= f * A_(k, c);
[ # # ][ # # ]
[ # ]
1186 : : }
1187 : 0 : }
1188 : :
1189 : : /*! Applies the householder vector stored in the given row to the
1190 : : downward rows using the given normalization factor. */
1191 : : template <class nr_type_t>
1192 : 0 : void eqnsys<nr_type_t>::householder_apply_right (int r, nr_type_t t) {
1193 : 0 : nr_type_t f;
1194 : : int k, c;
1195 : : // apply the householder vector to each downward row
1196 [ # # ]: 0 : for (c = r + 1; c < N; c++) {
1197 : : // calculate f = u' * A (a scalar product)
1198 [ # # ]: 0 : f = A_(c, r + 1);
1199 [ # # ][ # # ]: 0 : for (k = r + 2; k < N; k++) f += cond_conj (A_(r, k)) * A_(c, k);
[ # # ][ # # ]
[ # # ]
1200 : : // calculate A -= T * f * u
1201 [ # # ]: 0 : f *= cond_conj (t); A_(c, r + 1) -= f;
1202 [ # # ][ # # ]: 0 : for (k = r + 2; k < N; k++) A_(c, k) -= f * A_(r, k);
[ # # ][ # # ]
[ # ]
1203 : : }
1204 : 0 : }
1205 : :
1206 : : // Some helper defines for SVD.
1207 : : #define V_ (*V)
1208 : : #define S_ (*S)
1209 : : #define E_ (*E)
1210 : : #define U_ (*A)
1211 : :
1212 : : /*! The function does exactly the same as householder_apply_right()
1213 : : except that it applies the householder transformations to another
1214 : : matrix. */
1215 : : template <class nr_type_t>
1216 : 0 : void eqnsys<nr_type_t>::householder_apply_right_extern (int r, nr_type_t t) {
1217 : 0 : nr_type_t f;
1218 : : int k, c;
1219 : : // apply the householder vector to each downward row
1220 [ # # ]: 0 : for (c = r + 1; c < N; c++) {
1221 : : // calculate f = u' * A (a scalar product)
1222 [ # # ]: 0 : f = V_(c, r + 1);
1223 [ # # ][ # # ]: 0 : for (k = r + 2; k < N; k++) f += cond_conj (A_(r, k)) * V_(c, k);
[ # # ][ # # ]
[ # # ]
1224 : : // calculate A -= T * f * u
1225 [ # # ]: 0 : f *= cond_conj (t); V_(c, r + 1) -= f;
1226 [ # # ][ # # ]: 0 : for (k = r + 2; k < N; k++) V_(c, k) -= f * A_(r, k);
[ # # ][ # # ]
[ # ]
1227 : : }
1228 : 0 : }
1229 : :
1230 : : /*! This function solves the equation system AX = B using the singular
1231 : : value decomposition (Golub-Reinsch-SVD). */
1232 : : template <class nr_type_t>
1233 : 0 : void eqnsys<nr_type_t>::solve_svd (void) {
1234 : 0 : factorize_svd ();
1235 : 0 : chop_svd ();
1236 : 0 : substitute_svd ();
1237 : 0 : }
1238 : :
1239 : : //! Annihilates near-zero singular values.
1240 : : template <class nr_type_t>
1241 : 0 : void eqnsys<nr_type_t>::chop_svd (void) {
1242 : : int c;
1243 : : nr_double_t Max, Min;
1244 : 0 : Max = 0.0;
1245 [ # # ][ # # ]: 0 : for (c = 0; c < N; c++) if (fabs (S_(c)) > Max) Max = fabs (S_(c));
1246 : 0 : Min = Max * std::numeric_limits<nr_double_t>::max();
1247 [ # # ][ # # ]: 0 : for (c = 0; c < N; c++) if (fabs (S_(c)) < Min) S_(c) = 0.0;
1248 : 0 : }
1249 : :
1250 : : /*! The function uses the singular value decomposition A = USV' to
1251 : : solve the equation system AX = B by simple matrix multiplications.
1252 : : Remember that the factorization actually computed U, S and V'. */
1253 : : template <class nr_type_t>
1254 : 0 : void eqnsys<nr_type_t>::substitute_svd (void) {
1255 : : int c, r;
1256 : 0 : nr_type_t f;
1257 : : // calculate U'B
1258 [ # # ][ # # ]: 0 : for (c = 0; c < N; c++) {
1259 : 0 : f = 0.0;
1260 : : // non-zero result only if S is non-zero
1261 [ # # ]: 0 : if (S_(c) != 0.0) {
[ # # # # ]
1262 [ # # ][ # # ]: 0 : for (r = 0; r < N; r++) f += cond_conj (U_(r, c)) * B_(r);
[ # # ][ # # ]
1263 : : // this is the divide by S
1264 [ # # ]: 0 : f /= S_(c);
1265 : : }
1266 [ # # ]: 0 : R_(c) = f;
1267 : : }
1268 : : // matrix multiply by V to get the final solution
1269 [ # # ][ # # ]: 0 : for (r = 0; r < N; r++) {
1270 [ # # ][ # # ]: 0 : for (f = 0.0, c = 0; c < N; c++) f += cond_conj (V_(c, r)) * R_(c);
[ # # ][ # # ]
[ # ]
1271 [ # # ]: 0 : X_(r) = f;
1272 : : }
1273 : 0 : }
1274 : :
1275 : : /*! The function decomposes the matrix A into three other matrices U, S
1276 : : and V'. The matrix A is overwritten by the U matrix, S is stored
1277 : : in a separate vector and V in a separate matrix. */
1278 : :
1279 : : template <class nr_type_t>
1280 : 0 : void eqnsys<nr_type_t>::factorize_svd (void) {
1281 : : int i, j, l;
1282 : 0 : nr_type_t t;
1283 : :
1284 : : // allocate space for vectors and matrices
1285 [ # # ][ # # ]: 0 : if (R) delete R; R = new tvector<nr_type_t> (N);
[ # # ][ # # ]
[ # ]
1286 [ # # ][ # # ]: 0 : if (T) delete T; T = new tvector<nr_type_t> (N);
[ # # ][ # # ]
[ # ]
1287 [ # # ][ # # ]: 0 : if (V) delete V; V = new tmatrix<nr_type_t> (N);
[ # # ][ # # ]
[ # ]
1288 [ # # ][ # # ]: 0 : if (S) delete S; S = new tvector<nr_double_t> (N);
[ # # ][ # # ]
[ # ]
1289 [ # # ][ # # ]: 0 : if (E) delete E; E = new tvector<nr_double_t> (N);
[ # # ][ # # ]
[ # ]
1290 : :
1291 : : // bidiagonalization through householder transformations
1292 [ # # ]: 0 : for (i = 0; i < N; i++) {
1293 [ # # ][ # # ]: 0 : T_(i) = householder_left (i);
1294 [ # # ][ # # ]: 0 : if (i < N - 1) R_(i) = householder_right (i);
[ # # ][ # # ]
1295 : : }
1296 : :
1297 : : // copy over the real valued bidiagonal values
1298 [ # # ][ # # ]: 0 : for (i = 0; i < N; i++) S_(i) = real (A_(i, i));
[ # # ]
1299 [ # # ][ # # ]: 0 : for (E_(0) = 0, i = 1; i < N; i++) E_(i) = real (A_(i - 1, i));
[ # # ][ # # ]
[ # ]
1300 : :
1301 : : // backward accumulation of right-hand householder transformations
1302 : : // yields the V' matrix
1303 [ # # ]: 0 : for (l = N, i = N - 1; i >= 0; l = i--) {
1304 [ # # ]: 0 : if (i < N - 1) {
1305 [ # # ][ # # ]: 0 : if ((t = R_(i)) != 0.0) {
[ # ]
1306 [ # # ]: 0 : householder_apply_right_extern (i, cond_conj (t));
1307 : : }
1308 [ # # ]: 0 : else for (j = l; j < N; j++) // cleanup this row
1309 [ # # ][ # # ]: 0 : V_(i, j) = V_(j, i) = 0.0;
1310 : : }
1311 [ # # ]: 0 : V_(i, i) = 1.0;
1312 : : }
1313 : :
1314 : : // backward accumulation of left-hand householder transformations
1315 : : // yields the U matrix in place of the A matrix
1316 [ # # ]: 0 : for (l = N, i = N - 1; i >= 0; l = i--) {
1317 [ # # ]: 0 : for (j = l; j < N; j++) // cleanup upper row
1318 [ # # ]: 0 : A_(i, j) = 0.0;
1319 [ # # ][ # # ]: 0 : if ((t = T_(i)) != 0.0) {
[ # ]
1320 [ # # ]: 0 : householder_apply_left (i, cond_conj (t));
1321 [ # # ][ # # ]: 0 : for (j = l; j < N; j++) A_(j, i) *= -t;
1322 : : }
1323 [ # # ]: 0 : else for (j = l; j < N; j++) // cleanup this column
1324 [ # # ]: 0 : A_(j, i) = 0.0;
1325 [ # # ]: 0 : A_(i, i) = 1.0 - t;
1326 : : }
1327 : :
1328 : : // S and E contain diagonal and super-diagonal, A contains U, V'
1329 : : // calculated; now diagonalization can begin
1330 [ # # ]: 0 : diagonalize_svd ();
1331 : 0 : }
1332 : :
1333 : : #ifndef MAX
1334 : : # define MAX(x,y) (((x) > (y)) ? (x) : (y))
1335 : : #endif
1336 : :
1337 : : //! Helper function computes Givens rotation.
1338 : : static nr_double_t
1339 : 0 : givens (nr_double_t a, nr_double_t b, nr_double_t& c, nr_double_t& s) {
1340 : 0 : nr_double_t z = xhypot (a, b);
1341 : 0 : c = a / z;
1342 : 0 : s = b / z;
1343 : 0 : return z;
1344 : : }
1345 : :
1346 : : template <class nr_type_t>
1347 : 0 : void eqnsys<nr_type_t>::givens_apply_u (int c1, int c2,
1348 : : nr_double_t c, nr_double_t s) {
1349 [ # # ]: 0 : for (int i = 0; i < N; i++) {
1350 [ # # ]: 0 : nr_type_t y = U_(i, c1);
1351 [ # # ]: 0 : nr_type_t z = U_(i, c2);
1352 [ # # ]: 0 : U_(i, c1) = y * c + z * s;
1353 [ # # ]: 0 : U_(i, c2) = z * c - y * s;
1354 : : }
1355 : 0 : }
1356 : :
1357 : : template <class nr_type_t>
1358 : 0 : void eqnsys<nr_type_t>::givens_apply_v (int r1, int r2,
1359 : : nr_double_t c, nr_double_t s) {
1360 [ # # ]: 0 : for (int i = 0; i < N; i++) {
1361 [ # # ]: 0 : nr_type_t y = V_(r1, i);
1362 [ # # ]: 0 : nr_type_t z = V_(r2, i);
1363 [ # # ]: 0 : V_(r1, i) = y * c + z * s;
1364 [ # # ]: 0 : V_(r2, i) = z * c - y * s;
1365 : : }
1366 : 0 : }
1367 : :
1368 : : /*! This function diagonalizes the upper bidiagonal matrix fromed by
1369 : : the diagonal S and the super-diagonal vector E. Both vectors are
1370 : : real valued. Thus real valued calculations even when solving a
1371 : : complex valued equation systems is possible except for the matrix
1372 : : updates of U and V'. */
1373 : : template <class nr_type_t>
1374 : 0 : void eqnsys<nr_type_t>::diagonalize_svd (void) {
1375 : : bool split;
1376 : 0 : int i, l, j, its, k, n, MaxIters = 30;
1377 : : nr_double_t an, f, g, h, d, c, s, b, a;
1378 : :
1379 : : // find largest bidiagonal value
1380 [ # # ]: 0 : for (an = 0, i = 0; i < N; i++)
1381 [ # # ][ # # ]: 0 : an = MAX (an, fabs (S_(i)) + fabs (E_(i)));
[ # # ][ # # ]
[ # # ]
1382 : :
1383 : : // diagonalize the bidiagonal matrix (stored as super-diagonal
1384 : : // vector E and diagonal vector S)
1385 [ # # ]: 0 : for (k = N - 1; k >= 0; k--) {
1386 : : // loop over singular values
1387 [ # # ]: 0 : for (its = 0; its <= MaxIters; its++) {
1388 : 0 : split = true;
1389 : : // check for a zero entry along the super-diagonal E, if there
1390 : : // is one, it is possible to QR iterate on two separate matrices
1391 : : // above and below it
1392 [ # # ]: 0 : for (n = 0, l = k; l >= 1; l--) {
1393 : : // note that E_(0) is always zero
1394 : 0 : n = l - 1;
1395 [ # # ][ # # ]: 0 : if (fabs (E_(l)) + an == an) { split = false; break; }
1396 [ # # ][ # # ]: 0 : if (fabs (S_(n)) + an == an) break;
1397 : : }
1398 : : // if there is a zero on the diagonal S, it is possible to zero
1399 : : // out the corresponding super-diagonal E entry to its right
1400 [ # # ]: 0 : if (split) {
1401 : : // cancellation of E_(l), if l > 0
1402 : 0 : c = 0.0;
1403 : 0 : s = 1.0;
1404 [ # # ]: 0 : for (i = l; i <= k; i++) {
1405 [ # # ]: 0 : f = -s * E_(i);
1406 [ # # ]: 0 : E_(i) *= c;
1407 [ # # ]: 0 : if (fabs (f) + an == an) break;
1408 [ # # ]: 0 : g = S_(i);
1409 [ # # ][ # # ]: 0 : S_(i) = givens (f, g, c, s);
1410 : : // apply inverse rotation to U
1411 [ # # ]: 0 : givens_apply_u (n, i, c, s);
1412 : : }
1413 : : }
1414 : :
1415 [ # # ]: 0 : d = S_(k);
1416 : : // convergence
1417 [ # # ]: 0 : if (l == k) {
1418 : : // singular value is made non-negative
1419 [ # # ]: 0 : if (d < 0.0) {
1420 [ # # ]: 0 : S_(k) = -d;
1421 [ # # ][ # # ]: 0 : for (j = 0; j < N; j++) V_(k, j) = -V_(k, j);
[ # # # ]
1422 : : }
1423 : 0 : break;
1424 : : }
1425 [ # # ]: 0 : if (its == MaxIters) {
1426 : : logprint (LOG_ERROR, "WARNING: no convergence in %d SVD iterations\n",
1427 [ # # ]: 0 : MaxIters);
1428 : : }
1429 : :
1430 : : // shift from bottom 2-by-2 minor
1431 [ # # ]: 0 : a = S_(l);
1432 : 0 : n = k - 1;
1433 [ # # ]: 0 : b = S_(n);
1434 [ # # ]: 0 : g = E_(n);
1435 [ # # ]: 0 : h = E_(k);
1436 : :
1437 : : // compute QR shift value (as close as possible to the largest
1438 : : // eigenvalue of the 2-by-2 minor matrix)
1439 : 0 : f = (b - d) * (b + d) + (g - h) * (g + h);
1440 : 0 : f /= 2.0 * h * b;
1441 [ # # ][ # # ]: 0 : f += sign_(f) * xhypot (f, 1.0);
[ # # ]
1442 : 0 : f = ((a - d) * (a + d) + h * (b / f - h)) / a;
1443 : : // f => (B_{ll}^2 - u) / B_{ll}
1444 : : // u => eigenvalue of T = B' * B nearer T_{22} (Wilkinson shift)
1445 : :
1446 : : // next QR transformation
1447 : 0 : c = s = 1.0;
1448 [ # # ]: 0 : for (j = l; j <= n; j++) {
1449 : 0 : i = j + 1;
1450 [ # # ]: 0 : g = E_(i);
1451 [ # # ]: 0 : b = S_(i);
1452 : 0 : h = s * g; // h => right-hand non-zero to annihilate
1453 : 0 : g *= c;
1454 [ # # ][ # # ]: 0 : E_(j) = givens (f, h, c, s);
1455 : : // perform the rotation
1456 : 0 : f = a * c + g * s;
1457 : 0 : g = g * c - a * s;
1458 : 0 : h = b * s;
1459 : 0 : b *= c;
1460 : : // here: +- -+
1461 : : // | f g | = B * V'_j (also first V'_1)
1462 : : // | h b |
1463 : : // +- -+
1464 : :
1465 : : // accumulate the rotation in V'
1466 [ # # ]: 0 : givens_apply_v (j, i, c, s);
1467 [ # # ][ # # ]: 0 : d = S_(j) = xhypot (f, h);
1468 : : // rotation can be arbitrary if d = 0
1469 [ # # ]: 0 : if (d != 0.0) {
1470 : : // d => non-zero result on diagonal
1471 : 0 : d = 1.0 / d;
1472 : : // rotation coefficients to annihilate the lower non-zero
1473 : 0 : c = f * d;
1474 : 0 : s = h * d;
1475 : : }
1476 : 0 : f = c * g + s * b;
1477 : 0 : a = c * b - s * g;
1478 : : // here: +- -+
1479 : : // | d f | => U_j * B
1480 : : // | 0 a |
1481 : : // +- -+
1482 : :
1483 : : // accumulate rotation into U
1484 [ # # ]: 0 : givens_apply_u (j, i, c, s);
1485 : : }
1486 [ # # ]: 0 : E_(l) = 0;
1487 [ # # ]: 0 : E_(k) = f;
1488 [ # # ]: 0 : S_(k) = a;
1489 : : }
1490 : : }
1491 : 0 : }
1492 : :
1493 : : } // namespace qucs
1494 : :
1495 : : #undef V_
|
