NumLinAlg.cxx

Go to the documentation of this file.
00001 
00014 #include "NumLinAlg.h"
00015 
00016 #include <fstream>
00017 #include <iostream>
00018 #include <iomanip>
00019 #include <cmath>
00020 #include <cfloat>
00021 #include <cassert>
00022 
00023 using std::ofstream;
00024 using std::ifstream;
00025 using std::setw;
00026 using std::setprecision;
00027 using std::cout;
00028 using std::endl;
00029 using std::vector;
00030 using std::abs;
00031 
00032 namespace hippodraw {
00033   namespace Numeric {
00034 
00035 std::vector< double >
00036 operator + ( const std::vector< double >& x, 
00037              const std::vector< double >& y  )
00038 {
00039   int nrows = x.size();
00040   vector< double > z;
00041   
00042   z.resize( nrows, 0.0 );
00043   
00044   for ( int i = 0; i < nrows; i++)
00045       z[i] = x[i] + y[i];
00046 
00047   return z;
00048 }
00049 
00050 std::vector< double >
00051 operator - ( const std::vector< double >& x,
00052              const std::vector< double >& y  )
00053 {
00054   int nrows = x.size();
00055   vector< double > z;
00056   
00057   z.resize( nrows, 0.0 );
00058   
00059   for ( int i = 0; i < nrows; i++)
00060       z[i] = x[i] - y[i];
00061 
00062   return z;
00063 }
00064 
00065 std::vector< double >
00066 operator - ( const std::vector< double >& y  )
00067 {
00068   int nrows = y.size();
00069   vector< double > z;
00070   
00071   z.resize( nrows, 0.0 );
00072   
00073   for ( int i = 0; i < nrows; i++ )
00074       z[i] = - y[i];
00075 
00076   return z;
00077 }
00078 
00079 std::vector< vector< double > >
00080 operator + ( const std::vector< std::vector< double > >&A,
00081              const std::vector< std::vector< double > >&B )
00082 {
00083   int nrows = A.size();
00084   int ncols = A[0].size();
00085    
00086   vector< vector< double > > C( nrows );
00087   for( int i = 0; i < nrows; i++ )
00088     C[i].resize( ncols, 0.0 );
00089   
00090   for (int i = 0; i < nrows; i++) 
00091     for (int j = 0; j < ncols; j++)
00092       C[i][j] = A[i][j] +  B[i][j];
00093 
00094   return C;
00095 }
00096 
00097 std::vector< vector< double > >
00098 operator - ( const std::vector< std::vector< double > >&A,
00099              const std::vector< std::vector< double > >&B )
00100 {
00101   int nrows = A.size();
00102   int ncols = A[0].size();
00103    
00104   vector< vector< double > > C( nrows );
00105   for( int i = 0; i < nrows; i++ )
00106     C[i].resize( ncols, 0.0 );
00107   
00108   for (int i = 0; i < nrows; i++) 
00109     for (int j = 0; j < ncols; j++)
00110       C[i][j] = A[i][j] -  B[i][j];
00111 
00112   return C;
00113 }
00114 
00115 
00116 std::vector< double >
00117 operator * ( double a, const std::vector< double >& x )
00118 {
00119   int nrows = x.size();
00120   vector< double > y;
00121   
00122   y.resize( nrows, 0.0 );
00123       
00124   for ( int i = 0; i < nrows; i++)
00125       y[i] = a * x[i];
00126 
00127   return y;
00128 }
00129 
00130 std::vector< double >
00131 operator / ( const std::vector< double >& x, double a )
00132 {
00133   int nrows = x.size();
00134   vector< double > y;
00135   
00136   assert( abs( a ) > DBL_EPSILON );
00137 
00138   y.resize( nrows, 0.0 );
00139   
00140   for ( int i = 0; i < nrows; i++)
00141       y[i] = x[i] / a;
00142 
00143   return y;
00144 }
00145 
00146 std::vector< std::vector< double > >
00147 operator * ( double a, const std::vector< std::vector< double > >&A )
00148 {
00149   int nrows = A.size();
00150   int ncols = A[0].size();
00151     
00152   vector< vector< double > > B( nrows );
00153   for( int i = 0; i < nrows; i++ )
00154     B[i].resize( ncols, 0.0 );
00155     
00156   for (int i = 0; i < nrows; i++) 
00157     for (int j = 0; j < ncols; j++)
00158       B[i][j] = a * A[i][j];
00159 
00160   return B;
00161 }
00162 
00163 std::vector< std::vector< double > >
00164 operator / ( const std::vector< std::vector< double > >& A, double a )
00165 {
00166   int nrows = A.size();
00167   int ncols = A[0].size();
00168 
00169   assert( abs( a ) > DBL_EPSILON );
00170   
00171   vector< vector< double > > B( nrows );
00172   for( int i = 0; i < nrows; i++ )
00173     B[i].resize( ncols, 0.0 );
00174   
00175   for (int i = 0; i < nrows; i++) 
00176     for (int j = 0; j < ncols; j++)
00177       B[i][j] = A[i][j]/a;
00178 
00179   return B;
00180 }
00181 
00182 std::vector< double >
00183 operator * ( const std::vector< std::vector< double > >& A,
00184              const std::vector< double >& x )
00185 {
00186   int nrows = A.size();
00187   int ncols = A[0].size();
00188   vector< double > y;
00189   
00190   y.resize( nrows, 0.0 );
00191   
00192   for (int i = 0; i < nrows; i++) 
00193     for (int j = 0; j < ncols; j++)
00194       y[i] += A[i][j] * x[j];
00195 
00196   return y;
00197 }
00198 
00199 std::vector< double >
00200 operator * ( const std::vector< double >& x,
00201              const std::vector< std::vector< double > >& A )
00202 {
00203   int nrows = A.size();
00204   int ncols = A[0].size();
00205   vector< double > y;
00206   
00207   y.resize( ncols, 0.0 );
00208     
00209   for (int j = 0; j < ncols; j++)
00210     for (int i = 0; i < nrows; i++) 
00211       y[j] += A[i][j] * x[i];
00212 
00213   return y;
00214 }
00215 
00216 std::vector< vector< double > >
00217 operator * ( const std::vector< std::vector< double > >&A,
00218              const std::vector< std::vector< double > >&B )
00219 {
00220   int m = A.size();
00221   int r = A[0].size();
00222   int n = B[0].size();
00223   
00224   vector< vector< double > > C( m );
00225   for( int i = 0; i < m; i++ )
00226     C[i].resize( n, 0.0 );
00227   
00228   for (int i = 0; i < m; i++) 
00229     for (int j = 0; j < n; j++)
00230       for (int k = 0; k < r; k++)
00231         C[i][j] += A[i][k] * B[k][j];
00232 
00233   return C;
00234 }
00235 
00236 double
00237 innerProduct( const std::vector< double > & a,
00238               const std::vector< double > & b )
00239 {
00240   double prod = 0.0;
00241   
00242   for ( unsigned int i = 0; i < a.size(); i++ ) 
00243     prod += a[i] * b[i];
00244 
00245   return prod;
00246 }
00247 
00248 
00249 vector< vector< double > >
00250 outerProduct ( const std::vector< double >& a,
00251                const std::vector< double >& b )
00252 {
00253   vector< vector< double> > C( a.size() );
00254   for( unsigned int i = 0; i < a.size(); i++ ) 
00255     C[i].resize( b.size() );
00256   
00257   for( unsigned int i = 0; i < a.size(); i++ )
00258     for( unsigned int j = 0; j < b.size(); j++ )
00259       C[i][j] = a[i] * b[j];
00260   
00261   return C;
00262 }
00263 
00264 
00265 double
00266 quadraticProduct( const std::vector< std::vector< double >  > & A,
00267                   const std::vector< double > x )
00268 {
00269   double prod = 0.0;
00270   unsigned int n = A.size();
00271   
00272   assert ( x.size() == n );
00273 
00274   for ( unsigned int i = 0; i < n; i++ )
00275     for ( unsigned int j = 0; j < n; j++ )
00276       prod += x[i] * A[i][j] * x[j];
00277       
00278   return prod;
00279 }
00280 
00281 
00282 double
00283 norm ( const std::vector< double > & a )
00284 {
00285   return sqrt( innerProduct( a, a ) );
00286 }
00287 
00288 
00289 int
00290 cholFactor ( std::vector < std::vector< double > > & A )
00291 {
00292   unsigned int n = A.size();
00293   
00294   for ( unsigned int i  = 0; i  < n ; ++i )
00295     for ( unsigned int j = 0; j <= i ; ++j)
00296       {
00297         double  sum = A[i][j];
00298         
00299         A[j][i] = 0;
00300         
00301         for ( unsigned int k  = 0; k  < j; ++k )
00302           sum -= A[i][k] * A[j][k];     
00303         
00304         if (i  == j)
00305           {
00306             if (sum < 0) return EXIT_FAILURE;    
00307 
00308             sum = sqrt (sum);
00309             
00310             if ( fabs(sum) < DBL_EPSILON ) return EXIT_FAILURE;
00311             
00312             A[i][j] = sum;
00313           }
00314         else
00315           A[i][j] = sum / A[j][j];
00316       }
00317   
00318   return EXIT_SUCCESS;
00319 }
00320 
00321 
00322 int
00323 cholBackSolve ( const std::vector < std::vector< double >  > &  L,
00324                 std::vector< double > & x,
00325                 const std::vector< double > & b )
00326 {
00327   unsigned int n = L.size();
00328   
00329   double sum;
00330 
00331   // Solving L y = b 
00332   for ( unsigned int i  = 0; i  < n ; i++)
00333     {
00334       sum = b [i];
00335       for ( unsigned int j = 0; j < i ; ++j)
00336        sum -= x[j] * L[i][j];
00337      x[i] = sum / L[i][i];
00338    }
00339   
00340   // Solving L' x = y
00341   for ( int i = n - 1; i >= 0; i-- )
00342     {
00343       sum = x[i];
00344       for ( unsigned int j = i + 1; j < n ; j++)
00345         sum -= x[j] * L[j][i];
00346       x[i] = sum / L[i][i]; 
00347     }
00348 
00349   return EXIT_SUCCESS;
00350 }
00351 
00352 
00353 int
00354 invertMatrix ( const std::vector < std::vector< double >  > &  A,
00355                std::vector < std::vector < double >  > &  Ainv )
00356 {
00357   unsigned int n = A.size();
00358   vector< double > x, b;
00359   vector< vector< double >  > L;
00360   
00361   // Set appropriate sizes for the vectors and matrices 
00362   x.resize( n, 0.0 );
00363   b.resize( n, 0.0 );
00364   
00365   L.resize( n );
00366   Ainv.resize( n );
00367   
00368   for ( unsigned int i = 0; i < n; i++ )
00369     {
00370       L[i].clear ();
00371       L[i].resize ( n, 0.0 );
00372 
00373       Ainv[i].clear ();
00374       Ainv[i].resize ( n, 0.0 );
00375     }
00376   
00377   for ( unsigned int i = 0; i < n; i++ )
00378     for ( unsigned int j = 0; j < n; j++ )
00379       L[i][j] = A[i][j];
00380   
00381   // Do a cholesky factorization writing the lower triangular factor
00382   // in the lower triangular part of L and setting the upper part as 0
00383   cholFactor( L );
00384     
00385   for ( unsigned int j = 0; j < n; j++ )
00386     {
00387       // Set up right hand side i.e. set b = ei  
00388       for ( unsigned int i = 0; i < n; i++ )
00389         b[i] = ( i == j ) ? 1 : 0;
00390       
00391       // LL'x= b is being solved here
00392       cholBackSolve( L, x, b );
00393       
00394       // This x constitutes the j th coloumn of the inverse
00395       for ( unsigned int i = 0; i < n; i++ )
00396         Ainv[i][j] = x[i] ;
00397     }
00398 
00399   return EXIT_SUCCESS;
00400 }
00401 
00402 int
00403 eye ( std::vector < std::vector < double > >& I, int n )
00404 {
00405   I.resize( n );
00406   for( int i = 0; i < n; i++ )
00407     {
00408       I[i].clear();
00409       I[i].resize( n, 0.0 );
00410     }
00411 
00412   for( int i = 0; i < n; i++ )
00413     I[i][i] = 1.0; 
00414 
00415   return EXIT_SUCCESS;
00416 }
00417 
00418 int
00419 write ( const std::vector < double >  &  a )
00420 {
00421   unsigned int n = a.size();
00422   
00423   for ( unsigned int i = 0; i < n; ++i )
00424     cout <<  setprecision( 15 ) <<  a[i]  << endl;
00425   cout << endl;
00426   
00427   return EXIT_SUCCESS;
00428 }
00429 
00430 
00431 int
00432 write ( const std::vector < std::vector < double >  > &  A )
00433 {
00434   unsigned int n = A.size();
00435 
00436   for ( unsigned int i = 0; i < n; ++i )
00437     {
00438       for ( unsigned int j = 0; j < n; ++j )
00439         cout << setw( 8 ) <<  setprecision( 4 ) << A[i][j];
00440       cout << endl;
00441     }
00442 
00443   cout << endl;
00444   
00445   return EXIT_SUCCESS;
00446 }
00447 
00448 
00449 int
00450 allocateMatrix ( std::vector < std::vector < double > > & A,
00451                  int m, int n )
00452 {
00453   A.resize( m );
00454   for( int i = 0; i < m; i++ )
00455     {
00456       A[i].clear();
00457       A[i].resize( n, 0.0 );
00458     }
00459   
00460   return EXIT_SUCCESS;
00461 }
00462 
00463 
00464 int
00465 allocateVector ( std::vector < double > & x, int n )
00466 {
00467   x.clear();
00468   x.resize( n, 0.0 );
00469   
00470   return EXIT_SUCCESS;
00471 }
00472 
00473 
00474 /* The driver main subroutine to check a few of above function.
00475    Test with the following matlab code:
00476    n = 4; L = tril( randn(n,n), -1 ) + eye(n,n); A = L' * L;  B =  randn(n , n); x =  randn(n , 1);  y = randn(n , 1); save test.txt -ascii A B x y;
00477    Then perform the operations as stated in the following program.
00478 */
00479 /*int main()
00480 {
00481   int n = 4;
00482   vector< double > x, y, z;
00483   vector< vector < double > > A, B, Ainv;
00484   ifstream fin( "test.txt" );
00485 
00486   if( !fin )
00487     {
00488       cout << " Could not open the file for reading " << endl;
00489       return EXIT_SUCCESS;
00490     }
00491 
00492   allocateMatrix( A, n, n );
00493   allocateMatrix( B, n, n );
00494   allocateVector( x, n );
00495   allocateVector( y, n );
00496   
00497   for( int i = 0; i < n; i++ )
00498     for( int j = 0; j < n; j++ )
00499       fin >> A[i][j];
00500   
00501   for( int i = 0; i < n; i++ )
00502     for( int j = 0; j < n; j++ )
00503       fin >> B[i][j];
00504   
00505   for( int i = 0; i < n; i++ )
00506     fin >> x[i];
00507   
00508   for( int i = 0; i < n; i++ )
00509     fin >> y[i];
00510   
00511   fin.close();
00512   
00513   //cout << "x + 3.14 * x + y /1.4141 - 2.8 * A * x - y + (y' * A)'= " << endl;
00514   //write( x + 3.141 * x + y / 1.4141 - 2.8 * A * x - y + y * A );
00515   
00516   //cout << "A + 3.14 * B + A /1.4141 - A * B + 65.0  * x * y' - B = " << endl;
00517   //write( A + 3.14 * B + A /1.4141 - A * B + 65.0  * x * y ) - B );
00518 
00519   //cout << "inv(A) = " << endl;
00520   //invertMatrix( A, Ainv );
00521   //write( Ainv );
00522 
00523   
00524   double temp = ( 1 + innerProduct( y, B * y ) / ys ) / ys;
00525       
00526   t1 = ( outerProduct(s, y) * B)/ys + ( m_M * outerProduct(y , s) ) / ys;
00527   t2 =  temp * outerProduct( s, s );
00528   B  =  B - t1 + t2;
00529   
00530   
00531   return EXIT_SUCCESS;
00532 }
00533 */
00534 
00535 } // namespace Numeric
00536 } // namespace hippodraw

Generated for HippoDraw Class Library by doxygen