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