Qucs-core  0.0.19
eqnsys.cpp
Go to the documentation of this file.
00001 /*
00002  * eqnsys.cpp - equation system solver class implementation
00003  *
00004  * Copyright (C) 2004, 2005, 2006, 2007, 2008 Stefan Jahn <stefan@lkcc.org>
00005  *
00006  * This is free software; you can redistribute it and/or modify
00007  * it under the terms of the GNU General Public License as published by
00008  * the Free Software Foundation; either version 2, or (at your option)
00009  * any later version.
00010  *
00011  * This software is distributed in the hope that it will be useful,
00012  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  * GNU General Public License for more details.
00015  *
00016  * You should have received a copy of the GNU General Public License
00017  * along with this package; see the file COPYING.  If not, write to
00018  * the Free Software Foundation, Inc., 51 Franklin Street - Fifth Floor,
00019  * Boston, MA 02110-1301, USA.
00020  *
00021  * $Id$
00022  *
00023  */
00024 
00025 // the types required for qucs library files are defined
00026 // in qucs_typedefs.h, created by configure from
00027 // qucs_typedefs.h.in
00028 #include "qucs_typedefs.h"
00029 
00030 #include <assert.h>
00031 #include <time.h>
00032 #include <cmath>
00033 #include <float.h>
00034 
00035 #include <limits>
00036 
00037 #include "compat.h"
00038 #include "logging.h"
00039 #include "precision.h"
00040 #include "complex.h"
00041 #include "tmatrix.h"
00042 #include "eqnsys.h"
00043 #include "exception.h"
00044 #include "exceptionstack.h"
00045 
00047 #define Swap(type,a,b) { type t; t = a; a = b; b = t; }
00048 
00049 namespace qucs {
00050 
00052 template <class nr_type_t>
00053 eqnsys<nr_type_t>::eqnsys () {
00054   A = V = NULL;
00055   B = X = NULL;
00056   S = E = NULL;
00057   T = R = NULL;
00058   nPvt = NULL;
00059   cMap = rMap = NULL;
00060   update = 1;
00061   pivoting = PIVOT_PARTIAL;
00062   N = 0;
00063 }
00064 
00066 template <class nr_type_t>
00067 eqnsys<nr_type_t>::~eqnsys () {
00068   delete R;
00069   delete T;
00070   delete B;
00071   delete S;
00072   delete E;
00073   delete V;
00074   delete[] rMap;
00075   delete[] cMap;
00076   delete[] nPvt;
00077 }
00078 
00081 template <class nr_type_t>
00082 eqnsys<nr_type_t>::eqnsys (eqnsys & e) {
00083   A = e.A;
00084   V = NULL;
00085   S = E = NULL;
00086   T = R = NULL;
00087   B = e.B ? new tvector<nr_type_t> (*(e.B)) : NULL;
00088   cMap = rMap = NULL;
00089   nPvt = NULL;
00090   update = 1;
00091   X = e.X;
00092   N = 0;
00093 }
00094 
00100 template <class nr_type_t>
00101 void eqnsys<nr_type_t>::passEquationSys (tmatrix<nr_type_t> * nA,
00102                                          tvector<nr_type_t> * refX,
00103                                          tvector<nr_type_t> * nB) {
00104   if (nA != NULL) {
00105     A = nA;
00106     update = 1;
00107     if (N != A->getCols ()) {
00108       N = A->getCols ();
00109       delete[] cMap; cMap = new int[N];
00110       delete[] rMap; rMap = new int[N];
00111       delete[] nPvt; nPvt = new nr_double_t[N];
00112     }
00113   }
00114   else {
00115     update = 0;
00116   }
00117   delete B;
00118   B = new tvector<nr_type_t> (*nB);
00119   X = refX;
00120 }
00121 
00125 template <class nr_type_t>
00126 void eqnsys<nr_type_t>::solve (void) {
00127 #if DEBUG && 0
00128   time_t t = time (NULL);
00129 #endif
00130   switch (algo) {
00131   case ALGO_INVERSE:
00132     solve_inverse ();
00133     break;
00134   case ALGO_GAUSS:
00135     solve_gauss ();
00136     break;
00137   case ALGO_GAUSS_JORDAN:
00138     solve_gauss_jordan ();
00139     break;
00140   case ALGO_LU_DECOMPOSITION_CROUT:
00141     solve_lu_crout ();
00142     break;
00143   case ALGO_LU_DECOMPOSITION_DOOLITTLE:
00144     solve_lu_doolittle ();
00145     break;
00146   case ALGO_LU_FACTORIZATION_CROUT:
00147     factorize_lu_crout ();
00148     break;
00149   case ALGO_LU_FACTORIZATION_DOOLITTLE:
00150     factorize_lu_doolittle ();
00151     break;
00152   case ALGO_LU_SUBSTITUTION_CROUT:
00153     substitute_lu_crout ();
00154     break;
00155   case ALGO_LU_SUBSTITUTION_DOOLITTLE:
00156     substitute_lu_doolittle ();
00157     break;
00158   case ALGO_JACOBI: case ALGO_GAUSS_SEIDEL:
00159     solve_iterative ();
00160     break;
00161   case ALGO_SOR:
00162     solve_sor ();
00163     break;
00164   case ALGO_QR_DECOMPOSITION:
00165     solve_qr ();
00166     break;
00167   case ALGO_QR_DECOMPOSITION_LS:
00168     solve_qr_ls ();
00169     break;
00170   case ALGO_SV_DECOMPOSITION:
00171     solve_svd ();
00172     break;
00173   case ALGO_QR_DECOMPOSITION_2:
00174     solve_qrh ();
00175     break;
00176   }
00177 #if DEBUG && 0
00178   logprint (LOG_STATUS, "NOTIFY: %dx%d eqnsys solved in %ld seconds\n",
00179             A->getRows (), A->getCols (), time (NULL) - t);
00180 #endif
00181 }
00182 
00184 template <class nr_type_t>
00185 void eqnsys<nr_type_t>::solve_inverse (void) {
00186   *X = inverse (*A) * *B;
00187 }
00188 
00189 #define A_ (*A)
00190 #define X_ (*X)
00191 #define B_ (*B)
00192 
00195 template <class nr_type_t>
00196 void eqnsys<nr_type_t>::solve_gauss (void) {
00197   nr_double_t MaxPivot;
00198   nr_type_t f;
00199   int i, c, r, pivot;
00200 
00201   // triangulate the matrix
00202   for (i = 0; i < N; i++) {
00203     // find maximum column value for pivoting
00204     for (MaxPivot = 0, pivot = r = i; r < N; r++) {
00205       if (abs (A_(r, i)) > MaxPivot) {
00206         MaxPivot = abs (A_(r, i));
00207         pivot = r;
00208       }
00209     }
00210     // exchange rows if necessary
00211     assert (MaxPivot != 0);
00212     if (i != pivot) {
00213       A->exchangeRows (i, pivot);
00214       B->exchangeRows (i, pivot);
00215     }
00216     // compute new rows and columns
00217     for (r = i + 1; r < N; r++) {
00218       f = A_(r, i) / A_(i, i);
00219       for (c = i + 1; c < N; c++) A_(r, c) -= f * A_(i, c);
00220       B_(r) -= f * B_(i);
00221     }
00222   }
00223 
00224   // backward substitution
00225   for (i = N - 1; i >= 0; i--) {
00226     f = B_(i);
00227     for (c = i + 1; c < N; c++) f -= A_(i, c) * X_(c);
00228     X_(i) = f / A_(i, i);
00229   }
00230 }
00231 
00235 template <class nr_type_t>
00236 void eqnsys<nr_type_t>::solve_gauss_jordan (void) {
00237   nr_double_t MaxPivot;
00238   nr_type_t f;
00239   int i, c, r, pivot;
00240 
00241   // create the eye matrix
00242   for (i = 0; i < N; i++) {
00243     // find maximum column value for pivoting
00244     for (MaxPivot = 0, pivot = r = i; r < N; r++) {
00245       if (abs (A_(r, i)) > MaxPivot) {
00246         MaxPivot = abs (A_(r, i));
00247         pivot = r;
00248       }
00249     }
00250     // exchange rows if necessary
00251     assert (MaxPivot != 0);
00252     if (i != pivot) {
00253       A->exchangeRows (i, pivot);
00254       B->exchangeRows (i, pivot);
00255     }
00256 
00257     // compute current row
00258     f = A_(i, i);
00259     for (c = i + 1; c < N; c++) A_(i, c) /= f;
00260     B_(i) /= f;
00261 
00262     // compute new rows and columns
00263     for (r = 0; r < N; r++) {
00264       if (r != i) {
00265         f = A_(r, i);
00266         for (c = i + 1; c < N; c++) A_(r, c) -= f * A_(i, c);
00267         B_(r) -= f * B_(i);
00268       }
00269     }
00270   }
00271 
00272   // right hand side is now the solution
00273   *X = *B;
00274 }
00275 
00276 #define LU_FAILURE 0
00277 #define VIRTUAL_RES(txt,i) {                                      \
00278   qucs::exception * e = new qucs::exception (EXCEPTION_SINGULAR); \
00279   e->setText (txt);                                               \
00280   e->setData (rMap[i]);                                           \
00281   A_(i, i) = NR_TINY; /* virtual resistance to ground */          \
00282   throw_exception (e); }
00283 
00289 template <class nr_type_t>
00290 void eqnsys<nr_type_t>::solve_lu_crout (void) {
00291 
00292   // skip decomposition if requested
00293   if (update) {
00294     // perform LU composition
00295     factorize_lu_crout ();
00296   }
00297 
00298   // finally solve the equation system
00299   substitute_lu_crout ();
00300 }
00301 
00303 template <class nr_type_t>
00304 void eqnsys<nr_type_t>::solve_lu_doolittle (void) {
00305 
00306   // skip decomposition if requested
00307   if (update) {
00308     // perform LU composition
00309     factorize_lu_doolittle ();
00310   }
00311 
00312   // finally solve the equation system
00313   substitute_lu_doolittle ();
00314 }
00315 
00320 template <class nr_type_t>
00321 void eqnsys<nr_type_t>::factorize_lu_crout (void) {
00322   nr_double_t d, MaxPivot;
00323   nr_type_t f;
00324   int k, c, r, pivot;
00325 
00326   // initialize pivot exchange table
00327   for (r = 0; r < N; r++) {
00328     for (MaxPivot = 0, c = 0; c < N; c++)
00329       if ((d = abs (A_(r, c))) > MaxPivot)
00330         MaxPivot = d;
00331     if (MaxPivot <= 0) MaxPivot = NR_TINY;
00332     nPvt[r] = 1 / MaxPivot;
00333     rMap[r] = r;
00334   }
00335 
00336   // decompose the matrix into L (lower) and U (upper) matrix
00337   for (c = 0; c < N; c++) {
00338     // upper matrix entries
00339     for (r = 0; r < c; r++) {
00340       f = A_(r, c);
00341       for (k = 0; k < r; k++) f -= A_(r, k) * A_(k, c);
00342       A_(r, c) = f / A_(r, r);
00343     }
00344     // lower matrix entries
00345     for (MaxPivot = 0, pivot = r; r < N; r++) {
00346       f = A_(r, c);
00347       for (k = 0; k < c; k++) f -= A_(r, k) * A_(k, c);
00348       A_(r, c) = f;
00349       // larger pivot ?
00350       if ((d = nPvt[r] * abs (f)) > MaxPivot) {
00351         MaxPivot = d;
00352         pivot = r;
00353       }
00354     }
00355 
00356     // check pivot element and throw appropriate exception
00357     if (MaxPivot <= 0) {
00358 #if LU_FAILURE
00359       qucs::exception * e = new qucs::exception (EXCEPTION_PIVOT);
00360       e->setText ("no pivot != 0 found during Crout LU decomposition");
00361       e->setData (c);
00362       throw_exception (e);
00363       goto fail;
00364 #else /* insert virtual resistance */
00365       VIRTUAL_RES ("no pivot != 0 found during Crout LU decomposition", c);
00366 #endif
00367     }
00368 
00369     // swap matrix rows if necessary and remember that step in the
00370     // exchange table
00371     if (c != pivot) {
00372       A->exchangeRows (c, pivot);
00373       Swap (int, rMap[c], rMap[pivot]);
00374       Swap (nr_double_t, nPvt[c], nPvt[pivot]);
00375     }
00376   }
00377 #if LU_FAILURE
00378  fail:
00379 #endif
00380 }
00381 
00386 template <class nr_type_t>
00387 void eqnsys<nr_type_t>::factorize_lu_doolittle (void) {
00388   nr_double_t d, MaxPivot;
00389   nr_type_t f;
00390   int k, c, r, pivot;
00391 
00392   // initialize pivot exchange table
00393   for (r = 0; r < N; r++) {
00394     for (MaxPivot = 0, c = 0; c < N; c++)
00395       if ((d = abs (A_(r, c))) > MaxPivot)
00396         MaxPivot = d;
00397     if (MaxPivot <= 0) MaxPivot = NR_TINY;
00398     nPvt[r] = 1 / MaxPivot;
00399     rMap[r] = r;
00400   }
00401 
00402   // decompose the matrix into L (lower) and U (upper) matrix
00403   for (c = 0; c < N; c++) {
00404     // upper matrix entries
00405     for (r = 0; r < c; r++) {
00406       f = A_(r, c);
00407       for (k = 0; k < r; k++) f -= A_(r, k) * A_(k, c);
00408       A_(r, c) = f;
00409     }
00410     // lower matrix entries
00411     for (MaxPivot = 0, pivot = r; r < N; r++) {
00412       f = A_(r, c);
00413       for (k = 0; k < c; k++) f -= A_(r, k) * A_(k, c);
00414       A_(r, c) = f;
00415       // larger pivot ?
00416       if ((d = nPvt[r] * abs (f)) > MaxPivot) {
00417         MaxPivot = d;
00418         pivot = r;
00419       }
00420     }
00421 
00422     // check pivot element and throw appropriate exception
00423     if (MaxPivot <= 0) {
00424 #if LU_FAILURE
00425       qucs::exception * e = new qucs::exception (EXCEPTION_PIVOT);
00426       e->setText ("no pivot != 0 found during Doolittle LU decomposition");
00427       e->setData (c);
00428       throw_exception (e);
00429       goto fail;
00430 #else /* insert virtual resistance */
00431       VIRTUAL_RES ("no pivot != 0 found during Doolittle LU decomposition", c);
00432 #endif
00433     }
00434 
00435     // swap matrix rows if necessary and remember that step in the
00436     // exchange table
00437     if (c != pivot) {
00438       A->exchangeRows (c, pivot);
00439       Swap (int, rMap[c], rMap[pivot]);
00440       Swap (nr_double_t, nPvt[c], nPvt[pivot]);
00441     }
00442 
00443     // finally divide by the pivot element
00444     if (c < N - 1) {
00445       f = 1.0 / A_(c, c);
00446       for (r = c + 1; r < N; r++) A_(r, c) *= f;
00447     }
00448   }
00449 #if LU_FAILURE
00450  fail:
00451 #endif
00452 }
00453 
00457 template <class nr_type_t>
00458 void eqnsys<nr_type_t>::substitute_lu_crout (void) {
00459   nr_type_t f;
00460   int i, c;
00461 
00462   // forward substitution in order to solve LY = B
00463   for (i = 0; i < N; i++) {
00464     f = B_(rMap[i]);
00465     for (c = 0; c < i; c++) f -= A_(i, c) * X_(c);
00466     X_(i) = f / A_(i, i);
00467   }
00468 
00469   // backward substitution in order to solve UX = Y
00470   for (i = N - 1; i >= 0; i--) {
00471     f = X_(i);
00472     for (c = i + 1; c < N; c++) f -= A_(i, c) * X_(c);
00473     // remember that the Uii diagonal are ones only in Crout's definition
00474     X_(i) = f;
00475   }
00476 }
00477 
00482 template <class nr_type_t>
00483 void eqnsys<nr_type_t>::substitute_lu_doolittle (void) {
00484   nr_type_t f;
00485   int i, c;
00486 
00487   // forward substitution in order to solve LY = B
00488   for (i = 0; i < N; i++) {
00489     f = B_(rMap[i]);
00490     for (c = 0; c < i; c++) f -= A_(i, c) * X_(c);
00491     // remember that the Lii diagonal are ones only in Doolittle's definition
00492     X_(i) = f;
00493   }
00494 
00495   // backward substitution in order to solve UX = Y
00496   for (i = N - 1; i >= 0; i--) {
00497     f = X_(i);
00498     for (c = i + 1; c < N; c++) f -= A_(i, c) * X_(c);
00499     X_(i) = f / A_(i, i);
00500   }
00501 }
00502 
00510 template <class nr_type_t>
00511 void eqnsys<nr_type_t>::solve_iterative (void) {
00512   nr_type_t f;
00513   int error, conv, i, c, r;
00514   int MaxIter = N; // -> less than N^3 operations
00515   nr_double_t reltol = 1e-4;
00516   nr_double_t abstol = NR_TINY;
00517   nr_double_t diff, crit;
00518 
00519   // ensure that all diagonal values are non-zero
00520   ensure_diagonal ();
00521 
00522   // try to raise diagonal dominance
00523   preconditioner ();
00524 
00525   // decide here about possible convergence
00526   if ((crit = convergence_criteria ()) >= 1) {
00527 #if DEBUG && 0
00528     logprint (LOG_STATUS, "NOTIFY: convergence criteria: %g >= 1 (%dx%d)\n",
00529               crit, N, N);
00530 #endif
00531     //solve_lu ();
00532     //return;
00533   }
00534 
00535   // normalize the equation system to have ones on its diagonal
00536   for (r = 0; r < N; r++) {
00537     f = A_(r, r);
00538     assert (f != 0); // singular matrix
00539     for (c = 0; c < N; c++) A_(r, c) /= f;
00540     B_(r) /= f;
00541   }
00542 
00543   // the current X vector is a good initial guess for the iteration
00544   tvector<nr_type_t> * Xprev = new tvector<nr_type_t> (*X);
00545 
00546   // start iterating here
00547   i = 0; error = 0;
00548   do {
00549     // compute new solution vector
00550     for (r = 0; r < N; r++) {
00551       for (f = 0, c = 0; c < N; c++) {
00552         if (algo == ALGO_GAUSS_SEIDEL) {
00553           // Gauss-Seidel
00554           if (c < r)      f += A_(r, c) * X_(c);
00555           else if (c > r) f += A_(r, c) * Xprev->get (c);
00556         }
00557         else {
00558           // Jacobi
00559           if (c != r) f += A_(r, c) * Xprev->get (c);
00560         }
00561       }
00562       X_(r) = B_(r) - f;
00563     }
00564     // check for convergence
00565     for (conv = 1, r = 0; r < N; r++) {
00566       diff = abs (X_(r) - Xprev->get (r));
00567       if (diff >= abstol + reltol * abs (X_(r))) {
00568         conv = 0;
00569         break;
00570       }
00571       if (!std::isfinite (diff)) { error++; break; }
00572     }
00573     // save last values
00574     *Xprev = *X;
00575   }
00576   while (++i < MaxIter && !conv);
00577 
00578   delete Xprev;
00579 
00580   if (!conv || error) {
00581     logprint (LOG_ERROR,
00582               "WARNING: no convergence after %d %s iterations\n",
00583               i, algo == ALGO_JACOBI ? "jacobi" : "gauss-seidel");
00584     solve_lu_crout ();
00585   }
00586 #if DEBUG && 0
00587   else {
00588     logprint (LOG_STATUS,
00589               "NOTIFY: %s convergence after %d iterations\n",
00590               algo == ALGO_JACOBI ? "jacobi" : "gauss-seidel", i);
00591   }
00592 #endif
00593 }
00594 
00599 template <class nr_type_t>
00600 void eqnsys<nr_type_t>::solve_sor (void) {
00601   nr_type_t f;
00602   int error, conv, i, c, r;
00603   int MaxIter = N; // -> less than N^3 operations
00604   nr_double_t reltol = 1e-4;
00605   nr_double_t abstol = NR_TINY;
00606   nr_double_t diff, crit, l = 1, d, s;
00607 
00608   // ensure that all diagonal values are non-zero
00609   ensure_diagonal ();
00610 
00611   // try to raise diagonal dominance
00612   preconditioner ();
00613 
00614   // decide here about possible convergence
00615   if ((crit = convergence_criteria ()) >= 1) {
00616 #if DEBUG && 0
00617     logprint (LOG_STATUS, "NOTIFY: convergence criteria: %g >= 1 (%dx%d)\n",
00618               crit, N, N);
00619 #endif
00620     //solve_lu ();
00621     //return;
00622   }
00623 
00624   // normalize the equation system to have ones on its diagonal
00625   for (r = 0; r < N; r++) {
00626     f = A_(r, r);
00627     assert (f != 0); // singular matrix
00628     for (c = 0; c < N; c++) A_(r, c) /= f;
00629     B_(r) /= f;
00630   }
00631 
00632   // the current X vector is a good initial guess for the iteration
00633   tvector<nr_type_t> * Xprev = new tvector<nr_type_t> (*X);
00634 
00635   // start iterating here
00636   i = 0; error = 0;
00637   do {
00638     // compute new solution vector
00639     for (r = 0; r < N; r++) {
00640       for (f = 0, c = 0; c < N; c++) {
00641         if (c < r)      f += A_(r, c) * X_(c);
00642         else if (c > r) f += A_(r, c) * Xprev->get (c);
00643       }
00644       X_(r) = (1 - l) * Xprev->get (r) + l * (B_(r) - f);
00645     }
00646     // check for convergence
00647     for (s = 0, d = 0, conv = 1, r = 0; r < N; r++) {
00648       diff = abs (X_(r) - Xprev->get (r));
00649       if (diff >= abstol + reltol * abs (X_(r))) {
00650         conv = 0;
00651         break;
00652       }
00653       d += diff; s += abs (X_(r));
00654       if (!std::isfinite (diff)) { error++; break; }
00655     }
00656     if (!error) {
00657       // adjust relaxation based on average errors
00658       if ((s == 0 && d == 0) || d >= abstol * N + reltol * s) {
00659         // values <= 1 -> non-convergence to convergence
00660         if (l >= 0.6) l -= 0.1;
00661         if (l >= 1.0) l = 1.0;
00662       }
00663       else {
00664         // values >= 1 -> faster convergence
00665         if (l < 1.5) l += 0.01;
00666         if (l < 1.0) l = 1.0;
00667       }
00668     }
00669     // save last values
00670     *Xprev = *X;
00671   }
00672   while (++i < MaxIter && !conv);
00673 
00674   delete Xprev;
00675 
00676   if (!conv || error) {
00677     logprint (LOG_ERROR,
00678               "WARNING: no convergence after %d sor iterations (l = %g)\n",
00679               i, l);
00680     solve_lu_crout ();
00681   }
00682 #if DEBUG && 0
00683   else {
00684     logprint (LOG_STATUS,
00685               "NOTIFY: sor convergence after %d iterations\n", i);
00686   }
00687 #endif
00688 }
00689 
00693 template <class nr_type_t>
00694 nr_double_t eqnsys<nr_type_t>::convergence_criteria (void) {
00695   nr_double_t f = 0;
00696   for (int r = 0; r < A->getCols (); r++) {
00697     for (int c = 0; c < A->getCols (); c++) {
00698       if (r != c) f += norm (A_(r, c) / A_(r, r));
00699     }
00700   }
00701   return sqrt (f);
00702 }
00703 
00707 template <class nr_type_t>
00708 void eqnsys<nr_type_t>::ensure_diagonal (void) {
00709   ensure_diagonal_MNA ();
00710 }
00711 
00718 template <class nr_type_t>
00719 void eqnsys<nr_type_t>::ensure_diagonal_MNA (void) {
00720   int restart, exchanged, begin = 0, pairs;
00721   int pivot1, pivot2, i;
00722   do {
00723     restart = exchanged = 0;
00724     /* search for zero diagonals with lone pairs */
00725     for (i = begin; i < N; i++) {
00726       if (A_(i, i) == 0) {
00727         pairs = countPairs (i, pivot1, pivot2);
00728         if (pairs == 1) { /* lone pair found, substitute rows */
00729           A->exchangeRows (i, pivot1);
00730           B->exchangeRows (i, pivot1);
00731           exchanged = 1;
00732         }
00733         else if ((pairs > 1) && !restart) {
00734           restart = 1;
00735           begin = i;
00736         }
00737       }
00738     }
00739 
00740     /* all lone pairs are gone, look for zero diagonals with multiple pairs */
00741     if (restart) {
00742       for (i = begin; !exchanged && i < N; i++) {
00743         if (A_(i, i) == 0) {
00744           pairs = countPairs (i, pivot1, pivot2);
00745           A->exchangeRows (i, pivot1);
00746           B->exchangeRows (i, pivot1);
00747           exchanged = 1;
00748         }
00749       }
00750     }
00751   }
00752   while (restart);
00753 }
00754 
00757 template <class nr_type_t>
00758 int eqnsys<nr_type_t>::countPairs (int i, int& r1, int& r2) {
00759   int pairs = 0;
00760   for (int r = 0; r < N; r++) {
00761     if (fabs (real (A_(r, i))) == 1.0) {
00762       r1 = r;
00763       pairs++;
00764       for (r++; r < N; r++) {
00765         if (fabs (real (A_(r, i))) == 1.0) {
00766           r2 = r;
00767           if (++pairs >= 2) return pairs;
00768         }
00769       }
00770     }
00771   }
00772   return pairs;
00773 }
00774 
00778 template <class nr_type_t>
00779 void eqnsys<nr_type_t>::preconditioner (void) {
00780   int pivot, r;
00781   nr_double_t MaxPivot;
00782   for (int i = 0; i < N; i++) {
00783     // find maximum column value for pivoting
00784     for (MaxPivot = 0, pivot = i, r = 0; r < N; r++) {
00785       if (abs (A_(r, i)) > MaxPivot &&
00786           abs (A_(i, r)) >= abs (A_(r, r))) {
00787         MaxPivot = abs (A_(r, i));
00788         pivot = r;
00789       }
00790     }
00791     // swap matrix rows if possible
00792     if (i != pivot) {
00793       A->exchangeRows (i, pivot);
00794       B->exchangeRows (i, pivot);
00795     }
00796   }
00797 }
00798 
00799 #define R_ (*R)
00800 #define T_ (*T)
00801 
00804 template <class nr_type_t>
00805 void eqnsys<nr_type_t>::solve_qrh (void) {
00806   factorize_qrh ();
00807   substitute_qrh ();
00808 }
00809 
00812 template <class nr_type_t>
00813 void eqnsys<nr_type_t>::solve_qr (void) {
00814   factorize_qr_householder ();
00815   substitute_qr_householder ();
00816 }
00817 
00821 template <class nr_type_t>
00822 void eqnsys<nr_type_t>::solve_qr_ls (void) {
00823   A->transpose ();
00824   factorize_qr_householder ();
00825   substitute_qr_householder_ls ();
00826 }
00827 
00829 static inline void
00830 euclidian_update (nr_double_t a, nr_double_t& n, nr_double_t& scale) {
00831   nr_double_t x, ax;
00832   if ((x = a) != 0) {
00833     ax = fabs (x);
00834     if (scale < ax) {
00835       x = scale / ax;
00836       n = 1 + n * x * x;
00837       scale = ax;
00838     }
00839     else {
00840       x = ax / scale;
00841       n += x * x;
00842     }
00843   }
00844 }
00845 
00848 template <class nr_type_t>
00849 nr_double_t eqnsys<nr_type_t>::euclidian_c (int c, int r) {
00850   nr_double_t scale = 0, n = 1;
00851   for (int i = r; i < N; i++) {
00852     euclidian_update (real (A_(i, c)), n, scale);
00853     euclidian_update (imag (A_(i, c)), n, scale);
00854   }
00855   return scale * sqrt (n);
00856 }
00857 
00860 template <class nr_type_t>
00861 nr_double_t eqnsys<nr_type_t>::euclidian_r (int r, int c) {
00862   nr_double_t scale = 0, n = 1;
00863   for (int i = c; i < N; i++) {
00864     euclidian_update (real (A_(r, i)), n, scale);
00865     euclidian_update (imag (A_(r, i)), n, scale);
00866   }
00867   return scale * sqrt (n);
00868 }
00869 
00870 template <typename nr_type_t>
00871 inline nr_type_t cond_conj (nr_type_t t) {
00872   return std::conj(t);
00873 }
00874 
00875 template <>
00876 inline double cond_conj (double t) {
00877   return t;
00878 }
00879 
00885 template <class nr_type_t>
00886 void eqnsys<nr_type_t>::factorize_qrh (void) {
00887   int c, r, k, pivot;
00888   nr_type_t f, t;
00889   nr_double_t s, MaxPivot;
00890 
00891   delete R; R = new tvector<nr_type_t> (N);
00892 
00893   for (c = 0; c < N; c++) {
00894     // compute column norms and save in work array
00895     nPvt[c] = euclidian_c (c);
00896     cMap[c] = c; // initialize permutation vector
00897   }
00898 
00899   for (c = 0; c < N; c++) {
00900 
00901     // put column with largest norm into pivot position
00902     MaxPivot = nPvt[c]; pivot = c;
00903     for (r = c + 1; r < N; r++) {
00904       if ((s = nPvt[r]) > MaxPivot) {
00905         pivot = r;
00906         MaxPivot = s;
00907       }
00908     }
00909     if (pivot != c) {
00910       A->exchangeCols (pivot, c);
00911       Swap (int, cMap[pivot], cMap[c]);
00912       Swap (nr_double_t, nPvt[pivot], nPvt[c]);
00913     }
00914 
00915     // compute householder vector
00916     if (c < N) {
00917       nr_type_t a, b;
00918       s = euclidian_c (c, c + 1);
00919       a = A_(c, c);
00920       b = -sign (a) * xhypot (a, s); // Wj
00921       t = xhypot (s, a - b);         // || Vi - Wi ||
00922       R_(c) = b;
00923       // householder vector entries Ui
00924       A_(c, c) = (a - b) / t;
00925       for (r = c + 1; r < N; r++) A_(r, c) /= t;
00926     }
00927     else {
00928       R_(c) = A_(c, c);
00929     }
00930 
00931     // apply householder transformation to remaining columns
00932     for (r = c + 1; r < N; r++) {
00933       for (f = 0, k = c; k < N; k++) f += cond_conj (A_(k, c)) * A_(k, r);
00934       for (k = c; k < N; k++) A_(k, r) -= 2.0 * f * A_(k, c);
00935     }
00936 
00937     // update norms of remaining columns too
00938     for (r = c + 1; r < N; r++) {
00939       nPvt[r] = euclidian_c (r, c + 1);
00940     }
00941   }
00942 }
00943 
00950 template <class nr_type_t>
00951 void eqnsys<nr_type_t>::factorize_qr_householder (void) {
00952   int c, r, pivot;
00953   nr_double_t s, MaxPivot;
00954 
00955   delete T; T = new tvector<nr_type_t> (N);
00956 
00957   for (c = 0; c < N; c++) {
00958     // compute column norms and save in work array
00959     nPvt[c] = euclidian_c (c);
00960     cMap[c] = c; // initialize permutation vector
00961   }
00962 
00963   for (c = 0; c < N; c++) {
00964 
00965     // put column with largest norm into pivot position
00966     MaxPivot = nPvt[c]; pivot = c;
00967     for (r = c + 1; r < N; r++)
00968       if ((s = nPvt[r]) > MaxPivot) {
00969         pivot = r;
00970         MaxPivot = s;
00971       }
00972     if (pivot != c) {
00973       A->exchangeCols (pivot, c);
00974       Swap (int, cMap[pivot], cMap[c]);
00975       Swap (nr_double_t, nPvt[pivot], nPvt[c]);
00976     }
00977 
00978     // compute and apply householder vector
00979     T_(c) = householder_left (c);
00980 
00981     // update norms of remaining columns too
00982     for (r = c + 1; r < N; r++) {
00983       if ((s = nPvt[r]) > 0) {
00984         nr_double_t y = 0;
00985         nr_double_t t = norm (A_(c, r) / s);
00986         if (t < 1)
00987           y = s * sqrt (1 - t);
00988         if (fabs (y / s) < NR_TINY)
00989           nPvt[r] = euclidian_c (r, c + 1);
00990         else
00991           nPvt[r] = y;
00992       }
00993     }
00994   }
00995 }
00996 
00999 template <class nr_type_t>
01000 void eqnsys<nr_type_t>::substitute_qrh (void) {
01001   int c, r;
01002   nr_type_t f;
01003 
01004   // form the new right hand side Q'B
01005   for (c = 0; c < N - 1; c++) {
01006     // scalar product u_k^T * B
01007     for (f = 0, r = c; r < N; r++) f += cond_conj (A_(r, c)) * B_(r);
01008     // z - 2 * f * u_k
01009     for (r = c; r < N; r++) B_(r) -= 2.0 * f * A_(r, c);
01010   }
01011 
01012   // backward substitution in order to solve RX = Q'B
01013   for (r = N - 1; r >= 0; r--) {
01014     f = B_(r);
01015     for (c = r + 1; c < N; c++) f -= A_(r, c) * X_(cMap[c]);
01016     if (abs (R_(r)) > std::numeric_limits<nr_double_t>::epsilon())
01017       X_(cMap[r]) = f / R_(r);
01018     else
01019       X_(cMap[r]) = 0;
01020   }
01021 }
01022 
01025 template <class nr_type_t>
01026 void eqnsys<nr_type_t>::substitute_qr_householder (void) {
01027   int c, r;
01028   nr_type_t f;
01029 
01030   // form the new right hand side Q'B
01031   for (c = 0; c < N; c++) {
01032     if (T_(c) != 0) {
01033       // scalar product u' * B
01034       for (f = B_(c), r = c + 1; r < N; r++) f += cond_conj (A_(r, c)) * B_(r);
01035       // z - T * f * u
01036       f *= cond_conj (T_(c)); B_(c) -= f;
01037       for (r = c + 1; r < N; r++) B_(r) -= f * A_(r, c);
01038     }
01039   }
01040 
01041   // backward substitution in order to solve RX = Q'B
01042   for (r = N - 1; r >= 0; r--) {
01043     for (f = B_(r), c = r + 1; c < N; c++) f -= A_(r, c) * X_(cMap[c]);
01044     if (abs (A_(r, r)) > std::numeric_limits<nr_double_t>::epsilon())
01045       X_(cMap[r]) = f / A_(r, r);
01046     else
01047       X_(cMap[r]) = 0;
01048   }
01049 }
01050 
01055 template <class nr_type_t>
01056 void eqnsys<nr_type_t>::substitute_qr_householder_ls (void) {
01057   int c, r;
01058   nr_type_t f;
01059 
01060   // forward substitution in order to solve R'X = B
01061   for (r = 0; r < N; r++) {
01062     for (f = B_(r), c = 0; c < r; c++) f -= A_(c, r) * B_(c);
01063     if (abs (A_(r, r)) > std::numeric_limits<nr_double_t>::epsilon())
01064       B_(r) = f / A_(r, r);
01065     else
01066       B_(r) = 0;
01067   }
01068 
01069   // compute the least square solution QX
01070   for (c = N - 1; c >= 0; c--) {
01071     if (T_(c) != 0) {
01072       // scalar product u' * B
01073       for (f = B_(c), r = c + 1; r < N; r++) f += cond_conj (A_(r, c)) * B_(r);
01074       // z - T * f * u_k
01075       f *= T_(c); B_(c) -= f;
01076       for (r = c + 1; r < N; r++) B_(r) -= f * A_(r, c);
01077     }
01078   }
01079 
01080   // permute solution vector
01081   for (r = 0; r < N; r++) X_(cMap[r]) = B_(r);
01082 }
01083 
01084 #define sign_(a) (real (a) < 0 ? -1 : 1)
01085 
01092 template <class nr_type_t>
01093 nr_type_t eqnsys<nr_type_t>::householder_create_left (int c) {
01094   nr_type_t a, b, t;
01095   nr_double_t s, g;
01096   s = euclidian_c (c, c + 1);
01097   if (s == 0 && imag (A_(c, c)) == 0) {
01098     // no reflection necessary
01099     t = 0;
01100   }
01101   else {
01102     // calculate householder reflection
01103     a = A_(c, c);
01104     g = sign_(a) * xhypot (a, s);
01105     b = a + g;
01106     t = b / g;
01107     // store householder vector
01108     for (int r = c + 1; r < N; r++) A_(r, c) /= b;
01109     A_(c, c) = -g;
01110   }
01111   return t;
01112 }
01113 
01118 template <class nr_type_t>
01119 nr_type_t eqnsys<nr_type_t>::householder_left (int c) {
01120   // compute householder vector
01121   nr_type_t t = householder_create_left (c);
01122   // apply householder transformation to remaining columns if necessary
01123   if (t != 0) {
01124     householder_apply_left (c, t);
01125   }
01126   return t;
01127 }
01128 
01133 template <class nr_type_t>
01134 nr_type_t eqnsys<nr_type_t>::householder_right (int r) {
01135   // compute householder vector
01136   nr_type_t t = householder_create_right (r);
01137   // apply householder transformation to remaining rows if necessary
01138   if (t != 0) {
01139     householder_apply_right (r, t);
01140   }
01141   return t;
01142 }
01143 
01150 template <class nr_type_t>
01151 nr_type_t eqnsys<nr_type_t>::householder_create_right (int r) {
01152   nr_type_t a, b, t;
01153   nr_double_t s, g;
01154   s = euclidian_r (r, r + 2);
01155   if (s == 0 && imag (A_(r, r + 1)) == 0) {
01156     // no reflection necessary
01157     t = 0;
01158   }
01159   else {
01160     // calculate householder reflection
01161     a = A_(r, r + 1);
01162     g = sign_(a) * xhypot (a, s);
01163     b = a + g;
01164     t = b / g;
01165     // store householder vector
01166     for (int c = r + 2; c < N; c++) A_(r, c) /= b;
01167     A_(r, r + 1) = -g;
01168   }
01169   return t;
01170 }
01171 
01174 template <class nr_type_t>
01175 void eqnsys<nr_type_t>::householder_apply_left (int c, nr_type_t t) {
01176   nr_type_t f;
01177   int k, r;
01178   // apply the householder vector to each right-hand column
01179   for (r = c + 1; r < N; r++) {
01180     // calculate f = u' * A (a scalar product)
01181     f = A_(c, r);
01182     for (k = c + 1; k < N; k++) f += cond_conj (A_(k, c)) * A_(k, r);
01183     // calculate A -= T * f * u
01184     f *= cond_conj (t); A_(c, r) -= f;
01185     for (k = c + 1; k < N; k++) A_(k, r) -= f * A_(k, c);
01186   }
01187 }
01188 
01191 template <class nr_type_t>
01192 void eqnsys<nr_type_t>::householder_apply_right (int r, nr_type_t t) {
01193   nr_type_t f;
01194   int k, c;
01195   // apply the householder vector to each downward row
01196   for (c = r + 1; c < N; c++) {
01197     // calculate f = u' * A (a scalar product)
01198     f = A_(c, r + 1);
01199     for (k = r + 2; k < N; k++) f += cond_conj (A_(r, k)) * A_(c, k);
01200     // calculate A -= T * f * u
01201     f *= cond_conj (t); A_(c, r + 1) -= f;
01202     for (k = r + 2; k < N; k++) A_(c, k) -= f * A_(r, k);
01203   }
01204 }
01205 
01206 // Some helper defines for SVD.
01207 #define V_ (*V)
01208 #define S_ (*S)
01209 #define E_ (*E)
01210 #define U_ (*A)
01211 
01215 template <class nr_type_t>
01216 void eqnsys<nr_type_t>::householder_apply_right_extern (int r, nr_type_t t) {
01217   nr_type_t f;
01218   int k, c;
01219   // apply the householder vector to each downward row
01220   for (c = r + 1; c < N; c++) {
01221     // calculate f = u' * A (a scalar product)
01222     f = V_(c, r + 1);
01223     for (k = r + 2; k < N; k++) f += cond_conj (A_(r, k)) * V_(c, k);
01224     // calculate A -= T * f * u
01225     f *= cond_conj (t); V_(c, r + 1) -= f;
01226     for (k = r + 2; k < N; k++) V_(c, k) -= f * A_(r, k);
01227   }
01228 }
01229 
01232 template <class nr_type_t>
01233 void eqnsys<nr_type_t>::solve_svd (void) {
01234   factorize_svd ();
01235   chop_svd ();
01236   substitute_svd ();
01237 }
01238 
01240 template <class nr_type_t>
01241 void eqnsys<nr_type_t>::chop_svd (void) {
01242   int c;
01243   nr_double_t Max, Min;
01244   Max = 0.0;
01245   for (c = 0; c < N; c++) if (fabs (S_(c)) > Max) Max = fabs (S_(c));
01246   Min = Max * std::numeric_limits<nr_double_t>::max();
01247   for (c = 0; c < N; c++) if (fabs (S_(c)) < Min) S_(c) = 0.0;
01248 }
01249 
01253 template <class nr_type_t>
01254 void eqnsys<nr_type_t>::substitute_svd (void) {
01255   int c, r;
01256   nr_type_t f;
01257   // calculate U'B
01258   for (c = 0; c < N; c++) {
01259     f = 0.0;
01260     // non-zero result only if S is non-zero
01261     if (S_(c) != 0.0) {
01262       for (r = 0; r < N; r++) f += cond_conj (U_(r, c)) * B_(r);
01263       // this is the divide by S
01264       f /= S_(c);
01265     }
01266     R_(c) = f;
01267   }
01268   // matrix multiply by V to get the final solution
01269   for (r = 0; r < N; r++) {
01270     for (f = 0.0, c = 0; c < N; c++) f += cond_conj (V_(c, r)) * R_(c);
01271     X_(r) = f;
01272   }
01273 }
01274 
01279 template <class nr_type_t>
01280 void eqnsys<nr_type_t>::factorize_svd (void) {
01281   int i, j, l;
01282   nr_type_t t;
01283 
01284   // allocate space for vectors and matrices
01285   delete R; R = new tvector<nr_type_t> (N);
01286   delete T; T = new tvector<nr_type_t> (N);
01287   delete V; V = new tmatrix<nr_type_t> (N);
01288   delete S; S = new tvector<nr_double_t> (N);
01289   delete E; E = new tvector<nr_double_t> (N);
01290 
01291   // bidiagonalization through householder transformations
01292   for (i = 0; i < N; i++) {
01293     T_(i) = householder_left (i);
01294     if (i < N - 1) R_(i) = householder_right (i);
01295   }
01296 
01297   // copy over the real valued bidiagonal values
01298   for (i = 0; i < N; i++) S_(i) = real (A_(i, i));
01299   for (E_(0) = 0, i = 1; i < N; i++) E_(i) = real (A_(i - 1, i));
01300 
01301   // backward accumulation of right-hand householder transformations
01302   // yields the V' matrix
01303   for (l = N, i = N - 1; i >= 0; l = i--) {
01304     if (i < N - 1) {
01305       if ((t = R_(i)) != 0.0) {
01306         householder_apply_right_extern (i, cond_conj (t));
01307       }
01308       else for (j = l; j < N; j++) // cleanup this row
01309         V_(i, j) = V_(j, i) = 0.0;
01310     }
01311     V_(i, i) = 1.0;
01312   }
01313 
01314   // backward accumulation of left-hand householder transformations
01315   // yields the U matrix in place of the A matrix
01316   for (l = N, i = N - 1; i >= 0; l = i--) {
01317     for (j = l; j < N; j++) // cleanup upper row
01318       A_(i, j) = 0.0;
01319     if ((t = T_(i)) != 0.0) {
01320       householder_apply_left (i, cond_conj (t));
01321       for (j = l; j < N; j++) A_(j, i) *= -t;
01322     }
01323     else for (j = l; j < N; j++) // cleanup this column
01324       A_(j, i) = 0.0;
01325     A_(i, i) = 1.0 - t;
01326   }
01327 
01328   // S and E contain diagonal and super-diagonal, A contains U, V'
01329   // calculated; now diagonalization can begin
01330   diagonalize_svd ();
01331 }
01332 
01333 #ifndef MAX
01334 # define MAX(x,y) (((x) > (y)) ? (x) : (y))
01335 #endif
01336 
01338 static inline nr_double_t
01339 givens (nr_double_t a, nr_double_t b, nr_double_t& c, nr_double_t& s) {
01340   nr_double_t z = xhypot (a, b);
01341   c = a / z;
01342   s = b / z;
01343   return z;
01344 }
01345 
01346 template <class nr_type_t>
01347 void eqnsys<nr_type_t>::givens_apply_u (int c1, int c2,
01348                                         nr_double_t c, nr_double_t s) {
01349   for (int i = 0; i < N; i++) {
01350     nr_type_t y = U_(i, c1);
01351     nr_type_t z = U_(i, c2);
01352     U_(i, c1) = y * c + z * s;
01353     U_(i, c2) = z * c - y * s;
01354   }
01355 }
01356 
01357 template <class nr_type_t>
01358 void eqnsys<nr_type_t>::givens_apply_v (int r1, int r2,
01359                                         nr_double_t c, nr_double_t s) {
01360   for (int i = 0; i < N; i++) {
01361     nr_type_t y = V_(r1, i);
01362     nr_type_t z = V_(r2, i);
01363     V_(r1, i) = y * c + z * s;
01364     V_(r2, i) = z * c - y * s;
01365   }
01366 }
01367 
01373 template <class nr_type_t>
01374 void eqnsys<nr_type_t>::diagonalize_svd (void) {
01375   bool split;
01376   int i, l, j, its, k, n, MaxIters = 30;
01377   nr_double_t an, f, g, h, d, c, s, b, a;
01378 
01379   // find largest bidiagonal value
01380   for (an = 0, i = 0; i < N; i++)
01381     an = MAX (an, fabs (S_(i)) + fabs (E_(i)));
01382 
01383   // diagonalize the bidiagonal matrix (stored as super-diagonal
01384   // vector E and diagonal vector S)
01385   for (k = N - 1; k >= 0; k--) {
01386     // loop over singular values
01387     for (its = 0; its <= MaxIters; its++) {
01388       split = true;
01389       // check for a zero entry along the super-diagonal E, if there
01390       // is one, it is possible to QR iterate on two separate matrices
01391       // above and below it
01392       for (n = 0, l = k; l >= 1; l--) {
01393         // note that E_(0) is always zero
01394         n = l - 1;
01395         if (fabs (E_(l)) + an == an) { split = false; break; }
01396         if (fabs (S_(n)) + an == an) break;
01397       }
01398       // if there is a zero on the diagonal S, it is possible to zero
01399       // out the corresponding super-diagonal E entry to its right
01400       if (split) {
01401         // cancellation of E_(l), if l > 0
01402         c = 0.0;
01403         s = 1.0;
01404         for (i = l; i <= k; i++) {
01405           f = -s * E_(i);
01406           E_(i) *= c;
01407           if (fabs (f) + an == an) break;
01408           g = S_(i);
01409           S_(i) = givens (f, g, c, s);
01410           // apply inverse rotation to U
01411           givens_apply_u (n, i, c, s);
01412         }
01413       }
01414 
01415       d = S_(k);
01416       // convergence
01417       if (l == k) {
01418         // singular value is made non-negative
01419         if (d < 0.0) {
01420           S_(k) = -d;
01421           for (j = 0; j < N; j++) V_(k, j) = -V_(k, j);
01422         }
01423         break;
01424       }
01425       if (its == MaxIters) {
01426         logprint (LOG_ERROR, "WARNING: no convergence in %d SVD iterations\n",
01427                   MaxIters);
01428       }
01429 
01430       // shift from bottom 2-by-2 minor
01431       a = S_(l);
01432       n = k - 1;
01433       b = S_(n);
01434       g = E_(n);
01435       h = E_(k);
01436 
01437       // compute QR shift value (as close as possible to the largest
01438       // eigenvalue of the 2-by-2 minor matrix)
01439       f  = (b - d) * (b + d) + (g - h) * (g + h);
01440       f /= 2.0 * h * b;
01441       f += sign_(f) * xhypot (f, 1.0);
01442       f  = ((a - d) * (a + d) + h * (b / f - h)) / a;
01443       // f => (B_{ll}^2 - u) / B_{ll}
01444       // u => eigenvalue of T = B' * B nearer T_{22} (Wilkinson shift)
01445 
01446       // next QR transformation
01447       c = s = 1.0;
01448       for (j = l; j <= n; j++) {
01449         i = j + 1;
01450         g = E_(i);
01451         b = S_(i);
01452         h = s * g; // h => right-hand non-zero to annihilate
01453         g *= c;
01454         E_(j) = givens (f, h, c, s);
01455         // perform the rotation
01456         f = a * c + g * s;
01457         g = g * c - a * s;
01458         h = b * s;
01459         b *= c;
01460         // here: +-   -+
01461         //       | f g | = B * V'_j (also first V'_1)
01462         //       | h b |
01463         //       +-   -+
01464 
01465         // accumulate the rotation in V'
01466         givens_apply_v (j, i, c, s);
01467         d = S_(j) = xhypot (f, h);
01468         // rotation can be arbitrary if d = 0
01469         if (d != 0.0) {
01470           // d => non-zero result on diagonal
01471           d = 1.0 / d;
01472           // rotation coefficients to annihilate the lower non-zero
01473           c = f * d;
01474           s = h * d;
01475         }
01476         f = c * g + s * b;
01477         a = c * b - s * g;
01478         // here: +-   -+
01479         //       | d f | => U_j * B
01480         //       | 0 a |
01481         //       +-   -+
01482 
01483         // accumulate rotation into U
01484         givens_apply_u (j, i, c, s);
01485       }
01486       E_(l) = 0;
01487       E_(k) = f;
01488       S_(k) = a;
01489     }
01490   }
01491 }
01492 
01493 } // namespace qucs
01494 
01495 #undef V_