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
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
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
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
00382
00383 cholFactor( L );
00384
00385 for ( unsigned int j = 0; j < n; j++ )
00386 {
00387
00388 for ( unsigned int i = 0; i < n; i++ )
00389 b[i] = ( i == j ) ? 1 : 0;
00390
00391
00392 cholBackSolve( L, x, b );
00393
00394
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
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 }