Marsyas
0.6.0-alpha
|
00001 /* 00002 ** Copyright (C) 1998-2010 George Tzanetakis <gtzan@cs.uvic.ca> 00003 ** 00004 ** This program is free software; you can redistribute it and/or modify 00005 ** it under the terms of the GNU General Public License as published by 00006 ** the Free Software Foundation; either version 2 of the License, or 00007 ** (at your option) any later version. 00008 ** 00009 ** This program is distributed in the hope that it will be useful, 00010 ** but WITHOUT ANY WARRANTY; without even the implied warranty of 00011 ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00012 ** GNU General Public License for more details. 00013 ** 00014 ** You should have received a copy of the GNU General Public License 00015 ** along with this program; if not, write to the Free Software 00016 ** Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. 00017 */ 00018 00019 #include <marsyas/common_source.h> 00020 #include <marsyas/NumericLib.h> 00021 00022 //************************************************************* 00023 // Includes for determinant calculation (adapted from): 00024 // http://perso.wanadoo.fr/jean-pierre.moreau/c_matrices.html 00025 //************************************************************* 00026 #include <marsyas/basis.h> 00027 #include "vmblock.h" 00028 #include "lu.h" 00029 //************************************************************* 00030 00031 #include <cfloat> 00032 #include <limits> 00033 #include <stdlib.h> 00034 #include <iostream> 00035 #include <fstream> 00036 #include <sstream> 00037 #include <algorithm> 00038 00039 using std::ostringstream; 00040 using std::numeric_limits; 00041 using std::endl; 00042 using std::cout; 00043 using std::cerr; 00044 using std::min; 00045 using std::max; 00046 00047 00048 00049 using namespace Marsyas; 00050 00051 //#define DBL_EPSILON 2.2204460492503131E-16 //=> already defined in float.h... 00052 //#define DBL_MAX 1.7976931348623157E+308 //=> already defined in float.h... 00053 //#define PI 3.14159265358979323846 /* circular transcendental nb. */ //already defined in common_source.h 00054 00055 #define NumericLib_MAXCOEFF 5001 /* max. number of coefficients */ 00056 00057 #define NumLib_TRUE 1 00058 #define NumLib_FALSE 0 00059 00060 //macros for complex operations (to avoid modifying the legacy code...) 00061 //Use STL <complex> algorithms instead of proprietary (and now commented) methods 00062 #define Cadd(a,b) (a+b) 00063 #define Csub(a,b) (a-b) 00064 #define Cmul(a,b) (a*b) 00065 #define Cdiv(a,b) (a/b) 00066 #define Ccomplex(a,b) mrs_complex(a,b) 00067 #define Cabs(z) std::abs(z) 00068 #define Carg(z) std::arg(z) 00069 #define Conjg(z) std::conj(z) 00070 #define Csqrt(z) std::sqrt(z) 00071 #define RCmul(x,a) (x*a) 00072 #define RCadd(x,a) (x+a) 00073 #define CADD(x,a,b) {x = a + b;} //due to some legacy code... 00074 #define CMUL(x,a,b) {x = a * b;} //due to some legacy code... 00075 #define COMPLEXM(x,a,b) {x = mrs_complex(a,b);} //due to some legacy code... 00076 00077 //MULLER macros 00078 #define ITERMAXMULLER 150 /* max. number of iteration steps */ 00079 #define CONVERGENCE 100/* halve q2, when |P(x2)/P(x1)|^2 > CONVERGENCE */ 00080 #define MAXDIST 1e3 /* max. relative change of distance between */ 00081 /* x-values allowed in one step */ 00082 #define FACTORMULLER 1e5 /* if |f2|<FACTOR*macc and (x2-x1)/x2<FACTOR*macc */ 00083 /* then root is determined; end routine */ 00084 #define KITERMAX 1e3 /* halve distance between old and new x2 max. */ 00085 /* KITERMAX times in case of possible overflow */ 00086 #define FVALUEMULLER 1e36 /* initialisation of |P(x)|^2 */ 00087 #define BOUND1 1.01 /* improve convergence in case of small changes */ 00088 #define BOUND2 0.99 /* of |P(x)|^2 */ 00089 #define BOUND3 0.01 00090 #define BOUND4 sqrt(DBL_MAX)/1e4 /* if |P(x2).real()|+|P(x2).i|>BOUND4 => */ 00091 /* suppress overflow of |P(x2)|^2 */ 00092 #define BOUND6 log10(BOUND4)-4 /* if |x2|^nred>10^BOUND6 => */ 00093 /* suppress overflow of P(x2) */ 00094 #define BOUND7 1e-5 /* relative distance between determined root and */ 00095 /* real root bigger than BOUND7 => 2. iteration */ 00096 #define NOISESTART DBL_EPSILON*1e2 /* when noise starts counting */ 00097 #define NOISEMAX 5 /* if noise>NOISEMAX: terminate iteration */ 00098 00099 //NEWTON macros 00100 #define ITERMAXNEWTON 20 /* max. number of iterations */ 00101 #define FACTORNEWTON 5 /* calculate new dx, when change of x0 is smaller */ 00102 /* than FACTOR*(old change of x0) */ 00103 #define FVALUENEWTON 1E36 /* initialisation of |P(xmin)| */ 00104 #define BOUND sqrt(DBL_EPSILON) 00105 /* if the imaginary part of the root is smaller */ 00106 /* than BOUND5 => real root */ 00107 #define NOISEMAX 5 /* max. number of iterations with no better value */ 00108 00109 00110 NumericLib::NumericLib() 00111 { 00112 00113 } 00114 00115 NumericLib::~NumericLib() 00116 { 00117 00118 } 00119 00120 bool 00121 NumericLib::polyRoots(std::vector<mrs_complex> coefs, bool complexCoefs, mrs_natural order, std::vector<mrs_complex> &roots) 00122 { 00123 // complexCoefs == true => complex coefficients (UNTESTED!!) [!] 00124 // complexCoefs == false => real coefficients 00125 00126 unsigned char error; 00127 mrs_real maxerr; 00128 mrs_complex* pred = new mrs_complex[NumericLib_MAXCOEFF];//coeff. vector for deflated polynom (null() just needs it...) 00129 00130 //determine polynomial roots 00131 error = null(&coefs[0], pred, &order, &roots[0], &maxerr, complexCoefs); 00132 00133 delete [] pred; 00134 00135 if (error==0) 00136 return true; 00137 else 00138 { 00139 MRSERR("NumericLib::polyRoots() - numeric error in polynomial roots calculation!"); 00140 return false; 00141 } 00142 } 00143 00144 mrs_real 00145 NumericLib::determinant(realvec matrix) 00146 { 00147 //******************************************************************** 00148 // Determinant calculation adapted by <lmartins@inescporto.pt> from: 00149 // http://perso.orange.fr/jean-pierre.moreau/Cplus/deter1_cpp.txt 00150 //******************************************************************** 00151 00152 //input matrix must be square 00153 if(matrix.getCols() != matrix.getRows()) 00154 { 00155 MRSERR("NumericLib::determinant() : input matrix should be square! Returning invalid determinant value..."); 00156 return numeric_limits<mrs_real>::max(); 00157 } 00158 00159 /**************************************************** 00160 * Calculate the determinant of a real square matrix * 00161 * by the LU decomposition method * 00162 * ------------------------------------------------- * 00163 * SAMPLE RUN: * 00164 * (Calculate the determinant of real square matrix: * 00165 * | 1 0.5 0 0 0 0 0 0 0 | * 00166 * |-1 0.5 1 0 0 0 0 0 0 | * 00167 * | 0 -1 0 2.5 0 0 0 0 0 | * 00168 * | 0 0 -1 -1.5 4 0 0 0 0 | * 00169 * | 0 0 0 -1 -3 5.5 0 0 0 | * 00170 * | 0 0 0 0 -1 -4.5 7 0 0 | * 00171 * | 0 0 0 0 0 -1 -6 8.5 0 | * 00172 * | 0 0 0 0 0 0 -1 -7.5 10 | * 00173 * | 0 0 0 0 0 0 0 -1 -9 | ) * 00174 * * 00175 * * 00176 * Determinant = 1.000000 * 00177 * * 00178 * * 00179 * C++ version by J-P Moreau, Paris. * 00180 ***************************************************** 00181 Exact value is: 1 00182 ---------------------------------------------------*/ 00183 REAL **A; // input matrix of size n x n 00184 int *INDX; // dummy integer vector 00185 void *vmblock = NULL; 00186 00187 // NOTE: index zero not used here. 00188 00189 int i, id, j, n, rc; 00190 REAL det; 00191 00192 //n=9; 00193 n = matrix.getCols(); 00194 00195 vmblock = vminit(); 00196 A = (REAL **)vmalloc(vmblock, MATRIX, n+1, n+1); 00197 INDX = (int *) vmalloc(vmblock, VEKTOR, n+1, 0); 00198 00199 if (! vmcomplete(vmblock)) 00200 { 00201 MRSERR("NumericLib::determinant() : No memory! Returning invalid determinant value..."); 00202 return numeric_limits<mrs_real>::max(); 00203 } 00204 00205 for (i=0; i<=n; ++i) 00206 for (j=0; j<=n; j++) 00207 A[i][j] = 0.0; 00208 00209 // define matrix A column by column 00210 for (i=1; i<=n; ++i) 00211 for (j=1; j<=n; j++) 00212 A[i][j] = (REAL)matrix(i-1, j-1); 00213 00214 // A[1][1]=1.0; A[2][1]=-1.0; 00215 // A[1][2]=0.5; A[2][2]=0.5; A[3][2]=-1.0; 00216 // A[2][3]=1.0; A[4][3]=-1.0; 00217 // A[3][4]=2.5; A[4][4]=-1.5; A[5][4]=-1.0; 00218 // A[4][5]=4.0; A[5][5]=-3.0; A[6][5]=-1.0; 00219 // A[5][6]=5.5; A[6][6]=-4.5; A[7][6]=-1.0; 00220 // A[6][7]=7.0; A[7][7]=-6.0; A[8][7]=-1.0; 00221 // A[7][8]=8.5; A[8][8]=-7.5; A[9][8]=-1.0; 00222 // A[8][9]=10.0; A[9][9]=-9.0; 00223 // 00224 //call LU decomposition 00225 rc = LUDCMP(A,n,INDX,&id); 00226 00227 //calculate determinant 00228 det = id; 00229 if (rc==0) { 00230 for (i=1; i<=n; ++i) 00231 det *= A[i][i]; 00232 return (mrs_real)det; 00233 } 00234 else { 00235 if(rc==-1) 00236 { 00237 MRSERR("NumericLib::determinant() : Memory Allocation error in LUDCMP()! Returning invalid determinant value..."); 00238 return numeric_limits<mrs_real>::max(); 00239 } 00240 else 00241 { 00242 MRSWARN("NumericLib::determinant() : Error in LU decomposition: singular input matrix. Determinant equals to zero."); 00243 return 0.0; // det(singular matrix) = 0 00244 } 00245 } 00246 } 00247 00248 //************************************************************************************************ 00249 // PRIVATE METHODS 00250 //************************************************************************************************ 00251 00252 // MULLER 00253 /******************************************************************/ 00254 /* */ 00255 /* file: muller.c */ 00256 /* */ 00257 /* main function: muller() */ 00258 /* */ 00259 /* version: 1.2 */ 00260 /* */ 00261 /* author: B. Frenzel */ 00262 /* */ 00263 /* date: Jan 7 1993 */ 00264 /* */ 00265 /* input: pred[] coefficient vector of the deflated */ 00266 /* polynomial */ 00267 /* nred the highest exponent of the deflated */ 00268 /* polynomial */ 00269 /* */ 00270 /* return: xb determined root */ 00271 /* */ 00272 /* subroutines: initialize(),root_of_parabola(), */ 00273 /* iteration_equation(), compute_function(), */ 00274 /* check_x_value(), root_check() */ 00275 /* */ 00276 /* description: */ 00277 /* muller() determines the roots of a polynomial with complex */ 00278 /* coefficients with the Muller method; these roots are the */ 00279 /* initial estimations for the following Newton method */ 00280 /* */ 00281 /* Copyright: */ 00282 /* Lehrstuhl fuer Nachrichtentechnik Erlangen */ 00283 /* Cauerstr. 7, 91054 Erlangen, FRG, 1993 */ 00284 /* e-mail: int@nt.e-technik.uni-erlangen.de */ 00285 /* */ 00286 /******************************************************************/ 00287 00288 /***** main routine of Mueller's method *****/ 00289 mrs_complex 00290 NumericLib::muller(mrs_complex *pred,mrs_natural nred) 00291 /*mrs_complex *pred; coefficient vector of the deflated polynomial */ 00292 /*mrs_natural nred; the highest exponent of the deflated polynomial */ 00293 { 00294 mrs_real f1absq=FVALUEMULLER, /* f1absq=|f1MULLER_|^2 */ 00295 f2absq=FVALUEMULLER, /* f2absq=|f2MULLER_|^2 */ 00296 f2absqb=FVALUEMULLER, /* f2absqb=|P(xb)|^2 */ 00297 h2abs, /* h2abs=|h2MULLER_| */ 00298 epsilon; /* bound for |q2MULLER_| */ 00299 mrs_natural seconditer=0, /* second iteration, when root is too bad */ 00300 noise=0, /* noise counter */ 00301 rootd=NumLib_FALSE; /* rootd = TRUE => root determined */ 00302 /* rootd = FALSE => no root determined */ 00303 mrs_complex xb; /* best x-value */ 00304 00305 /* initializing routine */ 00306 initialize(pred,&xb,&epsilon); 00307 00308 fdvalue(pred,nred,&f0MULLER_,&f0MULLER_,x0MULLER_,NumLib_FALSE); /* compute exact function value */ 00309 fdvalue(pred,nred,&f1MULLER_,&f1MULLER_,x1MULLER_,NumLib_FALSE); /* oct-29-1993 ml */ 00310 fdvalue(pred,nred,&f2MULLER_,&f2MULLER_,x2MULLER_,NumLib_FALSE); 00311 00312 do { /* loop for possible second iteration */ 00313 do { /* main iteration loop */ 00314 /* calculate the roots of the parabola */ 00315 root_of_parabola(); 00316 00317 /* store values for the next iteration */ 00318 x0MULLER_ = x1MULLER_; 00319 x1MULLER_ = x2MULLER_; 00320 h2abs = Cabs(h2MULLER_); /* distance between x2MULLER_ and x1MULLER_ */ 00321 00322 /* main iteration-equation */ 00323 iteration_equation(&h2abs); 00324 00325 /* store values for the next iteration */ 00326 f0MULLER_ = f1MULLER_; 00327 f1MULLER_ = f2MULLER_; 00328 f1absq = f2absq; 00329 00330 /* compute P(x2MULLER_) and make some checks */ 00331 compute_function(pred,nred,f1absq,&f2absq,epsilon); 00332 00333 /* printf("betrag %10.5e %4.2d %4.2d\n",f2absq,iterMULLER_,seconditer);*/ 00334 00335 /* is the new x-value the best approximation? */ 00336 check_x_value(&xb,&f2absqb,&rootd,f1absq,f2absq, 00337 epsilon,&noise); 00338 00339 /* increase noise counter */ 00340 if (fabs((Cabs(xb)-Cabs(x2MULLER_))/Cabs(xb))<NOISESTART) 00341 noise++; 00342 } while ((iterMULLER_<=ITERMAXMULLER) && (!rootd) && (noise<=NOISEMAX)); 00343 00344 seconditer++; /* increase seconditer */ 00345 00346 /* check, if determined root is good enough */ 00347 root_check(pred,nred,f2absqb,&seconditer,&rootd,&noise,xb); 00348 } while (seconditer==2); 00349 00350 return xb;/* return best x value*/ 00351 } 00352 00353 /***** initializing routine *****/ 00354 void 00355 NumericLib::initialize(mrs_complex *pred,mrs_complex *xb,mrs_real *epsilon) 00356 /*mrs_complex *pred, coefficient vector of the deflated polynomial */ 00357 /* *xb; best x-value */ 00358 /*mrs_real *epsilon; bound for |q2MULLER_| */ 00359 { 00360 // FIXME Unused parameter; 00361 (void) pred; 00362 /* initial estimations for x0MULLER_,...,x2MULLER_ and its values */ 00363 /* ml, 12-21-94 changed */ 00364 00365 x0MULLER_ = Ccomplex(0.,0.); /* x0MULLER_ = 0 + j*1 */ 00366 x1MULLER_ = Ccomplex(-1./sqrt(2.0),-1./sqrt(2.0)); /* x1MULLER_ = 0 - j*1 */ 00367 x2MULLER_ = Ccomplex(1./sqrt(2.0),1./sqrt(2.0)); /* x2MULLER_ = (1 + j*1)/sqrt(2) */ 00368 00369 h1MULLER_ = Csub(x1MULLER_,x0MULLER_); /* h1MULLER_ = x1MULLER_ - x0MULLER_ */ 00370 h2MULLER_ = Csub(x2MULLER_,x1MULLER_); /* h2MULLER_ = x2MULLER_ - x1MULLER_ */ 00371 q2MULLER_ = Cdiv(h2MULLER_,h1MULLER_); /* q2MULLER_ = h2MULLER_/h1MULLER_ */ 00372 00373 *xb = x2MULLER_; /* best initial x-value = zero */ 00374 *epsilon = FACTORMULLER*DBL_EPSILON;/* accuracy for determined root */ 00375 iterMULLER_ = 0; /* reset iteration counter */ 00376 } 00377 00378 /***** root_of_parabola() calculate smaller root of Muller's parabola *****/ 00379 void 00380 NumericLib::root_of_parabola(void) 00381 { 00382 mrs_complex A2,B2,C2, /* variables to get q2MULLER_ */ 00383 discr, /* discriminante */ 00384 N1,N2; /* denominators of q2MULLER_ */ 00385 00386 /* A2 = q2MULLER_(f2MULLER_ - (1+q2MULLER_)f1MULLER_ + f0q2) */ 00387 /* B2 = q2MULLER_[q2MULLER_(f0MULLER_-f1MULLER_) + 2(f2MULLER_-f1MULLER_)] + (f2MULLER_-f1MULLER_)*/ 00388 /* C2 = (1+q2MULLER_)f[2]*/ 00389 A2 = Cmul(q2MULLER_,Csub(Cadd(f2MULLER_,Cmul(q2MULLER_,f0MULLER_)), 00390 Cmul(f1MULLER_,RCadd((mrs_real)1.,q2MULLER_)))); 00391 B2 = Cadd(Csub(f2MULLER_,f1MULLER_),Cmul(q2MULLER_,Cadd(Cmul(q2MULLER_, 00392 Csub(f0MULLER_,f1MULLER_)),RCmul((mrs_real)2.,Csub(f2MULLER_,f1MULLER_))))); 00393 C2 = Cmul(f2MULLER_,RCadd((mrs_real)1.,q2MULLER_)); 00394 /* discr = B2^2 - 4A2C2 */ 00395 discr = Csub(Cmul(B2,B2),RCmul((mrs_real)4.,Cmul(A2,C2))); 00396 /* denominators of q2MULLER_ */ 00397 N1 = Csub(B2,Csqrt(discr)); 00398 N2 = Cadd(B2,Csqrt(discr)); 00399 /* choose denominater with largest modulus */ 00400 if (Cabs(N1)>Cabs(N2) && Cabs(N1)>DBL_EPSILON) 00401 q2MULLER_ = Cdiv(RCmul((mrs_real)-2.,C2),N1); 00402 else if (Cabs(N2)>DBL_EPSILON) 00403 q2MULLER_ = Cdiv(RCmul((mrs_real)-2.,C2),N2); 00404 else 00405 q2MULLER_ = Ccomplex(cos((mrs_real)iterMULLER_),sin((mrs_real)iterMULLER_)); 00406 } 00407 00408 /***** main iteration equation: x2MULLER_ = h2MULLER_*q2MULLER_ + x2MULLER_ *****/ 00409 void 00410 NumericLib::iteration_equation(mrs_real *h2abs) 00411 /*mrs_real *h2abs; Absolute value of the old distance*/ 00412 { 00413 mrs_real h2absnew, /* Absolute value of the new h2MULLER_ */ 00414 help; /* help variable */ 00415 00416 h2MULLER_ = Cmul(h2MULLER_,q2MULLER_); 00417 h2absnew = Cabs(h2MULLER_); /* distance between old and new x2MULLER_ */ 00418 00419 if (h2absnew > (*h2abs*MAXDIST)) { /* maximum relative change */ 00420 help = MAXDIST/h2absnew; 00421 h2MULLER_ = RCmul(help,h2MULLER_); 00422 q2MULLER_ = RCmul(help,q2MULLER_); 00423 } 00424 00425 *h2abs = h2absnew; /* actualize old distance for next iteration*/ 00426 00427 x2MULLER_ = Cadd(x2MULLER_,h2MULLER_); 00428 } 00429 00430 /**** suppress overflow *****/ 00431 void 00432 NumericLib::suppress_overflow(mrs_natural nred) 00433 /*mrs_natural nred; the highest exponent of the deflated polynomial */ 00434 { 00435 mrs_natural kiter; /* internal iteration counter */ 00436 unsigned char loop; /* loop = FALSE => terminate loop */ 00437 mrs_real help; /* help variable */ 00438 00439 kiter = 0; /* reset iteration counter */ 00440 do { 00441 loop=NumLib_FALSE; /* initial estimation: no overflow */ 00442 help = Cabs(x2MULLER_); /* help = |x2MULLER_| */ 00443 if (help>1. && fabs(nred*log10(help))>BOUND6) { 00444 kiter++; /* if |x2MULLER_|>1 and |x2MULLER_|^nred>10^BOUND6 */ 00445 if (kiter<KITERMAX) { /* then halve the distance between */ 00446 h2MULLER_=RCmul((mrs_real).5,h2MULLER_); /* new and old x2MULLER_ */ 00447 q2MULLER_=RCmul((mrs_real).5,q2MULLER_); 00448 x2MULLER_=Csub(x2MULLER_,h2MULLER_); 00449 loop=NumLib_TRUE; 00450 } else 00451 kiter=0; 00452 } 00453 } while(loop); 00454 } 00455 00456 /***** check of too big function values *****/ 00457 void 00458 NumericLib::too_big_functionvalues(mrs_real *f2absq) 00459 /*mrs_real *f2absq; f2absq=|f2MULLER_|^2 */ 00460 { 00461 if ((fabs(f2MULLER_.real())+fabs(f2MULLER_.imag()))>BOUND4) /* limit |f2MULLER_|^2, when */ 00462 *f2absq = fabs(f2MULLER_.real())+fabs(f2MULLER_.imag()); /* |f2MULLER_.real()|+|f2MULLER_.imag()|>BOUND4 */ 00463 else 00464 *f2absq = (f2MULLER_.real())*(f2MULLER_.real())+(f2MULLER_.imag())*(f2MULLER_.imag()); /* |f2MULLER_|^2 = f2MULLER_.real()^2+f2MULLER_.imag()^2 */ 00465 } 00466 00467 /***** Muller's modification to improve convergence *****/ 00468 void 00469 NumericLib::convergence_check(mrs_natural *overflow,mrs_real f1absq,mrs_real f2absq, 00470 mrs_real epsilon) 00471 /*mrs_real f1absq, f1absq = |f1MULLER_|^2 */ 00472 /* f2absq, f2absq = |f2MULLER_|^2 */ 00473 /* epsilon; bound for |q2MULLER_| */ 00474 /*mrs_natural *overflow; *overflow = TRUE => overflow occures */ 00475 /* *overflow = FALSE => no overflow occures */ 00476 { 00477 if ((f2absq>(CONVERGENCE*f1absq)) && (Cabs(q2MULLER_)>epsilon) && 00478 (iterMULLER_<ITERMAXMULLER)) { 00479 q2MULLER_ = RCmul((mrs_real).5,q2MULLER_); /* in case of overflow: */ 00480 h2MULLER_ = RCmul((mrs_real).5,h2MULLER_); /* halve q2MULLER_ and h2MULLER_; compute new x2MULLER_ */ 00481 x2MULLER_ = Csub(x2MULLER_,h2MULLER_); 00482 *overflow = NumLib_TRUE; 00483 } 00484 } 00485 00486 /***** compute P(x2MULLER_) and make some checks *****/ 00487 void 00488 NumericLib::compute_function(mrs_complex *pred,mrs_natural nred,mrs_real f1absq, 00489 mrs_real *f2absq,mrs_real epsilon) 00490 /*mrs_complex *pred; coefficient vector of the deflated polynomial */ 00491 /*mrs_natural nred; the highest exponent of the deflated polynomial */ 00492 /*mrs_real f1absq, f1absq = |f1MULLER_|^2 */ 00493 /* *f2absq, f2absq = |f2MULLER_|^2 */ 00494 /* epsilon; bound for |q2MULLER_| */ 00495 { 00496 mrs_natural overflow; /* overflow = TRUE => overflow occures */ 00497 /* overflow = FALSE => no overflow occures */ 00498 00499 do { 00500 overflow = NumLib_FALSE; /* initial estimation: no overflow */ 00501 00502 /* suppress overflow */ 00503 suppress_overflow(nred); 00504 00505 /* calculate new value => result in f2MULLER_ */ 00506 fdvalue(pred,nred,&f2MULLER_,&f2MULLER_,x2MULLER_,NumLib_FALSE); 00507 00508 /* check of too big function values */ 00509 too_big_functionvalues(f2absq); 00510 00511 /* increase iterationcounter */ 00512 iterMULLER_++; 00513 00514 /* Muller's modification to improve convergence */ 00515 convergence_check(&overflow,f1absq,*f2absq,epsilon); 00516 } while (overflow); 00517 } 00518 00519 /***** is the new x2MULLER_ the best approximation? *****/ 00520 void 00521 NumericLib::check_x_value(mrs_complex *xb,mrs_real *f2absqb,mrs_natural *rootd, 00522 mrs_real f1absq,mrs_real f2absq,mrs_real epsilon, 00523 mrs_natural *noise) 00524 /*mrs_complex *xb; best x-value */ 00525 /*mrs_real *f2absqb, f2absqb |P(xb)|^2 */ 00526 /* f1absq, f1absq = |f1MULLER_|^2 */ 00527 /* f2absq, f2absq = |f2MULLER_|^2 */ 00528 /* epsilon; bound for |q2MULLER_| */ 00529 /*mrs_natural *rootd, *rootd = TRUE => root determined */ 00530 /* *rootd = FALSE => no root determined */ 00531 /* *noise; noisecounter */ 00532 { 00533 if ((f2absq<=(BOUND1*f1absq)) && (f2absq>=(BOUND2*f1absq))) { 00534 /* function-value changes slowly */ 00535 if (Cabs(h2MULLER_)<BOUND3) { /* if |h[2]| is small enough => */ 00536 q2MULLER_ = RCmul((mrs_real)2.,q2MULLER_); /* mrs_real q2MULLER_ and h[2] */ 00537 h2MULLER_ = RCmul((mrs_real)2.,h2MULLER_); 00538 } else { /* otherwise: |q2MULLER_| = 1 and */ 00539 /* h[2] = h[2]*q2MULLER_ */ 00540 q2MULLER_ = Ccomplex(cos((mrs_real)iterMULLER_),sin((mrs_real)iterMULLER_)); 00541 h2MULLER_ = Cmul(h2MULLER_,q2MULLER_); 00542 } 00543 } else if (f2absq<*f2absqb) { 00544 *f2absqb = f2absq; /* the new function value is the */ 00545 *xb = x2MULLER_; /* best approximation */ 00546 *noise = 0; /* reset noise counter */ 00547 if ((sqrt(f2absq)<epsilon) && 00548 (Cabs(Cdiv(Csub(x2MULLER_,x1MULLER_),x2MULLER_))<epsilon)) 00549 *rootd = NumLib_TRUE; /* root determined */ 00550 } 00551 } 00552 00553 /***** check, if determined root is good enough. *****/ 00554 void 00555 NumericLib::root_check(mrs_complex *pred,mrs_natural nred,mrs_real f2absqb,mrs_natural *seconditer, 00556 mrs_natural *rootd,mrs_natural *noise,mrs_complex xb) 00557 /*mrs_complex *pred, coefficient vector of the deflated polynomial */ 00558 /* xb; best x-value */ 00559 /*mrs_natural nred, the highest exponent of the deflated polynomial */ 00560 /* *noise, noisecounter */ 00561 /* *rootd, *rootd = TRUE => root determined */ 00562 /* *rootd = FALSE => no root determined */ 00563 /* *seconditer; *seconditer = TRUE => start second iteration with */ 00564 /* new initial estimations */ 00565 /* *seconditer = FALSE => end routine */ 00566 /*mrs_real f2absqb; f2absqb |P(xb)|^2 */ 00567 { 00568 00569 mrs_complex df; /* df=P'(x0MULLER_) */ 00570 00571 if ((*seconditer==1) && (f2absqb>0)) { 00572 fdvalue(pred,nred,&f2MULLER_,&df,xb,NumLib_TRUE); /* f2MULLER_=P(x0MULLER_), df=P'(x0MULLER_) */ 00573 if (Cabs(f2MULLER_)/(Cabs(df)*Cabs(xb))>BOUND7) { 00574 /* start second iteration with new initial estimations */ 00575 /* x0MULLER_ = Ccomplex(-1./sqrt(2),1./sqrt(2)); 00576 x1MULLER_ = Ccomplex(1./sqrt(2),-1./sqrt(2)); 00577 x2MULLER_ = Ccomplex(-1./sqrt(2),-1./sqrt(2)); */ 00578 /*ml, 12-21-94: former initial values: */ 00579 x0MULLER_ = Ccomplex(1.,0.); 00580 x1MULLER_ = Ccomplex(-1.,0.); 00581 x2MULLER_ = Ccomplex(0.,0.); /* */ 00582 fdvalue(pred,nred,&f0MULLER_,&df,x0MULLER_,NumLib_FALSE); /* f0MULLER_ = P(x0MULLER_) */ 00583 fdvalue(pred,nred,&f1MULLER_,&df,x1MULLER_,NumLib_FALSE); /* f1MULLER_ = P(x1MULLER_) */ 00584 fdvalue(pred,nred,&f2MULLER_,&df,x2MULLER_,NumLib_FALSE); /* f2MULLER_ = P(x2MULLER_) */ 00585 iterMULLER_ = 0; /* reset iteration counter */ 00586 (*seconditer)++; /* increase seconditer */ 00587 *rootd = NumLib_FALSE; /* no root determined */ 00588 *noise = 0; /* reset noise counter */ 00589 } 00590 } 00591 } 00592 00593 //NEWTON 00594 /******************************************************************/ 00595 /* */ 00596 /* file: newton.c */ 00597 /* */ 00598 /* main function: newton() */ 00599 /* */ 00600 /* version: 1.2 */ 00601 /* */ 00602 /* author: B. Frenzel */ 00603 /* */ 00604 /* date: Jan 7 1993 */ 00605 /* */ 00606 /* input: p[] coefficient vector of the original */ 00607 /* polynomial */ 00608 /* n the highest exponent of the original */ 00609 /* polynomial */ 00610 /* ns determined root with Muller method */ 00611 /* */ 00612 /* output: *dxabs error of determined root */ 00613 /* */ 00614 /* return: xmin determined root */ 00615 /* subroutines: fdvalue() */ 00616 /* */ 00617 /* description: */ 00618 /* newton() determines the root of a polynomial with complex */ 00619 /* coefficients; the initial estimation is the root determined */ 00620 /* by Muller's method */ 00621 /* */ 00622 /* Copyright: */ 00623 /* Lehrstuhl fuer Nachrichtentechnik Erlangen */ 00624 /* Cauerstr. 7, 8520 Erlangen, FRG, 1993 */ 00625 /* e-mail: int@nt.e-technik.uni-erlangen.de */ 00626 /* */ 00627 /******************************************************************/ 00628 mrs_complex 00629 NumericLib::newton(mrs_complex *p,mrs_natural n,mrs_complex ns,mrs_real *dxabs, 00630 unsigned char flag) 00631 /*mrs_complex *p, coefficient vector of the original polynomial */ 00632 /* ns; determined root with Muller method */ 00633 /*mrs_natural n; highest exponent of the original polynomial */ 00634 /*mrs_real *dxabs; dxabs = |P(x0)/P'(x0)| */ 00635 /*unsigned char flag; flag = TRUE => complex coefficients */ 00636 /* flag = FALSE => real coefficients */ 00637 { 00638 mrs_complex x0, /* iteration variable for x-value */ 00639 xmin, /* best x determined in newton() */ 00640 f, /* f = P(x0) */ 00641 df, /* df = P'(x0) */ 00642 dx, /* dx = P(x0)/P'(x0) */ 00643 dxh; /* help variable dxh = P(x0)/P'(x0) */ 00644 mrs_real fabsmin=FVALUENEWTON, /* fabsmin = |P(xmin)| */ 00645 eps=DBL_EPSILON; /* routine ends, when estimated dist. */ 00646 /* between x0 and root is less or */ 00647 /* equal eps */ 00648 mrs_natural iter =0, /* counter */ 00649 noise =0; /* noisecounter */ 00650 00651 x0 = ns; /* initial estimation = root determined */ 00652 /* with Muller method */ 00653 xmin = x0; /* initial estimation for the best x-value */ 00654 dx = Ccomplex(1.,0.); /* initial value: P(x0)/P'(x0)=1+j*0 */ 00655 *dxabs = Cabs(dx); /* initial value: |P(x0)/P'(x0)|=1 */ 00656 /* printf("%8.4e %8.4e\n",xmin.real(),xmin.imag()); */ 00657 for (iter=0; iter<ITERMAXNEWTON; iter++) { /* main loop */ 00658 fdvalue(p,n,&f,&df,x0,NumLib_TRUE); /* f=P(x0), df=P'(x0) */ 00659 00660 if (Cabs(f)<fabsmin) { /* the new x0 is a better */ 00661 xmin = x0; /* approximation than the old xmin */ 00662 fabsmin = Cabs(f); /* store new xmin and fabsmin */ 00663 noise = 0; /* reset noise counter */ 00664 } 00665 00666 if (Cabs(df)!=0.) { /* calculate new dx */ 00667 dxh=Cdiv(f,df); 00668 if (Cabs(dxh)<*dxabs*FACTORNEWTON) { /* new dx small enough? */ 00669 dx = dxh; /* store new dx for next */ 00670 *dxabs = Cabs(dx); /* iteration */ 00671 } 00672 } 00673 00674 if (Cabs(xmin)!=0.) { 00675 if (*dxabs/Cabs(xmin)<eps || noise == NOISEMAX) { 00676 /* routine ends */ 00677 if (fabs(xmin.imag())<BOUND && flag==0) { 00678 /* define determined root as real,*/ 00679 xmin = mrs_complex(xmin.real(),0.);//xmin.imag()=0.; /* if imag. part<BOUND */ 00680 } 00681 *dxabs=*dxabs/Cabs(xmin); /* return relative error */ 00682 return xmin; /* return best approximation */ 00683 } 00684 } 00685 00686 x0 = Csub(x0,dx); /* main iteration: x0 = x0 - P(x0)/P'(x0) */ 00687 00688 noise++; /* increase noise counter */ 00689 } 00690 00691 00692 if (fabs(xmin.imag())<BOUND && flag==0) /* define determined root */ 00693 xmin = mrs_complex(xmin.real(),0.);//xmin.imag()=0.;/* as real, if imag. part<BOUND */ 00694 if (Cabs(xmin)!=0.) 00695 *dxabs=*dxabs/Cabs(xmin); /* return relative error */ 00696 /* maximum number of iterations exceeded: */ 00697 return xmin; /* return best xmin until now */ 00698 } 00699 00700 //NULL 00701 /******************************************************************/ 00702 /* */ 00703 /* file: null.cpp */ 00704 /* */ 00705 /* main function: null() */ 00706 /* */ 00707 /* version: 1.3 (adapted by lmartins@inescporto.pt 15.06.06)*/ 00708 /* */ 00709 /* author: B. Frenzel */ 00710 /* */ 00711 /* date: Jan 19 1993 */ 00712 /* */ 00713 /* input: p[] coefficient vector of the original */ 00714 /* polynomial */ 00715 /* pred[] coefficient vector of the deflated */ 00716 /* polynomial */ 00717 /* *n the highest exponent of the original */ 00718 /* polynomial */ 00719 /* flag flag = TRUE => complex coefficients */ 00720 /* flag = FALSE => real coefficients */ 00721 /* */ 00722 /* output: root[] vector of determined roots */ 00723 /* *maxerr estimation of max. error of all */ 00724 /* determined roots */ 00725 /* */ 00726 /* subroutines: poly_check(),quadratic(),lin_or_quad(),monic(), */ 00727 /* muller(),newton() */ 00728 /* */ 00729 /* description: */ 00730 /* main rootfinding routine for polynomials with complex or real */ 00731 /* coefficients using Muller's method combined with Newton's m. */ 00732 /* */ 00733 /* Copyright: */ 00734 /* Lehrstuhl fuer Nachrichtentechnik Erlangen */ 00735 /* Cauerstr. 7, 8520 Erlangen, FRG, 1993 */ 00736 /* e-mail: int@nt.e-technik.uni-erlangen.de */ 00737 /* */ 00738 /******************************************************************/ 00739 unsigned char 00740 NumericLib::null(mrs_complex *p,mrs_complex *pred,mrs_natural *n,mrs_complex *root, 00741 mrs_real *maxerr,unsigned char flag) 00742 /*mrs_complex *p, coefficient vector of the original polynomial */ 00743 /* *pred, coefficient vector of the deflated polynomial */ 00744 /* *root; determined roots */ 00745 /*mrs_natural *n; the highest exponent of the original polynomial */ 00746 /*mrs_real *maxerr; max. error of all determined roots */ 00747 /*unsigned char flag; flag = TRUE => complex coefficients */ 00748 /* flag = FALSE => real coefficients */ 00749 { 00750 mrs_real newerr; /* error of actual root */ 00751 mrs_complex ns; /* root determined by Muller's method */ 00752 mrs_natural nred, /* highest exponent of deflated polynomial */ 00753 i; /* counter */ 00754 unsigned char error; /* indicates an error in poly_check */ 00755 mrs_natural red, 00756 diff; /* number of roots at 0 */ 00757 00758 *maxerr = 0.; /* initialize max. error of determined roots */ 00759 nred = *n; /* At the beginning: degree defl. polyn. = */ 00760 /* degree of original polyn. */ 00761 00762 /* check input of the polynomial */ 00763 error = poly_check(p,&nred,n,root); 00764 diff = (*n-nred); /* reduce polynomial, if roots at 0 */ 00765 p += diff; 00766 *n = nred; 00767 00768 if (error) 00769 return error; /* error in poly_check(); return error */ 00770 00771 /* polynomial is linear or quadratic */ 00772 if (lin_or_quad(p,nred,root)==0) { 00773 *n += diff; /* remember roots at 0 */ 00774 *maxerr = DBL_EPSILON; 00775 return 0; /* return no error */ 00776 } 00777 00778 monic(p,n); /* get monic polynom */ 00779 00780 for (i=0; i<=*n; ++i) pred[i]=p[i]; /* original polynomial */ 00781 /* = deflated polynomial at beginning */ 00782 /* of Muller */ 00783 00784 do { /* main loop of null() */ 00785 /* Muller method */ 00786 ns = muller(pred,nred); 00787 00788 00789 00790 /* Newton method */ 00791 root[nred-1] = newton(p,*n,ns,&newerr,flag); 00792 00793 /* stores max. error of all roots */ 00794 if (newerr>*maxerr) 00795 *maxerr=newerr; 00796 /* deflate polynomial */ 00797 red = poldef(pred,nred,root,flag); 00798 pred += red; /* forget lowest coefficients */ 00799 nred -= red; /* reduce degree of polynomial */ 00800 } while (nred>2); 00801 /* last one or two roots */ 00802 (void) lin_or_quad(pred,nred,root); 00803 if (nred==2) { 00804 root[1] = newton(p,*n,root[1],&newerr,flag); 00805 if (newerr>*maxerr) 00806 *maxerr=newerr; 00807 } 00808 root[0] = newton(p,*n,root[0],&newerr,flag); 00809 if (newerr>*maxerr) 00810 *maxerr=newerr; 00811 00812 *n += diff; /* remember roots at 0 */ 00813 return 0; /* return no error */ 00814 } 00815 00816 /***** poly_check() check the formal correctness of input *****/ 00817 unsigned char 00818 NumericLib::poly_check(mrs_complex *pred,mrs_natural *nred,mrs_natural *n,mrs_complex *root) 00819 /*mrs_complex *pred, coefficient vector of the original polynomial */ 00820 /* *root; determined roots */ 00821 /*mrs_natural *nred, highest exponent of the deflated polynomial */ 00822 /* *n; highest exponent of the original polynomial */ 00823 { 00824 mrs_natural i = -1, /* i stores the (reduced) real degree */ 00825 j; /* counter variable */ 00826 unsigned char 00827 notfound=NumLib_TRUE; /* indicates, whether a coefficient */ 00828 /* unequal zero was found */ 00829 00830 if (*n<0) return 1; /* degree of polynomial less than zero */ 00831 /* return error */ 00832 00833 for (j=0; j<=*n; j++) { /* determines the "real" degree of */ 00834 if(Cabs(pred[j])!=0.) /* polynomial, cancel leading roots */ 00835 i=j; 00836 } 00837 if (i==-1) return 2; /* polynomial is a null vector; return error */ 00838 if (i==0) return 3; /* polynomial is constant unequal null; */ 00839 /* return error */ 00840 00841 *n=i; /* set new exponent of polynomial */ 00842 i=0; /* reset variable for exponent */ 00843 do { /* find roots at 0 */ 00844 if (Cabs(pred[i])==0.) 00845 ++i; 00846 else 00847 notfound=NumLib_FALSE; 00848 } while (i<=*n && notfound); 00849 00850 if (i==0) { /* no root determined at 0 */ 00851 *nred = *n; /* original degree=deflated degree and */ 00852 return 0; /* return no error */ 00853 } else { /* roots determined at 0: */ 00854 for (j=0; j<=i-1; j++) /* store roots at 0 */ 00855 root[*n-j-1] = Ccomplex(0.,0.); 00856 *nred = *n-i; /* reduce degree of deflated polynomial */ 00857 return 0; /* and return no error */ 00858 } 00859 } 00860 00861 /***** quadratic() calculates the roots of a quadratic polynomial *****/ 00862 void 00863 NumericLib::quadratic(mrs_complex *pred,mrs_complex *root) 00864 /*mrs_complex *pred, coefficient vector of the deflated polynomial */ 00865 /* *root; determined roots */ 00866 00867 { 00868 mrs_complex discr, /* discriminate */ 00869 Z1,Z2, /* numerators of the quadratic formula */ 00870 N; /* denominator of the quadratic formula */ 00871 00872 /* discr = p1^2-4*p2*p0 */ 00873 discr = Csub(Cmul(pred[1],pred[1]), 00874 RCmul((mrs_real)4.,Cmul(pred[2],pred[0]))); 00875 /* Z1 = -p1+sqrt(discr) */ 00876 Z1 = Cadd(RCmul((mrs_real)-1.,pred[1]),Csqrt(discr)); 00877 /* Z2 = -p1-sqrt(discr) */ 00878 Z2 = Csub(RCmul((mrs_real)-1.,pred[1]),Csqrt(discr)); 00879 /* N = 2*p2 */ 00880 N = RCmul((mrs_real)2.,pred[2]); 00881 root[0] = Cdiv(Z1,N); /* first root = Z1/N */ 00882 root[1] = Cdiv(Z2,N); /* second root = Z2/N */ 00883 } 00884 00885 /***** lin_or_quad() calculates roots of lin. or quadratic equation *****/ 00886 unsigned char 00887 NumericLib::lin_or_quad(mrs_complex *pred,mrs_natural nred,mrs_complex *root) 00888 /*mrs_complex *pred, coefficient vector of the deflated polynomial */ 00889 /* *root; determined roots */ 00890 /*mrs_natural nred; highest exponent of the deflated polynomial */ 00891 { 00892 if (nred==1) { /* root = -p0/p1 */ 00893 root[0] = Cdiv(RCmul((mrs_real)-1.,pred[0]),pred[1]); 00894 return 0; /* and return no error */ 00895 } else if (nred==2) { /* quadratic polynomial */ 00896 quadratic(pred,root); 00897 return 0; /* return no error */ 00898 } 00899 00900 return 1; /* nred>2 => no roots were calculated */ 00901 } 00902 00903 /***** monic() computes monic polynomial for original polynomial *****/ 00904 void 00905 NumericLib::monic(mrs_complex *p,mrs_natural *n) 00906 /*mrs_complex *p; coefficient vector of the original polynomial */ 00907 /*mrs_natural *n; the highest exponent of the original polynomial */ 00908 { 00909 mrs_real factor; /* stores absolute value of the coefficient */ 00910 /* with highest exponent */ 00911 mrs_natural i; /* counter variable */ 00912 00913 factor=1./Cabs(p[*n]); /* factor = |1/pn| */ 00914 if ( factor!=1.) /* get monic pol., when |pn| != 1 */ 00915 for (i=0; i<=*n; ++i) 00916 p[i]=RCmul(factor,p[i]); 00917 } 00918 00919 //TOOLS 00920 /******************************************************************/ 00921 /* */ 00922 /* file: tools.c */ 00923 /* */ 00924 /* version: 1.2 */ 00925 /* */ 00926 /* author: B. Frenzel */ 00927 /* */ 00928 /* date: Jan 7 1993 */ 00929 /* */ 00930 /* function: description: */ 00931 /* */ 00932 /* hornc() hornc() deflates the polynomial with coeff. */ 00933 /* stored in pred[0],...,pred[n] by Horner's */ 00934 /* method (one root) */ 00935 /* */ 00936 /* horncd() horncd() deflates the polynomial with coeff. */ 00937 /* stored in pred[0],...,pred[n] by Horner's */ 00938 /* method (two roots) */ 00939 /* */ 00940 /* poldef() decides whether to call hornc() or horncd() */ 00941 /* */ 00942 /* fdvalue() fdvalue() computes f=P(x0) by Horner's method; */ 00943 /* if flag=TRUE, it additionally computes the */ 00944 /* derivative df=P'(x0) */ 00945 /* */ 00946 /* Copyright: */ 00947 /* Lehrstuhl fuer Nachrichtentechnik Erlangen */ 00948 /* Cauerstr. 7, 8520 Erlangen, FRG, 1993 */ 00949 /* e-mail: int@nt.e-technik.uni-erlangen.de */ 00950 /* */ 00951 /******************************************************************/ 00952 00953 /***** Horner method to deflate one root *****/ 00954 void 00955 NumericLib::hornc(mrs_complex *pred,mrs_natural nred,mrs_complex x0,unsigned char flag) 00956 /*mrs_complex *pred, coefficient vector of the polynomial */ 00957 /* x0; root to be deflated */ 00958 /*mrs_natural nred; the highest exponent of (deflated) polynomial */ 00959 /*unsigned char flag; indicates how to reduce polynomial */ 00960 { 00961 mrs_natural i; /* counter */ 00962 mrs_complex help1; /* help variable */ 00963 00964 if ((flag&1)==0) /* real coefficients */ 00965 for(i=nred-1; i>0; i--) 00966 pred[i] = mrs_complex(pred[i].real() + (x0.real()*pred[i+1].real()), pred[i].imag()); //pred[i].real() += (x0.real()*pred[i+1].real()); 00967 else /* complex coefficients */ 00968 for (i=nred-1; i>0; i--) { 00969 CMUL(help1,pred[i+1],x0); 00970 CADD(pred[i],help1,pred[i]); 00971 } 00972 } 00973 00974 /***** Horner method to deflate two roots *****/ 00975 void 00976 NumericLib::horncd(mrs_complex *pred,mrs_natural nred,mrs_real a,mrs_real b) 00977 /*mrs_complex *pred; coefficient vector of the polynomial */ 00978 /*mrs_real a, coefficients of the quadratic polynomial */ 00979 /* b; x^2+ax+b */ 00980 /*mrs_natural nred; the highest exponent of (deflated) polynomial */ 00981 { 00982 mrs_natural i; /* counter */ 00983 00984 //lmartins: pred[nred-1].real() += pred[nred].real()*a; 00985 pred[nred-1] = mrs_complex(pred[nred-1].real() + pred[nred].real()*a, pred[nred-1].imag()); 00986 00987 for (i=nred-2; i>1; i--) 00988 //lmartins pred[i].real() += (a*pred[i+1].real()+b*pred[i+2].real()); 00989 pred[i] = mrs_complex(pred[i].real() + (a*pred[i+1].real()+b*pred[i+2].real()), pred[i].imag()); 00990 } 00991 00992 /***** main routine to deflate polynomial *****/ 00993 mrs_natural 00994 NumericLib::poldef(mrs_complex *pred,mrs_natural nred,mrs_complex *root,unsigned char flag) 00995 /*mrs_complex *pred, coefficient vector of the polynomial */ 00996 /* *root; vector of determined roots */ 00997 /*mrs_natural nred; the highest exponent of (deflated) polynomial */ 00998 /*unsigned char flag; indicates how to reduce polynomial */ 00999 { 01000 mrs_real a, /* coefficients of the quadratic polynomial */ 01001 b; /* x^2+ax+b */ 01002 mrs_complex x0; /* root to be deflated */ 01003 01004 01005 x0 = root[nred-1]; 01006 if (x0.imag()!=0.) /* x0 is complex */ 01007 flag |=2; 01008 01009 if (flag==2) { /* real coefficients and complex root */ 01010 a = 2*x0.real(); /* => deflate x0 and Conjg(x0) */ 01011 b = -(x0.real()*x0.real()+x0.imag()*x0.imag()); 01012 root[nred-2]=Conjg(x0); /* store second root = Conjg(x0) */ 01013 horncd(pred,nred,a,b); 01014 return 2; /* two roots deflated */ 01015 } else { 01016 hornc(pred,nred,x0,flag); /* deflate only one root */ 01017 return 1; /* one root deflated */ 01018 } 01019 } 01020 01021 /***** fdvalue computes P(x0) and optional P'(x0) *****/ 01022 void 01023 NumericLib::fdvalue(mrs_complex *p,mrs_natural n,mrs_complex *f,mrs_complex *df,mrs_complex x0, 01024 unsigned char flag) 01025 /*mrs_complex *p, coefficient vector of the polynomial P(x) */ 01026 /* *f, the result f=P(x0) */ 01027 /* *df, the result df=P'(x0), if flag=TRUE */ 01028 /* x0; polynomial will be computed at x0 */ 01029 /*mrs_natural n; the highest exponent of p */ 01030 /*unsigned char flag; flag==TRUE => compute P'(x0) */ 01031 { 01032 mrs_natural i; /* counter */ 01033 mrs_complex help1; /* help variable */ 01034 01035 *f = p[n]; 01036 if (flag==NumLib_TRUE) { /* if flag=TRUE, compute P(x0) */ 01037 COMPLEXM(*df,0.,0.); /* and P'(x0) */ 01038 for (i=n-1; i>=0; i--) { 01039 CMUL(help1,*df,x0); /* *df = *f + *df * x0 */ 01040 CADD(*df,help1,*f); 01041 CMUL(help1,*f,x0); /* *f = p[i] + *f * x0 */ 01042 CADD(*f,help1,p[i]); 01043 } 01044 } else /* otherwise: compute only P(x0)*/ 01045 for (i=n-1; i>=0; i--) { 01046 CMUL(help1,*f,x0); /* *f = p[i] + *f * x0 */ 01047 CADD(*f,help1,p[i]); 01048 } 01049 } 01050 01051 01052 //********************************************************************************************************** 01053 01054 01055 // Used to force A and B to be stored prior to 01056 // doing the addition of A and B, for use in 01057 // situations where optimizers might hold one 01058 // of these in a register 01059 mrs_real NumericLib::add(mrs_real *a, mrs_real *b) 01060 { 01061 mrs_real ret; 01062 ret = *a + *b; 01063 return ret; 01064 } 01065 01066 mrs_real NumericLib::pow_di(mrs_real *ap, mrs_natural *bp) 01067 { 01068 mrs_real pow, x; 01069 mrs_natural n; 01070 unsigned long u; 01071 01072 pow = 1; 01073 x = *ap; 01074 n = *bp; 01075 01076 if(n != 0) 01077 { 01078 if(n < 0) 01079 { 01080 n = -n; 01081 x = 1/x; 01082 } 01083 for(u = n; ; ) 01084 { 01085 if(u & 01) 01086 pow *= x; 01087 if(u >>= 1) 01088 x *= x; 01089 else 01090 break; 01091 } 01092 } 01093 return(pow); 01094 } 01095 01096 // Machine parameters 01097 // cmach : 01098 // 'B' | 'b' --> base 01099 // 'M' | 'm' --> digits in the mantissa 01100 // 'R' | 'r' --> approximation method : 1=rounding 0=chopping 01101 // 'E' | 'e' --> eps 01102 mrs_real NumericLib::machp(const char *cmach) 01103 { 01104 mrs_real zero, one, two, half, sixth, third, a, b, c, f, d__1, d__2, d__3, d__4, d__5, qtr, eps; 01105 mrs_real base; 01106 mrs_natural lt, rnd, i__1; 01107 lt = 0; 01108 rnd = 0; 01109 eps = 0.0; 01110 01111 one = 1.; 01112 a = 1.; 01113 c = 1.; 01114 01115 01116 while( c == one ) { 01117 a *= 2; 01118 c = add(&a, &one); 01119 d__1 = -a; 01120 c = add(&c, &d__1); 01121 } 01122 01123 b = 1.; 01124 c = add( &a, &b ); 01125 01126 while( c == a ) 01127 { 01128 b *= 2; 01129 c = add( &a, &b ); 01130 } 01131 01132 qtr = one / 4; 01133 d__1 = -a; 01134 c = add(&c, &d__1); 01135 base = (mrs_natural)(c+qtr); 01136 01137 01138 if( *cmach == 'M' || *cmach == 'm' || *cmach == 'E' || *cmach == 'e' ) 01139 { 01140 lt = 0; 01141 a = 1.; 01142 c = 1.; 01143 printf("%g %g %g %g\n", c, one, a, d__1); 01144 while( c == one ) { 01145 ++lt; 01146 a *= base; 01147 c = add( &a, &one ); 01148 d__1 = -a; 01149 c = add( &c, &d__1 ); 01150 } 01151 } 01152 01153 if( *cmach == 'R' || *cmach == 'r' || *cmach == 'E' || *cmach == 'e' ) 01154 { 01155 b = (mrs_real) base; 01156 d__1 = b / 2; 01157 d__2 = -b / 100; 01158 f = add( &d__1, &d__2 ); 01159 c = add( &f, &a ); 01160 if( c == a ) { 01161 rnd = 1; // true 01162 } else { 01163 rnd = 0; // false 01164 } 01165 d__1 = b / 2; 01166 d__2 = b / 100; 01167 f = add( &d__1 , &d__2 ); 01168 c = add( &f, &a ); 01169 if( rnd && c == a ) { 01170 rnd = 0; // false 01171 } 01172 } 01173 01174 if( *cmach == 'E' || *cmach == 'e' ) 01175 { 01176 zero = 0.; 01177 two = 2.; 01178 01179 i__1 = -lt; 01180 a = pow_di( &base, &i__1); 01181 eps = a; 01182 01183 b = two / 3; 01184 half = one / 2; 01185 d__1 = -half; 01186 sixth = add( &b, &d__1 ); 01187 third = add( &sixth, &sixth ); 01188 d__1 = -half; 01189 b = add( &third, &d__1 ); 01190 b = add( &b, &sixth ); 01191 b = fabs(b); 01192 if( b < eps ) 01193 b = eps; 01194 01195 eps = 1.; 01196 01197 while( eps > b && b > zero ) 01198 { 01199 eps = b; 01200 d__1 = half * eps; 01201 /* Computing 5th power */ 01202 d__3 = two, d__4 = d__3, d__3 *= d__3; 01203 /* Computing 2nd power */ 01204 d__5 = eps; 01205 d__2 = d__4 * (d__3 * d__3) * (d__5 * d__5); 01206 c = add(&d__1, &d__2); 01207 d__1 = -c; 01208 c = add(&half, &d__1); 01209 b = add(&half, &c); 01210 d__1 = -b; 01211 c = add(&half, &d__1); 01212 b = add(&half, &c); 01213 } 01214 01215 if( a < eps ) 01216 eps = a; 01217 01218 if( rnd == 1 ) { 01219 i__1 = 1 - lt; 01220 eps = pow_di(&base, &i__1) / 2; 01221 } else { 01222 i__1 = 1 - lt; 01223 eps = pow_di( &base, &i__1 ); 01224 } 01225 01226 } 01227 01228 switch(*cmach) { 01229 case 'B' : 01230 case 'b' : return base; break; 01231 01232 case 'M' : 01233 case 'm' : return lt; break; 01234 01235 case 'R' : 01236 case 'r' : return rnd; break; 01237 01238 case 'E' : 01239 case 'e' : return eps; break; 01240 01241 default : return -1; 01242 } 01243 } 01244 01245 01246 /* Reduce a real, symmetric matrix to a symmetric, tridiag. matrix. */ 01247 void 01248 NumericLib::tred2(realvec &a, mrs_natural m, realvec &d, realvec &e) 01249 /* Householder reductiom of matrix a to tridiagomal form. 01250 Algorithm: Martim et al., Num. Math. 11, 181-195, 1968. 01251 Ref: Smith et al., Matrix Eigemsystem Routimes -- EISPACK Guide 01252 Sprimger-Verlag, 1976, pp. 489-494. 01253 W H Press et al., Numerical Recipes im C, Cambridge U P, 01254 1988, pp. 373-374. 01255 01256 Source code adapted from F. Murtagh, Munich, 6 June 1989 01257 http://astro.u-strasbg.fr/~fmurtagh/mda-sw/pca.c 01258 */ 01259 { 01260 mrs_natural l, k, j, i; 01261 mrs_real scale, hh, h, g, f; 01262 01263 for (i = m-1; i > 0; i--) 01264 { 01265 l = i - 1; 01266 h = scale = 0.0; 01267 if (l > 0) 01268 { 01269 for (k = 0; k <= l; k++) 01270 scale += fabs(a(k*m+i)); 01271 if (scale == 0.0) 01272 e(i) = a(l*m+i); 01273 else 01274 { 01275 for (k = 0; k <= l; k++) 01276 { 01277 a(k*m+i) /= scale; 01278 h += a(k*m+i) * a(k*m+i); 01279 } 01280 f = a(l*m+i); 01281 g = f>0 ? -sqrt(h) : sqrt(h); 01282 e(i) = scale * g; 01283 01284 h -= f * g; 01285 a(l*m+i) = f - g ; 01286 f = 0.0; 01287 for (j = 0; j <= l; j++) 01288 { 01289 a(i*m+j) = a(j*m+i)/h; 01290 g = 0.0; 01291 for (k = 0; k <= j; k++) 01292 g += a(k*m+j) * a(k*m+i) ; 01293 for (k = j+1; k <= l; k++) 01294 g += a(j*m+k) * a(k*m+i); 01295 e(j) = g / h; 01296 f += e(j) * a(j*m+i); 01297 } 01298 hh = f / (h + h); 01299 for (j = 0; j <= l; j++) 01300 { 01301 f = a(j*m+i); 01302 e(j) = g = e(j) - hh * f; 01303 for (k = 0; k <= j; k++) 01304 a(k*m+j) -= (f * e(k) + g * a(k*m+i)); 01305 } 01306 } 01307 } 01308 else 01309 e(i) = a(l*m+i); 01310 d(i) = h; 01311 } 01312 d(0) = 0.0; 01313 e(0) = 0.0; 01314 for (i = 0; i < m; ++i) 01315 { 01316 l = i - 1; 01317 if (d(i)) 01318 { 01319 for (j = 0; j <= l; j++) 01320 { 01321 g = 0.0; 01322 for (k = 0; k <= l; k++) 01323 g += a(k*m+i) * a(j*m+k); 01324 for (k = 0; k <= l; k++) 01325 a(j*m+k) -= g * a(i*m+k); 01326 } 01327 } 01328 d(i) = a(i*m+i); 01329 a(i*m+i) = 1.0 ; 01330 01331 for (j = 0; j <= l; j++) 01332 a(i*m+j) = a(j*m+i) = 0.0; 01333 } 01334 } 01335 01336 /* Tridiagonal QL algorithm -- Implicit */ 01337 void 01338 NumericLib::tqli(realvec &d, realvec &e, mrs_natural m, realvec &z) 01339 /* 01340 Source code adapted from F. Murtagh, Munich, 6 June 1989 01341 http://astro.u-strasbg.fr/~fmurtagh/mda-sw/pca.c 01342 */ 01343 { 01344 mrs_natural n, l, iter, i, j, k; 01345 mrs_real s, r, p, g, f, dd, c, b, tmp; 01346 01347 for (i = 1; i < m; ++i) 01348 e(i-1) = e(i); 01349 e(m-1) = 0.0; 01350 for (l = 0; l < m; l++) 01351 { 01352 iter = 0; 01353 do 01354 { 01355 for (n = l; n < m-1; n++) 01356 { 01357 dd = fabs(d(n)) + fabs(d(n+1)); 01358 if (fabs(e(n)) + dd == dd) break; 01359 } 01360 if (n != l) 01361 { 01362 //MRSASSERT( iter++ != 30 ); // No convergence 01363 if( iter++ == 30 ) 01364 { 01365 cerr << "tqli did not converge!" << endl; 01366 MRSERR("NumericLib.cpp: tqli did not converge!"); 01367 MRSASSERT(0); 01368 return; 01369 //exit(EXIT_SUCCESS); 01370 } 01371 01372 g = (d(l+1) - d(l)) / (2.0 * e(l)); 01373 r = sqrt((g * g) + 1.0); 01374 g = d(n) - d(l) + e(l) / (g + SIGN(r, g)); 01375 s = c = 1.0; 01376 p = 0.0; 01377 for (i = n-1; i >= l; i--) 01378 { 01379 f = s * e(i); 01380 b = c * e(i); 01381 if (fabs(f) >= fabs(g)) 01382 { 01383 c = g / f; 01384 r = sqrt((c * c) + 1.0); 01385 e(i+1) = f * r; 01386 c *= (s = 1.0/r); 01387 } 01388 else 01389 { 01390 s = f / g; 01391 r = sqrt((s * s) + 1.0); 01392 e(i+1) = g * r; 01393 s *= (c = 1.0/r); 01394 } 01395 g = d(i+1) - p; 01396 r = (d(i) - g) * s + 2.0 * c * b; 01397 p = s * r; 01398 d(i+1) = g + p; 01399 g = c * r - b; 01400 for (k = 0; k < m; k++) 01401 { 01402 f = z((i+1)*m+k); 01403 z((i+1)*m+k) = s * z(i*m+k) + c * f; 01404 z(i*m+k) = c * z(i*m+k) - s * f; 01405 } 01406 } 01407 d(l) = d(l) - p; 01408 e(l) = g; 01409 e(n) = 0.0; 01410 } 01411 } while (n != l); 01412 01413 } 01414 01415 for (i = 0; i < m-1; ++i) { 01416 k = i; 01417 tmp = d(i); 01418 for (j = i+1; j < m; j++) { 01419 if (d(j) < tmp) { 01420 k = j; 01421 tmp = d(j); 01422 } 01423 } 01424 if (k != i) { 01425 d(k) = d(i); 01426 d(i) = tmp; 01427 for (j = 0; j < m; j++) { 01428 tmp = z(i*m+j); 01429 z(i*m+j) = z(k*m+j); 01430 z(k*m+j) = tmp; 01431 } 01432 } 01433 } 01434 01435 } 01436 01437 // A is m x n 01438 // U is m x m 01439 // V is n x n 01440 // s is max(m,n)+1 x 1 01441 void NumericLib::svd(mrs_natural m, mrs_natural n, realvec &A, realvec &U, realvec &V, realvec &s) { 01442 01443 mrs_natural nu = min(m,n); 01444 realvec e(n); 01445 realvec work(m); 01446 mrs_natural wantu = 1; /* boolean */ 01447 mrs_natural wantv = 1; /* boolean */ 01448 mrs_natural i=0, j=0, k=0; 01449 01450 // Reduce A to bidiagonal form, storing the diagonal elements 01451 // in s and the super-diagonal elements in e. 01452 mrs_natural nct = min(m-1,n); 01453 mrs_natural nrt = max((mrs_natural) 0,min(n-2,m)); 01454 for (k = 0; k < max(nct,nrt); k++) { 01455 if (k < nct) { 01456 01457 // Compute the transformation for the k-th column and 01458 // place the k-th diagonal in s(k). 01459 // Compute 2-norm of k-th column without under/overflow. 01460 s(k) = 0; 01461 for (i = k; i < m; ++i) { 01462 #ifdef MARSYAS_WIN32 01463 s(k) = _hypot(s(k),A(k*m+i)); 01464 #else 01465 s(k) = hypot(s(k),A(k*m+i)); 01466 #endif 01467 } 01468 if (s(k) != 0.0) { 01469 if (A(k*m+k) < 0.0) { 01470 s(k) = -s(k); 01471 } 01472 for (i = k; i < m; ++i) { 01473 A(k*m+i) /= s(k); 01474 } 01475 A(k*m+k) += 1.0; 01476 } 01477 s(k) = -s(k); 01478 } 01479 for (j = k+1; j < n; j++) { 01480 if ((k < nct) && (s(k) != 0.0)) { 01481 01482 // Apply the transformation. 01483 01484 mrs_real t = 0; 01485 for (i = k; i < m; ++i) { 01486 t += A(k*m+i)*A(j*m+i); 01487 } 01488 t = -t/A(k*m+k); 01489 for (i = k; i < m; ++i) { 01490 A(j*m+i) += t*A(k*m+i); 01491 } 01492 } 01493 01494 // Place the k-th row of A into e for the 01495 // subsequent calculation of the row transformation. 01496 01497 e(j) = A(j*m+k); 01498 } 01499 if (wantu & (k < nct)) { 01500 01501 // Place the transformation in U for subsequent back 01502 // multiplication. 01503 01504 for (i = k; i < m; ++i) { 01505 U(k*m+i) = A(k*m+i); 01506 } 01507 } 01508 if (k < nrt) { 01509 01510 // Compute the k-th row transformation and place the 01511 // k-th super-diagonal in e(k). 01512 // Compute 2-norm without under/overflow. 01513 e(k) = 0; 01514 for (i = k+1; i < n; ++i) { 01515 #ifdef MARSYAS_WIN32 01516 e(k) = _hypot(e(k),e(i)); 01517 #else 01518 e(k) = hypot(e(k),e(i)); 01519 #endif 01520 01521 } 01522 if (e(k) != 0.0) { 01523 if (e(k+1) < 0.0) { 01524 e(k) = -e(k); 01525 } 01526 for (i = k+1; i < n; ++i) { 01527 e(i) /= e(k); 01528 } 01529 e(k+1) += 1.0; 01530 } 01531 e(k) = -e(k); 01532 if ((k+1 < m) & (e(k) != 0.0)) { 01533 01534 // Apply the transformation. 01535 01536 for (i = k+1; i < m; ++i) { 01537 work(i) = 0.0; 01538 } 01539 for (j = k+1; j < n; j++) { 01540 for (i = k+1; i < m; ++i) { 01541 work(i) += e(j)*A(j*m+i); 01542 } 01543 } 01544 for (j = k+1; j < n; j++) { 01545 mrs_real t = -e(j)/e(k+1); 01546 for (i = k+1; i < m; ++i) { 01547 A(j*m+i) += t*work(i); 01548 } 01549 } 01550 } 01551 if (wantv) { 01552 01553 // Place the transformation in V for subsequent 01554 // back multiplication. 01555 01556 for (i = k+1; i < n; ++i) { 01557 V(k*n+i) = e(i); 01558 } 01559 } 01560 } 01561 } 01562 01563 // Set up the final bidiagonal matrix or order p. 01564 01565 mrs_natural p = min(n,m+1); 01566 if (nct < n) { 01567 s(nct) = A(nct*m+nct); 01568 } 01569 if (m < p) { 01570 s(p-1) = 0.0; 01571 } 01572 if (nrt+1 < p) { 01573 e(nrt) = A((p-1)*m+nrt); 01574 } 01575 e(p-1) = 0.0; 01576 01577 // If required, generate U. 01578 01579 if (wantu) { 01580 for (j = nct; j < nu; j++) { 01581 for (i = 0; i < m; ++i) { 01582 U(j*m+i) = 0.0; 01583 } 01584 U(j*m+j) = 1.0; 01585 } 01586 for (k = nct-1; k >= 0; k--) { 01587 if (s(k) != 0.0) { 01588 for (j = k+1; j < nu; j++) { 01589 mrs_real t = 0; 01590 for (i = k; i < m; ++i) { 01591 t += U(k*m+i)*U(j*m+i); 01592 } 01593 t = -t/U(k*m+k); 01594 for (i = k; i < m; ++i) { 01595 U(j*m+i) += t*U(k*m+i); 01596 } 01597 } 01598 for (i = k; i < m; ++i ) { 01599 U(k*m+i) = -U(k*m+i); 01600 } 01601 U(k*m+k) = 1.0 + U(k*m+k); 01602 for (i = 0; i < k-1; ++i) { 01603 U(k*m+i) = 0.0; 01604 } 01605 } else { 01606 for (i = 0; i < m; ++i) { 01607 U(k*m+i) = 0.0; 01608 } 01609 U(k*m+k) = 1.0; 01610 } 01611 } 01612 } 01613 01614 // If required, generate V. 01615 01616 if (wantv) { 01617 for (k = n-1; k >= 0; k--) { 01618 if ((k < nrt) & (e(k) != 0.0)) { 01619 for (j = k+1; j < nu; j++) { 01620 mrs_real t = 0; 01621 for (i = k+1; i < n; ++i) { 01622 t += V(k*n+i)*V(j*n+i); 01623 } 01624 t = -t/V(k*n+(k+1)); 01625 for (i = k+1; i < n; ++i) { 01626 V(j*n+i) += t*V(k*n+i); 01627 } 01628 } 01629 } 01630 for (i = 0; i < n; ++i) { 01631 V(k*n+i) = 0.0; 01632 } 01633 V(k*n+k) = 1.0; 01634 } 01635 } 01636 01637 // Main iteration loop for the singular values. 01638 01639 mrs_natural pp = p-1; 01640 mrs_natural iter = 0; 01641 // mrs_real eps = machp("E"); //pow(2.0,-52.0); 01642 mrs_real eps = std::numeric_limits<double>::epsilon(); 01643 01644 01645 01646 while (p > 0) { 01647 mrs_natural k=0; 01648 mrs_natural kase=0; 01649 01650 // Here is where a test for too many iterations would go. 01651 01652 // This section of the program inspects for 01653 // negligible elements in the s and e arrays. On 01654 // completion the variables kase and k are set as follows. 01655 01656 // kase = 1 if s(p) and e(k-1) are negligible and k<p 01657 // kase = 2 if s(k) is negligible and k<p 01658 // kase = 3 if e(k-1) is negligible, k<p, and 01659 // s(k), ..., s(p) are not negligible (qr step). 01660 // kase = 4 if e(p-1) is negligible (convergence). 01661 01662 for (k = p-2; k >= -1; k--) { 01663 if (k == -1) { 01664 break; 01665 } 01666 if (fabs(e(k)) <= eps*(fabs(s(k)) + fabs(s(k+1)))) { 01667 e(k) = 0.0; 01668 break; 01669 } 01670 } 01671 if (k == p-2) { 01672 kase = 4; 01673 } else { 01674 mrs_natural ks; 01675 for (ks = p-1; ks >= k; ks--) { 01676 if (ks == k) { 01677 break; 01678 } 01679 mrs_real t = (ks != p ? fabs(e(ks)) : 0.) + 01680 (ks != k+1 ? fabs(e(ks-1)) : 0.); 01681 if (fabs(s(ks)) <= eps*t) { 01682 s(ks) = 0.0; 01683 break; 01684 } 01685 } 01686 if (ks == k) { 01687 kase = 3; 01688 } else if (ks == p-1) { 01689 kase = 1; 01690 } else { 01691 kase = 2; 01692 k = ks; 01693 } 01694 } 01695 k++; 01696 01697 // Perform the task indicated by kase. 01698 01699 switch (kase) { 01700 01701 // Deflate negligible s(p). 01702 01703 case 1: { 01704 mrs_real f = e(p-2); 01705 e(p-2) = 0.0; 01706 for (j = p-2; j >= k; j--) { 01707 #ifdef MARSYAS_WIN32 01708 mrs_real t = _hypot(s(j),f); 01709 #else 01710 mrs_real t = hypot(s(j),f); 01711 #endif 01712 01713 mrs_real cs = s(j)/t; 01714 mrs_real sn = f/t; 01715 s(j) = t; 01716 if (j != k) { 01717 f = -sn*e(j-1); 01718 e(j-1) = cs*e(j-1); 01719 } 01720 if (wantv) { 01721 for (i = 0; i < n; ++i) { 01722 t = cs*V(j*n+i) + sn*V((p-1)*n+i); 01723 V((p-1)*n+i) = -sn*V(j*n+i) + cs*V((p-1)*n+i); 01724 V(j*n+i) = t; 01725 } 01726 } 01727 } 01728 } 01729 break; 01730 01731 // Split at negligible s(k). 01732 01733 case 2: { 01734 mrs_real f = e(k-1); 01735 e(k-1) = 0.0; 01736 for (j = k; j < p; j++) { 01737 #ifdef MARSYAS_WIN32 01738 mrs_real t = _hypot(s(j),f); 01739 #else 01740 mrs_real t = hypot(s(j),f); 01741 #endif 01742 mrs_real cs = s(j)/t; 01743 mrs_real sn = f/t; 01744 s(j) = t; 01745 f = -sn*e(j); 01746 e(j) = cs*e(j); 01747 if (wantu) { 01748 for (i = 0; i < m; ++i) { 01749 t = cs*U(j*m+i) + sn*U((k-1)*m+i); 01750 U((k-1)*m+i) = -sn*U(j*m+i) + cs*U((k-1)*m+i); 01751 U(j*m+i) = t; 01752 } 01753 } 01754 } 01755 } 01756 break; 01757 01758 // Perform one qr step. 01759 01760 case 3: { 01761 01762 // Calculate the shift. 01763 01764 mrs_real scale = max(max(max(max( 01765 fabs(s(p-1)),fabs(s(p-2))),fabs(e(p-2))), 01766 fabs(s(k))),fabs(e(k))); 01767 mrs_real sp = s(p-1)/scale; 01768 mrs_real spm1 = s(p-2)/scale; 01769 mrs_real epm1 = e(p-2)/scale; 01770 mrs_real sk = s(k)/scale; 01771 mrs_real ek = e(k)/scale; 01772 mrs_real b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)/2.0; 01773 mrs_real c = (sp*epm1)*(sp*epm1); 01774 mrs_real shift = 0.0; 01775 if ((b != 0.0) || (c != 0.0)) { 01776 shift = sqrt(b*b + c); 01777 if (b < 0.0) { 01778 shift = -shift; 01779 } 01780 shift = c/(b + shift); 01781 } 01782 mrs_real f = (sk + sp)*(sk - sp) + shift; 01783 mrs_real g = sk*ek; 01784 01785 // Chase zeros. 01786 01787 for (j = k; j < p-1; j++) { 01788 #ifdef MARSYAS_WIN32 01789 mrs_real t = _hypot(f,g); 01790 #else 01791 mrs_real t = hypot(f,g); 01792 #endif 01793 01794 mrs_real cs = f/t; 01795 mrs_real sn = g/t; 01796 if (j != k) { 01797 e(j-1) = t; 01798 } 01799 f = cs*s(j) + sn*e(j); 01800 e(j) = cs*e(j) - sn*s(j); 01801 g = sn*s(j+1); 01802 s(j+1) = cs*s(j+1); 01803 if (wantv) { 01804 for (i = 0; i < n; ++i) { 01805 t = cs*V(j*n+i) + sn*V((j+1)*n+i); 01806 V((j+1)*n+i) = -sn*V(j*n+i) + cs*V((j+1)*n+i); 01807 V(j*n+i) = t; 01808 } 01809 } 01810 #ifdef MARSYAS_WIN32 01811 t = _hypot(f,g); 01812 #else 01813 t = hypot(f,g); 01814 #endif 01815 cs = f/t; 01816 sn = g/t; 01817 s(j) = t; 01818 f = cs*e(j) + sn*s(j+1); 01819 s(j+1) = -sn*e(j) + cs*s(j+1); 01820 g = sn*e(j+1); 01821 e(j+1) = cs*e(j+1); 01822 if (wantu && (j < m-1)) { 01823 for (i = 0; i < m; ++i) { 01824 t = cs*U(j*m+i) + sn*U((j+1)*m+i); 01825 U((j+1)*m+i) = -sn*U(j*m+i) + cs*U((j+1)*m+i); 01826 U(j*m+i) = t; 01827 } 01828 } 01829 } 01830 e(p-2) = f; 01831 iter = iter + 1; 01832 } 01833 break; 01834 01835 // Convergence. 01836 01837 case 4: { 01838 01839 // Make the singular values positive. 01840 01841 if (s(k) <= 0.0) { 01842 s(k) = (s(k) < 0.0 ? -s(k) : 0.0); 01843 if (wantv) { 01844 for (i = 0; i <= pp; ++i) { 01845 V(k*n+i) = -V(k*n+i); 01846 } 01847 } 01848 } 01849 01850 // Order the singular values. 01851 01852 while (k < pp) { 01853 if (s(k) >= s(k+1)) { 01854 break; 01855 } 01856 mrs_real t = s(k); 01857 s(k) = s(k+1); 01858 s(k+1) = t; 01859 if (wantv && (k < n-1)) { 01860 for (i = 0; i < n; ++i) { 01861 t = V((k+1)*n+i); V((k+1)*n+i) = V(k*n+i); V(k*n+i) = t; 01862 } 01863 } 01864 if (wantu && (k < m-1)) { 01865 for (i = 0; i < m; ++i) { 01866 t = U((k+1)*m+i); U((k+1)*m+i) = U(k*m+i); U(k*m+i) = t; 01867 } 01868 } 01869 k++; 01870 } 01871 iter = 0; 01872 p--; 01873 } 01874 break; 01875 } 01876 } 01877 } 01878 01880 // METRICS 01882 mrs_real 01883 NumericLib::euclideanDistance(const realvec& Vi, const realvec& Vj, const realvec& covMatrix) 01884 { 01885 mrs_real res1; 01886 mrs_real res = 0; 01887 01888 //if no variances are passed as argument (i.e. empty covMatrix), 01889 //just do a plain euclidean computation 01890 if(covMatrix.getSize() == 0) 01891 { 01892 for (mrs_natural r=0 ; r < Vi.getSize() ; ++r) 01893 { 01894 res1 = Vi(r)-Vj(r); 01895 res1 *= res1; //square 01896 res += res1; //summation 01897 } 01898 res = sqrt(res); 01899 } 01900 else if (covMatrix.sum () > 0) 01901 { 01902 // do a standardized L2 euclidean distance 01903 //(i.e. just use the diagonal elements of covMatrix) 01904 for (mrs_natural r=0 ; r < Vi.getSize() ; ++r) 01905 { 01906 res1 = Vi(r)-Vj(r); 01907 res1 *= res1; //square 01908 res1 /= covMatrix(r,r); //divide by var of each feature 01909 res += res1; //summation 01910 } 01911 res = sqrt(res); 01912 } 01913 return res; 01914 } 01915 01916 mrs_real 01917 NumericLib::mahalanobisDistance(const realvec& Vi, const realvec& Vj, const realvec& covMatrix) 01918 { 01919 // These remove warnings about unused parameters. 01920 (void) Vi; 01921 (void) Vj; 01922 (void) covMatrix; 01923 //realvec invCovMatrix; 01924 //covMatrix.invert(invCovMatrix); 01925 01926 //NOT IMPLEMENTED YET!!!! [!] 01927 return MINREAL; 01928 } 01929 01930 mrs_real 01931 NumericLib::cosineDistance(const realvec& Vi, const realvec& Vj, const realvec& dummy) 01932 { 01933 (void) dummy; 01934 //as defined in: 01935 //http://www.mathworks.com/access/helpdesk/help/toolbox/stats/index.html?/access/helpdesk/help/toolbox/stats/pdist.html 01936 mrs_real res1 = 0; 01937 mrs_real res2 = 0; 01938 mrs_real res3 = 0; 01939 mrs_real res = 0; 01940 for (mrs_natural r=0 ; r < Vi.getSize() ; ++r) 01941 { 01942 res1 += Vi(r)*Vj(r); 01943 res2 += Vi(r)*Vi(r); 01944 res3 += Vj(r)*Vj(r); 01945 } 01946 if (res2!=0 && res3!=0) 01947 { 01948 res = res1/sqrt(res2 * res3); 01949 if(res > 1.0) //cosine similarity should never be bigger than 1.0!! 01950 { 01951 if (res-1.0 > 1e-6) { 01952 MRSWARN("NumericLib::cosineDistance() - cosine similarity value is > 1.0 by " << res-1.0 << " -> setting value to 1.0!"); 01953 } 01954 res = 1.0; 01955 } 01956 return (1.0 - res); //return DISTANCE (and not the cosine similarity) 01957 } 01958 else 01959 { 01960 MRSERR("NumericLib::cosineDistance() - at least one of the input points have small relative magnitudes, making it effectively zero... returning invalid value of -1.0!"); 01961 return -1.0; 01962 } 01963 } 01964 01965 mrs_real 01966 NumericLib::cityblockDistance(const realvec& Vi, const realvec& Vj, const realvec& dummy) 01967 { 01968 (void) Vi; (void) Vj; (void) dummy; // Remove warnings about unused parameters 01969 return MINREAL; //NOT IMPLEMENTED YET!!!! [!] 01970 } 01971 01972 mrs_real 01973 NumericLib::correlationDistance(const realvec& Vi, const realvec& Vj, const realvec& dummy) 01974 { 01975 (void) Vi; (void) Vj; (void) dummy; // Remove warnings about unused parameters 01976 return MINREAL; //NOT IMPLEMENTED YET!!!! [!] 01977 } 01978 01979 mrs_real 01980 NumericLib::divergenceShape(const realvec& Ci, const realvec& Cj, const realvec& dummy) 01981 { 01982 (void) dummy; 01984 if(Ci.getCols() != Cj.getCols() && Ci.getRows() != Cj.getRows() && 01985 Ci.getCols()!= Ci.getRows()) 01986 { 01987 MRSERR("realvec::divergenceShape() : input matrices should be square and equal sized. Returning invalid value (-1.0)"); 01988 return -1.0; 01989 } 01990 01991 realvec Cii = Ci; 01992 realvec Cjj = Cj; 01993 01994 mrs_real res; 01995 01996 realvec Citemp(Cii.getRows(), Cii.getCols()); 01997 realvec invCi(Cii); 01998 01999 realvec Cjtemp(Cjj.getRows(),Cjj.getCols()); 02000 realvec invCj(Cjj); 02001 02002 #ifdef _MATLAB_REALVEC_ 02003 MATLAB_PUT(Cii, "Ci"); 02004 MATLAB_PUT(Cjj, "Cj"); 02005 MATLAB_PUT(Citemp, "Citemp"); 02006 MATLAB_PUT(Cjtemp, "Cjtemp"); 02007 #endif 02008 02009 02010 invCi.invert(Citemp); 02011 invCj.invert(Cjtemp); 02012 02013 #ifdef _MATLAB_REALVEC_ 02014 MATLAB_PUT(Citemp, "Citemp2"); 02015 MATLAB_PUT(Cjtemp, "Cjtemp2"); 02016 MATLAB_PUT(invCi, "invCi"); 02017 MATLAB_PUT(invCj, "invCj"); 02018 #endif 02019 02020 Cjj *= (-1.0); 02021 Cii += Cjj; 02022 02023 #ifdef _MATLAB_REALVEC_ 02024 MATLAB_PUT(Cii, "Ci_minus_Cj"); 02025 #endif 02026 02027 invCi *= (-1.0); 02028 invCj += invCi; 02029 02030 #ifdef _MATLAB_REALVEC_ 02031 MATLAB_PUT(invCj, "invCj_minus_invCi"); 02032 #endif 02033 02034 Cii *= invCj; 02035 02036 res = Cii.trace() / 2.0; 02037 02038 #ifdef _MATLAB_REALVEC_ 02039 MATLAB_PUT(Cii, "divergenceMatrix"); 02040 MATLAB_PUT(res, "divergence"); 02041 #endif 02042 02043 return res; 02044 } 02045 02046 mrs_real 02047 NumericLib::bhattacharyyaShape(const realvec& Ci, const realvec& Cj, const realvec& dummy) 02048 { 02049 (void) dummy; 02051 if(Ci.getCols() != Cj.getCols() && Ci.getRows() != Cj.getRows() && 02052 Ci.getCols()!= Ci.getRows()) 02053 { 02054 MRSERR("realvec::bhattacharyyaShape() : input matrices should be square and equal sized. Returning invalid value (-1.0)"); 02055 return -1.0; 02056 } 02057 02058 realvec Cii = Ci; 02059 realvec Cjj = Cj; 02060 02061 //denominator 02062 mrs_real sqrtdetCi = sqrt(Cii.det()); 02063 mrs_real sqrtdetCj = sqrt(Cjj.det()); 02064 mrs_real den = sqrtdetCi * sqrtdetCj; 02065 #ifdef _MATLAB_REALVEC_ 02066 MATLAB_PUT(Cii, "Ci"); 02067 MATLAB_PUT(Cjj, "Cj"); 02068 MATLAB_PUT(sqrtdetCi, "sqrtdetCi"); 02069 MATLAB_PUT(sqrtdetCj, "sqrtdetCj"); 02070 MATLAB_PUT(den, "den"); 02071 #endif 02072 02073 //numerator 02074 Cii += Cjj; 02075 Cii /= 2.0; 02076 mrs_real num = Cii.det(); 02077 02078 //bhattacharyyaShape 02079 return log(num/den); 02080 } 02081 02083 // hungarian assignement methods 02085 mrs_real 02086 NumericLib::hungarianAssignment(realvec& matrixdist, realvec& matrixAssign) 02087 { 02088 mrs_real cost; 02089 02090 mrs_natural rows = matrixdist.getRows(); 02091 mrs_natural cols = matrixdist.getCols(); 02092 02093 if(matrixAssign.getCols() != cols || matrixAssign.getRows() != 1) 02094 { 02095 MRSERR("NumericLib::hungarianAssignemnt - wrong size for matrix Assign!"); 02096 return -1.0; 02097 } 02098 02099 //copy input data [!] 02100 mrs_real* distMatrix = new mrs_real[rows * cols]; 02101 for(mrs_natural r=0; r < rows; ++r) 02102 for(mrs_natural c = 0; c < cols; ++c) 02103 distMatrix[r*cols + c] = matrixdist(r,c); 02104 02105 mrs_natural* assignement = new mrs_natural[cols]; 02106 02107 assignmentoptimal(assignement, &cost, distMatrix, rows, cols); 02108 02109 //copy resulting assignment to matrixAssign [!] 02110 for(mrs_natural i = 0; i < cols; ++i) 02111 matrixAssign(i) = assignement[i]; 02112 02113 delete [] distMatrix; 02114 delete [] assignement; 02115 02116 return cost; 02117 } 02118 02119 mrs_real 02120 NumericLib::mxGetInf() 02121 { 02122 return numeric_limits<double>::infinity(); 02123 } 02124 bool 02125 NumericLib::mxIsInf(mrs_real s) 02126 { 02127 return s == numeric_limits<double>::infinity() || s == -numeric_limits<double>::infinity(); 02128 } 02129 02130 void 02131 NumericLib::mxFree( void * s ) 02132 { 02133 free ( s ); 02134 } 02135 02136 void 02137 NumericLib::mexErrMsgTxt(const char * s) 02138 { 02139 cout << s << endl; 02140 } 02141 02142 void 02143 NumericLib::assignmentoptimal(mrs_natural *assignment, mrs_real *cost, mrs_real *distMatrixIn, mrs_natural nOfRows, mrs_natural nOfColumns) 02144 { 02145 02146 mrs_real *distMatrix, *distMatrixTemp, value, minValue; //*columnEnd, 02147 bool *coveredColumns, *coveredRows, *starMatrix, *newStarMatrix, *primeMatrix; 02148 mrs_natural nOfElements, minDim, row, col; 02149 #ifdef CHECK_FOR_INF 02150 bool infiniteValueFound; 02151 mrs_real maxFiniteValue, infValue; 02152 #endif 02153 02154 /* initialization */ 02155 *cost = 0; 02156 for(row=0; row<nOfRows; row++) 02157 //#ifdef ONE_INDEXING 02158 // assignment[row] = 0.0; 02159 //#else 02160 assignment[row] = -1;//.0; 02161 //#endif 02162 02163 /* generate working copy of distance Matrix */ 02164 /* check if all matrix elements are positive */ 02165 nOfElements = nOfRows * nOfColumns; 02166 distMatrix = (mrs_real *)malloc(nOfElements * sizeof(mrs_real)); 02167 //distMatrix aponta para o primeiro elemento por causa do malloc; 02168 //quando soma mais nOfElements aponta para o final 02169 for(row=0; row<nOfElements; row++) 02170 { 02171 value = distMatrixIn[row]; 02172 //if(mxIsFinite(value) && (value < 0)) 02173 if((( value > -numeric_limits<double>::infinity() && value < numeric_limits<double>::infinity())) && (value < 0)) 02174 mexErrMsgTxt("All matrix elements have to be non-negative."); 02175 distMatrix[row] = value; 02176 } 02177 02178 02179 02180 #ifdef CHECK_FOR_INF 02181 mrs_real *distMatrixEnd = distMatrix + nOfElements; 02182 /* check for infinite values */ 02183 maxFiniteValue = -1; 02184 infiniteValueFound = false; 02185 02186 distMatrixTemp = distMatrix; 02187 while(distMatrixTemp < distMatrixEnd) 02188 { 02189 value = *distMatrixTemp++; 02190 //if(mxIsFinite(value)) 02191 if ( value > -numeric_limits<double>::infinity() && value < numeric_limits<double>::infinity()) 02192 { 02193 if(value > maxFiniteValue) 02194 maxFiniteValue = value; 02195 } 02196 else 02197 infiniteValueFound = true; 02198 } 02199 if(infiniteValueFound) 02200 { 02201 if(maxFiniteValue == -1) /* all elements are infinite */ 02202 return; 02203 02204 /* set all infinite elements to big finite value */ 02205 if(maxFiniteValue > 0) 02206 infValue = 10 * maxFiniteValue * nOfElements; 02207 else 02208 infValue = 10; 02209 distMatrixTemp = distMatrix; 02210 while(distMatrixTemp < distMatrixEnd) 02211 if(mxIsInf(*distMatrixTemp++)) 02212 *(distMatrixTemp-1) = infValue; 02213 } 02214 #endif 02215 02216 /* memory allocation */ 02217 /* 02218 coveredColumns = (bool *)mxCalloc(nOfColumns, sizeof(bool)); 02219 coveredRows = (bool *)mxCalloc(nOfRows, sizeof(bool)); 02220 starMatrix = (bool *)mxCalloc(nOfElements, sizeof(bool)); 02221 primeMatrix = (bool *)mxCalloc(nOfElements, sizeof(bool)); 02222 newStarMatrix = (bool *)mxCalloc(nOfElements, sizeof(bool)); *//* used in step4 */ 02223 02224 coveredColumns = (bool *)calloc(nOfColumns, sizeof(bool)); 02225 coveredRows = (bool *)calloc(nOfRows, sizeof(bool)); 02226 starMatrix = (bool *)calloc(nOfElements, sizeof(bool)); 02227 primeMatrix = (bool *)calloc(nOfElements, sizeof(bool)); 02228 newStarMatrix = (bool *)calloc(nOfElements, sizeof(bool)); 02229 02230 /* preliminary steps */ 02231 if(nOfRows <= nOfColumns) 02232 { 02233 minDim = nOfRows; 02234 02235 for(row=0; row<nOfRows; row++) 02236 { 02237 /* find the smallest element in the row */ 02238 //distMatrixTemp = distMatrix + row; 02239 //minValue = *distMatrixTemp; 02240 //distMatrixTemp += nOfRows; 02241 distMatrixTemp = distMatrix + row*nOfColumns; 02242 minValue = *distMatrixTemp; 02243 //while(distMatrixTemp < distMatrixEnd) 02244 //{ 02245 // value = *distMatrixTemp; 02246 // if(value < minValue) 02247 // minValue = value; 02248 // distMatrixTemp += nOfRows; 02249 //} 02250 for (mrs_natural i=1; i< nOfColumns; ++i) { 02251 value = *(distMatrixTemp++); 02252 if (value < minValue) 02253 minValue = value; 02254 } 02255 02256 /* subtract the smallest element from each element of the row */ 02257 //distMatrixTemp = distMatrix + row; 02258 //while(distMatrixTemp < distMatrixEnd) 02259 //{ 02260 // *distMatrixTemp -= minValue; 02261 // distMatrixTemp += nOfRows; 02262 //} 02263 distMatrixTemp = distMatrix + row*nOfColumns; 02264 for (mrs_natural i=0; i< nOfColumns; ++i) 02265 *(distMatrixTemp++) -= minValue; 02266 02267 } 02268 02269 /* Steps 1 and 2a */ 02270 //for(row=0; row<nOfRows; row++) 02271 // for(col=0; col<nOfColumns; col++) 02272 // if(distMatrix[row + nOfRows*col] == 0) 02273 // if(!coveredColumns[col]) 02274 // { 02275 // starMatrix[row + nOfRows*col] = true; 02276 // coveredColumns[col] = true; 02277 // break; 02278 // } 02279 for(row=0; row<nOfRows; row++) 02280 for(col=0; col<nOfColumns; col++) 02281 if(distMatrix[row*nOfColumns + col] == 0) 02282 if(!coveredColumns[col]) 02283 { 02284 starMatrix[row*nOfColumns + col] = true; 02285 coveredColumns[col] = true; 02286 break; 02287 } 02288 02289 } 02290 else /* if(nOfRows > nOfColumns) */ 02291 { 02292 minDim = nOfColumns; 02293 02294 for(col=0; col<nOfColumns; col++) 02295 { 02296 /* find the smallest element in the column */ 02297 //distMatrixTemp = distMatrix + nOfRows*col; 02298 //columnEnd = distMatrixTemp + nOfRows; 02299 distMatrixTemp = distMatrix + col; 02300 02301 //minValue = *distMatrixTemp++; 02302 minValue = *distMatrixTemp; 02303 //while(distMatrixTemp < columnEnd) 02304 //{ 02305 // value = *distMatrixTemp++; 02306 // if(value < minValue) 02307 // minValue = value; 02308 //} 02309 for (mrs_natural i=1; i<nOfRows; ++i) { 02310 value = *(distMatrixTemp + nOfColumns*i); 02311 if (value < minValue) 02312 minValue = value; 02313 } 02314 02315 /* subtract the smallest element from each element of the column */ 02316 //distMatrixTemp = distMatrix + nOfRows*col; 02317 //while(distMatrixTemp < columnEnd) 02318 // *distMatrixTemp++ -= minValue; 02319 distMatrixTemp = distMatrix + col; 02320 for (mrs_natural i=0; i<nOfRows; ++i) 02321 *(distMatrixTemp + nOfColumns*i) -= minValue; 02322 02323 } 02324 02325 /* Steps 1 and 2a */ 02326 for(col=0; col<nOfColumns; col++) 02327 for(row=0; row<nOfRows; row++) 02328 if(distMatrix[row*nOfColumns + col] == 0) 02329 if(!coveredRows[row]) 02330 { 02331 starMatrix[row*nOfColumns + col] = true; 02332 coveredColumns[col] = true; 02333 coveredRows[row] = true; 02334 break; 02335 } 02336 for(row=0; row<nOfRows; row++) 02337 coveredRows[row] = false; 02338 02339 } 02340 02341 02342 /* move to step 2b */ 02343 step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); 02344 02345 /* compute cost and remove invalid assignments */ 02346 computeassignmentcost(assignment, cost, distMatrixIn, nOfRows, nOfColumns); 02347 02348 /* free allocated memory */ 02349 mxFree(distMatrix); 02350 mxFree(coveredColumns); 02351 mxFree(coveredRows); 02352 mxFree(starMatrix); 02353 mxFree(primeMatrix); 02354 mxFree(newStarMatrix); 02355 02356 return; 02357 } 02358 02359 void 02360 NumericLib::buildassignmentvector(mrs_natural *assignment, bool *starMatrix, mrs_natural nOfRows, mrs_natural nOfColumns) 02361 { 02362 mrs_natural row, col; 02363 02364 for(row=0; row<nOfRows; row++) 02365 for(col=0; col<nOfColumns; col++) 02366 //if(starMatrix[row + nOfRows*col]) 02367 if(starMatrix[row*nOfColumns + col]) 02368 { 02369 //#ifdef ONE_INDEXING 02370 // assignment[row] = col + 1; /* MATLAB-Indexing */ 02371 //#else 02372 assignment[row] = col; 02373 //#endif 02374 break; 02375 } 02376 } 02377 02378 void 02379 NumericLib::computeassignmentcost(mrs_natural *assignment, mrs_real *cost, mrs_real *distMatrix, mrs_natural nOfRows, mrs_natural nOfColumns) 02380 { 02381 mrs_natural row, col; 02382 #ifdef CHECK_FOR_INF 02383 mrs_real value; 02384 #endif 02385 02386 for(row=0; row<nOfRows; row++) 02387 { 02388 //#ifdef ONE_INDEXING 02389 // col = assignment[row]-1; /* MATLAB-Indexing */ 02390 //#else 02391 col = assignment[row]; 02392 //#endif 02393 02394 if(col >= 0) 02395 { 02396 #ifdef CHECK_FOR_INF 02397 value = distMatrix[row + nOfRows*col]; 02398 //if(mxIsFinite(value)) 02399 if( value > -numeric_limits<double>::infinity() && value < numeric_limits<double>::infinity()) 02400 *cost += value; 02401 else 02402 //#ifdef ONE_INDEXING 02403 // assignment[row] = 0.0; 02404 //#else 02405 assignment[row] = -1.0; 02406 //#endif 02407 02408 #else 02409 //*cost += distMatrix[row + nOfRows*col]; 02410 *cost += distMatrix[row*nOfColumns + col]; 02411 #endif 02412 } 02413 } 02414 } 02415 02416 void 02417 NumericLib::step2a(mrs_natural *assignment, mrs_real *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, mrs_natural nOfRows, mrs_natural nOfColumns, mrs_natural minDim) 02418 { 02419 bool *starMatrixTemp; //*columnEnd; 02420 mrs_natural col; 02421 02422 /* cover every column containing a starred zero */ 02423 for(col=0; col<nOfColumns; col++) 02424 { 02425 //starMatrixTemp = starMatrix + nOfRows*col; 02426 //columnEnd = starMatrixTemp + nOfRows; 02427 //while(starMatrixTemp < columnEnd){ 02428 // if(*starMatrixTemp++) 02429 // { 02430 // coveredColumns[col] = true; 02431 // break; 02432 // } 02433 //} 02434 starMatrixTemp = starMatrix + col; 02435 for (mrs_natural i=0; i< nOfRows; ++i) { 02436 if (*(starMatrixTemp + nOfColumns*i)) { 02437 coveredColumns[col] = true; 02438 break; 02439 } 02440 } 02441 } 02442 02443 /* move to step 3 */ 02444 step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); 02445 } 02446 02447 void 02448 NumericLib::step2b(mrs_natural *assignment, mrs_real *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, mrs_natural nOfRows, mrs_natural nOfColumns, mrs_natural minDim) 02449 { 02450 mrs_natural col, nOfCoveredColumns; 02451 02452 /* count covered columns */ 02453 nOfCoveredColumns = 0; 02454 for(col=0; col<nOfColumns; col++) 02455 if(coveredColumns[col]) 02456 nOfCoveredColumns++; 02457 02458 if(nOfCoveredColumns == minDim) 02459 { 02460 /* algorithm finished */ 02461 buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns); 02462 } 02463 else 02464 { 02465 /* move to step 3 */ 02466 step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); 02467 } 02468 02469 } 02470 02471 void 02472 NumericLib::step3(mrs_natural *assignment, mrs_real *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, mrs_natural nOfRows, mrs_natural nOfColumns, mrs_natural minDim) 02473 { 02474 bool zerosFound; 02475 mrs_natural row, col, starCol; 02476 02477 zerosFound = true; 02478 while(zerosFound) 02479 { 02480 zerosFound = false; 02481 for(col=0; col<nOfColumns; col++) 02482 if(!coveredColumns[col]) 02483 for(row=0; row<nOfRows; row++) 02484 //if((!coveredRows[row]) && (distMatrix[row + nOfRows*col] == 0)) 02485 if((!coveredRows[row]) && (distMatrix[row*nOfColumns + col] == 0)) 02486 { 02487 /* prime zero */ 02488 //primeMatrix[row + nOfRows*col] = true; 02489 primeMatrix[row*nOfColumns + col] = true; 02490 02491 /* find starred zero in current row */ 02492 for(starCol=0; starCol<nOfColumns; starCol++) 02493 //if(starMatrix[row + nOfRows*starCol]) 02494 if(starMatrix[row*nOfColumns + starCol]) 02495 break; 02496 02497 02498 if(starCol == nOfColumns) /* no starred zero found */ 02499 { 02500 /* move to step 4 */ 02501 step4(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim, row, col); 02502 return; 02503 } 02504 else 02505 { 02506 coveredRows[row] = true; 02507 coveredColumns[starCol] = false; 02508 zerosFound = true; 02509 break; 02510 } 02511 } 02512 } 02513 02514 /* move to step 5 */ 02515 step5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); 02516 } 02517 02518 void 02519 NumericLib::step4(mrs_natural *assignment, mrs_real *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, mrs_natural nOfRows, mrs_natural nOfColumns, mrs_natural minDim, mrs_natural row, mrs_natural col) 02520 { 02521 mrs_natural n, starRow, starCol, primeRow, primeCol; 02522 mrs_natural nOfElements = nOfRows*nOfColumns; 02523 02524 /* generate temporary copy of starMatrix */ 02525 for(n=0; n<nOfElements; n++) 02526 newStarMatrix[n] = starMatrix[n]; 02527 02528 /* star current zero */ 02529 //newStarMatrix[row + nOfRows*col] = true; 02530 newStarMatrix[row*nOfColumns + col] = true; 02531 02532 /* find starred zero in current column */ 02533 starCol = col; 02534 for(starRow=0; starRow<nOfRows; starRow++) 02535 //if(starMatrix[starRow + nOfRows*starCol]) 02536 if(starMatrix[starRow*nOfColumns + starCol]) 02537 break; 02538 02539 while(starRow<nOfRows) 02540 { 02541 /* unstar the starred zero */ 02542 //newStarMatrix[starRow + nOfRows*starCol] = false; 02543 newStarMatrix[starRow*nOfColumns + starCol] = false; 02544 02545 /* find primed zero in current row */ 02546 primeRow = starRow; 02547 for(primeCol=0; primeCol<nOfColumns; primeCol++) 02548 //if(primeMatrix[primeRow + nOfRows*primeCol]) 02549 if(primeMatrix[primeRow*nOfColumns + primeCol]) 02550 break; 02551 02552 /* star the primed zero */ 02553 //newStarMatrix[primeRow + nOfRows*primeCol] = true; 02554 newStarMatrix[primeRow*nOfColumns + primeCol] = true; 02555 02556 /* find starred zero in current column */ 02557 starCol = primeCol; 02558 for(starRow=0; starRow<nOfRows; starRow++) 02559 /*if(starMatrix[starRow + nOfRows*starCol])*/ 02560 if(starMatrix[starRow*nOfColumns + starCol]) 02561 break; 02562 } 02563 02564 /* use temporary copy as new starMatrix */ 02565 /* delete all primes, uncover all rows */ 02566 for(n=0; n<nOfElements; n++) 02567 { 02568 primeMatrix[n] = false; 02569 starMatrix[n] = newStarMatrix[n]; 02570 } 02571 for(n=0; n<nOfRows; n++) 02572 coveredRows[n] = false; 02573 02574 /* move to step 2a */ 02575 step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); 02576 } 02577 02578 void 02579 NumericLib::step5(mrs_natural *assignment, mrs_real *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, mrs_natural nOfRows, mrs_natural nOfColumns, mrs_natural minDim) 02580 { 02581 mrs_real h, value; 02582 mrs_natural row, col; 02583 02584 /* find smallest uncovered element h */ 02585 h = mxGetInf(); 02586 for(row=0; row<nOfRows; row++) 02587 if(!coveredRows[row]) 02588 for(col=0; col<nOfColumns; col++) 02589 if(!coveredColumns[col]) 02590 { 02591 //value = distMatrix[row + nOfRows*col]; 02592 value = distMatrix[row*nOfColumns + col]; 02593 if(value < h) 02594 h = value; 02595 } 02596 02597 /* add h to each covered row */ 02598 for(row=0; row<nOfRows; row++) 02599 if(coveredRows[row]) 02600 for(col=0; col<nOfColumns; col++) 02601 //distMatrix[row + nOfRows*col] += h; 02602 distMatrix[row*nOfColumns + col] += h; 02603 02604 /* subtract h from each uncovered column */ 02605 for(col=0; col<nOfColumns; col++) 02606 if(!coveredColumns[col]) 02607 for(row=0; row<nOfRows; row++) 02608 /*distMatrix[row + nOfRows*col] -= h;*/ 02609 distMatrix[row*nOfColumns + col] -= h; 02610 02611 /* move to step 3 */ 02612 step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); 02613 }