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