CO2INJECTION: numericmethods.cpp Source File

00001 #include "numericmethods.h"
00002 #include <fstream>
00003 #define NUMERIC_METHODS_TOLERANCE (1.e-30)
00004 
00005 
00006 unsigned NumericMethods::CellDirectionToCellNeighborsIndexTbl[4][2] = {{2,1},{1,2},{0,1},{1,0}};
00007 
00008 
00009 double NumericMethods::interpolateLinear1D(double X,double Px1,double Py1,double Px2,double Py2)
00010 {
00011   return (X - Px1)/(Px2 - Px1)*(Py2 - Py1) + Py1;
00012 }
00013 
00014 
00015 
00016 double NumericMethods::interpolateLinear1D(double X,double Px1,double Py1,double Px2,double Py2,double Px3,double Py3)
00017 {
00018   if (X < Px2)
00019     return interpolateLinear1D(X,Px1,Py1,Px2,Py2);
00020   else
00021     return interpolateLinear1D(X,Px2,Py2,Px3,Py3);
00022 }
00023 
00024 
00025 
00026 /***************************************************/
00033 void NumericMethods::rotate90(Point2D &X)
00034 {
00035   double aux;
00036   aux = X(0);
00037   X(0) = -X(1);
00038   X(1) = aux;
00039 }
00040 
00041 /***************************************************/
00049 void NumericMethods::rotate270(Point2D &X)
00050 {
00051   double aux;
00052   aux = X(0);
00053   X(0) = X(1);
00054   X(1) = -aux;
00055 }
00056 
00057 #if DIM == 2
00058 VertexDirection NumericMethods::getVertexFromCellDirection(CellDirection dir1,CellDirection dir2)
00059 {
00060   static int auxTable[4][2] = {{BOTTOM_CELL,LEFT_CELL},
00061                                   {BOTTOM_CELL,RIGHT_CELL},
00062                                   {UP_CELL,RIGHT_CELL},
00063                                   {UP_CELL,LEFT_CELL}};
00064 
00065   for (unsigned i=0;i<4;i++)
00066   {
00067     if ( (auxTable[i][0] == dir1 && auxTable[i][1] == dir2) ||
00068          (auxTable[i][0] == dir2 && auxTable[i][1] == dir1) )
00069       return (VertexDirection) i;
00070   }
00071   //Combinacao de direcoes erradas,isso jamais deveria
00072   //acontecer.
00073   assert(1);
00074   return INVALID_VERTEX;
00075 
00076 }
00077 #endif
00078 
00079 
00080 
00081 double NumericMethods::intThreePointsLinear(double P1,double Sw1,double P2,double Sw2, double P3,double Sw3, double Xstart,double Xend)
00082 {
00083 
00084   double IntXstart,IntXend;
00085 
00086   if (Xstart >= P1 && Xstart <=P2)
00087     IntXstart = (interpolateLinear1D(Xstart,P1,Sw1,P2,Sw2) + Sw2)/2.0*(P2-Xstart) + (Sw2 + Sw3)/2.0*(P3-P2);
00088   else if (Xstart > P2 && Xstart <=P3)
00089     IntXstart = (interpolateLinear1D(Xstart,P2,Sw2,P3,Sw3) + Sw3)/2.0*(P3-Xstart);
00090   else
00091     return NAN;
00092 
00093   if (Xend >= P1 && Xend <=P2)
00094     IntXend = (interpolateLinear1D(Xend,P1,Sw1,P2,Sw2) + Sw2)/2.0*(P2-Xend) + (Sw2 + Sw3)/2.0*(P3-P2);
00095   else if (Xend > P2 && Xend <=P3)
00096     IntXend = (interpolateLinear1D(Xend,P2,Sw2,P3,Sw3) + Sw3)/2.0*(P3-Xend);
00097   else
00098     return NAN;
00099 
00100   return IntXstart- IntXend;
00101 }
00102 
00103 
00104 
00105 
00106 /***************************************************/
00113 double NumericMethods::minMod(double a,double b,double c)
00114 {
00115   double resp;
00116   if (a > 0 && b > 0 && c > 0)
00117   {
00118     resp = a > b ? b : a;
00119     if (resp > c) resp = c;
00120     return resp;
00121   }
00122   else if (a<0 && b < 0 && c < 0)
00123   {
00124     resp = a > b ? a : b;
00125     if (resp < c) resp =c;
00126     return resp;
00127   }
00128   else
00129     return 0.0;
00130 }
00131 
00132 double NumericMethods::minMod(double a,double b) 
00133 {
00134   if (a > 0 && b > 0)
00135   {
00136     return (a<b) ? a : b;
00137   }
00138 
00139   if (a < 0 && b < 0)
00140   {
00141     return (a<b) ? b : a;
00142   }
00143   else
00144     return 0.0;    
00145 }
00146 
00147 double NumericMethods::maxMod(double a,double b,double c)
00148 {
00149   double resp;
00150   if (a > 0 && b > 0 && c > 0)
00151   {
00152     resp = a > b ? a : b;
00153     if (resp < c) resp = c;
00154     return resp;
00155   }
00156   else if (a<0 && b < 0 && c < 0)
00157   {
00158     resp = a > b ? b : a;
00159     if (resp < c) resp =c;
00160     return resp;
00161   }
00162   else
00163     return 0.0;
00164 }
00165 
00166 
00167 
00168 
00169 
00170 
00171 
00172 /***************************************************/
00179 void NumericMethods::solve3x3(double M[3][4])
00180 {
00181 
00182   double factor;
00183   double pivot=M[0][0];
00184   assert(fabs(pivot) > 1.0E-15);
00185   M[0][0]=1;
00186   M[0][1]/=pivot;
00187   M[0][2]/=pivot;
00188   M[0][3]/=pivot;
00189 
00190   factor = M[1][0];
00191   M[1][1]-=M[0][1]*factor;
00192   M[1][2]-=M[0][2]*factor;
00193   M[1][3]-=M[0][3]*factor;
00194 
00195   factor = M[2][0];
00196   M[2][1]-=M[0][1]*factor;
00197   M[2][2]-=M[0][2]*factor;
00198   M[2][3]-=M[0][3]*factor;
00199 
00200   pivot = M[1][1];
00201   assert(fabs(pivot) > 1.0E-15);
00202   M[1][2]/=pivot;
00203   M[1][3]/=pivot;
00204 
00205   factor  = M[0][1];
00206   M[0][2] -= M[1][2]*factor;
00207   M[0][3] -= M[1][3]*factor;
00208 
00209 
00210   factor  = M[2][1];
00211   M[2][2]-= M[1][2]*factor;
00212   M[2][3]-= M[1][3]*factor;
00213 
00214   pivot = M[2][2];
00215   assert(fabs(pivot) > 1.0E-15);
00216   M[2][3]/=pivot;
00217 
00218   M[1][3] -= M[2][3]*M[1][2];
00219   M[0][3] -= M[2][3]*M[0][2];
00220 
00221 
00222 }
00223 
00224 /***************************************************/
00227 void NumericMethods::multiplyEachElement(VecDouble &values,const VecDouble &v1,const VecDouble &v2)
00228 {
00229   assert(values.size() == v1.size() && v2.size() == v1.size());
00230   for (unsigned i=0;i<values.size();i++)
00231   {
00232     values(i) = v1(i)*v2(i);
00233   }
00234 }
00235 
00236 /***************************************************/
00244 void NumericMethods::divideEachElement(VecDouble &values,const VecDouble &v1,const VecDouble &v2)
00245 {
00246   assert(values.size() == v1.size() && v2.size() == v1.size());
00247   for (unsigned i=0;i<values.size();i++)
00248   {
00249     values(i) = _div(v1(i),v2(i));
00250   }
00251 
00252 }
00253 
00254 
00255 /***************************************************/
00259 void NumericMethods::IterateOrder1EDOEuler(double &u,unsigned numIteracoes,double dTau,Function1D &f,double c)
00260 {
00261   unsigned i=0;
00262   for (i=0;i<numIteracoes;i++)
00263   {
00264     u+= dTau*f(u)*c;
00265   }
00266 }
00267 
00268 
00269 /***************************************************/
00274 void NumericMethods::IterateOrder1EDOSystem(double &u,double &x,unsigned numIteracoes,double dTau,Function1D &dfV,double por,double v,Function1D &fR,double r)
00275 {
00276   unsigned i=0;
00277   for (i=0;i<numIteracoes;i++)
00278   {
00279     x+= dTau*dfV(u)*v/por;
00280     u+= dTau*fR(u)*r;
00281   }
00282 }
00283 
00284 
00285 
00286 void NumericMethods::normalizePoint2D (Point2D &p)
00287 {
00288   double hyp = hypot(p[0],p[1]);
00289   p[0]/=hyp;
00290   p[1]/=hyp;
00291 
00292 }
00293 
00294 void  NumericMethods::vectorBounds(const VecDouble &values,double &min,double &max)
00295 {
00296   assert(values.size() != 0);
00297 
00298   min=max=values(0);
00299   for (unsigned i=1;i<values.size();i++)
00300   {
00301     if (values(i) < min)
00302       min = values(i);
00303     if (values(i) > max)
00304       max = values(i);
00305   }
00306 }
00307 
00308 /***************************************************/
00313 void NumericMethods::StlVectorDoubleToVecDouble(const std::vector<double> &vV,VecDouble &vValues)
00314 {
00315   vValues.reinit(vV.size());
00316   for (unsigned i=0;i<vV.size();i++)
00317   {
00318     vValues(i)=vV[i];
00319   }
00320 }
00321 
00322 /***************************************************/
00325 void NumericMethods::readVecDouble(std::ifstream &fIn,VecDouble &vValues)
00326 {
00327   unsigned size;
00328   std::string strLixo;
00329   fIn >> size;
00330   if (fIn.fail())
00331   {
00332     printf("NumericMethods::readVecDouble = Error, expected the size of the Vector");
00333     abort();
00334   }
00335   fIn >> strLixo;
00336   if (strLixo != "[" || fIn.fail())
00337   {
00338     printf("NumericMethods::readVecDouble = Error, expected [ but got it %s\n",strLixo.c_str());
00339     abort();
00340   }
00341   vValues.reinit(size);
00342   for (unsigned i=0;i<size;i++)
00343   {
00344     fIn >> vValues(i);
00345   }
00346   fIn >> strLixo;
00347   if (strLixo != "]")
00348   {
00349     printf("NumericMethods::readVecDouble = Error, expected ]");
00350     abort();
00351   }
00352 
00353 
00354 }
00355 
00356 /***************************************************/
00359 void NumericMethods::writeVecDouble(std::ofstream &fOut,VecDouble &vValues)
00360 {
00361    fOut.precision(5);
00362    fOut << "\n" << vValues.size() << "\n";
00363    fOut << "[ ";
00364    for (unsigned i=0;i<vValues.size();i++)
00365    {
00366      fOut << vValues(i) << " ";
00367    }
00368    fOut << "]";
00369 
00370 }
00371 
00372 
00373 /***************************************************/
00385 void NumericMethods::getSparseMatrixWithoutZeroLines(SparseMatrix<double> &M,SparseMatrix<double> &C,SparsityPattern &spStripped)
00386 {
00387   //Primeiramente conte o numero de linhas nao nulas de M.
00388   unsigned nrows=0;
00389   const SparsityPattern &patternM = M.get_sparsity_pattern();
00390   for (unsigned i =0;i<M.m();i++)
00391   {
00392     SparseMatrix<double>::iterator it  = M.begin(i);
00393     SparseMatrix<double>::iterator end = M.end(i);
00394     for (;it!=end;it++)
00395     {
00396       if (it->value() != 0.0)
00397       {
00398         nrows++;
00399         break;
00400       }
00401     }
00402 
00403   }
00404   spStripped.reinit(nrows,M.n(),patternM.max_entries_per_row());
00405   unsigned iAcum=0;
00406   for (unsigned i =0;i<M.m();i++)
00407   {
00408     SparseMatrix<double>::iterator it  = M.begin(i);
00409     SparseMatrix<double>::iterator end = M.end(i);
00410     bool bNotZeroLine=false;
00411     for (;it!=end;it++)
00412     {
00413       if (it->value() != 0.0)
00414       {
00415         spStripped.add(iAcum,it->column());
00416         bNotZeroLine = true;
00417       }
00418     }
00419     if (bNotZeroLine)
00420     {
00421       iAcum++;
00422     }
00423   }
00424   spStripped.compress();
00425   C.reinit(spStripped);
00426   C = 0.0;
00427   iAcum=0;
00428   for (unsigned i =0;i<M.m();i++)
00429   {
00430     SparseMatrix<double>::iterator it  = M.begin(i);
00431     SparseMatrix<double>::iterator end = M.end(i);
00432     bool bNotZeroLine=false;
00433     for (;it!=end;it++)
00434     {
00435       if (it->value() != 0.0)
00436       {
00437         C.set(iAcum,it->column(),it->value());
00438         bNotZeroLine=true;
00439       }
00440     }
00441     if (bNotZeroLine)
00442     {
00443       iAcum++;
00444     }
00445   }
00446 }
00447 
00448 /***************************************************/
00458 void NumericMethods::getTranspose(SparseMatrix<double> &M,SparseMatrix<double> &MT,SparsityPattern &patternMT)
00459 {
00460 
00461 
00462   //Configura Pattern.
00463   patternMT.reinit(M.n(),M.m(),M.get_sparsity_pattern().max_entries_per_row());
00464   SparseMatrix<double>::iterator it  = M.begin();
00465   SparseMatrix<double>::iterator end = M.end();
00466   for (;it!=end;it++)
00467   {
00468     if (it->value() != 0.0)
00469     {
00470       patternMT.add(it->column(),it->row());
00471     }
00472   }
00473 
00474   patternMT.compress();
00475   MT.reinit(patternMT);
00476 
00477   for (it=M.begin();it!=end;it++)
00478   {
00479     if (it->value() != 0.0)
00480     {
00481       MT.set(it->column(),it->row(),it->value());
00482     }
00483   }
00484 
00485 
00486 }
00487 
00488 /***************************************************/
00496 void NumericMethods::matrixMultiply(SparseMatrix<double> &A,SparseMatrix<double> &B,SparseMatrix<double> &C,SparsityPattern &patternC)
00497 {
00498   assert(A.n() == B.m());
00499   std::vector<unsigned> rowSizes(A.m(),0);
00500 
00501   //Sao na verdade duas multiplicacaoes: Uma configura a pattern da matriz, a outra configura a matriz C.
00502   for (unsigned i =0;i<A.m();i++)
00503   {
00504     for (unsigned bColumn=0;bColumn < B.n();bColumn++)
00505     {
00506       SparseMatrix<double>::iterator it  = A.begin(i);
00507       SparseMatrix<double>::iterator end = A.end(i);
00508       for (;it!=end;it++)
00509       {
00510         if (fabs (B.el(it->column(),bColumn)) != 0.0)
00511         {
00512           rowSizes[i]++;
00513           break;
00514         }
00515       }
00516     }
00517   }
00518   patternC.reinit(A.m(),B.n(),rowSizes);
00519 
00520 
00521 
00522   for (unsigned i =0;i<A.m();i++)
00523   {
00524     for (unsigned bColumn=0;bColumn < B.n();bColumn++)
00525     {
00526       SparseMatrix<double>::iterator it  = A.begin(i);
00527       SparseMatrix<double>::iterator end = A.end(i);
00528       for (;it!=end;it++)
00529       {
00530         if (fabs (B.el(it->column(),bColumn)) != 0.0)
00531         {
00532           patternC.add(i,bColumn);
00533           break;
00534         }
00535       }
00536     }
00537   }
00538   patternC.compress();
00539   C.reinit(patternC);
00540   //Preencher as entradas de C.
00541   //Para todas as entradas
00542   SparseMatrix<double>::iterator Cend = C.end();
00543   for (SparseMatrix<double>::iterator Cit  = C.begin();Cit!=Cend;Cit++)
00544   {
00545     SparseMatrix<double>::iterator Ait  = A.begin(Cit->row());
00546     SparseMatrix<double>::iterator Aend = A.end(Cit->row());
00547     double acum=0;
00548     for (;Ait!=Aend;Ait++)
00549     {
00550       acum+=Ait->value()*B.el(Ait->column(),Cit->column());
00551     }
00552     Cit->value()=acum;
00553   }
00554 
00555 }
00556 
00557 /***************************************************/
00565 void NumericMethods::getInvMatrix(unsigned dim,SparseDirectUMFPACK &umf_solver,SparseMatrix<double> &A,SparsityPattern &patternA)
00566 {
00567   VecDouble V1(dim);
00568   V1 = 0;
00569   std::vector<unsigned> entries_per_row(dim,0);
00570 
00571 
00572   for (unsigned i=0;i<dim;i++)
00573   {
00574     V1(i)=1.0;
00575     umf_solver.solve(V1);
00576     for (unsigned j=0;j<dim;j++)
00577     {
00578       if (fabs(V1(j)) != 0.0)
00579       {
00580         entries_per_row[j]++;
00581       }
00582       V1(j)=0.0;
00583     }
00584   }
00585   patternA.reinit(dim,dim,entries_per_row);
00586   for (unsigned i=0;i<dim;i++)
00587   {
00588     V1(i)=1.0;
00589     umf_solver.solve(V1);
00590     for (unsigned j=0;j<dim;j++)
00591     {
00592       if (fabs(V1(j)) != 0.0 )
00593       {
00594         patternA.add(j,i);
00595       }
00596       V1(j)=0.0;
00597     }
00598   }
00599   patternA.compress();
00600   A.reinit(patternA);
00601   for (unsigned i=0;i<dim;i++)
00602   {
00603     V1(i)=1.0;
00604     umf_solver.solve(V1);
00605     for (unsigned j=0;j<dim;j++)
00606     {
00607       if (fabs(V1(j)) != 0.0 )
00608       {
00609         A.set(j,i,V1(j));
00610       }
00611       V1(j)=0.0;
00612     }
00613   }
00614 
00615 
00616 }
00617 
00618 void NumericMethods::makeSumPattern(SparseMatrix<double> &A,SparseMatrix<double> &B,
00619                                            SparsityPattern &patternS)
00620 {
00621   if ((A.m()!=B.m()) || (A.n()!=B.n()))
00622   {
00623     printf("NumericMethods::makeSumPattern: Matrizes precisam ter iguais dimensoes.\n");
00624   }
00625   patternS.reinit(A.m(),A.n(),A.get_sparsity_pattern().max_entries_per_row() +
00626                   B.get_sparsity_pattern().max_entries_per_row());
00627 
00628   SparseMatrix<double>::iterator it  = A.begin();
00629   SparseMatrix<double>::iterator end = A.end();
00630   for (;it!=end;it++)
00631   {
00632       patternS.add(it->row(),it->column());
00633   }
00634   it = B.begin();
00635   end = B.end();
00636   for (;it!=end;it++)
00637   {
00638       patternS.add(it->row(),it->column());
00639   }
00640   patternS.compress();
00641 
00642 
00643 
00644 }
00645 
00646 /***************************************************/
00654 void NumericMethods::matrixSum(SparseMatrix<double> &A,SparseMatrix<double> &B,SparseMatrix<double> &C)
00655 {
00656   C = 0;
00657   SparseMatrix<double>::iterator it  = A.begin();
00658   SparseMatrix<double>::iterator end = A.end();
00659   for (;it!=end;it++)
00660   {
00661     C.add(it->row(),it->column(),it->value());
00662   }
00663   it = B.begin();
00664   end = B.end();
00665   for (;it!=end;it++)
00666   {
00667     C.add(it->row(),it->column(),it->value());
00668   }
00669 }
00670 
00671 
00672 /***************************************************/
00680 void NumericMethods::matrixFill(SparseMatrix<double> &A,double start,double inc)
00681 {
00682 
00683   for (SparseMatrix<double>::iterator it  = A.begin();it!=A.end();it++,start+=inc)
00684   {
00685     it->value()=start;
00686   }
00687 }
00688 
00689  void NumericMethods::vectorFill(VecDouble &vec,double start,double inc)
00690 {
00691   for (unsigned i=0;i<vec.size();start+=inc,i++)
00692   {
00693     vec(i)=start;
00694   }
00695 }
00696 
00697 /***************************************************/
00705 double  NumericMethods::matrixEfficience(SparseMatrix<double> &A,double tol)
00706 {
00707   SparseMatrix<double>::iterator it  = A.begin();
00708   SparseMatrix<double>::iterator end = A.end();
00709   double inc = 0.0;
00710   for (;it!=end;it++)
00711   {
00712     if (fabs (it->value())< tol)
00713       inc++;
00714   }
00715   return (((double) A.n_nonzero_elements())-inc)/((double) A.n_nonzero_elements());
00716 }
00717 
00718 /***************************************************/
00723 bool NumericMethods::equal(const VecDouble &V1,const VecDouble &V2,double tol)
00724 {
00725   //Vetores de tamanho iguais.
00726   assert(V1.size() == V2.size());
00727   for (unsigned i=0;i<V1.size();i++)
00728   {
00729     if (fabs(V1(i) - V2(i)) > tol)
00730       return false;
00731   }
00732   return true;
00733 }
00734 
00735 
00736 
00737 /***************************************************/
00756 void NumericMethods::multiplySubMatrix(const SparseMatrix<double> &C,std::vector<unsigned> &equations,std::vector<unsigned> &degs,VecDouble &dst,const VecDouble &src)
00757 {
00758   SparseMatrix<double> &M = (SparseMatrix<double>&) C;
00759   //Check Dimensions
00760   assert(M.m() == dst.size());
00761   assert(M.n() == src.size());
00762   assert(M.m() == M.n());
00763   assert(M.m() % equations.size() == 0);
00764   assert(equations.size() == degs.size());
00765 
00766   assert(M.get_sparsity_pattern().optimize_diagonal());
00767 
00768 
00769   double &number = M.global_entry(0);
00770   double *val_init=&number;
00771 
00772   const SparsityPattern &cols = M.get_sparsity_pattern();
00773   unsigned n_rows=M.m();
00774   const unsigned int* rowstart = (const unsigned int*) cols.get_rowstart_indices();
00775   const unsigned int* colnums  = cols.get_column_numbers();
00776   VecDouble::iterator dst_ptr  = dst.begin();
00777   const unsigned nDegs = degs.size();
00778   unsigned j;
00779   unsigned row =0;
00780   double s;
00781   while (row < n_rows)
00782   {
00783     for (unsigned i=0;i<nDegs;i++,row++,dst_ptr++)
00784     {
00785       if (equations[i])
00786       {
00787         const double *val_ptr = val_init + rowstart[row];
00788         const double *val_end_of_row = (val_init + rowstart[row+1]);
00789         const unsigned  *colnum_ptr = (colnums + rowstart[row]);
00790 
00791         //Incorpora a diagonal na multiplicacao se for o caso.
00792         (degs[i]) ? s=*val_ptr * src(*colnum_ptr) : s = 0.0;
00793         colnum_ptr++;
00794         val_ptr++;
00795         while (1)
00796         {
00797           for (j=0;j<i;j++,val_ptr++,colnum_ptr++)
00798           {
00799             if (degs[j])
00800               s+=*val_ptr * src(*colnum_ptr);
00801           }
00802           if (val_ptr >= val_end_of_row || *colnum_ptr > row)
00803             break;
00804           for (;j<nDegs;j++,val_ptr++,colnum_ptr++)
00805           {
00806             if (degs[j])
00807               s+=*val_ptr * src(*colnum_ptr);
00808           }
00809         }
00810         //A posicao (i,i) ja foi computada e esta localizada na primeira posicao
00811         //do vetor de colunas da linha i. Dessa forma o grau de liberdade nao eh mais
00812         //j mas sim j+1
00813         j++;
00814         while(val_ptr < val_end_of_row)
00815         {
00816           while(j<nDegs)
00817           {
00818             if (degs[j])
00819               s+=*val_ptr * src(*colnum_ptr);
00820             val_ptr++;
00821             colnum_ptr++;
00822             j++;
00823           }
00824           j=0;
00825         }
00826         *dst_ptr=s;
00827       }
00828     }
00829   }
00830 }
00831 
00832 /***************************************************/
00838 bool NumericMethods::IsSymetric(const SparseMatrix<double> &C, double tol)
00839 {
00840   for (unsigned i=0;i<C.m();i++)
00841     for (unsigned j=i+1;j<C.n();j++)
00842     {
00843       if (fabs(C.el(i,j) - C.el(j,i)) > tol)
00844       {
00845         printf("Error in Entry M(%d,%d)=%g != %g\n",i,j,C.el(i,j),C.el(j,i));
00846         return false;
00847       }
00848     }
00849   return true;
00850 }
00851 
00852 /***************************************************/
00860 void NumericMethods::multiplyBlockSubMatrix(const SparseMatrix<double> &C,unsigned sRow,unsigned eRow,unsigned sCol,unsigned eCol,VecDouble &dst,const VecDouble &src)
00861 {
00862   assert(eRow < C.m() && eCol < C.n());
00863   assert(C.get_sparsity_pattern().optimize_diagonal());
00864 
00865   for (unsigned i=sRow;i<=eRow;i++)
00866   {
00867     SparseMatrix<double>::const_iterator eIt = C.end(i);
00868     SparseMatrix<double>::const_iterator it = C.begin(i);
00869     double acum=0.0;
00870 
00871     //First Entry is diagonal, verify if we should
00872     //put it in the acum variable.
00873     if (it->column() >=sCol && it->column() <= eCol)
00874       acum+=it->value()*src(it->column());
00875     it++;
00876     while(it!=eIt && it->column() < sCol)
00877       it++;
00878 
00879 
00880     //Make the multiplication
00881     while (it != eIt &&  it->column() <= eCol)
00882     {
00883        acum+=it->value()*src(it->column());
00884        it++;
00885     }
00886     dst(it->row())=acum;
00887   }
00888 }
00889 
00890 /***************************************************/
00897 void NumericMethods::vectorModBounds(const VecDouble &values,double &min,double &media,double &max)
00898 {
00899 
00900   assert(values.size() != 0);
00901 
00902 
00903   min=max=media=fabs(values(0));
00904   for (unsigned i=1;i<values.size();i++)
00905   {
00906     double d = fabs(values(i));
00907 
00908     if (d < min)
00909       min = d;
00910     if (d > max)
00911       max = d;
00912     media+=d;
00913   }
00914   media/=values.size();
00915 }
00916 
00917 /***************************************************/
00923 void NumericMethods::checkLinearSolution(const SparseMatrix<double> &C,const VecDouble &vSol,const VecDouble &vB,double &min,double &media,double &max)
00924 {
00925   VecDouble vAux(vSol.size());
00926   C.vmult(vAux,vSol);
00927   vAux-=vB;
00928   vectorModBounds(vAux,min,media,max);
00929 
00930 }
00931 
00932 /***************************************************/
00935 void NumericMethods::printVectorWithIndices(const VecDouble &vV,double tol,bool printZeros)
00936 {
00937   for (unsigned i=0;i<vV.size();i++)
00938   {
00939     if (fabs(vV(i)) >  tol)
00940       printf("%d) %g\n",i,vV(i));
00941     else if (printZeros)
00942       printf("%d) 0\n",i);
00943   }
00944 }
00945 
00946 void NumericMethods::printVectorWithIndices(const VecTag &vV,double tol,bool printZeros) 
00947 {
00948   for (unsigned i=0;i<vV.size();i++)
00949   {
00950     if (fabs(vV[i]) >  tol)
00951       printf("%d) %d\n",i,vV[i]);
00952     else if (printZeros)
00953       printf("%d) 0\n",i);
00954   }
00955 
00956 }
00957 
00958 void NumericMethods::printVectorWithIndices(const BlockVecDouble &vV,double tol)
00959 {
00960   for (unsigned i=0;i<vV.size();i++)
00961   {
00962     if (fabs(vV(i)) < tol)
00963       printf("%d) 0\n",i);
00964     else
00965       printf("%d) %g\n",i,vV(i));
00966   }
00967 }
00968 
00969 void NumericMethods::printVectorWithIndices(const VecIndex &vI,std::ostream &out) 
00970 {
00971   for (unsigned i=0;i<vI.size();i++)
00972   {
00973     out << i << ") " << vI[i] << "\n";
00974   }
00975 
00976 }
00977 
00978 void NumericMethods::printVector(const BlockVecDouble &vV,double tol)
00979 {
00980   
00981   for (unsigned i=0;i<vV.size();i++)
00982   {
00983     if (fabs(vV(i)) < tol)
00984       printf("0 ");
00985     else
00986       printf("%g ",vV(i));
00987 
00988   }
00989 }
00990 
00991 void NumericMethods::printVector(const VecDouble &vV,double tol)
00992 {
00993   
00994   for (unsigned i=0;i<vV.size();i++)
00995   {
00996     if (fabs(vV(i)) < tol)
00997       printf("0 ");
00998     else
00999       printf("%g ",vV(i));
01000 
01001   }
01002     
01003 
01004 }
01005 
01006 
01007 bool NumericMethods::isInCube(const Point3D &X,const Point3D &P1,const Point3D &P2)
01008 {
01009   assert(P1(0) < P2(0) &&P1(1) < P2(1) &&P1(2) < P2(2) ); 
01010   return (X(0) >= P1(0) && X(1) >= P1(1) && X(2) >= P1(2) && X(0) <= P2(0) && X(1) <= P2(1) && X(2) <= P2(2));
01011 }
01012 
01013 bool NumericMethods::isInCube(const VecDouble &X,const VecDouble &P1,const VecDouble &P2)
01014 {
01015   assert(P1(0) < P2(0) &&P1(1) < P2(1) &&P1(2) < P2(2) ); 
01016   return (X(0) >= P1(0) && X(1) >= P1(1) && X(2) >= P1(2) && X(0) <= P2(0) && X(1) <= P2(1) && X(2) <= P2(2));
01017 }
01018 
01019 
01020 
01032 void NumericMethods::fillVector(VecDouble &v,double s) 
01033 {
01034   assert (v.size()!=0);
01035   if (v.size()!=0)
01036     std::fill (v.begin(), v.end(), s);
01037   return;
01038 
01039 }
01040 
01045 double NumericMethods::triangleMeasure(const Point3D &u,const Point3D &v) 
01046 {
01047   double a = u[1]*v[2] - u[2]*v[1];
01048   double b = u[0]*v[2] - u[2]*v[0];
01049   double c = u[0]*v[1] - u[1]*v[0];
01050   return sqrt(a*a + b*b + c*c)/2.0;
01051 }
01052 
01059 double NumericMethods::squareMeasure(const Point3D &p1,const Point3D &p2,const Point3D &p3,const Point3D &p4) 
01060 {
01061   Point3D u = p2-p1;
01062   Point3D v = p4-p1;
01063   Point3D u2 = p2-p3;
01064   Point3D v2 = p4-p3;
01065 
01066   return triangleMeasure(u,v) + triangleMeasure(u2,v2);
01067 
01068   
01069 }
01070 
01071 
01072  void NumericMethods::printMatrix(const Matrix &M) 
01073 {
01074   for (unsigned i =0;i<M.m();i++)
01075   {
01076     printf("%d)",i);
01077     for (unsigned j =0;j<M.n();j++)
01078       printf("%g ",M(i,j));
01079     printf("\n");
01080   }
01081 }
01082 
01083 
01084 void NumericMethods::printNonZeroEntriesPerLine(const SparseMatrix<double> &M) 
01085 {
01086   for (unsigned i =0;i<M.m();i++)
01087   {
01088     int count =0;
01089     for (unsigned j=0;j<M.n();j++)
01090     {
01091       if (!isNearZero(M.el(i,j)))
01092         count++;
01093     }
01094     printf("line %d) has %d non zero entries\n",i,count);
01095   }
01096 }
01097 
01098 void NumericMethods::printSparseMatrixOctave(std::ostream &out, const SparseMatrix<double> &M) 
01099 {
01100   unsigned lastColIndex = M.n() -1;
01101   unsigned j;
01102   for (unsigned i=0;i<M.m();i++)
01103   {
01104     for (j=0;j<lastColIndex;j++)
01105       out << M.el(i,j) << ", ";
01106     out << M.el(i,j);
01107     out << std::endl;
01108   }
01109 }
01110 
01111 
01112 
01119 void NumericMethods::getMinMaxValueByColumn(Matrix &M,VecDouble &vMinValues,VecDouble &vMaxValues) 
01120 {
01121   assert(vMinValues.size() == M.n());
01122   assert(vMaxValues.size() == M.n());
01123   
01124   for (unsigned j=0;j<M.n();j++)
01125   {
01126     vMinValues(j) = M(0,j);
01127     vMaxValues(j) = M(0,j);
01128   }
01129 
01130   for (unsigned i=1;i<M.m();i++)
01131   {
01132     for (unsigned j=0;j<M.n();j++)
01133     {
01134       if (vMinValues(j) > M(i,j))
01135         vMinValues(j) = M(i,j);
01136       if (vMaxValues(j) < M(i,j))
01137         vMaxValues(j) = M(i,j);
01138     }
01139 
01140   }
01141 }
01142 
01143 
01153 void NumericMethods::getMinMaxValueByColumn(Matrix &M,VecDouble &vMinValues,VecDouble &vMaxValues,VecDouble &vIndexMin,VecDouble &vIndexMax) 
01154 {
01155   assert(vMinValues.size() == M.n());
01156   assert(vMaxValues.size() == M.n());
01157   
01158   for (unsigned j=0;j<M.n();j++)
01159   {
01160     vMinValues(j) = M(0,j);
01161     vMaxValues(j) = M(0,j);
01162     vIndexMin(j)=0;
01163     vIndexMax(j)=0;
01164   }
01165 
01166   for (unsigned i=1;i<M.m();i++)
01167   {
01168     for (unsigned j=0;j<M.n();j++)
01169     {
01170       if (vMinValues(j) > M(i,j))
01171       {
01172         vMinValues(j) = M(i,j);
01173         vIndexMin(j) = i;
01174       }
01175       if (vMaxValues(j) < M(i,j))
01176       {
01177         vMaxValues(j) = M(i,j);
01178         vIndexMax(j)=i;
01179       }
01180     }
01181   }
01182 }
01183 
01184 
01185 
01191 double NumericMethods::maxMod(double a,double b) 
01192 {
01193   if (fabs(a) > fabs(b))
01194     return fabs(a);
01195   else
01196     return fabs(b);
01197 }
01198 
01199 
01200 
01201 
01202 bool NumericMethods::a_equal(double a, double b,double tol) 
01203 {
01204   return (fabs(a-b) < tol);
01205 }
01206 
01207 
01208 void NumericMethods::adjustBounds(double &c,const double a,const double b) 
01209 {
01210   assert(a<=b);
01211   if (c <= a)
01212     c = a;
01213   else if (c >= b)
01214     c = b;
01215 }
01216 
01217 
01218  double NumericMethods::vectorMinValue(const VecDouble &v) 
01219 {
01220   assert(v.size() >= 1);
01221   double dd = v(0);
01222   for (unsigned i=1;i<v.size();i++)
01223   {
01224     if (dd > v(i))
01225       dd = v(i);
01226   }
01227   return dd;
01228 }
01229 
01230  double NumericMethods::vectorMaxValue(const VecDouble &v) 
01231 {
01232   assert(v.size() >= 1);
01233   double dd = v(0);
01234   for (unsigned i=1;i<v.size();i++)
01235   {
01236     if (dd < v(i))
01237       dd = v(i);
01238   }
01239   return dd;
01240 }
01241 
01242 
01247  double NumericMethods::vectorMaxAbsValue(const VecDouble &v) 
01248 {
01249   assert(v.size()>0);
01250   double result=fabs(v(0));
01251   for (unsigned i=1;i<v.size();i++)
01252   {
01253     double dd=fabs(v(i));
01254     if (result<dd)
01255       result=dd;
01256   }
01257   return result;
01258 }
01259 
01264  double NumericMethods::vectorMinAbsValue(const VecDouble &v) 
01265 {
01266   assert(v.size()>0);
01267   double result=fabs(v(0));
01268   for (unsigned i=1;i<v.size();i++)
01269   {
01270     double dd=fabs(v(i));
01271     if (result>dd)
01272       result=dd;
01273   }
01274   return result;
01275 }
01276 
01277 
01278 
01279 
01280 void NumericMethods::VecDoubleToSTL(const VecDouble &v1,std::vector<double> &v2)
01281 {
01282   assert(v1.size() == v2.size());
01283   for (unsigned i=0;i<v1.size();i++)
01284     v2[i] = v1(i);
01285 }
01286 
01287 void NumericMethods::STLToVecDouble(VecDouble &v1,const std::vector<double> &v2) 
01288 {
01289   v1.reinit(v2.size());
01290   for (unsigned i=0;i<v2.size();i++)
01291   {
01292     v1(i)=v2[i];
01293   }
01294 }
01295 
01296 
01297 
01298 void NumericMethods::getCompactRowFormat(SparseMatrix<double> &A,std::vector<int> &cnt,std::vector<int> &col, std::vector<double> &ele) 
01299 {
01300   assert(A.m() == cnt.size());
01301   assert(A.n_nonzero_elements() == col.size());
01302   assert(ele.size() == col.size());
01303 
01304 
01305   
01306   const SparsityPattern &pattern = A.get_sparsity_pattern();
01307   for (unsigned i=0;i<cnt.size();i++)
01308   {
01309     cnt[i] = pattern.row_length(i); 
01310   }
01311   
01312   std::vector<double>::iterator itElem = ele.begin();
01313   std::vector<int>::iterator itCols = col.begin();
01314   for (SparseMatrix<double>::iterator it  = A.begin();it!=A.end();it++,itElem++,itCols++)
01315   {
01316     *itElem = it->value();
01317     *itCols = it->column();
01318   }
01319 
01320   
01321   if (pattern.optimize_diagonal())
01322   {
01323     itElem = ele.begin();
01324     itCols = col.begin();
01325     unsigned nRows = A.m();
01326     //for each row do
01327     for (unsigned i=0;i<nRows;i++)
01328     {
01329       //number of non zero entries in row i excluding the diagonal
01330       int nNonDiagColsPerRow = pattern.row_length(i) - 1; 
01331       assert(nNonDiagColsPerRow >= 0);
01332       std::vector<double>::iterator itElemDiag = itElem++;
01333       std::vector<int>::iterator itColDiag = itCols++;
01334       for (int j=0;j<nNonDiagColsPerRow;j++)
01335       {
01336         if (*itColDiag > *itCols)
01337         {
01338           std::swap(*itColDiag++,*itCols++);
01339           std::swap(*itElemDiag++,*itElem++);
01340         }
01341         else
01342         {
01343           itCols++;
01344           itElem++;
01345         }
01346       }
01347     }
01348   }
01349 }
01350 
01351 
01352  bool NumericMethods::isNearZero(double dd) 
01353 {
01354   return fabs(dd) < NUMERIC_METHODS_TOLERANCE;
01355 }
01356 
01357 
01358  void NumericMethods::printPrescBoundMapping(MapIntDouble &prescValuesMap) 
01359 {
01360     for (  std::map<unsigned int,double>::const_iterator dof  = prescValuesMap.begin();
01361            dof != prescValuesMap.end();dof++)
01362     {
01363       printf("dof %d =%g\n",dof->first,dof->second);
01364     }
01365 }
01366 
01367 
01368 void NumericMethods::addMapValues(MapIntDouble &map,VecDouble &values) 
01369 {
01370   for (MapIntDouble::iterator it = map.begin();it!=map.end();it++)
01371   {
01372     assert(it->first < values.size());
01373     values(it->first) += it->second;
01374   }
01375 }
01376 
01377 
01378 
01379 
01380 double NumericMethods::vectorMaxDiff(const VecDouble &v1,const VecDouble &v2) 
01381 {
01382   assert(v1.size() == v2.size());
01383   double diff;
01384   double maxDiff=0;
01385   for (unsigned i=0;i<v1.size();i++)
01386   {
01387     diff = fabs(v1(i) - v2(i));
01388     if (diff > maxDiff)
01389       maxDiff = diff;
01390   }
01391   return maxDiff;
01392 }
01393 
01394 
01395 
01396 
01397  void NumericMethods::printPoint(const Point3D p) 
01398 {
01399   printf("%g,%g,%g\n",p[0],p[1],p[2]);
01400 }
01401 
01402 
01403  double NumericMethods::min(double a,double b,double c) 
01404 {
01405   return std::min(a,std::min(b,c));
01406 }
01407 
01408 
01409 
01410 
01411  double NumericMethods::vectorRelError(const VecDouble &v1,const VecDouble &v2) 
01412 {
01413   assert(v1.size() == v2.size());
01414   unsigned size=v1.size();
01415   double diff=0.0,norm=0.0;
01416   for (unsigned i=0;i<size;i++)
01417   {
01418     
01419     //diff += (v1(i)-v2(i))*(v1(i)-v2(i));
01420     //norm +=v1(i)*v1(i); 
01421     
01422     diff += fabs(v1(i)-v2(i));
01423     norm += fabs(v1(i)); 
01424   }
01425   
01426   if (norm < 1.e-8)
01427     norm=1;
01428   return diff/norm;
01429 
01430 }
01431 
01432 
01433  double NumericMethods::vectorMaxRelDiff(const VecDouble &v1,const VecDouble &v2) 
01434 {
01435   assert(v1.size() == v2.size());
01436   unsigned size=v1.size();
01437   double max=0.0;
01438   
01439   for (unsigned i=0;i<size;i++)
01440   {
01441     
01442     //diff += (v1(i)-v2(i))*(v1(i)-v2(i));
01443     //norm +=v1(i)*v1(i); 
01444     
01445     double diff = fabs( (v1(i)-v2(i))/v1(i));
01446     if (max < diff)
01447       max=diff;
01448   }
01449   
01450   return max;
01451 
01452 }
01453 
01454 
01455 
01456 void NumericMethods::getSubVector(const VecDouble &values,const VecIndex &vI,VecDouble &subV) 
01457 {
01458   subV.reinit(vI.size());
01459   for (Index i=0;i<vI.size();i++)
01460   {
01461     subV(i)=values(vI[i]);
01462   }
01463 }
01464 
01465 
01466 
01467  void NumericMethods::vectorDivideEntries(VecDouble &V1,const VecDouble &vQuot) 
01468 {
01469   assert(V1.size()==vQuot.size());
01470   for (unsigned i=0;i<V1.size();i++)
01471   {
01472     V1(i)/=vQuot(i);
01473   }
01474 }
01475 
01476  void NumericMethods::vectorDivideEntriesAvoidingNAN(VecDouble &V1,const VecDouble &vQuot) 
01477 {
01478   assert(V1.size()==vQuot.size());
01479   for (unsigned i=0;i<V1.size();i++)
01480   {
01481     if (vQuot(i)!=0.0)
01482       V1(i)/=vQuot(i);
01483     else
01484     {
01485       assert(V1(i) == 0.0);
01486     }
01487   }
01488 }
01489 
01490 
01491 
01496  double NumericMethods::vectorSum(const VecDouble &v) 
01497 {
01498   assert(v.size() != 0);
01499   double acum=0.0;
01500   for (unsigned i=0;i<v.size();i++)
01501     acum+=v(i);
01502   return acum;
01503 }
01504 
01505 
01506 void NumericMethods::vectorMinMaxValues(const VecDouble &v,double &min,double &max) 
01507 {
01508   assert(v.size());
01509   min=max=v(0);
01510   for (unsigned i=1;i<v.size();i++)
01511   {
01512     if (v(i) < min)
01513       min=v(i);
01514     if (v(i) > max)
01515       max=v(i);
01516   }
01517 }
01518 
01519 
01527 double NumericMethods::_div(double a,double b) 
01528 {
01529   if (b!=0.0)
01530     return a/b;
01531   else
01532   {
01533     //assert(fabs(a)==0.0);
01534     return 0.0;
01535   } 
01536 }
01537 
01538 
01539 void NumericMethods::matrixGetColumn(VecDouble &v, const Matrix &M, int col) 
01540 {
01541   assert(v.size() == M.m());
01542   for (unsigned i=0;i<v.size();i++)
01543     v(i)=M(i,col);
01544 }
01545 
01546 
01547 
01548 
01549 
01550 
01551 void NumericMethods::printMatrix(std::string file,const SparseMatrix<double> &M) 
01552 {
01553   FILE *f = fopen(file.c_str(),"w");
01554   fprintf(f,"%d %d %d\n", M.m(), M.n(), M.get_sparsity_pattern().max_entries_per_row());
01555     SparseMatrix<double>::const_iterator it  = M.begin();
01556     SparseMatrix<double>::const_iterator end = M.end();
01557 
01558     for(;it!=end;it++)
01559     {
01560       fprintf(f,"%d %d %.55lf\n",it->row(),it->column(),it->value());
01561     }
01562     fclose(f);  
01563 }
01564 
01565 
01566 double NumericMethods::det3x3(const Matrix &M) 
01567 {
01568   assert(M.m() == M.n());
01569   assert(M.m() == 3);
01570 
01571   const double *d = &(M(0,0));
01572 
01573   //return M(0,0)*M(1,1)*M(2,2) + M(0,1)*M(1,2)*M(2,0) + M(1,0)*M(2,1)*M(0,2)  - M(0,2)*M(1,1)*M(2,0) - M(0,1)*M(1,0)*M(2,2) - M(1,2)*M(2,1)*M(0,0)
01574   return +d[0]*d[4]*d[8] + d[1]*d[5]*d[6] + d[3]*d[7]*d[2]
01575          -d[2]*d[4]*d[6] - d[1]*d[3]*d[8] - d[5]*d[7]*d[0]; 
01576 }
01577 
01578 
01584 double NumericMethods::abs_vector_product(const VecDouble &a,const VecDouble &b) 
01585 {
01586   assert(a.size() == 3);
01587   assert(b.size() == 3);
01588 
01589   double aux0=a(1)*b(2) - a(2)*b(1);
01590   double aux1=a(2)*b(0) - a(0)*b(2);
01591   double aux2=a(0)*b(1) - a(1)*b(0);
01592   return sqrt(aux0*aux0 + aux1*aux1 + aux2*aux2);
01593 
01594   
01595 }
01596 
01597 
01598 void NumericMethods::matrixFill(Matrix &A,double value) 
01599 {
01600   std::fill_n (&(A(0,0)), A.n_elements(), value);
01601 }