00001 #include <stdio.h>
00002 #include <math.h>
00003 #include <string.h>
00004 #include "matrix.h"
00005 #include "eigsol.h"
00006
00007 #ifdef DEBUG_MATRIX
00008 static unsigned long Acm;
00009 #endif
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 matrix :: matrix(long m, long n)
00022 {
00023 matrix::m = m;
00024 matrix::n = n;
00025 a = new double[m*n];
00026 if (a == NULL)
00027 {
00028 print_err("cannot allocate memory for matrix a(%ld,%ld)", __FILE__, __LINE__, __func__, m, n);
00029 }
00030 memset (a,0,m*n*sizeof(double));
00031 #ifdef DEBUG_MATRIX
00032 Acm++;
00033 #endif
00034 }
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045 matrix :: matrix (const matrix &mat)
00046 {
00047 long i, j;
00048 if ((mat.m != m) || (mat.n != n) || (a == NULL))
00049 {
00050 delete [] a;
00051 m = mat.m;
00052 n = mat.n;
00053 a = new double[m*n];
00054 }
00055 if (a == NULL)
00056 {
00057 print_err("cannot allocate memory for matrix a(%ld,%ld)", __FILE__, __LINE__, __func__, mat.m, mat.n);
00058 }
00059 for (i = 0; i < m; i++)
00060 for (j = 0; j < n; j++)
00061 a[i*n+j] = mat.a[i*n+j];
00062 #ifdef DEBUG_MATRIX
00063 print_warning("copy constructor of matrix is called.\n"
00064 "Please check your code and make sure you want use a copy constructor.\n",
00065 __FILE__, __LINE__, __func__);
00066 Acm++;
00067 #endif
00068 }
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078 matrix :: ~matrix()
00079 {
00080 #ifdef DEBUG_MATRIX
00081 if (a != NULL)
00082 Acm--;
00083 #endif
00084 m = 0;
00085 n = 0;
00086 delete [] a;
00087 a = NULL;
00088 }
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099 long condense_vector(matrix &sm, vector &nf, ivector &cu)
00100 {
00101
00102 long n = sm.m;
00103 long x,i,j,z,v=0;
00104 ivector q;
00105
00106 for(i=0;i<n;i++)
00107 {
00108 if(cu[i])
00109 v++;
00110 }
00111
00112 allocv(v, q);
00113
00114
00115 for(i=0,j=0;i<n && j<v;i++)
00116 {
00117 if(cu[i])
00118 {
00119 q[j]=i;
00120 j++;
00121 }
00122 }
00123
00124 for(z=0;z<v;z++)
00125 {
00126 x = q[z];
00127
00128 if(sm[x][x]==0.0)
00129 {
00130 return 1;
00131 }
00132
00133 for(i=0;i<n;i++)
00134 nf[i] -= ((nf[x]*sm[i][x])/sm[x][x]);
00135 }
00136
00137 return 0;
00138 }
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150 imatrix :: imatrix(long m, long n)
00151 {
00152 imatrix::m = m;
00153 imatrix::n = n;
00154 a = new long[m*n];
00155 if (a == NULL)
00156 {
00157 print_err("cannot allocate memory for imatrix a(%ld,%ld)", __FILE__, __LINE__, __func__, m, n);
00158 }
00159 memset (a,0,m*n*sizeof(long));
00160 #ifdef DEBUG_MATRIX
00161 Acm++;
00162 #endif
00163 }
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174 imatrix :: imatrix (const imatrix &mat)
00175 {
00176 long i, j;
00177 if ((mat.m != m) || (mat.n != n) || (a == NULL))
00178 {
00179 m = mat.m;
00180 n = mat.n;
00181 a = new long[m*n];
00182 }
00183 if (a == NULL)
00184 {
00185 print_err("cannot allocate memory for imatrix a(%ld,%ld)", __FILE__, __LINE__, __func__, mat.m, mat.n);
00186 }
00187 for (i = 0; i < m; i++)
00188 for (j = 0; j < n; j++)
00189 a[i*n+j] = mat.a[i*n+j];
00190 #ifdef DEBUG_MATRIX
00191 print_warning("copy constructor of imatrix is called.\n"
00192 "Please check your code and make sure you want use copy constructor.",
00193 __FILE__, __LINE__, __func__);
00194 Acm++;
00195 #endif
00196 }
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206 imatrix :: ~imatrix()
00207 {
00208 #ifdef DEBUG_MATRIX
00209 if (a != NULL)
00210 Acm--;
00211 #endif
00212 m = 0;
00213 n = 0;
00214 delete [] a;
00215 a = NULL;
00216 }
00217
00218
00219
00220 #ifdef DEBUG_MATRIX
00221 long give_acm()
00222 {
00223 return Acm;
00224 }
00225 #endif
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239 long allocm(long m, long n, matrix &mat)
00240 {
00241 mat.m = m;
00242 mat.n = n;
00243 mat.a = new double[m*n];
00244 if (mat.a == NULL)
00245 {
00246 print_err("cannot allocate memory for matrix a(%ld,%ld)", __FILE__, __LINE__, __func__, mat.m, mat.n);
00247 return (1);
00248 }
00249 memset (mat.a,0,m*n*sizeof(double));
00250 #ifdef DEBUG_MATRIX
00251 Acm++;
00252 #endif
00253 return (0);
00254 }
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268 long allocm(long m, long n, imatrix &mat)
00269 {
00270 mat.m = m;
00271 mat.n = n;
00272 mat.a = new long[m*n];
00273 memset (mat.a,0,m*n*sizeof(long));
00274 if (mat.a == NULL)
00275 {
00276 print_err("cannot allocate memory for imatrix a(%ld,%ld)", __FILE__, __LINE__, __func__, mat.m, mat.n);
00277 return (1);
00278 }
00279 #ifdef DEBUG_MATRIX
00280 Acm++;
00281 #endif
00282 return (0);
00283 }
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293 long condense_matrix(matrix &sm, ivector &cu)
00294 {
00295
00296 long n = sm.m;
00297 long x,i,j,z,v=0;
00298 ivector q;
00299 matrix auxm(n,n);
00300
00301 for(i=0;i<n;i++)
00302 {
00303 if(cu[i])
00304 v++;
00305 }
00306 allocv(v, q);
00307
00308
00309 for(i=0,j=0;i<n && j<v;i++)
00310 {
00311 if(cu[i])
00312 {
00313 q[j]=i;
00314 j++;
00315 }
00316 }
00317
00318
00319
00320 for(z=0;z<v;z++)
00321 {
00322 x = q[z];
00323
00324 if(sm[x][x]==0.0)
00325 return 1;
00326
00327 for(i=0;i<n;i++)
00328 {
00329 for(j=0;j<n;j++)
00330 auxm[i][j] = (sm[i][j]-((sm[i][x]*sm[x][j])/sm[x][x]));
00331 }
00332 copym(auxm, sm);
00333 }
00334
00335 return 0;
00336 }
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352 long reallocm(long m, long n, matrix &mat)
00353 {
00354 if (m*n > mat.m*mat.n)
00355 {
00356 delete [] mat.a;
00357 #ifdef DEBUG_MATRIX
00358 Acm--;
00359 #endif
00360 mat.a = new double[m*n];
00361 if (mat.a == NULL)
00362 {
00363 print_err("cannot allocate memory for matrix a(%ld,%ld)", __FILE__, __LINE__, __func__, m, n);
00364 return (1);
00365 }
00366 #ifdef DEBUG_MATRIX
00367 Acm++;
00368 #endif
00369 }
00370 mat.m = m;
00371 mat.n = n;
00372 memset (mat.a,0,m*n*sizeof(*mat.a));
00373 return (0);
00374 }
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390 long reallocm(long m, long n, imatrix &mat)
00391 {
00392 if (m*n > mat.m*mat.n)
00393 {
00394 delete [] mat.a;
00395 #ifdef DEBUG_MATRIX
00396 Acm--;
00397 #endif
00398 mat.a = new long[m*n];
00399 if (mat.a == NULL)
00400 {
00401 print_err("cannot allocate memory for imatrix a(%ld,%ld)", __FILE__, __LINE__, __func__, m, n);
00402 return (1);
00403 }
00404 #ifdef DEBUG_MATRIX
00405 Acm++;
00406 #endif
00407 }
00408 mat.m = m;
00409 mat.n = n;
00410 memset (mat.a,0,m*n*sizeof(*mat.a));
00411 return (0);
00412 }
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425 long allocm(long m, long n, double **&mat)
00426 {
00427 mat = new double* [m];
00428 for (register long i=0; i<m; i++)
00429 mat[i] = new double [n];
00430
00431 if (mat == NULL)
00432 {
00433 print_err("cannot allocate memory for array=matrix a(%ld,%ld)", __FILE__, __LINE__, __func__, m, n);
00434 return (1);
00435 }
00436 return (0);
00437 }
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457 long copym(const matrix &src, matrix &dest)
00458 {
00459 if ((src.m != dest.m) || (src.n != dest.n))
00460 {
00461 print_err("cannot copy matrix - incompatible size of matrices\n"
00462 " src(%ld,%ld) X dest(%ld,%ld)", __FILE__, __LINE__, __func__, src.m, src.n, dest.m, dest.n);
00463 return (1);
00464 }
00465 memcpy(dest.a, src.a, sizeof(double)*src.m*src.n);
00466 return (0);
00467 }
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481 long fillm(double c, matrix &mat)
00482 {
00483 long i, j;
00484 for (i = 0; i < mat.m; i++)
00485 for (j = 0; j < mat.n; mat[i][j] = c, j++);
00486 return (0);
00487 }
00488
00489
00490
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502 long fillrow(double c, long i, matrix &mat)
00503 {
00504 long j;
00505 if ((i >= mat.m) || (i < 0))
00506 {
00507 print_err("matrix row index %ld is out of range <0,%ld>",
00508 __FILE__, __LINE__, __func__, i, mat.m-1);
00509 return (1);
00510 }
00511 for (j=0; j < mat.n; j++)
00512 mat[i][j] = c;
00513 return 0;
00514 }
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529 long fillcol(double c, long i, matrix &mat)
00530 {
00531 long j;
00532 if (( i >= mat.n) || (i < 0))
00533 {
00534 print_err("matrix column index %ld is out of range <0,%ld>",
00535 __FILE__, __LINE__, __func__, i, mat.n-1);
00536 return (1);
00537 }
00538 for (j=0; j < mat.m; j++)
00539 mat[j][i] = c;
00540 return 0;
00541 }
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555 long destrm(matrix &mat)
00556 {
00557 #ifdef DEBUG_MATRIX
00558 if (mat.a != NULL)
00559 Acm--;
00560 #endif
00561 mat.m = 0;
00562 mat.n = 0;
00563 delete [] mat.a;
00564 mat.a = NULL;
00565 return (0);
00566 }
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580 long destrm(double **&mat, long m)
00581 {
00582 for (register int i=0; i<m; i++)
00583 delete [] mat[i];
00584
00585 delete [] mat;
00586 mat = NULL;
00587 return (0);
00588 }
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602 long destrm(long **&mat, long m)
00603 {
00604 for (register int i=0; i<m; i++)
00605 delete [] mat[i];
00606
00607 delete [] mat;
00608 mat = NULL;
00609 return (0);
00610 }
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631 long addm(const matrix &a, const matrix &b, matrix &c)
00632 {
00633 long i,j;
00634
00635 if ((a.m != b.m) || (a.n != b.n) ||
00636 (c.m != b.m) || (c.n != b.n))
00637 {
00638 print_err("cannot add matrices due to their incompatible dimensions\n"
00639 " a(%ld,%ld) X b(%ld,%ld) X c(%ld,%ld)", __FILE__, __LINE__, __func__, a.m, a.n, b.m, b.n, c.m, c.n);
00640 return (1);
00641 }
00642 for (i = 0; i < a.m; i++)
00643 for (j = 0; j < a.n; j++)
00644 c(i,j) = a(i,j) + b(i,j);
00645 return(0);
00646 }
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667 long subm(const matrix &a, const matrix &b, matrix &c)
00668 {
00669 long i,j;
00670
00671 if ((a.m != b.m) || (a.n != b.n) ||
00672 (b.m != c.m) || (b.n != c.n))
00673 {
00674 print_err("cannot subtract matrices due to their incompatible dimensions\n"
00675 " a(%ld,%ld) X b(%ld,%ld) X c(%ld,%ld)", __FILE__, __LINE__, __func__, a.m, a.n, b.m, b.n, c.m, c.n);
00676 return (1);
00677 }
00678 for (i = 0; i < a.m; i++)
00679 for (j = 0; j < a.n; j++)
00680 c(i,j) = a(i,j) - b(i,j);
00681
00682 return(0);
00683 }
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696
00697
00698
00699
00700
00701
00702
00703 long tensprd (const vector &a, const vector &b,matrix &c)
00704 {
00705 long i,j;
00706
00707 if ((a.n != c.m) || (b.n != c.n))
00708 {
00709 print_err("cannot make tensor product of vectors due to incompatible dimensions\n"
00710 " a(%ld) X b(%ld) X c(%ld,%ld)", __FILE__, __LINE__, __func__, a.n, b.n, c.m, c.n);
00711 return (1);
00712 }
00713 for (i=0;i<a.n;i++)
00714 for (j=0;j<b.n;j++)
00715 c(i,j)=a[i]*b[j];
00716
00717 return(0);
00718 }
00719
00720
00721
00722
00723
00724
00725
00726
00727
00728
00729
00730
00731
00732
00733
00734
00735
00736
00737
00738
00739
00740
00741
00742 long mxm(const matrix &a, const matrix &b, matrix &c)
00743 {
00744 long i,j,k;
00745
00746 if ((a.n != b.m) ||
00747 (c.m != a.m) || (c.n != b.n))
00748 {
00749 print_err("cannot multiply matrices due to their incompatible dimensions\n"
00750 " a(%ld) X b(%ld) X c(%ld,%ld)", __FILE__, __LINE__, __func__, a.m, a.n, b.m, b.n, c.m, c.n);
00751 return (1);
00752 }
00753 for (i=0; i < a.m; i++)
00754 for (j=0; j < b.n; j++)
00755 {
00756 c(i,j) = 0.0;
00757 for (k=0; k < a.n; k++)
00758 c(i,j) += a(i,k) * b(k,j);
00759 }
00760 return(0);
00761 }
00762
00763
00764
00765
00766
00767
00768
00769
00770
00771
00772
00773
00774
00775
00776
00777
00778
00779
00780
00781
00782 void mxm(const double *a, const double *b, double *c, long l, long m, long n)
00783 {
00784 long i,j,k,ac,acb,acu,acl;
00785 double s;
00786
00787 acl=0; ac=0;
00788 for (i=0;i<l;i++){
00789 acu=acl+m;
00790 for (j=0;j<n;j++){
00791 s=0.0;
00792 acb=j;
00793 for (k=acl;k<acu;k++){
00794 s+=a[k]*b[acb];
00795 acb+=n;
00796 }
00797 c[ac]=s;
00798 ac++;
00799 }
00800 acl+=m;
00801 }
00802 }
00803
00804
00805
00806
00807
00808
00809
00810
00811
00812
00813
00814
00815
00816
00817
00818
00819
00820
00821
00822
00823
00824
00825
00826 long mxmt(const matrix &a, const matrix &b, matrix &c)
00827 {
00828 long i,j,k;
00829
00830 if ((a.n != b.n) ||
00831 (c.m != a.m) || (c.n != b.m))
00832 {
00833 print_err("cannot multiply matrices due to their incompatible dimensions\n"
00834 " a(%ld,%ld) X b(%ld,%ld) X c(%ld,%ld)", __FILE__, __LINE__, __func__, a.m, a.n, b.m, b.n, c.m, c.n);
00835 return (1);
00836 }
00837 for (i=0; i < a.m; i++)
00838 for (j=0; j < b.m; j++)
00839 {
00840 c(i,j) = 0.0;
00841 for (k=0; k < a.n; k++)
00842 c(i,j) += a(i,k) * b(j,k);
00843 }
00844 return(0);
00845 }
00846
00847
00848
00849
00850
00851
00852
00853
00854
00855
00856
00857
00858
00859
00860
00861
00862
00863
00864
00865
00866 void mxmt(const double *a, const double *b, double *c, long l, long m, long n)
00867 {
00868 long i,j,k,ii,kk,lk,uk;
00869 double s;
00870
00871 ii=0;
00872 for (i=0;i<l;i++){
00873 lk=i*m; uk=lk+m;
00874 for (j=0;j<n;j++){
00875 s=0.0; kk=j*m;
00876 for (k=lk;k<uk;k++){
00877 s+=a[k]*b[kk]; kk++;
00878 }
00879 c[ii]=s; ii++;
00880 }
00881 }
00882 }
00883
00884
00885
00886
00887
00888
00889
00890
00891
00892
00893
00894
00895
00896
00897
00898
00899
00900
00901
00902
00903
00904
00905
00906 long mtxm(const matrix &a, const matrix &b, matrix &c)
00907 {
00908 long i,j,k;
00909
00910 if ((a.m != b.m) ||
00911 (c.m != a.n) || (c.n != b.n))
00912 {
00913 print_err("cannot multiply matrices due to their incompatible dimensions\n"
00914 " a(%ld,%ld) X b(%ld,%ld) X c(%ld,%ld)", __FILE__, __LINE__, __func__, a.m, a.n, b.m, b.n, c.m, c.n);
00915 return (1);
00916 }
00917 for (i=0; i < a.n; i++)
00918 for (j=0; j < b.n; j++)
00919 {
00920 c(i,j) = 0.0;
00921 for (k=0; k < a.m; k++)
00922 c(i,j) += a(k,i) * b(k,j);
00923 }
00924 return(0);
00925 }
00926
00927
00928
00929
00930
00931
00932
00933
00934
00935
00936
00937
00938
00939
00940
00941
00942
00943
00944
00945
00946 void mtxm (const double *a, const double *b, double *c, long l, long m, long n)
00947 {
00948 long i,j,k,aca,acb,acc;
00949 double s;
00950
00951 acc=0;
00952 for (i=0;i<m;i++){
00953 for (j=0;j<n;j++){
00954 s=0.0; aca=i; acb=j;
00955 for (k=0;k<l;k++){
00956 s+=a[aca]*b[acb];
00957 aca+=m; acb+=n;
00958 }
00959 c[acc]=s; acc++;
00960 }
00961 }
00962 }
00963
00964
00965
00966
00967
00968
00969
00970
00971
00972
00973
00974
00975
00976
00977
00978
00979
00980
00981
00982 void mtxmccr (const double *a, const double *b, double *c, long l, long m, long n)
00983 {
00984 long i,j,k,ii,lk,uk,ac;
00985 double s;
00986
00987 ac=0;
00988 for (i=0;i<m;i++){
00989 for (j=0;j<n;j++){
00990 s=0.0; lk=i*l; uk=lk+l; ii=j*l;
00991 for (k=lk;k<uk;k++){
00992 s+=a[k]*b[ii]; ii++;
00993 }
00994 c[ac]=s; ac++;
00995 }
00996 }
00997 }
00998
00999
01000
01001
01002
01003
01004
01005
01006
01007
01008
01009
01010
01011
01012
01013
01014
01015
01016
01017
01018
01019 long cmulm(const double c, const matrix &a, matrix &b)
01020 {
01021 long i,j;
01022
01023 if ((a.m != b.m) || (a.n != b.n))
01024 {
01025 print_err("incompatible dimensions of matrices - a(%ld,%ld) X b(%ld,%ld)",
01026 __FILE__, __LINE__, __func__, a.m, a.n, b.m, b.n);
01027 return (1);
01028 }
01029 for (i=0; i < a.m; i++)
01030 for (j=0; j < a.n; j++)
01031 b(i,j) = c * a(i,j);
01032
01033 return (0);
01034 }
01035
01036
01037
01038
01039
01040
01041
01042
01043
01044
01045
01046
01047
01048
01049
01050 long cmulm(double c, matrix &a)
01051 {
01052 long i,j;
01053
01054 for (i=0; i < a.m; i++)
01055 for (j=0; j < a.n; j++)
01056 a(i,j) *= c;
01057
01058 return (0);
01059 }
01060
01061
01062
01063
01064
01065
01066
01067
01068
01069
01070
01071
01072
01073
01074
01075
01076
01077
01078
01079
01080 long tranm(const matrix &a, matrix &at)
01081 {
01082 long i,j;
01083
01084 if ((a.m != at.n) || (a.n != at.m))
01085 {
01086 print_err("cannot transpose matrix - incompatible dimensions of matrices\n"
01087 " a(%ld,%ld) X at(%ld,%ld)", __FILE__, __LINE__, __func__, a.m, a.n, at.m, at.n);
01088 return (1);
01089 }
01090 for (i = 0; i < at.m; i++)
01091 for (j = 0; j < at.n; j++)
01092 at(i,j) = a(j,i);
01093
01094 return(0);
01095 }
01096
01097
01098
01099
01100
01101
01102
01103
01104
01105
01106
01107
01108
01109
01110 long tranm(matrix &a)
01111 {
01112 long i,j;
01113 double *temp;
01114 temp = new double[a.m*a.n];
01115 for (i = 0; i < a.m; i++)
01116 for (j = 0; j < a.n; j++)
01117 temp[j*a.m+i] = a(i,j);
01118 memcpy(a.a, temp, a.m*a.n*sizeof(*a.a));
01119 long t = a.n;
01120 a.n = a.m;
01121 a.m = t;
01122 delete temp;
01123 return(0);
01124 }
01125
01126
01127
01128
01129
01130
01131
01132
01133
01134
01135
01136
01137
01138
01139
01140
01141
01142
01143
01144
01145
01146
01147
01148 long vxm(const vector &u, const matrix &a, vector &v)
01149 {
01150 long i,j;
01151
01152 if ((u.n != a.m) || (v.n != a.n))
01153 {
01154 print_err("cannot multiply vector by matrix due to their incompatible dimensions\n"
01155 " u(%ld) X a(%ld,%ld) X v(%ld)", __FILE__, __LINE__, __func__, u.n, a.m, a.n, v.n);
01156 return (1);
01157 }
01158 for (i=0; i < v.n; i++)
01159 {
01160 v[i] = 0.0;
01161 for (j=0; j < a.m; j++)
01162 v[i] += u[j] * a(j,i);
01163 }
01164 return(0);
01165 }
01166
01167
01168
01169
01170
01171
01172
01173
01174
01175
01176
01177
01178
01179
01180
01181
01182
01183
01184
01185
01186
01187
01188
01189 long vxmt(const vector &u, const matrix &a, vector &v)
01190 {
01191 long i,j;
01192
01193 if ((u.n != a.n) || (v.n != a.n))
01194 {
01195 print_err("cannot multiply vector by matrix due to their incompatible dimensions\n"
01196 "u(%ld) X a(%ld,%ld) X v(%ld)", __FILE__, __LINE__, __func__, u.n, a.m, a.n, v.n);
01197
01198 return (1);
01199 }
01200 for (i=0; i < v.n; i++)
01201 {
01202 v(i) = 0.0;
01203 for (j=0; j < a.n; j++)
01204 v(i) += u(j) * a(i,j);
01205 }
01206 return(0);
01207 }
01208
01209
01210
01211
01212
01213
01214
01215
01216
01217
01218
01219
01220
01221
01222
01223
01224
01225
01226
01227
01228
01229
01230
01231 long mxv(const matrix &a, const vector &u, vector &v)
01232 {
01233 long i,j;
01234
01235 if ((a.n != u.n) || (v.n != a.m))
01236 {
01237 print_err("cannot multiply matrix by vector due to their incompatible dimensions\n"
01238 " a(%ld,%ld) X u(%ld) X v(%ld)",__FILE__, __LINE__, __func__, a.m, a.n, u.n, v.n);
01239 return (1);
01240 }
01241 for (i=0; i < v.n; i++)
01242 {
01243 v(i) = 0.0;
01244 for (j=0; j < a.n; j++)
01245 v(i) += u(j) * a(i,j);
01246 }
01247 return(0);
01248 }
01249
01250
01251
01252
01253
01254
01255
01256
01257
01258
01259
01260
01261
01262
01263
01264 void mxv (const double *a, const double *b, double *c, long m, long n)
01265 {
01266 long i,j,aca;
01267 double s;
01268
01269 aca=0;
01270 for (i=0;i<m;i++){
01271 s=0.0;
01272 for (j=0;j<n;j++){
01273 s+=a[aca]*b[j];
01274 aca++;
01275 }
01276 c[i]=s;
01277 }
01278 }
01279
01280
01281
01282
01283
01284
01285
01286
01287
01288
01289
01290
01291
01292
01293
01294 void mxvc (const double *a, const double *b, double *c, long m, long n)
01295 {
01296 long i,j,k;
01297 double s;
01298
01299 for (i=0;i<m;i++){
01300 s=0.0; k=i;
01301 for (j=0;j<n;j++){
01302 s+=a[k]*b[j]; k+=m;
01303 }
01304 c[i]=s;
01305 }
01306 }
01307
01308
01309
01310
01311
01312
01313
01314
01315
01316
01317
01318
01319
01320
01321
01322
01323
01324
01325
01326
01327
01328
01329
01330 long mtxv(const matrix &a, const vector &u, vector &v)
01331 {
01332 long i,j;
01333
01334 if ((a.m != u.n) || (v.n != a.n))
01335 {
01336 print_err("cannot multiply matrix by vector due to their incompatible dimensions\n"
01337 " a(%ld,%ld) X u(%ld) X v(%ld)", __FILE__, __LINE__, __func__, a.m, a.n, u.n, v.n);
01338 return (1);
01339 }
01340 for (i=0; i < v.n; i++)
01341 {
01342 v(i) = 0.0;
01343 for (j=0; j < a.m; j++)
01344 v(i) += u(j) * a(j,i);
01345 }
01346 return(0);
01347 }
01348
01349
01350
01351
01352
01353
01354
01355
01356
01357
01358
01359
01360
01361 void mtxv(const double *a, const double *b, double *c, long m, long n)
01362 {
01363 long i,j,aca;
01364 double s;
01365
01366 for (i=0;i<n;i++){
01367 s=0.0; aca=i;
01368 for (j=0;j<m;j++){
01369 s+=a[aca]*b[j];
01370 aca+=n;
01371 }
01372 c[i]=s;
01373 }
01374 }
01375
01376
01377
01378
01379
01380
01381
01382
01383
01384
01385
01386
01387
01388
01389
01390
01391 void mtxvc(const double *a, const double *b, double *c, long m, long n)
01392 {
01393 long i,j,k;
01394 double s;
01395
01396 for (i=0;i<n;i++){
01397 s=0.0; k=i*m;
01398 for (j=0;j<m;j++){
01399 s+=a[k]*b[j]; k++;
01400 }
01401 c[i]=s;
01402 }
01403 }
01404
01405
01406
01407
01408
01409
01410
01411
01412
01413
01414
01415
01416
01417
01418
01419
01420
01421
01422
01423
01424
01425
01426
01427 long vxv(const vector &u, const vector &v, matrix &a)
01428 {
01429 long i,j;
01430
01431 if ((u.n != a.m) || (v.n != a.n)){
01432 print_err("cannot perform tensor product of vectors due to their incompatible dimensions\n"
01433 " u(%ld) X v(%ld) X a(%ld,%ld)", __FILE__, __LINE__, __func__, u.n, v.n, a.m, a.n);
01434 return (1);
01435 }
01436 for (i=0; i < a.m; i++){
01437 for (j=0; j < a.n; j++){
01438 a(i,j)=u(i)*v(j);
01439 }
01440 }
01441 return(0);
01442 }
01443
01444
01445
01446
01447
01448
01449
01450
01451
01452
01453
01454
01455
01456
01457
01458
01459
01460
01461
01462
01463
01464
01465
01466
01467
01468
01469 long gause(const matrix &a, matrix &b,double zero)
01470 {
01471 long i,j,k;
01472
01473 if ((a.m != b.m) || (a.n != b.n) || (a.m != a.n))
01474 {
01475 print_err("Gauss elimination - incompatible dimension of matrices"
01476 "a(%ld,%ld) X b(%ld,%ld)", __FILE__, __LINE__, __func__, a.m, a.n, b.m, b.n);
01477 return (1);
01478 }
01479 if (a.m < 2)
01480 {
01481 print_err("matrix has less than 2 rows", __FILE__, __LINE__, __func__);
01482 return (2);
01483 }
01484
01485 for (i = 0; i < a.n; i++)
01486 b(0,i) = a(0,i);
01487
01488 for (i = 0; i < a.m-1; i++)
01489 {
01490 for (j = i+1; j < a.m; j++)
01491 {
01492
01493 if (fabs(b(i,i)) <= zero)
01494 {
01495 print_err("diagonal element %ld is too small or zero (%le)", __FILE__, __LINE__, __func__, i+1, b(i,i));
01496 return (3);
01497 }
01498 if ((i == 0) && (fabs(a(j,i)) <= zero))
01499
01500 {
01501
01502
01503 for (k = 0; k < b.n; k++)
01504 b(j,k) = a(j,k);
01505 continue;
01506 }
01507 if ((i != 0) && (fabs(b(j,i)) <= zero))
01508
01509 continue;
01510
01511 double r;
01512
01513 if (i == 0)
01514
01515 r = a(i,i)/a(j,i);
01516 else
01517 r = b(i,i)/b(j,i);
01518
01519 b(j, i) = 0.0;
01520 for (k = i+1; k < a.n; k++)
01521 {
01522 if (i == 0)
01523
01524 b(j,k) = a(i,k) - a(j,k) * r;
01525 else
01526
01527 b(j,k) = b(i,k) - b(j,k) * r;
01528 }
01529 }
01530 }
01531 return (0);
01532 }
01533
01534
01535
01536
01537
01538
01539
01540
01541
01542
01543
01544
01545
01546
01547
01548
01549
01550
01551
01552
01553
01554
01555
01556
01557
01558
01559
01560
01561 long gause(const matrix &a, matrix &b, vector &r,double zero)
01562 {
01563 long i,j,k;
01564
01565 if ((a.m != b.m) || (a.n != b.n) || (r.n != a.m) || (a.m != a.n))
01566 {
01567 print_err("Gauss elimination - incompatible dimension of matrices or vector"
01568 "a(%ld,%ld) X b(%ld,%ld) X r(%ld)", __FILE__, __LINE__, __func__, a.m, a.n, b.m, b.n, r.n);
01569 return (1);
01570 }
01571 if (a.m < 2)
01572 {
01573 print_err("matrix has less than 2 rows", __FILE__, __LINE__, __func__);
01574 return (2);
01575 }
01576
01577 for (i = 0; i < a.n; i++)
01578 b(0,i) = a(0,i);
01579
01580 for (i = 0; i < a.m-1; i++)
01581 {
01582 for (j = i+1; j < a.m; j++)
01583 {
01584 if (fabs(b(i,i)) <= zero)
01585
01586 {
01587 print_err("diagonal element %ld is too small or zero (%le)", __FILE__, __LINE__, __func__, i+1, b(i,i));
01588 return (3);
01589 }
01590 if ((i == 0) && (fabs(a(j,i)) <= zero))
01591
01592 {
01593
01594
01595 for (k = 0; k < b.n; k++)
01596 b(j,k) = a(j,k);
01597 continue;
01598 }
01599 if ((i != 0) && (fabs(b(j,i)) <= zero))
01600
01601 continue;
01602
01603 double q;
01604
01605 if (i == 0)
01606
01607 q = a(i,i)/a(j,i);
01608 else
01609 q = b(i,i)/b(j,i);
01610
01611 b(j, i) = 0.0;
01612 r[j] = r[i] - r[j] * q;
01613 for (k = i+1; k < a.n; k++)
01614 {
01615 if (i == 0)
01616
01617 b(j,k) = a(i,k) - a(j,k) * q;
01618 else
01619
01620 b(j,k) = b(i,k) - b(j,k) * q;
01621 }
01622 }
01623 }
01624
01625 if (fabs(b(b.m-1,b.n-1)) < zero)
01626 {
01627 print_err("diagonal element %ld is too small or zero (%le)", __FILE__, __LINE__, __func__, b.m, b(b.m-1,b.n-1));
01628 return (3);
01629 }
01630
01631 r[r.n-1] = r[r.n-1] / b(b.m-1, b.n-1);
01632 for (i = b.m-2; i >= 0; i--)
01633 {
01634 for (j = b.n-1; j > i; j--)
01635 r[i] -= b(i,j) * r[j];
01636 r[i] /= b(i,i);
01637 }
01638 return (0);
01639 }
01640
01641
01642
01643
01644
01645
01646
01647
01648
01649
01650
01651
01652
01653
01654
01655
01656
01657
01658
01659
01660
01661
01662
01663
01664
01665
01666
01667
01668
01669
01670
01671
01672 long gause(const matrix &a, matrix &b, vector &r, vector &sol,double zero)
01673 {
01674 long i,j,k;
01675
01676 if ((a.m != b.m) || (a.n != b.n) || (r.n != a.m) ||
01677 (sol.n != a.m) || (a.m != a.n))
01678 {
01679 print_err("Gauss elimination - incompatible dimensions of matrices or vectors"
01680 "a(%ld,%ld) X b(%ld,%ld) X r(%ld) X sol(%ld)", __FILE__, __LINE__, __func__, a.m, a.n, b.m, b.n, r.n, sol.n);
01681 return (1);
01682 }
01683 if (a.m < 2)
01684 {
01685 print_err("matrix a has less than 2 rows", __FILE__, __LINE__, __func__);
01686 return (2);
01687 }
01688
01689 for (i = 0; i < a.n; i++)
01690 b(0,i) = a(0,i);
01691
01692 for (i = 0; i < a.m-1; i++)
01693 {
01694 for (j = i+1; j < a.m; j++)
01695 {
01696 if (fabs(b(i,i)) <= zero)
01697
01698 {
01699 print_err("diagonal element %ld is too small or zero (%le)", __FILE__, __LINE__, __func__, i+1, b(i,i));
01700 return (4);
01701 }
01702 if ((i == 0) && (fabs(a(j,i)) <= zero))
01703
01704 {
01705
01706
01707 for (k = 0; k < b.n; k++)
01708 b(j,k) = a(j,k);
01709 continue;
01710 }
01711 if ((i != 0) && (fabs(b(j,i)) <= zero))
01712
01713 continue;
01714
01715 double q;
01716
01717 if (i == 0)
01718
01719 q = a(i,i)/a(j,i);
01720 else
01721 q = b(i,i)/b(j,i);
01722
01723 b(j, i) = 0.0;
01724 r[j] = r[i] - r[j] * q;
01725 for (k = i+1; k < a.n; k++)
01726 {
01727 if (i == 0)
01728
01729 b(j,k) = a(i,k) - a(j,k) * q;
01730 else
01731
01732 b(j,k) = b(i,k) - b(j,k) * q;
01733 }
01734 }
01735 }
01736
01737 if (fabs(b(b.m-1,b.n-1)) < zero)
01738 {
01739 print_err("diagonal element %ld is too small or zero (%le)", __FILE__, __LINE__, __func__, b.m, b(b.m-1,b.n-1));
01740 return (3);
01741 }
01742
01743 sol[sol.n-1] = r[r.n-1] / b(b.m-1, b.n-1);
01744 for (i = b.m-2; i >= 0; i--)
01745 {
01746 sol[i] = r[i];
01747 for (j = b.n-1; j > i; j--)
01748 sol[i] -= b(i,j) * sol[j];
01749 sol[i] /= b(i,i);
01750 }
01751 return (0);
01752 }
01753
01754
01755
01756
01757
01758
01759
01760
01761
01762
01763
01764
01765
01766
01767
01768
01769
01770
01771
01772
01773
01774
01775
01776 long invm(const matrix &a, matrix &b,double zero)
01777 {
01778 long i,j,k;
01779 double r;
01780
01781
01782 if ((a.m != b.m) || (a.n != b.n) || (a.m != a.n))
01783 {
01784 print_err("cannot invert matrix due to incompatible dimensions of inverted and resulting matrices"
01785 "a(%ld,%ld) X b(%ld,%ld)", __FILE__, __LINE__, __func__, a.m, a.n, b.m, b.n);
01786 return (1);
01787 }
01788 if (a.m < 2)
01789 {
01790 print_err("matrix has less than 2 rows", __FILE__, __LINE__, __func__);
01791 return (2);
01792 }
01793
01794
01795
01796
01797
01798
01799
01800
01801
01802
01803
01804 matrix c(a.m, a.n);
01805
01806
01807 for (i = 0; i < b.m; i++)
01808 for (j = 0; j < b.n; j++)
01809 {
01810 c(i,j) = a(i,j);
01811 i == j ? b(i,j) = 1.0 : b(i,j) = 0.0;
01812 }
01813
01814
01815 for (i = 0; i < c.m-1; i++)
01816 {
01817
01818
01819
01820
01821
01822
01823
01824
01825
01826
01827
01828
01829
01830
01831
01832
01833
01834
01835
01836
01837
01838
01839 for (j = i+1; j < c.m; j++)
01840 {
01841 if (fabs(c(i,i)) < zero)
01842
01843 {
01844 print_err("diagonal element %ld is too small or zero (%le)", __FILE__, __LINE__, __func__, i+1, c(i,i));
01845 return (3);
01846 }
01847 if (fabs(c(j,i)) <= zero)
01848
01849 continue;
01850
01851
01852
01853 r = c(j,i)/c(i,i);
01854
01855 c(j,i) = 0.0;
01856 for (k = 0; k < c.n; k++)
01857 {
01858 if (k > i)
01859
01860 c(j,k) = c(j,k) - c(i,k) * r;
01861 b(j,k) = b(j,k) - b(i,k) * r;
01862
01863
01864 }
01865 }
01866 }
01867
01868
01869 for (i = c.m-1; i > 0; i--)
01870 {
01871
01872
01873
01874
01875
01876
01877
01878
01879
01880
01881
01882
01883
01884
01885
01886
01887
01888
01889
01890
01891
01892
01893
01894 for (j = i-1; j >= 0; j--)
01895 {
01896 if (fabs(c(i,i)) < zero)
01897
01898 {
01899 print_err("diagonal element %ld is too small or zero (%le)", __FILE__, __LINE__, __func__, i+1, c(i,i));
01900 return (3);
01901 }
01902 if (fabs(c(j,i)) <= zero)
01903
01904 continue;
01905
01906
01907 r = c(j,i)/c(i,i);
01908
01909 c(j,i) = 0.0;
01910 for (k = c.n-1; k >= 0; k--)
01911 {
01912 if (k < i)
01913
01914 c(j,k) = c(j,k) - c(i,k) * r;
01915 b(j,k) = b(j,k) - b(i,k) * r;
01916
01917
01918 }
01919 }
01920 }
01921
01922
01923 for (i = 0; i < c.m; i++)
01924 for (j = 0; j < b.n; j++)
01925 {
01926 if (fabs(c(i,i)) < zero)
01927
01928 {
01929 print_err("diagonal element %ld is too small or zero (%le)", __FILE__, __LINE__, __func__, i+1, c(i,i));
01930 return (3);
01931 }
01932 b(i,j) /= c(i,i);
01933 }
01934
01935
01936
01937
01938
01939
01940
01941
01942
01943
01944
01945
01946
01947
01948
01949
01950
01951
01952
01953
01954
01955 return (0);
01956 }
01957
01958
01959
01960
01961
01962
01963
01964
01965
01966
01967
01968
01969
01970
01971
01972
01973
01974
01975
01976 long detm(const matrix &a, double &det)
01977 {
01978 long i;
01979 double zero=1.0e-20;
01980
01981 if (a.m != a.n)
01982 {
01983 print_err("wrong dimensions of matrix a(%ld,%ld)", __FILE__, __LINE__, __func__, a.m, a.n);
01984 return (1);
01985 }
01986
01987 matrix b(a.m, a.n);
01988 if (gause(a, b,zero) == 3)
01989
01990
01991 {
01992 det = 0.0;
01993 return (0);
01994 }
01995
01996 det = b(0,0);
01997 for (i = 1; i < b.m; i++)
01998 det *= b(i,i);
01999 return (0);
02000 }
02001
02002
02003
02004
02005
02006
02007
02008
02009
02010
02011
02012
02013
02014
02015
02016
02017
02018
02019
02020
02021 long readm(FILE *in, matrix &a)
02022 {
02023 long i,j;
02024
02025 for (i = 0; i < a.m; i++)
02026 {
02027 for (j = 0; j < a.n; j++)
02028 fscanf(in, "%le", &a(i,j));
02029 }
02030 return (0);
02031 }
02032
02033
02034
02035
02036
02037
02038
02039
02040
02041
02042
02043
02044
02045
02046
02047
02048
02049
02050
02051
02052 long printm(const matrix &a, FILE *out = stdout, int prec = 3, int width = 11)
02053 {
02054 long i,j;
02055
02056 for (i = 0; i < a.m; i++)
02057 {
02058 for (j = 0; j < a.n; j++)
02059 fprintf(out, "%*.*e ", width, prec, a(i,j));
02060 fprintf(out, "\n");
02061 }
02062 return (0);
02063 }
02064
02065
02066
02067
02068
02069
02070
02071
02072
02073
02074
02075
02076
02077
02078 long mswapc(matrix &mat, long i, long j)
02079 {
02080 long k;
02081 double s;
02082 for (k = 0; k < mat.m; k++)
02083 {
02084 s = mat(k,i);
02085 mat(k,i) = mat(k,j);
02086 mat(k,j) = s;
02087 }
02088 return (0);
02089 }
02090
02091
02092
02093
02094
02095
02096
02097
02098
02099
02100
02101
02102
02103
02104 long mswapr(matrix &mat, long i, long j)
02105 {
02106 long k;
02107 double s;
02108 for (k = 0; k < mat.n; k++)
02109 {
02110 s = mat(i,k);
02111 mat(i,k) = mat(j,k);
02112 mat(j,k) = s;
02113 }
02114 return (0);
02115 }
02116
02117
02118
02119
02120
02121
02122
02123
02124
02125
02126
02127
02128
02129
02130
02131
02132
02133 void bdbj (double *k, const double *b, const double *d,double jac,long m,long n)
02134 {
02135 long i,j,l,ii,jj,mm,ll,ul;
02136 double s;
02137 double *am;
02138
02139
02140 am = new double [m*n];
02141
02142
02143 ll=0;
02144 for (i=0;i<n;i++){
02145 for (j=0;j<m;j++){
02146 s=0.0; ii=i; jj=j;
02147 for (l=0;l<m;l++){
02148 s+=b[ii]*d[jj];
02149 ii+=n; jj+=m;
02150 }
02151 am[ll]=s; ll++;
02152 }
02153 }
02154
02155
02156 for (i=0;i<n;i++){
02157 ii=i*n+i; mm=ii;
02158 s=0.0; ll=i*m; ul=ll+m; jj=i;
02159 for (l=ll;l<ul;l++){
02160 s+=am[l]*b[jj];
02161 jj+=n;
02162 }
02163 k[ii]+=s*jac;
02164 ii++; mm+=n;
02165
02166 for (j=i+1;j<n;j++){
02167 s=0.0; jj=j;
02168 for (l=ll;l<ul;l++){
02169 s+=am[l]*b[jj];
02170 jj+=n;
02171 }
02172 k[ii]+=s*jac; k[mm]+=s*jac;
02173 ii++; mm+=n;
02174 }
02175 }
02176
02177 delete [] am;
02178 }
02179
02180
02181
02182
02183
02184
02185
02186
02187 void bdbjac (matrix &d, const matrix &a, const matrix &b, const matrix &c,double jac)
02188 {
02189 matrix x,y;
02190 allocm (a.n,b.n,x);
02191 allocm (d.m,d.n,y);
02192 mtxm (a,b,x);
02193 mxm (x,c,y);
02194 cmulm (jac,y);
02195 addm (y,d,d);
02196 destrm (y);
02197 destrm (x);
02198 }
02199
02200
02201
02202
02203
02204
02205
02206
02207
02208
02209
02210
02211
02212 void nnj (double *m,const double *n,double jac,long mm,long nn)
02213 {
02214 long i,j,k,ii,jj,kk,ll;
02215 double s;
02216
02217 for (i=0;i<nn;i++){
02218 kk=i*nn+i; ll=kk;
02219 s=0.0; ii=i; jj=i;
02220 for (k=0;k<mm;k++){
02221 s+=n[ii]*n[jj];
02222 ii+=nn; jj+=nn;
02223 }
02224 m[kk]+=s*jac;
02225 kk++; ll+=nn;
02226
02227 for (j=i+1;j<nn;j++){
02228 s=0.0; ii=i; jj=j;
02229 for (k=0;k<mm;k++){
02230 s+=n[ii]*n[jj];
02231 ii+=nn; jj+=nn;
02232 }
02233 m[kk]+=s*jac; m[ll]+=s*jac;
02234 kk++; ll+=nn;
02235 }
02236 }
02237 }
02238
02239
02240
02241
02242
02243
02244
02245
02246 void nnjac (matrix &d,const matrix &a,const matrix &b,double jac)
02247 {
02248 matrix x(a.n, b.n);
02249
02250 mtxm (a,b,x);
02251 cmulm (jac,x);
02252 addm (x,d,d);
02253 }
02254
02255
02256
02257
02258
02259
02260
02261
02262
02263
02264
02265
02266
02267
02268
02269
02270 void lgvectortransf (vector &g,const vector &l,const matrix &tmat)
02271 {
02272 mxv (tmat,l,g);
02273 }
02274
02275
02276
02277
02278
02279
02280
02281
02282
02283
02284
02285
02286
02287
02288
02289
02290 void glvectortransf (const vector &g,vector &l,const matrix &tmat)
02291 {
02292 long n;
02293 n=tmat.m;
02294 matrix tmt(n,n);
02295 tranm (tmat,tmt);
02296 mxv (tmt,g,l);
02297 }
02298
02299
02300
02301
02302
02303
02304
02305
02306
02307
02308
02309
02310
02311
02312
02313 void glmatrixtransf (matrix &a,const matrix &tmat)
02314 {
02315 long n;
02316 n=a.m;
02317 matrix am(n,n);
02318
02319 mtxm (tmat,a,am);
02320 mxm (am,tmat,a);
02321 }
02322
02323
02324
02325
02326
02327
02328
02329
02330
02331
02332
02333
02334
02335
02336
02337 void lgmatrixtransf (matrix &a,const matrix &tmat)
02338 {
02339 long n;
02340 n=a.m;
02341 matrix am(n,n);
02342
02343 mxm (tmat,a,am);
02344 mxmt (am,tmat,a);
02345 }
02346
02347
02348
02349
02350
02351
02352
02353
02354
02355
02356
02357
02358
02359
02360
02361 void lgmatrixtransfblock (matrix &a, const matrix &t)
02362 {
02363 long i,j,k,ii,jj,dim;
02364 double s;
02365 matrix b;
02366
02367
02368 dim=t.n;
02369 allocm (dim,dim,b);
02370
02371 for (ii=0;ii<a.n;ii+=dim){
02372 for (jj=0;jj<a.n;jj+=dim){
02373 for (i=0;i<dim;i++){
02374 for (j=0;j<dim;j++){
02375 s=0.0;
02376 for (k=0;k<dim;k++){
02377 s+=a[ii+i][jj+k]*t[j][k];
02378 }
02379 b[i][j]=s;
02380 }
02381 }
02382 for (i=0;i<dim;i++){
02383 for (j=0;j<dim;j++){
02384 s=0.0;
02385 for (k=0;k<dim;k++){
02386 s+=t[i][k]*b[k][j];
02387 }
02388 a[ii+i][jj+j]=s;
02389 }
02390 }
02391 }
02392 }
02393 destrm (b);
02394 }
02395
02396
02397
02398
02399
02400
02401
02402
02403
02404
02405
02406
02407
02408
02409
02410
02411 void glvectortransfblock (vector &v, const matrix &t)
02412 {
02413
02414
02415
02416
02417
02418
02419
02420
02421
02422
02423
02424
02425
02426
02427 long i,j,ii,dim;
02428 double s;
02429 vector b;
02430
02431
02432 dim=t.n;
02433 allocv (dim,b);
02434
02435 for (ii=0;ii<v.n;ii=ii+dim){
02436 for (i=0;i<dim;i++){
02437 s=0.0;
02438 for (j=0;j<dim;j++){
02439 s+=t[j][i]*v[ii+j];
02440 }
02441 b[i]=s;
02442 }
02443 for (i=0;i<dim;i++){
02444 v[ii+i]=b[i];
02445 }
02446 }
02447
02448 destrv (b);
02449 }
02450
02451
02452
02453
02454
02455
02456
02457
02458
02459
02460
02461
02462
02463
02464
02465
02466 void lgvectortransfblock (vector &v, const matrix &t)
02467 {
02468
02469
02470
02471
02472
02473
02474
02475
02476
02477
02478
02479
02480
02481
02482 long i,j,ii,dim;
02483 double s;
02484 vector b;
02485
02486
02487 dim=t.n;
02488 allocv (dim,b);
02489
02490 for (ii=0;ii<v.n;ii=ii+dim){
02491 for (i=0;i<dim;i++){
02492 s=0.0;
02493 for (j=0;j<dim;j++){
02494 s+=t[i][j]*v[ii+j];
02495 }
02496 b[i]=s;
02497 }
02498 for (i=0;i<dim;i++){
02499 v[ii+i]=b[i];
02500 }
02501 }
02502
02503 destrv (b);
02504 }
02505
02506
02507
02508
02509
02510
02511
02512
02513
02514
02515
02516
02517
02518
02519 void locglob (double *gv,const double *lv,const long *cn,long n)
02520 {
02521 long i,j;
02522
02523 for (i=0;i<n;i++){
02524 j=cn[i];
02525 if (j<1) continue;
02526 else{
02527 j--;
02528 gv[j]+=lv[i];
02529 }
02530 }
02531 }
02532
02533
02534
02535
02536
02537
02538
02539
02540
02541
02542
02543
02544
02545
02546 void globloc (const double *gv,double *lv,const long *cn,long n)
02547 {
02548 long i,j;
02549
02550 for (i=0;i<n;i++){
02551 j=cn[i];
02552 if (j<1) continue;
02553 else{
02554 j--;
02555 lv[i]=gv[j];
02556 }
02557 }
02558 }
02559
02560
02561
02562 void locvecmat(double *mat, const double *vect, const long *cn, long ci, long m,long n)
02563 {
02564 long i;
02565
02566 for (i = 0; i < n; i++){
02567 if (cn[i] < 1)
02568 continue;
02569 mat[(cn[i]-1)*m+ci] += vect[i];
02570 }
02571 }
02572
02573
02574
02575
02576
02577
02578
02579
02580
02581
02582
02583
02584
02585
02586
02587
02588
02589
02590
02591 double det2d (const double *x,const double *y)
02592 {
02593 double d;
02594 d = x[0]*(y[1]-y[2])+ x[1]*(y[2]-y[0]) + x[2]*(y[0]-y[1]);
02595 return d;
02596 }
02597
02598
02599
02600
02601
02602
02603
02604
02605
02606
02607
02608
02609
02610
02611
02612
02613
02614
02615
02616
02617
02618
02619 double det3d (const double *x,const double *y,const double *z)
02620 {
02621 double d;
02622
02623
02624
02625
02626
02627
02628
02629
02630
02631
02632
02633
02634 vector a(3),b(3),c(3);
02635
02636 a[0]=x[0]-x[3];
02637 a[1]=y[0]-y[3];
02638 a[2]=z[0]-z[3];
02639
02640 b[0]=x[1]-x[3];
02641 b[1]=y[1]-y[3];
02642 b[2]=z[1]-z[3];
02643
02644 c[0]=x[2]-x[3];
02645 c[1]=y[2]-y[3];
02646 c[2]=z[2]-z[3];
02647
02648 d = a[0]*b[1]*c[2] + b[0]*c[1]*a[2] + c[0]*a[1]*b[2] - c[0]*b[1]*a[2] - c[1]*b[2]*a[0] - c[2]*b[0]*a[1];
02649
02650 return d;
02651 }
02652
02653
02654
02655
02656
02657
02658
02659
02660
02661
02662
02663
02664
02665
02666
02667
02668
02669
02670
02671
02672
02673 void princ_val (matrix &v,vector &pval,matrix &pvect,long ni,double err,double zero,long n, long normalize)
02674 {
02675 long ani;
02676
02677 if (n==2){
02678 double i1,i2,d,s;
02679
02680 i1=v[0][0]+v[1][1];
02681
02682 i2=v[0][0]*v[1][1]-v[0][1]*v[1][0];
02683
02684 d=i1*i1-4.0*i2;
02685 if (d<0.0) print_err("negative discriminant (%le)",__FILE__,__LINE__,__func__,d);
02686 pval[0]=(i1+sqrt(d))/2.0;
02687 pval[1]=(i1-sqrt(d))/2.0;
02688
02689 s=v[0][0]-pval[0];
02690 if (fabs(s)<zero){
02691 pvect[0][0]=1.0;
02692 pvect[1][0]=0.0;
02693 }
02694 else{
02695 pvect[0][0]=-v[0][1]/s;
02696 pvect[1][0]=1.0;
02697 }
02698
02699 s=v[0][0]-pval[1];
02700 if (fabs(s)<zero){
02701 pvect[0][1]=1.0;
02702 pvect[1][1]=0.0;
02703 }
02704 else{
02705 pvect[0][1]=-v[0][1]/s;
02706 pvect[1][1]=1.0;
02707 }
02708
02709
02710 s=pvect[0][0]*pvect[0][0]+pvect[1][0]*pvect[1][0];
02711 s=sqrt(s);
02712 pvect[0][0]/=s; pvect[1][0]/=s;
02713
02714 s=pvect[0][1]*pvect[0][1]+pvect[1][1]*pvect[1][1];
02715 s=sqrt(s);
02716 pvect[0][1]/=s; pvect[1][1]/=s;
02717
02718
02719 if (pval[0]>pval[1]){
02720 s=pval[0];
02721 pval[0]=pval[1];
02722 pval[1]=s;
02723
02724 s=pvect[0][0]; pvect[0][0]=pvect[0][1]; pvect[0][1]=s;
02725 s=pvect[1][0]; pvect[1][0]=pvect[1][1]; pvect[1][1]=s;
02726 }
02727 }
02728
02729 if (n==3){
02730 double a[9],b[9];
02731
02732 a[0]=v[0][0]; a[1]=v[0][1]; a[2]=v[0][2];
02733 a[3]=v[1][0]; a[4]=v[1][1]; a[5]=v[1][2];
02734 a[6]=v[2][0]; a[7]=v[2][1]; a[8]=v[2][2];
02735
02736 jacobi (a,b,pval.a,n,ni,ani,err, normalize);
02737
02738 pvect[0][0]=b[0]; pvect[0][1]=b[1]; pvect[0][2]=b[2];
02739 pvect[1][0]=b[3]; pvect[1][1]=b[4]; pvect[1][2]=b[5];
02740 pvect[2][0]=b[6]; pvect[2][1]=b[7]; pvect[2][2]=b[8];
02741
02742 double temp;
02743 if (pval[0] > pval[1])
02744 {
02745 temp = pval[0];
02746 pval[0] = pval[1];
02747 pval[1] = temp;
02748 mswapc(pvect,0,1);
02749 }
02750 if (pval[0] > pval[2])
02751 {
02752 temp = pval[0];
02753 pval[0] = pval[2];
02754 pval[2] = temp;
02755 mswapc(pvect,0,2);
02756 }
02757 if (pval[1] > pval[2])
02758 {
02759 temp = pval[1];
02760 pval[1] = pval[2];
02761 pval[2] = temp;
02762 mswapc(pvect,1,2);
02763 }
02764 }
02765 }
02766
02767
02768
02769
02770
02771
02772
02773
02774
02775
02776
02777
02778
02779
02780
02781
02782
02783
02784
02785
02786 void gemp (double *a,double *x,double *y,long n,long m,double limit,long pivot)
02787 {
02788 long i,j,k,ii,jj,kk,ir,ic,li,ui,*order;
02789 double f,s;
02790
02791 order = new long [n];
02792
02793
02794 for (i=0;i<n;i++){
02795 order[i]=i;
02796 }
02797
02798 for (k=0;k<n-1;k++){
02799 ir=k; ic=k;
02800
02801 if (pivot==1){
02802
02803 if (fabs(a[k*n+k])<limit){
02804 s=0.0; ii=k*n+k;
02805 for (i=k;i<n;i++){
02806 for (j=k;j<n;j++){
02807 f=fabs(a[ii]);
02808 if (f>s){
02809 s=f; ir=i; ic=j;
02810 }
02811 ii++;
02812 }
02813 ii+=k;
02814 }
02815 if (s<limit){
02816 print_err("singular matrix (%le)", __FILE__, __LINE__, __func__,s);
02817 break;
02818 }
02819 }
02820 }
02821
02822 if (pivot==2){
02823
02824 s=0.0; ii=k*n+k;
02825 for (i=k;i<n;i++){
02826 for (j=k;j<n;j++){
02827 f=fabs(a[ii]);
02828 if (f>s){
02829 s=f; ir=i; ic=j;
02830 }
02831 ii++;
02832 }
02833 ii+=k;
02834 }
02835 if (s<limit){
02836 print_err("singular matrix (%le)", __FILE__, __LINE__, __func__,s);
02837 break;
02838 }
02839 }
02840
02841
02842 if (ir!=k){
02843 li=k*n+k; ui=(k+1)*n; j=ir*n+k;
02844 for (i=li;i<ui;i++){
02845 s=a[i];
02846 a[i]=a[j];
02847 a[j]=s;
02848 j++;
02849 }
02850
02851 li=k*m; ui=li+m; j=ir*m;
02852 for (i=li;i<ui;i++){
02853 s=y[i];
02854 y[i]=y[j];
02855 y[j]=s;
02856 j++;
02857 }
02858 }
02859
02860
02861 if (ic!=k){
02862 i=order[k];
02863 order[k]=order[ic];
02864 order[ic]=i;
02865
02866 j=k; ii=ic;
02867 for (i=0;i<n;i++){
02868 s=a[j];
02869 a[j]=a[ii];
02870 a[ii]=s;
02871 j+=n; ii+=n;
02872 }
02873 }
02874
02875
02876
02877
02878 for (i=k+1;i<n;i++){
02879 ii=i*n+k; kk=k*n+k;
02880 s=a[ii]/a[kk];
02881
02882
02883 for (j=k;j<n;j++){
02884 a[ii]-=s*a[kk];
02885 ii++; kk++;
02886 }
02887
02888
02889 ii=i*m; kk=k*m;
02890 for (j=0;j<m;j++){
02891 y[ii]-=s*y[kk];
02892 ii++; kk++;
02893 }
02894 }
02895
02896
02897 }
02898
02899 if (fabs(a[(n-1)*n+n-1])<limit){
02900 print_err("singular matrix (%le)", __FILE__, __LINE__, __func__,a[(n-1)*n+n-1]);
02901 }
02902
02903
02904
02905
02906 for (k=n-1;k>-1;k--){
02907 f=a[k*n+k]; kk=k*m;
02908 for (i=0;i<m;i++){
02909 s=0.0; ii=k*n+n-1; jj=(n-1)*m+i;
02910 for (j=n-1;j>k;j--){
02911 s+=a[ii]*x[jj];
02912 ii--; jj-=m;
02913 }
02914 x[kk]=(y[kk]-s)/f;
02915 kk++;
02916 }
02917 }
02918
02919
02920 for (i=0;i<n;i++){
02921 if (order[i]!=i){
02922 for (j=i;j<n;j++){
02923 if (order[j]==i){
02924 jj=j; break;
02925 }
02926 }
02927
02928 ii=order[i];
02929 order[i]=order[jj];
02930 order[jj]=ii;
02931
02932 ii=i*m; kk=jj*m;
02933 for (j=0;j<m;j++){
02934 s=x[ii];
02935 x[ii]=x[kk];
02936 x[kk]=s;
02937 ii++; kk++;
02938 }
02939 }
02940 }
02941
02942 delete [] order;
02943 }
02944
02945
02946
02947
02948
02949
02950
02951
02952
02953
02954
02955
02956
02957
02958
02959
02960
02961
02962
02963 void lu_full (double *a,double *x,double *y,long n,double zero,long tc)
02964 {
02965 long i,j,k;
02966 double s;
02967 char emsg[200];
02968
02969 if (tc==1 || tc==2){
02970 for (i=0;i<n;i++){
02971 for (j=0;j<i;j++){
02972 s=0.0;
02973 for (k=0;k<j;k++){
02974 s+=a[i*n+k]*a[k*n+j];
02975 }
02976 a[i*n+j]-=s;
02977 }
02978
02979 s=0.0;
02980 for (k=0;k<i;k++){
02981 s+=a[i*n+k]*a[k*n+i];
02982 }
02983 a[i*n+i]-=s;
02984 if (fabs(a[i*n+i])<zero){
02985 sprintf(emsg, "zero diagonal entry (%le) in LU decomposition in matrix row %ld", a[i*n+i], i+1);
02986 print_err(emsg, __FILE__, __LINE__, __func__);
02987 }
02988
02989 for (j=i+1;j<n;j++){
02990 s=0.0;
02991 for (k=0;k<i;k++){
02992 s+=a[i*n+k]*a[k*n+j];
02993 }
02994 a[i*n+j]=(a[i*n+j]-s)/a[i*n+i];
02995 }
02996 }
02997 }
02998
02999 if (tc==1 || tc==3){
03000 for (i=0;i<n;i++){
03001 s=0.0;
03002 for (j=0;j<i;j++){
03003 s+=a[i*n+j]*y[j];
03004 }
03005 y[i]=(y[i]-s)/a[i*n+i];
03006 }
03007 for (i=n-1;i>-1;i--){
03008 s=0.0;
03009 for (j=i+1;j<n;j++){
03010 s+=a[i*n+j]*x[j];
03011 }
03012 x[i]=y[i]-s;
03013 }
03014 }
03015
03016 }
03017
03018
03019
03020 void mat_localize (matrix &gm,matrix &lm,long *rcn,long *ccn)
03021 {
03022 long i,j,m,n;
03023
03024 m=lm.m; n=lm.n;
03025
03026 for (i=0;i<m;i++){
03027 if (rcn[i]<=0) continue;
03028 for (j=0;j<n;j++){
03029 if (ccn[j]<=0) continue;
03030 gm[rcn[i]-1][ccn[j]-1]+=lm[i][j];
03031 }
03032 }
03033
03034 }
03035
03036
03037
03038
03039
03040
03041
03042
03043
03044
03045
03046
03047 void matassem_lsm (double *lsm,vector &natcoord)
03048 {
03049 if (natcoord.n==1){
03050 lsm[0*2+0]++; lsm[0*2+1]+=natcoord[0];
03051 lsm[1*2+1]+=natcoord[0]*natcoord[0];
03052 }
03053 if (natcoord.n==2){
03054 lsm[0*3+0]++; lsm[0*3+1]+=natcoord[0]; lsm[0*3+2]+=natcoord[1];
03055 lsm[1*3+1]+=natcoord[0]*natcoord[0]; lsm[1*3+2]+=natcoord[1]*natcoord[0];
03056 lsm[2*3+2]+=natcoord[1]*natcoord[1];
03057 }
03058 if (natcoord.n==3){
03059 lsm[0*4+0]++; lsm[0*4+1]+=natcoord[0]; lsm[0*4+2]+=natcoord[1]; lsm[0*4+3]+=natcoord[2];
03060 lsm[1*4+1]+=natcoord[0]*natcoord[0]; lsm[1*4+2]+=natcoord[1]*natcoord[0]; lsm[1*4+3]+=natcoord[2]*natcoord[0];
03061 lsm[2*4+2]+=natcoord[1]*natcoord[1]; lsm[2*4+3]+=natcoord[2]*natcoord[1];
03062 lsm[3*4+3]+=natcoord[2]*natcoord[2];
03063 }
03064 }
03065
03066
03067
03068
03069
03070
03071
03072
03073
03074
03075
03076
03077
03078 void rhsassem_lsm (double *rhs,vector &natcoord,vector &values)
03079 {
03080 long i;
03081 if (natcoord.n==1){
03082 for (i=0;i<values.n;i++){
03083 rhs[i*2+0]+=values[i];
03084 rhs[i*2+1]+=values[i]*natcoord[0];
03085 }
03086 }
03087 if (natcoord.n==2){
03088 for (i=0;i<values.n;i++){
03089 rhs[i*3+0]+=values[i];
03090 rhs[i*3+1]+=values[i]*natcoord[0];
03091 rhs[i*3+2]+=values[i]*natcoord[1];
03092 }
03093 }
03094 if (natcoord.n==3){
03095 for (i=0;i<values.n;i++){
03096 rhs[i*4+0]+=values[i];
03097 rhs[i*4+1]+=values[i]*natcoord[0];
03098 rhs[i*4+2]+=values[i]*natcoord[1];
03099 rhs[i*4+3]+=values[i]*natcoord[2];
03100 }
03101 }
03102 }
03103
03104
03105
03106
03107
03108
03109
03110
03111
03112
03113
03114
03115
03116
03117
03118 void solve_lsm (double *lsm,double *lhs,double *rhs,double zero,long n,long m)
03119 {
03120 long i,j,k;
03121
03122 if (n==3){
03123 lsm[1*3+0]=lsm[0*3+1];
03124 lsm[2*3+0]=lsm[0*3+2]; lsm[2*3+1]=lsm[1*3+2];
03125 }
03126 if (n==4){
03127 lsm[1*4+0]=lsm[0*4+1];
03128 lsm[2*4+0]=lsm[0*4+2]; lsm[2*4+1]=lsm[1*4+2];
03129 lsm[3*4+0]=lsm[0*4+3]; lsm[3*4+1]=lsm[1*4+3]; lsm[3*4+2]=lsm[2*4+3];
03130 }
03131
03132 if (lsm[0]<3.0){
03133 k=0;
03134 for (i=0;i<m;i++){
03135 lhs[k]=rhs[k]/lsm[0];
03136 k++;
03137 for (j=1;j<n;j++){
03138 lhs[k]=0.0;
03139 k++;
03140 }
03141 }
03142 }
03143 else{
03144 lu_full (lsm,lhs,rhs,n,zero,2);
03145 for (i=0;i<m;i++){
03146 lu_full (lsm,lhs+i*n,rhs+i*n,n,zero,3);
03147 }
03148 }
03149
03150 }
03151
03152
03153
03154
03155
03156
03157
03158
03159
03160
03161
03162
03163
03164
03165
03166
03167
03168
03169
03170 long vxmxv (const vector &v, const matrix &m, double &answer)
03171 {
03172 long i,j;
03173
03174 if ((v.n != m.n) || (v.n != m.m)) {
03175 print_err("cannot vxmxv cannot be performed due to incompatible dimensions\n"
03176 "v(%ld) X m(%ld,%ld)", __FILE__, __LINE__, __func__, v.n, m.m, m.n);
03177 return (1);
03178 }
03179
03180 answer = 0.0;
03181
03182 for (i=0;i<v.n-1;i++)
03183 for (j=i+1;j<v.n;j++)
03184 answer += 2*v[i]*m[i][j]*v[j];
03185
03186 for (i=0;i<v.n;i++)
03187 answer += v[i]*m[i][i]*v[i];
03188
03189 return(0);
03190 }
03191
03192
03193
03194
03195
03196
03197
03198
03199
03200
03201
03202
03203
03204
03205
03206
03207
03208
03209
03210 long vxmxv (const double *v, const double *m, long dim, double &answer)
03211 {
03212 long i,j;
03213
03214 answer = 0.0;
03215
03216 for (i=0;i<dim-1;i++)
03217 for (j=i+1;j<dim;j++)
03218 answer += 2*v[i]*m[i*dim+j]*v[j];
03219
03220 for (i=0;i<dim;i++)
03221 answer += v[i]*m[i*dim+i]*v[i];
03222
03223 return(0);
03224 }
03225
03226
03227
03228
03229
03230
03231
03232
03233
03234
03235
03236
03237
03238
03239 void extractm (matrix &a,matrix &b,long fi, long ncomp)
03240 {
03241 long i,j;
03242
03243 for (i=0;i<ncomp;i++){
03244 for (j=0;j<ncomp;j++){
03245 a[i][j]=b[i+fi][j+fi];
03246 }
03247 }
03248 }
03249
03250
03251
03252
03253
03254
03255
03256
03257
03258
03259
03260
03261
03262
03263
03264 void extractblock (double *a,double *b,long n,long nr,long nc,long fri,long fci)
03265 {
03266 long i,j;
03267
03268 for (i=0;i<nr;i++){
03269 for (j=0;j<nc;j++){
03270 b[i*nc+j]=a[(fri+i)*n+(fci+j)];
03271 }
03272 }
03273 }