MatematicaAlgebra.cpp
```00001 #include "../include/Matematica.h"
00002 #include <iostream>
00003 #include <fstream>
00004 void Matrice::Shout(const char * s, ...){
00005 #ifdef MAT_DEBUG
00006   va_list args;
00007   va_start(args, s);
00008   fprintf(stderr, "Matrice] ");
00009   vfprintf(stderr, s, args);
00010   fprintf(stderr, "\n");
00011   va_end(args);
00012 #else
00013   return;
00014 #endif
00015 }
00016 // Matrice::Matrice(int newNSize, int newNRow)  { // the only public ctor
00017 //   if (newNSize <= 0) newNSize = 5;
00018 //   NSize = newNSize;
00019 //   if ((newNRow <= newNSize)&&(newNRow>0))
00020 //     NRow = newNRow;
00021 //   else
00022 //     NRow = newNSize;
00023 //   // since allocate() will first call delete[] on data:
00024 //   //data = new double [NSize*NSize];
00025 //   data = (double *)calloc(NSize*NSize,sizeof(double));
00026 //   NCol = NRow = NSize;
00027 //   //    allocate();
00028 // }
00029 //----------------Constructors------------------------
00030 Matrice::Matrice(int ExtNRow, int ExtNCol){
00031   Shout("Matrice(row,col)");
00032   NRow = ExtNRow;
00033   NCol = ExtNCol;
00034   NZed = 0;
00035   NSize = NCol*NRow;
00036   NDim = 2;
00037   data = (double *)calloc(NSize,sizeof(double));
00038 }
00039 Matrice::Matrice(int ExtNRow, int ExtNCol, double *pointer){
00040   Shout("Matrice(row,col)");
00041   NRow = ExtNRow;
00042   NCol = ExtNCol;
00043   NSize = NCol*NRow;
00044   NDim = 2;
00045   data = (double *)malloc(sizeof(double));
00046   data = pointer;
00047 }
00048 Matrice::Matrice(int ExtNRow){ // the only public  data = (double *)calloc(NSize*NSize,sizeof(double));
00049   Shout("Matrice(row=col)");
00050   NRow = ExtNRow;
00051   NCol = 0;
00052   NZed = 0;
00053   NSize = NRow;
00054   NDim = 1;
00055   data = (double *)calloc(NSize,sizeof(double));
00056 }
00057 Matrice::Matrice(int ExtNRow,int ExtNCol,int ExtNZed){
00058   Shout("Matrice(row,col)");
00059   NRow = ExtNRow;
00060   NCol = ExtNCol;
00061   NZed = ExtNZed;
00062   NSize = NCol*NRow*NZed;
00063   NDim = 3;
00064   data = (double *)calloc(NSize,sizeof(double));
00065 }
00066 Matrice::Matrice(SPLINE Wg){
00067   Shout("Matrice(SPLINE)");
00068   NRow = NCol = 3;
00069   NZed = 0;
00070   NSize = NRow * NCol;
00071   data = (double *)calloc(NSize,sizeof(double));
00072   double MenoDue = - .75*Wg.a3 + 1.5*Wg.a4;
00073   //MenoDue += .125*Wg.a1 - .125*Wg.a2; //O(h^4)
00074   double MenoUno = -.5*Wg.a1 + Wg.a2 + 1.5*Wg.a3 - 6.*Wg.a4;
00075   //MenoUno += -.125*Wg.a1 - .5*Wg.a2;
00076   double Zero = Wg.a0 - 2.*Wg.a2 + 9.*Wg.a4;
00077   //Zero += 0.75*Wg.a2;
00078   double PiuUno = .5*Wg.a1 + Wg.a2 - 1.5*Wg.a3 - 6.*Wg.a4;
00079   //PiuUno += .25*Wg.a1 + .5*Wg.a2;
00080   double PiuDue = .75*Wg.a3 + 1.5*Wg.a4;
00081   //PiuDue += -.125*Wg.a1 -.125*Wg.a2;
00082   double Norm=0.;
00083   for(int r=0;r<NCol;r++){
00084     data[NRow*0+r] = MenoUno;
00085     data[NRow*1+r] = Zero;
00086     data[NRow*2+r] = PiuUno;
00087   }
00088   for(int r=0;r<NRow;r++)
00089     for(int c=0;c<NCol;c++)
00090       //Norm += data[NSize*r+c] > 0. ? data[NSize*r+c] : 0;
00091       Norm += ASS(data[NRow*c+r]);
00092   for(int r=0;r<NRow;r++)
00093     for(int c=0;c<NCol;c++)
00094       data[NRow*c+r] /= Norm*.5;
00095   NCol = NRow;
00096   NSize = NCol * NRow;
00097   NDim = 2;
00098 }
00099 Matrice::Matrice(double *Axis,double Angle,int ExtNRow){
00100   double c = cos(Angle);
00101   double s = sin(Angle);
00102   double t = 1. - c;
00103   double Norm = 0.;
00104   for(int d=0;d<3;d++){
00105     Norm += SQR(Axis[d]);
00106   }
00107   Norm = Norm > 0. ? sqrt(Norm) : 1.;
00108   double x = Axis[0]/Norm;
00109   double y = Axis[1]/Norm;
00110   double z = Axis[2]/Norm;
00111   NDim = 2;
00112   NZed = 0;
00113   if(ExtNRow == 4){
00114     NCol = NRow = 4;
00115     NSize = NCol * NRow;
00116     data = (double *)calloc(NSize,sizeof(double));
00117     data[NRow*0+0]  = t*x*x + c;
00118     data[NRow*0+1]  = t*x*y + z*s;
00119     data[NRow*0+2]  = t*x*z - y*s;
00120     data[NRow*0+3]  = 0.;
00121
00122     data[NRow*1+0]  = t*x*y - z*s;
00123     data[NRow*1+1]  = t*y*y + c;
00124     data[NRow*1+2]  = t*y*z + x*s;
00125     data[NRow*1+3]  = 0.;
00126
00127     data[NRow*2+0]  = t*x*z + y*s;
00128     data[NRow*2+1]  = t*y*z - x*s;
00129     data[NRow*2+2]  = t*z*z + c;
00130     data[NRow*2+3]  = 0.;
00131
00132     data[NRow*3+0]  = 0.;
00133     data[NRow*3+1]  = 0.;
00134     data[NRow*3+2]  = 0.;
00135     data[NRow*3+3]  = 1.;
00136   }
00137   else{
00138     NCol = NRow = 3;
00139     NSize = NCol * NRow;
00140     data = (double *)calloc(NSize,sizeof(double));
00141     data[NRow*0+0]  = t*x*x + c;
00142     data[NRow*0+1]  = t*x*y + z*s;
00143     data[NRow*0+2]  = t*x*z - y*s;
00144
00145     data[NRow*1+0]  = t*x*y - z*s;
00146     data[NRow*1+1]  = t*y*y + c;
00147     data[NRow*1+2]  = t*y*z + x*s;
00148
00149     data[NRow*2+0]  = t*x*z + y*s;
00150     data[NRow*2+1]  = t*y*z - x*s;
00151     data[NRow*2+2]  = t*z*z + c;
00152   }
00153 }
00155   //FIXME: the determinant is not zero!
00156   NDim = 2;
00157   NZed = 0;
00158   if(dim == 4){
00159     NCol = NRow = 4;
00160     NSize = NCol * NRow;
00161     data = (double *)calloc(NSize,sizeof(double));
00162     data[NRow*0+0]  = q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z;
00163     data[NRow*0+1]  = 2.*q.x*q.y + 2.*q.w*q.z;
00164     data[NRow*0+2]  = 2.*q.x*q.z - 2.*q.w*q.y;
00165     data[NRow*0+3]  = 0.;
00166
00167     data[NRow*1+0]  = 2.*q.x*q.y - 2.*q.w*q.z;
00168     data[NRow*1+1]  = q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z;
00169     data[NRow*1+2]  = 2.*q.y*q.z + 2.*q.w*q.x;
00170     data[NRow*1+3]  = 0.;
00171
00172     data[NRow*2+0]  = 2.*q.x*q.z + 2.*q.w*q.y;
00173     data[NRow*2+1]  = 2.*q.y*q.z - 2.*q.w*q.x;
00174     data[NRow*2+2]  = q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z;
00175     data[NRow*2+3]  = 0.;
00176
00177     data[NRow*3+0]  = 0.;
00178     data[NRow*3+1]  = 0.;
00179     data[NRow*3+2]  = 0.;
00180     data[NRow*3+3]  = q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z;
00181   }
00182   else{
00183     NCol = NRow = 3;
00184     NSize = NCol * NRow;
00185     data = (double *)calloc(NSize,sizeof(double));
00186     data[NRow*0+0]  = 1. - 2.*SQR(q.y) - 2.*SQR(q.z);
00187     data[NRow*0+1]  = 2.*q.x*q.y + 2.*q.w*q.z;
00188     data[NRow*0+2]  = 2.*q.x*q.z - 2.*q.w*q.y;
00189
00190     data[NRow*1+0]  = 2.*q.x*q.y - 2.*q.w*q.z;
00191     data[NRow*1+1]  = 1. - 2.*SQR(q.x) - 2.*SQR(q.z);
00192     data[NRow*1+2]  = 2.*q.y*q.z + 2.*q.w*q.x;
00193
00194     data[NRow*2+0]  = 2.*q.x*q.z + 2.*q.w*q.y;
00195     data[NRow*2+1]  = 2.*q.y*q.z - 2.*q.w*q.x;
00196     data[NRow*2+2]  = 1. - 2.*SQR(q.x) - 2.*SQR(q.y);
00197   }
00198 }
00199 Matrice::Matrice(double Roll,double Pitch,double Yaw,int ExtNRow){
00200   //Defining Rrpy(r,p,y) := RzrRypRxy
00201   double CR = cos(Roll);
00202   double SR = sin(Roll);
00203   double CP = cos(Pitch);
00204   double SP = sin(Pitch);
00205   double CY = cos(Yaw);
00206   double SY = sin(Yaw);
00207   NDim = 2;
00208   NZed = 0;
00209   if(ExtNRow == 4){
00210     NCol = NRow = 4;
00211     NSize = NCol * NRow;
00212     data = (double *)calloc(NSize,sizeof(double));
00213     data[NRow*0+0] = CY*CP;
00214     data[NRow*0+1] = SY*CP;
00215     data[NRow*0+2] = -SP;
00216     data[NRow*0+3] = 0.;
00217
00218     data[NRow*1+0] = CY*SP*SR - SY*CR;
00219     data[NRow*1+1] = SY*SP*SR + CY*CR;
00220     data[NRow*1+2] = CP*SR;
00221     data[NRow*1+3] = 0.;
00222
00223     data[NRow*2+0] = CY*SP*CR + SY*SR;
00224     data[NRow*2+1] = SY*SP*CR - CY*SR;
00225     data[NRow*2+2] = CP*CR;
00226     data[NRow*2+3] = 0.;
00227
00228     data[NRow*3+0] = 0.;
00229     data[NRow*3+1] = 0.;
00230     data[NRow*3+2] = 0.;
00231     data[NRow*3+3] = 1.;
00232     // Other definition of the angles (to check)
00233     // data[NRow*0+0] = CR*CP*CY - SR*SY;
00234     // data[NRow*0+1] = CR*CP*SY - SR*CY;
00235     // data[NRow*0+2] = -CR*SP;
00236     // data[NRow*0+3] = 0.;
00237
00238     // data[NRow*1+0] = -SR*CP*CY - CR*SY;
00239     // data[NRow*1+1] = -SR*CP*SY + CR*CY;
00240     // data[NRow*1+2] = SR*SP;
00241     // data[NRow*1+3] = 0.;
00242
00243     // data[NRow*2+0] = SP*CY;
00244     // data[NRow*2+1] = SP*SY;
00245     // data[NRow*2+2] = CP;
00246     // data[NRow*2+3] = 0.;
00247
00248     // data[NRow*3+0] = 0.;
00249     // data[NRow*3+1] = 0.;
00250     // data[NRow*3+2] = 0.;
00251     // data[NRow*3+3] = 1.;
00252   }
00253   if(ExtNRow == 3){
00254     NCol = NRow = 3;
00255     NSize = NCol * NRow;
00256     data = (double *)calloc(NSize,sizeof(double));
00257     data[NRow*0+0] = CR*CP;
00258     data[NRow*0+1] = SR*CP;
00259     data[NRow*0+2] = -SP;
00260
00261     data[NRow*1+0] = CR*SP*SY - SR*CY;
00262     data[NRow*1+1] = SR*SP*SY + CR*SY;
00263     data[NRow*1+2] = CP*SY;
00264
00265     data[NRow*2+0] = CR*SP*CY+SR*SY;
00266     data[NRow*2+1] = SR*SP*CY-CR*SY;
00267     data[NRow*2+2] = CR*CY;
00268   }
00269 }
00270 Matrice::Matrice(double *M,int Row,int Col){
00271   NRow = Row;
00272   NCol = Col;
00273   NZed = 0;
00274   NSize = NRow*NCol;
00275   //data = M;
00276   NDim = 2;
00277   data = (double *)calloc(NSize,sizeof(double));
00278   for(int c=0;c<NCol;c++)
00279     for(int r=0;r<NRow;r++)
00280       data[NRow*c+r] = M[NRow*c+r];
00281 }
00282 Matrice::Matrice(SPLINE Wg,int Dim){
00283   Shout("Matrice(SPLINE,dim)");
00284   NCol = NRow = 5;
00285   NZed = 0;
00286   NSize = NCol * NRow;
00287   data = (double *)calloc(NSize,sizeof(double));
00288   FillDiffOperator(Wg,Dim);
00289 }
00290 Matrice::~Matrice() {
00291   //  delete[] data;
00292   free(data);
00293 }
00294 void Matrice::RandomFill(double Max){
00295   Shout("RandomFill");
00296   for(int r=0;r<NRow;r++)
00297     for(int c=0;c<NCol;c++)
00298       data[NRow*c+r] = Max*drand48();//Casuale();
00299 }
00300 void Matrice::FillDiffOperator(SPLINE Wg,int Dim){
00301   Shout("Matrice(SPLINE,dim)");
00302   double MenoDue = - .75*Wg.a3 + 2.*Wg.a4;//1.5
00303   //MenoDue += .125*Wg.a1 - .125*Wg.a2; //O(h^4)
00304   MenoDue -= 1./12.*Wg.a2; //O(h^4)
00305   double MenoUno = -.5*Wg.a1 + Wg.a2 + 1.5*Wg.a3 - 8.*Wg.a4;//6
00306   //MenoUno += -.125*Wg.a1 - .5*Wg.a2;
00307   MenoUno += 1./3.*Wg.a2; //O(h^4)
00308   double Zero = Wg.a0 - 2.*Wg.a2 + 12.*Wg.a4;//9
00309   //Zero += 0.75*Wg.a2;
00310   Zero -= 0.5*Wg.a2; //O(h^4)
00311   double PiuUno = .5*Wg.a1 + Wg.a2 - 1.5*Wg.a3 - 8.*Wg.a4;
00312   //PiuUno += .25*Wg.a1 + .5*Wg.a2;
00313   PiuUno += 1./3.*Wg.a2;
00314   double PiuDue = .75*Wg.a3 + 2.*Wg.a4;
00315   //PiuDue += -.125*Wg.a1 -.125*Wg.a2;
00316   PiuDue -= 1./12.*Wg.a2; //O(h^4)
00317   double Norm=0.;
00318   if (Dim == 0){
00319     for(int r=0;r<NCol;r++){
00320       data[NRow*0+r] = MenoDue;
00321       data[NRow*1+r] = MenoUno;
00322       data[NRow*2+r] = Zero;
00323       data[NRow*3+r] = PiuUno;
00324       data[NRow*4+r] = PiuDue;
00325     }
00326   }
00327   else if(Dim == 1){
00328     int r=2;
00329     data[NRow*0+r] = MenoDue;
00330     data[NRow*1+r] = MenoUno;
00331     data[NRow*2+r] = Zero;
00332     data[NRow*3+r] = PiuUno;
00333     data[NRow*4+r] = PiuDue;
00334   }
00335   else if (Dim == 2){
00336     int r=2;
00337     data[NRow*0+r] = MenoDue;
00338     data[NRow*1+r] = MenoUno;
00339     data[NRow*2+r] = Zero;
00340     data[NRow*3+r] = PiuUno;
00341     data[NRow*4+r] = PiuDue;
00342     int c=2;
00343     data[NRow*c+0] = MenoDue;
00344     data[NRow*c+1] = MenoUno;
00345     data[NRow*c+2] += Zero;
00346     data[NRow*c+3] = PiuUno;
00347     data[NRow*c+4] = PiuDue;
00348   }
00349   else if (Dim == 3){
00350     for(int r=0;r<NCol;r++){
00351       data[NRow*0+r] = MenoDue;
00352       data[NRow*1+r] = MenoUno;
00353       data[NRow*2+r] = Zero;
00354       data[NRow*3+r] = PiuUno;
00355       data[NRow*4+r] = PiuDue;
00356     }
00357   }
00358 }
00359 void Matrice::FillCanny(){
00360   if(NRow != 5){
00361     printf("I don't know the expresion for matrices of NRow != 5\n");
00362     return ;
00363   }
00364   if(NCol != 5){
00365     printf("I don't know the expresion for matrices of NRow != 5\n");
00366     return ;
00367   }
00368   Set(0,0,2.); Set(0,1,4.); Set(0,2,5.); Set(0,3,4.); Set(0,4,2.);
00369   Set(1,0,4.); Set(1,1,9.); Set(1,2,12.); Set(1,3,9.); Set(1,4,4.);
00370   Set(2,0,5.); Set(2,1,12.); Set(2,2,15.); Set(2,3,12.); Set(2,4,5.);
00371   Set(3,0,4.); Set(3,1,9.); Set(3,2,12.); Set(3,3,9.); Set(3,4,4.);
00372   Set(4,0,2.); Set(4,1,4.); Set(4,2,5.); Set(4,3,4.); Set(4,4,2.);
00373   Multiply(1./159.);
00374 }
00375 void Matrice::FillGaussian(double Sigma,double CutOff){
00376   int Half = NRow/2;
00377   double Norm = 0.;
00378   if(NDim == 3){
00379     for(int r=0;r<NRow;r++){
00380       double x = CutOff*(r-Half)/(double)NRow;
00381       for(int c=0;c<NCol;c++){
00382    double y = CutOff*(c-Half)/(double)NCol;
00383    for(int q=0;q<NCol;q++){
00384      double z = CutOff*(q-Half)/(double)NCol;
00385      double r2 = SQR(x) + SQR(y) + SQR(z);
00386      double Gauss = exp(-r2*.5/SQR(Sigma));
00387      data[(NRow*c+r)*NCol+q] = Gauss;
00388      Norm += Gauss;
00389    }
00390       }
00391     }
00392   }
00393   else if(NDim == 2){
00394     for(int r=0;r<NRow;r++){
00395       double x = CutOff*(r-Half)/(double)NRow;
00396       for(int c=0;c<NCol;c++){
00397    double y = CutOff*(c-Half)/(double)NCol;
00398    double r2 = SQR(x) + SQR(y);
00399    double Gauss = exp(-r2*.5/SQR(Sigma));
00400    data[NRow*c+r] = Gauss;
00401    Norm += Gauss;
00402       }
00403     }
00404   }
00405   else if(NDim == 1){
00406     for(int r=0;r<NRow;r++){
00407       double x = CutOff*(r-Half)/(double)NRow;
00408       double r2 = SQR(x);
00409       double Gauss = exp(-r2*.5/SQR(Sigma));
00410       data[r] = Gauss;
00411       Norm += Gauss;
00412     }
00413   }
00414   Multiply(1./Norm);
00415 }
00416 void Matrice::FillGaussian5(){
00417   if(NRow != 5 && NCol != 5){
00418     printf("Invalid number of rows and cols %dx%d != 7x7\n",NRow,NCol);
00419     return;
00420   }
00421   data[NRow*0+0] = 1;
00422   data[NRow*0+1] = 4;
00423   data[NRow*0+2] = 7;
00424   data[NRow*0+3] = 4;
00425   data[NRow*0+4] = 1;
00426
00427   data[NRow*1+0] = 4;
00428   data[NRow*1+1] = 16;
00429   data[NRow*1+2] = 26;
00430   data[NRow*1+3] = 16;
00431   data[NRow*1+4] = 4;
00432
00433   data[NRow*2+0] = 7;
00434   data[NRow*2+1] = 26;
00435   data[NRow*2+2] = 41;
00436   data[NRow*2+3] = 26;
00437   data[NRow*2+4] = 7;
00438
00439   data[NRow*3+0] = 4;
00440   data[NRow*3+1] = 16;
00441   data[NRow*3+2] = 26;
00442   data[NRow*3+3] = 16;
00443   data[NRow*3+4] = 4;
00444
00445   data[NRow*4+0] = 1;
00446   data[NRow*4+1] = 4;
00447   data[NRow*4+2] = 7;
00448   data[NRow*4+3] = 4;
00449   data[NRow*4+4] = 1;
00450
00451   Multiply(1./273.);
00452 }
00453
00454
00455 //---------------Manage-entries-------------------
00456 double Matrice ::Val(int row){
00457 #ifdef MATR_DEBUG
00458   if ( (row>=NRow) || (row<0) )
00459     {  printf("Values %d %d out of range %d %d\n",row,NRow);
00460       return 1.;    }
00461 #endif
00462   return data[row];
00463 }
00464 double Matrice ::Val(int row,int col){
00465 #ifdef MATR_DEBUG
00466   if ( (row>=NRow) || (col>=NCol)
00467        || (row<0) || (col<0) )
00468     {  printf("Values %d %d out of range %d %d\n",row,col,NRow,NCol);
00469       return 1.;    }
00470 #endif
00471   return data[ NRow*col+row ];
00472 }
00473 double Matrice ::Val(int row,int col,int zed){
00474   return data[ (NRow*col+row)*NCol+zed ];
00475 }
00476 bool Matrice ::Set(int row, int column, double newvalue)  {
00477 #ifdef MATR_DEBUG
00478   if ( (row >= NRow) || (column >= NCol)
00479        || (row<0) || (column<0) ) return false;
00480 #endif
00481   data[ NRow*column + row ] = newvalue;
00482   return true;
00483 }
00484 bool Matrice ::Add(int row, int column, double Value)  {
00485 #ifdef MATR_DEBUG
00486   if ( (row >= NRow) || (column >= NCol)
00487        || (row<0) || (column<0) ) return false;
00488 #endif
00489   data[ NRow*column + row ] += Value;
00490   return true;
00491 }
00492 void Matrice ::Print(){
00493   if(NDim == 3){
00494     for(int q=0;q<NZed;q++){
00495       printf("%d)\n",q);
00496       for(int r=0;r<NRow;r++){
00497    printf("|");
00498    for(int c=0;c<NCol;c++){
00499      printf("%.2g ",data[(NRow*c+r)*NCol+q]);
00500    }
00501    printf("|\n");
00502       }
00503       printf("\n");
00504     }
00505     return;
00506   }
00507   else if(NDim == 1){
00508     printf("|");
00509     for(int r=0;r<NRow;r++){
00510       printf("%.2g ",data[r]);
00511     }
00512     printf("|\n");
00513     printf("\n");
00514     return;
00515   }
00516   for(int r=0;r<NRow;r++){
00517     printf("|");
00518     for(int c=0;c<NCol;c++){
00519       printf("%.2g ",data[NRow*c+r]);
00520     }
00521     printf("|\n");
00522   }
00523   printf("\n");
00524 }
00525 //---------------inversion-(-stolen,-to-be-checked-)---------
00526 void Matrice ::comparetoidentity()  {
00527   int worstdiagonal = 0;
00528   double maxunitydeviation = 0.0;
00529   double currentunitydeviation;
00530   for ( int i = 0; i < NRow; i++ )  {
00531     currentunitydeviation = data[i+i*NRow] - 1.;
00532     if ( currentunitydeviation < 0.0) currentunitydeviation *= -1.;
00533     if ( currentunitydeviation > maxunitydeviation )  {
00534       maxunitydeviation = currentunitydeviation;
00535       worstdiagonal = i;
00536     }
00537   }
00538   int worstoffdiagonalrow = 0;
00539   int worstoffdiagonalcolumn = 0;
00540   double maxzerodeviation = 0.0;
00541   double currentzerodeviation ;
00542   for ( int i = 0; i < NRow; i++ )  {
00543     for ( int j = 0; j < NRow; j++ )  {
00544       if ( i == j ) continue;  // we look only at non-diagonal terms
00545       currentzerodeviation = data[i+j*NRow];
00546       if ( currentzerodeviation < 0.0) currentzerodeviation *= -1.0;
00547       if ( currentzerodeviation > maxzerodeviation )  {
00548    maxzerodeviation = currentzerodeviation;
00549    worstoffdiagonalrow = i;
00550    worstoffdiagonalcolumn = j;
00551       }
00552
00553     }
00554   }
00555   cout << "Worst diagonal value deviation from unity: "
00556        << maxunitydeviation << " at row/column " << worstdiagonal << endl;
00557   cout << "Worst off-diagonal value deviation from zero: "
00558        << maxzerodeviation << " at row = " << worstoffdiagonalrow
00559        << ", column = " << worstoffdiagonalcolumn << endl;
00560 }
00561 void Matrice ::settoproduct(Matrice& left, Matrice& right)  {
00562 //   NRow = left.getNRow();
00563 //   if ( NSize < left.getNRow() )   {
00564 //     NSize = left.getNRow();
00565 //     allocate();
00566 //   }
00567   for ( int i = 0; i < NRow; i++ )
00568     for ( int j = 0; j < NRow; j++ )  {
00569       double sum = 0.0;
00570       double leftvalue, rightvalue;
00571       bool success;
00572       for (int c = 0; c < NRow; c++)  {
00573    left.getvalue(i,c,leftvalue,success);
00574    right.getvalue(c,j,rightvalue,success);
00575    sum += leftvalue * rightvalue;
00576       }
00577       Set(i,j,sum);
00578     }
00579 }
00580 void Matrice ::copymatrix(Matrice&  source)  {
00581 //   NRow = source.getNRow();
00582 //   if ( NSize < source.getNRow() )  {
00583 //     NSize = source.getNRow();
00584 //     allocate();
00585 //   }
00586   for ( int i = 0; i < NRow; i++ )
00587     for ( int j = 0; j < NRow; j++ )  {
00588       double value;
00589       bool success;
00590       source.getvalue(i,j,value,success);
00591       data[i+j*NRow] = value;
00592     }
00593 }
00594 void Matrice ::setNRow(int newNRow) {
00595 //   if ( newNRow > NSize )
00596 //     {
00597 //       NSize = newNRow ; // * 2;  // wastes memory but saves
00598 //       // time otherwise required for
00599 //       // operation new[]
00600 //       allocate();
00601 //     }
00602 //   if (newNRow >= 0) NRow = newNRow;
00603 }
00604 int Matrice ::getNRow() { return NRow; }
00605 void Matrice ::getvalue(int row, int column, double& returnvalue, bool& success)   {
00606 #ifdef MATR_DEBUG
00607   if ( (row>=NRow) || (column>=NCol)
00608        || (row<0) || (column<0) )
00609     {  success = false;
00610       return;    }
00611 #endif
00612   returnvalue = data[ row + column*NRow ];
00613   success = true;
00614 }
00615 void Matrice::Invert()  {
00616 #ifdef MATR_DEBUG //CHECKIT
00617   if (NRow <= 0) return;  // sanity check
00618   if (NRow == 1) return;  // must be of dimension >= 2
00619 #endif
00620   for (int i=1; i < NRow; i++) data[i] /= data[0]; // normalize row 0
00621   for (int i=1; i < NRow; i++)  {
00622     for (int j=i; j < NRow; j++)  { // do a column of L
00623       double sum = 0.0;
00624       for (int k = 0; k < i; k++)
00625    sum += data[j+k*NRow] * data[k+i*NRow];
00626       data[j+i*NRow] -= sum;
00627     }
00628     if (i == NRow-1) continue;
00629     for (int j=i+1; j < NRow; j++)  {  // do a row of U
00630       double sum = 0.0;
00631       for (int k = 0; k < i; k++)
00632    sum += data[i+k*NRow]*data[k+j*NRow];
00633       data[i+j*NRow] =
00634    (data[i+j*NRow]-sum) / data[i+i*NRow];
00635     }
00636   }
00637   for ( int i = 0; i < NRow; i++ )  // invert L
00638     for ( int j = i; j < NRow; j++ )  {
00639       double x = 1.0;
00640       if ( i != j ) {
00641    x = 0.0;
00642    for ( int k = i; k < j; k++ )
00643      x -= data[j+k*NRow]*data[k+i*NRow];
00644       }
00645       data[j+i*NRow] = x / data[j+j*NRow];
00646     }
00647   for ( int i = 0; i < NRow; i++ )   // invert U
00648     for ( int j = i; j < NRow; j++ )  {
00649       if ( i == j ) continue;
00650       double sum = 0.0;
00651       for ( int k = i; k < j; k++ )
00652    sum += data[k+j*NRow]*( (i==k) ? 1.0 : data[i+k*NRow] );
00653       data[i+j*NRow] = -sum;
00654     }
00655   for ( int i = 0; i < NRow; i++ )   // final inversion
00656     for ( int j = 0; j < NRow; j++ )  {
00657       double sum = 0.0;
00658       for ( int k = ((i>j)?i:j); k < NRow; k++ )
00659    sum += ((j==k)?1.0:data[j+k*NRow])*data[k+i*NRow];
00660       data[j+i*NRow] = sum;
00661     }
00662 };
00663 //-------------Other-operations-----------------
00664 int Matrice ::Solve(double *Known,double *UnKnown){
00665   Shout("Solve");
00666 #ifdef MATR_DEBUG
00667 #endif
00668   Matrice Lower(NCol,NCol);
00669   Matrice Upper(NCol,NCol);
00670   double *AlmostKnown = (double *)calloc(NCol,sizeof(double));
00671   if(!AlmostKnown) {printf("Not allocated\n");return 0;}
00672   /* assegna i valori della diagonale di L */
00673   for(int r = 0; r < NRow ; r ++){
00674     Lower.Set(r,r,1.);
00675   }
00676   /* calcola gli elementi fuori della diagonale */
00677   for(int j = 0; j < NCol ; j ++){
00678     for(int i = 0; i <= j ; i++){
00679       Upper.Set(i,j,Val(i,j));
00680       for (int k = 0; k <= i - 1; k++) {
00682       }
00683     }
00684     for (int  i = j + 1; i < NCol ; i++) {
00685       Lower.Set(i,j,Val(i,j));
00686       for (int k = 0; k <= j - 1; k ++) {
00688       }//FIXME
00689       double Diag = POS(Upper.Val(j,j)) > 0. ? Lower.Val(i,j)/Upper.Val(j,j) : 1.;
00690       Lower.Set(i,j,Diag);
00691     }
00692   }//Solve the Uz = y
00693   //Upper = NULL;
00694   for(int r=0;r<NRow;r++){
00695     AlmostKnown[r] = Known[r];
00696     for(int c=r-1;c>=0;c--){
00697       AlmostKnown[r] -= AlmostKnown[c]*Lower.Val(r,c);
00698       //printf("Coeff[%d][%d]=%lf %lf\n",r,c,UnKnown[r],Lower.Val(r,c));
00699     }
00700     if(POS(Lower.Val(r,r))> 0.)
00701       AlmostKnown[r] /=  POS(Lower.Val(r,r))>0. ? Lower.Val(r,r) : 1.;
00702     else
00703       AlmostKnown[r] = 0.;
00704     //printf("Coeff[%d][%d]=%lf %lf\n",r,r,AlmostKnown[r],Lower.Val(r,r));
00705   }
00706   //Lower.Print();
00707   for(int r=NRow-1;r>=0;r--){
00708     UnKnown[r]  = AlmostKnown[r];
00709    for(int c=r+1;c<NCol;c++){
00710      UnKnown[r] -= UnKnown[c]*Upper.Val(r,c);
00711      // printf("Coeff[%d][%d]=%lf %lf\n",r,c,AlmostKnown[r],Upper.Val(r,c));
00712     }
00713    if(POS(Upper.Val(r,r))> 0.)
00714       UnKnown[r] /=  Upper.Val(r,r);
00715     else
00716       UnKnown[r] = 0.;
00717    //printf("Coeff[%d][%d]=%lf %lf\n",r,r,UnKnown[r],Upper.Val(r,r));
00718   }//finish solving Lx = z
00719   //Upper.Print();
00720   free(AlmostKnown);
00721   return 1;
00722 };
00723 void Matrice::Apply(double *Known,double *UnKnown){
00724   Shout("Apply");
00725   for(int r=0;r<NRow;r++){
00726     for(int c=0;c<NCol;c++){
00727       UnKnown[r] += Known[c]*data[r+c*NRow];
00728     }
00729   }
00730 };
00731 double Matrice::Det(){
00732   Shout("Det");
00733   Matrice Lower(NCol);
00734   Matrice Upper(NCol);
00735   /* assegna i valori della diagonale di L */
00736   for (int r = 0; r < NCol ; r ++) {
00737     Lower.Set(r,r,1.);
00738   }
00739   /* calcola gli elementi fuori della diagonale */
00740   for (int j = 0; j < NCol ; j ++) {
00741     for (int i = 0; i <= j ; i++) {
00742       Upper.Set(i,j,Val(i,j));
00743       for (int k = 0; k <= i - 1; k++) {
00745       }
00746     }
00747     for (int  i = j + 1; i < NCol ; i++) {
00748       Lower.Set(i,j,Val(i,j));
00749       for (int k = 0; k <= j - 1; k ++) {
00751       }
00752       Lower.Set(i,j,Lower.Val(i,j)/Upper.Val(j,j));
00753     }
00754   }
00755   double Det = 1.;
00756   for(int r=0;r<NCol;r++)
00757     Det *= Upper.Val(r,r);
00758   return Det;
00759 };
00760 void Matrice::Transpose(){
00761   Shout("Transpose");
00762   double *Temp = (double *)calloc(NCol*NRow,sizeof(double));
00763   for(int r=0;r<NRow;r++){
00764     for(int c=0;c<NCol;c++){
00765       Temp[r+c*NRow] = data[r+c*NRow];
00766     }
00767   }
00768   for(int r=0;r<NRow;r++){
00769     for(int c=0;c<NCol;c++){
00770       data[r+c*NRow] = Temp[c+r*NRow];
00771     }
00772   }
00773   free(Temp);
00774 }
00775 void Matrice::Clear(){
00776   Shout("Clear");
00777   for(int r=0;r<NRow;r++){
00778     for(int c=0;c<NCol;c++){
00779       data[r+c*NRow] = 0.;
00780     }
00781   }
00782 }
00783 void Matrice::Normalize(){
00784   Shout("Normalize");
00785   double Max=0.;
00786   double Min=0.;
00787   for(int r=0;r<NRow;r++){
00788     for(int c=0;c<NCol;c++){
00789       if(Max < data[r+c*NRow])
00790    Max = data[r+c*NRow];
00791       if(Min > data[r+c*NRow])
00792    Min = data[r+c*NRow];
00793     }
00794   }
00795   if(fabs(Min) > 0.0e-5){
00796     for(int r=0;r<NRow;r++)
00797       for(int c=0;c<NCol;c++)
00798    data[r+c*NRow] = data[r+c*NRow]/(Max-Min)-Min;
00799
00800   }
00801   else {
00802     for(int r=0;r<NRow;r++)
00803       for(int c=0;c<NCol;c++)
00804    data[r+c*NRow] = data[r+c*NRow]/Max;
00805   }
00806 }
00807 void Matrice::Multiply(double Val){
00808   for(int s=0;s<NSize;s++)
00809     data[s] *= Val;
00810   // for(int r=0;r<NRow;r++)
00811   //   for(int c=0;c<NCol;c++)
00812   //     data[r+c*NRow] = data[r+c*NRow]*Val;
00813 }
00814 void Matrice::CopyOn(Matrice *B){
00815   for(int r=0;r<NRow;r++)
00816     for(int c=0;c<NCol;c++)
00817       B->Set(r,c,data[r+c*NRow]);
00818 }
00819 Matrice Matrice::operator+ (Matrice& A){
00820   if(NCol != A.NCol || NRow != A.NCol){
00821     printf("Incompatible matrices. Dim: %d/%d\n",NSize,A.NSize);
00822     return 0;
00823   }
00824   Matrice Resp(NRow,NCol);
00825   for(int r=0;r<NRow;r++)
00826     for(int c=0;c<NCol;c++)
00827       Resp.Set(r,c,Val(r,c)+A.Val(r,c));
00828   return Resp;
00829 }
00830 Matrice Matrice::operator* (Matrice &A){
00831   if(NCol != A.NRow || NRow != A.NCol){
00832     printf("Incompatible matrices. Dim: %d/%d\n",NSize,A.NSize);
00833     return 0;
00834   }
00835   Matrice Resp(NRow,A.NCol);
00836   for(int r=0;r<NRow;r++)
00837     for(int c=0;c<A.NCol;c++){
00838       double Temp=0.;
00839       for(int i=0;i<NRow;i++)
00840    //Temp += Val(r,i)*A.Val(i,c);
00841    Temp += Resp.data[r+i*NRow]*A.data[i+c*NRow];
00842       Resp.Set(r,c,Temp);
00843     }
00844   Resp.Print();
00845   return Resp;
00846 }
00847 Matrice Matrice::operator= (Matrice& A){
00848   if(NRow != A.NRow || NCol != A.NCol){
00849     printf("Incompatible matrices. Dim: %d/%d\n",NSize,A.NSize);
00850     return 0;
00851   }
00852   for(int r=0;r<NRow;r++)
00853     for(int c=0;c<NCol;c++)
00854       Set(r,c,A.Val(r,c));
00855   return *this;
00856 }
00857 Matrice Matrice::operator* (const double& Molt)const{
00858   for(int r=0;r<NRow;r++)
00859     for(int c=0;c<NCol;c++)
00860       data[r+c*NRow] *= Molt;
00861   return *this;
00862 }
00863 Matrice Matrice::operator^(Matrice& A)const{
00864   assert(  NCol == A.NRow || NRow == A.NCol);
00865   printf("Still to be written!!!\n");
00866   Matrice B(NCol,NRow);
00867   return B;
00868 }
00869 Vettore Matrice::operator* (const Vettore& u)const {
00870   if(NCol != u.NDim){
00871     printf("Incompatible matrices. Dim: %d/%d\n",NSize,u.NDim);
00872     return 0;
00873   }
00874   Vettore Resp(NRow);
00875   for(int r=0;r<NRow;r++){
00876       double Temp=0.;
00877     for(int c=0;c<NCol;c++)
00878       Temp += data[r+c*NRow]*u.x[c];
00879     Resp.x[r] = Temp;
00880   }
00881   return Resp;
00882 }
00883 void Matrice::Mult(Matrice &A,Matrice &B){
00884   if(B.NCol != A.NRow || B.NRow != A.NCol){
00885     printf("Incompatible matrices. Dim: %d/%d\n",B.NSize,A.NSize);
00886     return ;
00887   }
00888   for(int r=0;r<A.NRow;r++)
00889     for(int c=0;c<B.NCol;c++){
00890       double Temp=0.;
00891       for(int i=0;i<A.NRow;i++)
00892    Temp += A.Val(r,i)*B.Val(i,c);
00893       Set(r,c,Temp);
00894     }
00895 }
00896 void Matrice::Mult(Matrice &A){
00897   if(NCol != A.NRow || NRow != A.NCol){
00898     printf("Incompatible matrices. Dim: %d/%d\n",NSize,A.NSize);
00899     return ;
00900   }
00901   for(int r=0;r<A.NRow;r++)
00902     for(int c=0;c<NCol;c++){
00903       double Temp=0.;
00904       for(int i=0;i<A.NRow;i++)
00905    Temp += A.Val(r,i)*Val(i,c);
00906       Set(r,c,Temp);
00907     }
00908 }
00909 Vettore Matrice::Mult(Matrice &A,Vettore &v){
00910   Vettore Resp(NRow);
00911   for(int r=0;r<NRow;r++){
00912     double Temp=0.;
00913     for(int c=0;c<NCol;c++)
00914       Temp += A.data[r+c*NRow]*v.x[c];
00915     Resp.x[r] = Temp;
00916   }
00917   return Resp;
00918 }
00919 Vettore Matrice::Mult(Vettore &v){
00920   Vettore Resp(NRow);
00921   for(int r=0;r<NRow;r++){
00922     double Temp=0.;
00923     for(int c=0;c<NCol;c++)
00924       Temp += data[r+c*NRow]*v.x[c];
00925     Resp.x[r] = Temp;
00926   }
00927   return Resp;
00928 }
00929 void Matrice::Mult(Vettore &v,Vettore &u){
00930   for(int r=0;r<NRow;r++){
00931     double Temp=0.;
00932     for(int c=0;c<NCol;c++)
00933       Temp += data[r+c*NRow]*v.x[c];
00934     u.x[r] = Temp;
00935   }
00936 }
00937 void Matrice::ConvoluteMatrix(double *Plot,int NGrid,int NDim,int IfMinImConv){
00938   if(NDim == 1){
00939     if(!IfMinImConv) ConvoluteMatrix1(Plot,NGrid);
00940     else ConvoluteMatrix1MinImConv(Plot,NGrid);
00941   }
00942   else if(NDim == 2){
00943     if(!IfMinImConv) ConvoluteMatrix2(Plot,NGrid);
00944     else ConvoluteMatrix2MinImConv(Plot,NGrid);
00945   }
00946   else if(NDim == 3){
00947     ConvoluteMatrix3(Plot,NGrid);
00948   }
00949 }
00950 //without minimum image convention
00951 void Matrice::ConvoluteMatrix1(double *Plot,int NGrid){
00952   double *Plot2 = (double *)calloc(NGrid,sizeof(double));
00953   int NMat2 = (int)pNRow()/2;
00954   for(int gx=0;gx<NGrid;gx++){
00955     if(gx <= NMat2  || gx >= NGrid - NMat2){
00956       Plot2[gx] = Plot[gx];
00957       continue;
00958     }
00959     for(int mx=0;mx<pNRow();mx++){
00960       int g1x = gx + mx - NMat2;
00961       if(g1x >= NGrid){
00962    Plot2[gx] = Plot[gx];
00963    break;
00964       }
00965       if(g1x < 0){
00966    Plot2[gx] = Plot[gx];
00967    break;
00968       }
00969       Plot2[gx] += Plot[g1x]*Val(mx);
00970     }
00971   }
00972   for(int gx=0;gx<NGrid;gx++){
00973     Plot[gx] = Plot2[gx];
00974   }
00975   free(Plot2);
00976 }
00977 //with minimum image convention
00978 void Matrice::ConvoluteMatrix1MinImConv(double *Plot,int NGrid){
00979   double *Plot2 = (double *)calloc(NGrid,sizeof(double));
00980   int NMat2 = (int)pNRow()/2;
00981   for(int gx=0;gx<NGrid;gx++){
00982     if(gx <= NMat2  || gx >= NGrid - NMat2){
00983       Plot2[gx] = Plot[gx];
00984       continue;
00985     }
00986     for(int mx=0;mx<pNRow();mx++){
00987       int g1x = gx + mx - NMat2;
00988       if(g1x >= NGrid) g1x -= NGrid;
00989       if(g1x < 0) g1x + NGrid;
00990       Plot2[gx] += Plot[g1x]*Val(mx);
00991     }
00992   }
00993   for(int gx=0;gx<NGrid;gx++){
00994     Plot[gx] = Plot2[gx];
00995   }
00996   free(Plot2);
00997 }
00999 void Matrice::ConvoluteMatrix2(double *Plot,int NGrid){
01000   double *Plot2 = (double *)calloc(SQR(NGrid),sizeof(double));
01001   int NMat2 = (int)pNCol()/2;
01002   for(int gx=0;gx<NGrid;gx++){
01003     for(int gy=0;gy<NGrid;gy++){
01004       for(int mx=0;mx<pNRow();mx++){
01005    int g1x = gx + mx - NMat2;
01006    if(g1x >= NGrid){
01007      Plot2[gx*NGrid+gy] = Plot[gx*NGrid+gy];
01008      break;
01009    }
01010    if(g1x < 0) {
01011      Plot2[gx*NGrid+gy] = Plot[gx*NGrid+gy];
01012      break;
01013    }
01014    for(int my=0;my<pNCol();my++){
01015      int g1y = gy + my - NMat2;
01016      if(g1y >= NGrid){
01017        Plot2[gx*NGrid+gy] = Plot[gx*NGrid+gy];
01018        break;
01019      }
01020      if(g1y < 0){
01021        Plot2[gx*NGrid+gy] = Plot[gx*NGrid+gy];
01022        break;
01023      }
01024      Plot2[gx*NGrid+gy] += Plot[g1x*NGrid+g1y]*Val(mx,my);
01025    }
01026       }
01027     }
01028   }
01029   for(int gx=0;gx<NGrid;gx++){
01030     for(int gy=0;gy<NGrid;gy++){
01031       Plot[gx*NGrid+gy] = Plot2[gx*NGrid+gy];
01032     }
01033   }
01034   free(Plot2);
01035 }
01037 void Matrice::ConvoluteMatrix2MinImConv(double *Plot,int NGrid){
01038   double *Plot2 = (double *)calloc(SQR(NGrid),sizeof(double));
01039   int NMat2 = (int)pNCol()/2;
01040   for(int gx=0;gx<NGrid;gx++){
01041     for(int gy=0;gy<NGrid;gy++){
01042       for(int mx=0;mx<pNRow();mx++){
01043    int g1x = gx + mx - NMat2;
01044    if(g1x >= NGrid) continue;//g1x -= NGrid;
01045    if(g1x < 0) continue;//g1x + NGrid;
01046    for(int my=0;my<pNCol();my++){
01047      int g1y = gy + my - NMat2;
01048      if(g1y >= NGrid) continue;//g1y -= NGrid;
01049      if(g1y < 0) continue;//g1y + NGrid;
01050      Plot2[gx*NGrid+gy] += Plot[g1x*NGrid+g1y]*Val(mx,my);
01051    }
01052       }
01053     }
01054   }
01055   for(int gx=0;gx<NGrid;gx++){
01056     for(int gy=0;gy<NGrid;gy++){
01057       Plot[gx*NGrid+gy] = Plot2[gx*NGrid+gy];
01058     }
01059   }
01060   free(Plot2);
01061 }
01062 //with minimum image convention
01063 void Matrice::ConvoluteMatrix3(double *Plot,int NGrid){
01064   double *Plot2 = (double *)calloc(CUBE(NGrid),sizeof(double));
01065   int NMat2 = (int)pNCol()/2;
01066   printf("NMat2 %d\n",NMat2);
01067   for(int gx=0;gx<NGrid;gx++){
01068     for(int gy=0;gy<NGrid;gy++){
01069       for(int gz=0;gz<NGrid;gz++){
01070    for(int mx=0;mx<pNRow();mx++){
01071      int g1x = gx + mx - NMat2;
01072      if(g1x >= NGrid) g1x -= NGrid;
01073      if(g1x < 0) g1x += NGrid;
01074      for(int my=0;my<pNCol();my++){
01075        int g1y = gy + my - NMat2;
01076        if(g1y >= NGrid) g1y -= NGrid;
01077        if(g1y < 0) g1y += NGrid;
01078        for(int mz=0;mz<pNZed();mz++){
01079          int g1z = gz + mz - NMat2;
01080          if(g1z >= NGrid) g1z -= NGrid;
01081          if(g1z < 0) g1z += NGrid;
01082          Plot2[(gx*NGrid+gy)*NGrid+gz] += Plot[(g1x*NGrid+g1y)*NGrid+g1z]*Val(mx,my,mz);
01083        }
01084      }
01085    }
01086       }
01087     }
01088   }
01089   for(int gx=0;gx<NGrid;gx++){
01090     for(int gy=0;gy<NGrid;gy++){
01091       for(int gz=0;gz<NGrid;gz++){
01092    Plot[(gx*NGrid+gy)*NGrid+gz] = Plot2[(gx*NGrid+gy)*NGrid+gz];
01093       }
01094     }
01095   }
01096   free(Plot2);
01097 }
```