Marsyas  0.6.0-alpha
/usr/src/RPM/BUILD/marsyas-0.6.0/src/marsyas/NumericLib.cpp
Go to the documentation of this file.
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 }