00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057 #include <assert.h>
00058 #include "TCernLib.h"
00059 #include "TMath.h"
00060 #include "TArrayD.h"
00061 #include "TError.h"
00062
00063 ClassImp(TCL)
00064
00065 #define TCL_MXMAD(n_,a,b,c,i,j,k) \
00066 \
00067 int l, m, n, ia, ic, ib, ja, jb, iia, iib, ioa, iob; \
00068 \
00069 \
00070 --a; --b; --c; \
00071 \
00072 \
00073 \
00074 const int iandj1[] = {2, 2 , 2 , 2 , 1 , 1 , 1 , 1 , 3 , 3 , 3 , 3 }; \
00075 const int iandj2[] = { 1, 2, 3, 4, 1, 2, 3, 4, 1, 2, 3, 4 }; \
00076 int n1 = iandj1[n_]; \
00077 int n2 = iandj2[n_]; \
00078 if (i == 0 || k == 0) return 0; \
00079 \
00080 switch (n2) { \
00081 case 1: iia = 1; ioa = j; iib = k; iob = 1; break; \
00082 case 2: iia = 1; ioa = j; iib = 1; iob = j; break; \
00083 case 3: iia = i; ioa = 1; iib = k; iob = 1; break; \
00084 case 4: iia = i; ioa = 1; iib = 1; iob = j; break; \
00085 default: iia = ioa = iib = iob = 0; assert(iob); \
00086 }; \
00087 \
00088 ia = 1; ic = 1; \
00089 for (l = 1; l <= i; ++l) { \
00090 ib = 1; \
00091 for (m = 1; m <= k; ++m,++ic) { \
00092 switch (n1) { \
00093 case 1: c[ic] = 0.; break; \
00094 case 3: c[ic] = -c[ic]; break; \
00095 }; \
00096 if (j == 0) continue; \
00097 ja = ia; jb = ib; \
00098 double cic = c[ic]; \
00099 for (n = 1; n <= j; ++n, ja+=iia, jb+=iib) \
00100 cic += a[ja] * b[jb]; \
00101 c[ic] = cic; \
00102 ib += iob; \
00103 } \
00104 ia += ioa; \
00105 }
00106
00107
00108 float *TCL::mxmad_0_(int n_, const float *a, const float *b, float *c, int i, int j, int k)
00109 {
00110 TCL_MXMAD(n_,a,b,c,i,j,k)
00111 return c;
00112 }
00113
00114
00115 double *TCL::mxmad_0_(int n_, const double *a, const double *b, double *c, int i, int j, int k)
00116 {
00117 TCL_MXMAD(n_,a,b,c,i,j,k)
00118 return c;
00119 }
00120
00121 #undef TCL_MXMAD
00122
00123
00124
00125
00126
00127
00128 #define TCL_MXMLRT( n__, a, b, c, ni,nj) \
00129 if (ni <= 0 || nj <= 0) return 0; \
00130 double x; \
00131 int ia, ib, ic, ja, kc, ii, jj, kj, ki, ia1, ib1, ic1, ja1; \
00132 int ipa = 1; int jpa = nj; \
00133 if (n__ == 1) { ipa = ni; jpa = 1; } \
00134 \
00135 --a; --b; --c; \
00136 \
00137 ic1 = 1; ia1 = 1; \
00138 for (ii = 1; ii <= ni; ++ii, ic1+=ni, ia1+=jpa) { \
00139 ic = ic1; \
00140 for (kc = 1; kc <= ni; ++kc,ic++) c[ic] = 0.; \
00141 ib1 = 1; ja1 = 1; \
00142 for (jj = 1; jj <= nj; ++jj,++ib1,ja1 += ipa) { \
00143 ib = ib1; ia = ia1; \
00144 x = 0.; \
00145 for (kj = 1;kj <= nj;++kj,ia+=ipa,ib += nj) \
00146 x += a[ia] * b[ib]; \
00147 ja = ja1; ic = ic1; \
00148 for (ki = 1; ki <= ni; ++ki,++ic,ja += jpa) \
00149 c[ic] += x * a[ja]; \
00150 } \
00151 }
00152
00153
00154 float *TCL::mxmlrt_0_(int n__, const float *a, const float *b, float *c, int ni,int nj)
00155 {
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185 TCL_MXMLRT( n__, a, b, c, ni,nj)
00186 return c;
00187 }
00188
00189
00190 double *TCL::mxmlrt_0_(int n__, const double *a, const double *b, double *c, int ni,int nj)
00191 {
00192
00193
00194 TCL_MXMLRT( n__, a, b, c, ni,nj)
00195 return c;
00196
00197 }
00198
00199 #undef TCL_MXMLRT
00200
00201
00202
00203
00204
00205
00206 #define TCL_MXTRP(a, b, i, j) \
00207 if (i == 0 || j == 0) return 0; \
00208 --b; --a; \
00209 int ib = 1; \
00210 for (int k = 1; k <= j; ++k) \
00211 { int ia = k; \
00212 for (int l = 1; l <= i; ++l,ia += j,++ib) b[ib] = a[ia]; }
00213
00214
00215 float *TCL::mxtrp(const float *a, float *b, int i, int j)
00216 {
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227 TCL_MXTRP(a, b, i, j)
00228 return b;
00229 }
00230
00231
00232 double *TCL::mxtrp(const double *a, double *b, int i, int j)
00233 {
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243 TCL_MXTRP(a, b, i, j)
00244 return b;
00245
00246 }
00247 #undef TCL_MXTRP
00248
00249
00250
00251
00252
00253
00254
00255
00256 #define TCL_TRAAT(a, s, m, n) \
00257 \
00258 int ipiv, i, j, ipivn, ia, is, iat; \
00259 double sum; \
00260 --s; --a; \
00261 ia = 0; is = 0; \
00262 for (i = 1; i <= m; ++i) { \
00263 ipiv = ia; \
00264 ipivn = ipiv + n; \
00265 iat = 0; \
00266 for (j = 1; j <= i; ++j) { \
00267 ia = ipiv; \
00268 sum = 0.; \
00269 do { \
00270 ++ia; ++iat; \
00271 sum += a[ia] * a[iat]; \
00272 } while (ia < ipivn); \
00273 ++is; \
00274 s[is] = sum; \
00275 } \
00276 } \
00277 s++;
00278
00279
00280
00281 float *TCL::traat(const float *a, float *s, int m, int n)
00282 {
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294 TCL_TRAAT(a, s, m, n)
00295 return s;
00296 }
00297
00298
00299 double *TCL::traat(const double *a, double *s, int m, int n)
00300 {
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311 TCL_TRAAT(a, s, m, n)
00312 return s;
00313 }
00314
00315 #undef TCL_TRAAT
00316
00317 #define TCL_TRAL(a, u, b, m, n) \
00318 int indu, i, j, k, ia, ib, iu; \
00319 double sum; \
00320 --b; --u; --a; \
00321 ib = 1; \
00322 for (i = 1; i <= m; ++i) { \
00323 indu = 0; \
00324 for (j = 1; j <= n; ++j) { \
00325 indu += j; \
00326 ia = ib; \
00327 iu = indu; \
00328 sum = 0.; \
00329 for (k = j; k <= n; ++k) {\
00330 sum += a[ia] * u[iu]; \
00331 ++ia; \
00332 iu += k; \
00333 } \
00334 b[ib] = sum; \
00335 ++ib; \
00336 } \
00337 } \
00338 b++;
00339
00340
00341 float *TCL::tral(const float *a, const float *u, float *b, int m, int n)
00342 {
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352 TCL_TRAL(a, u, b, m, n)
00353 return b;
00354 }
00355
00356
00357 double *TCL::tral(const double *a, const double *u, double *b, int m, int n)
00358 {
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368 TCL_TRAL(a, u, b, m, n)
00369 return b;
00370 }
00371
00372 #undef TCL_TRAL
00373
00374
00375 #define TCL_TRALT(a, u, b, m, n) \
00376 int indu, j, k, ia, ib, iu; \
00377 double sum; \
00378 --b; --u; --a; \
00379 ib = m * n; \
00380 indu = (n * n + n) / 2; \
00381 do { \
00382 iu = indu; \
00383 for (j = 1; j <= n; ++j) { \
00384 ia = ib; \
00385 sum = 0.; \
00386 for (k = j; k <= n; ++k) {\
00387 sum += a[ia] * u[iu]; \
00388 --ia; --iu; \
00389 } \
00390 b[ib] = sum; \
00391 --ib; \
00392 } \
00393 } while (ib > 0); \
00394 ++b;
00395
00396
00397 float *TCL::tralt(const float *a, const float *u, float *b, int m, int n)
00398 {
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408 TCL_TRALT(a, u, b, m, n)
00409 return b;
00410 }
00411
00412
00413 double *TCL::tralt(const double *a, const double *u, double *b, int m, int n)
00414 {
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424 TCL_TRALT(a, u, b, m, n)
00425 return b;
00426 }
00427
00428 #undef TCL_TRALT
00429
00430
00431
00432 #define TCL_TRAS(a, s, b, m, n) \
00433 int inds, i__, j, k, ia, ib, is; \
00434 double sum; \
00435 --b; --s; --a; \
00436 ib = 0; inds = 0; i__ = 0; \
00437 do { \
00438 inds += i__; \
00439 ia = 0; \
00440 ib = i__ + 1; \
00441 for (j = 1; j <= m; ++j) { \
00442 is = inds; \
00443 sum = 0.; \
00444 k = 0; \
00445 do { \
00446 if (k > i__) is += k; \
00447 else ++is; \
00448 ++ia; \
00449 sum += a[ia] * s[is]; \
00450 ++k; \
00451 } while (k < n); \
00452 b[ib] = sum; \
00453 ib += n; \
00454 } \
00455 ++i__; \
00456 } while (i__ < n); \
00457 ++b;
00458
00459
00460 float *TCL::tras(const float *a, const float *s, float *b, int m, int n)
00461 {
00462
00463
00464
00465
00466
00467
00468
00469
00470
00471 TCL_TRAS(a, s, b, m, n)
00472 return b;
00473 }
00474
00475
00476 double *TCL::tras(const double *a, const double *s, double *b, int m, int n)
00477 {
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487 TCL_TRAS(a, s, b, m, n)
00488 return b;
00489 }
00490
00491 #undef TCL_TRAS
00492
00493
00494
00495 #define TCL_TRASAT(a, s, r__, m, n) \
00496 int imax, k; \
00497 int ia, mn, ir, is, iaa; \
00498 double sum; \
00499 --r__; --s; --a; \
00500 imax = (m * m + m) / 2; \
00501 vzero(&r__[1], imax); \
00502 mn = m * n; \
00503 int ind = 0; \
00504 int i__ = 0; \
00505 do { \
00506 ind += i__; \
00507 ia = 0; ir = 0; \
00508 do { \
00509 is = ind; \
00510 sum = 0.; k = 0; \
00511 do { \
00512 if (k > i__) is += k; \
00513 else ++is; \
00514 ++ia; \
00515 sum += s[is] * a[ia]; \
00516 ++k; \
00517 } while (k < n); \
00518 iaa = i__ + 1; \
00519 do { \
00520 ++ir; \
00521 r__[ir] += sum * a[iaa];\
00522 iaa += n; \
00523 } while (iaa <= ia); \
00524 } while (ia < mn); \
00525 ++i__; \
00526 } while (i__ < n); \
00527 ++r__;
00528
00529
00530 float *TCL::trasat(const float *a, const float *s, float *r__, int m, int n)
00531 {
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541 TCL_TRASAT(a, s, r__, m, n)
00542 return r__;
00543 }
00544
00545
00546 double *TCL::trasat(const double *a, const double *s, double *r__, int m, int n)
00547 {
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557 TCL_TRASAT(a, s, r__, m, n)
00558 return r__;
00559 }
00560
00561
00562 float *TCL::trasat(const double *a, const float *s, float *r__, int m, int n)
00563 {
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573 TCL_TRASAT(a, s, r__, m, n)
00574 return r__;
00575 }
00576
00577 #undef TCL_TRASAT
00578
00579
00580 float *TCL::trata(const float *a, float *r__, int m, int n)
00581 {
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592 int i__, j, ia, mn, ir, iat;
00593 double sum;
00594
00595
00596 --r__; --a;
00597
00598
00599 mn = m * n;
00600 ir = 0;
00601
00602 for (i__ = 1; i__ <= m; ++i__) {
00603 for (j = 1; j <= i__; ++j) {
00604 ia = i__;
00605 iat = j;
00606 sum = 0.;
00607 do {
00608 sum += a[ia] * a[iat];
00609 ia += m;
00610 iat += m;
00611 } while (ia <= mn);
00612 ++ir;
00613 r__[ir] = sum;
00614 }
00615 }
00616 ++r__;
00617 return r__;
00618 }
00619
00620
00621
00622 float *TCL::trats(const float *a, const float *s, float *b, int m, int n)
00623 {
00624
00625
00626
00627
00628
00629
00630 int inds, i__, j, k, ia, ib, is;
00631 double sum;
00632
00633
00634
00635
00636
00637 --b; --s; --a;
00638
00639
00640 ib = 0; inds = 0; i__ = 0;
00641 do {
00642 inds += i__;
00643 ib = i__ + 1;
00644
00645 for (j = 1; j <= m; ++j) {
00646 ia = j;
00647 is = inds;
00648 sum = 0.;
00649 k = 0;
00650
00651 do {
00652 if (k > i__) is += k;
00653 else ++is;
00654 sum += a[ia] * s[is];
00655 ia += m;
00656 ++k;
00657 } while (k < n);
00658
00659 b[ib] = sum;
00660 ib += n;
00661 }
00662 ++i__;
00663 } while (i__ < n);
00664 ++b;
00665 return b;
00666 }
00667
00668
00669
00670 float *TCL::tratsa(const float *a, const float *s, float *r__, int m, int n)
00671 {
00672
00673
00674
00675
00676
00677
00678
00679 int imax, i__, j, k;
00680 int ia, ir, is, iaa, ind;
00681 double sum;
00682
00683
00684
00685
00686
00687
00688 --r__; --s; --a;
00689
00690
00691 imax = (m * m + m) / 2;
00692 vzero(&r__[1], imax);
00693 ind = 0;
00694 i__ = 0;
00695
00696 do {
00697 ind += i__;
00698 ir = 0;
00699
00700 for (j = 1; j <= m; ++j) {
00701 is = ind;
00702 ia = j;
00703 sum = 0.;
00704 k = 0;
00705
00706 do {
00707 if (k > i__) is += k;
00708 else ++is;
00709 sum += s[is] * a[ia];
00710 ia += m;
00711 ++k;
00712 } while (k < n);
00713 iaa = i__ * m;
00714
00715 for (k = 1; k <= j; ++k) {
00716 ++iaa;
00717 ++ir;
00718 r__[ir] += sum * a[iaa];
00719 }
00720 }
00721 ++i__;
00722 } while (i__ < n);
00723 ++r__;
00724 return r__;
00725 }
00726
00727
00728
00729 float *TCL::trchlu(const float *a, float *b, int n)
00730 {
00731
00732
00733
00734
00735
00736
00737 int ipiv, kpiv, i__, j;
00738 double r__, dc;
00739 int id, kd;
00740 double sum;
00741
00742
00743
00744
00745
00746
00747
00748 --b; --a;
00749
00750
00751 ipiv = 0;
00752
00753 i__ = 0;
00754
00755 do {
00756 ++i__;
00757 ipiv += i__;
00758 kpiv = ipiv;
00759 r__ = a[ipiv];
00760
00761 for (j = i__; j <= n; ++j) {
00762 sum = 0.;
00763 if (i__ == 1) goto L40;
00764 if (r__ == 0.) goto L42;
00765 id = ipiv - i__ + 1;
00766 kd = kpiv - i__ + 1;
00767
00768 do {
00769 sum += b[kd] * b[id];
00770 ++kd; ++id;
00771 } while (id < ipiv);
00772
00773 L40:
00774 sum = a[kpiv] - sum;
00775 L42:
00776 if (j != i__) b[kpiv] = sum * r__;
00777 else {
00778 dc = TMath::Sqrt(sum);
00779 b[kpiv] = dc;
00780 if (r__ > 0.) r__ = 1. / dc;
00781 }
00782 kpiv += j;
00783 }
00784
00785 } while (i__ < n);
00786 ++b;
00787 return b;
00788 }
00789
00790
00791
00792 float *TCL::trchul(const float *a, float *b, int n)
00793 {
00794
00795
00796
00797
00798
00799
00800 int ipiv, kpiv, i__;
00801 double r__;
00802 int nTep;
00803 double dc;
00804 int id, kd;
00805 double sum;
00806
00807
00808
00809
00810
00811
00812
00813 --b; --a;
00814
00815
00816 kpiv = (n * n + n) / 2;
00817
00818 i__ = n;
00819 do {
00820 ipiv = kpiv;
00821 r__ = a[ipiv];
00822
00823 do {
00824 sum = 0.;
00825 if (i__ == n) goto L40;
00826 if (r__ == 0.) goto L42;
00827 id = ipiv;
00828 kd = kpiv;
00829 nTep = i__;
00830
00831 do {
00832 kd += nTep;
00833 id += nTep;
00834 ++nTep;
00835 sum += b[id] * b[kd];
00836 } while (nTep < n);
00837
00838 L40:
00839 sum = a[kpiv] - sum;
00840 L42:
00841 if (kpiv < ipiv) b[kpiv] = sum * r__;
00842 else {
00843 dc = TMath::Sqrt(sum);
00844 b[kpiv] = dc;
00845 if (r__ > 0.) r__ = 1. / dc;
00846 }
00847 --kpiv;
00848 } while (kpiv > ipiv - i__);
00849
00850 --i__;
00851 } while (i__ > 0);
00852
00853 ++b;
00854 return b;
00855 }
00856
00857
00858 float *TCL::trinv(const float *t, float *s, int n)
00859 {
00860
00861
00862
00863
00864
00865
00866
00867
00868
00869 int lhor, ipiv, lver, j;
00870 double sum = 0;
00871 double r__ = 0;
00872 int mx, ndTep, ind;
00873
00874
00875
00876 --s; --t;
00877
00878
00879 mx = (n * n + n) / 2;
00880 ipiv = mx;
00881
00882 int i = n;
00883 do {
00884 r__ = 0.;
00885 if (t[ipiv] > 0.) r__ = 1. / t[ipiv];
00886 s[ipiv] = r__;
00887 ndTep = n;
00888 ind = mx - n + i;
00889
00890 while (ind != ipiv) {
00891 sum = 0.;
00892 if (r__ != 0.) {
00893 lhor = ipiv;
00894 lver = ind;
00895 j = i;
00896
00897 do {
00898 lhor += j;
00899 ++lver;
00900 sum += t[lhor] * s[lver];
00901 ++j;
00902 } while (lhor < ind);
00903 }
00904 s[ind] = -sum * r__;
00905 --ndTep;
00906 ind -= ndTep;
00907 }
00908
00909 ipiv -= i;
00910 --i;
00911 } while (i > 0);
00912
00913 ++s;
00914 return s;
00915 }
00916
00917
00918
00919 float *TCL::trla(const float *u, const float *a, float *b, int m, int n)
00920 {
00921 int ipiv, ia, ib, iu;
00922 double sum;
00923
00924
00925
00926
00927
00928
00929
00930
00931
00932
00933
00934 --b; --a; --u;
00935
00936
00937 ib = m * n;
00938 ipiv = (m * m + m) / 2;
00939
00940 do {
00941 do {
00942 ia = ib;
00943 iu = ipiv;
00944
00945 sum = 0.;
00946 do {
00947 sum += a[ia] * u[iu];
00948 --iu;
00949 ia -= n;
00950 } while (ia > 0);
00951
00952 b[ib] = sum;
00953 --ib;
00954 } while (ia > 1 - n);
00955
00956 ipiv = iu;
00957 } while (iu > 0);
00958
00959 ++b;
00960 return b;
00961 }
00962
00963
00964
00965 float *TCL::trlta(const float *u, const float *a, float *b, int m, int n)
00966 {
00967 int ipiv, mxpn, i__, nTep, ia, ib, iu, mx;
00968 double sum;
00969
00970
00971
00972
00973
00974
00975
00976
00977
00978
00979
00980 --b; --a; --u;
00981
00982
00983 ipiv = 0;
00984 mx = m * n;
00985 mxpn = mx + n;
00986 ib = 0;
00987
00988 i__ = 0;
00989 do {
00990 ++i__;
00991 ipiv += i__;
00992
00993 do {
00994 iu = ipiv;
00995 nTep = i__;
00996 ++ib;
00997 ia = ib;
00998
00999 sum = 0.;
01000 do {
01001 sum += a[ia] * u[iu];
01002 ia += n;
01003 iu += nTep;
01004 ++nTep;
01005 } while (ia <= mx);
01006
01007 b[ib] = sum;
01008 } while (ia < mxpn);
01009
01010 } while (i__ < m);
01011
01012 ++b;
01013 return b;
01014 }
01015
01016
01017 float *TCL::trpck(const float *s, float *u, int n)
01018 {
01019
01020
01021
01022
01023
01024
01025
01026
01027 int i__, ia, ind, ipiv;
01028
01029
01030 --u; --s;
01031
01032
01033 ia = 0;
01034 ind = 0;
01035 ipiv = 0;
01036
01037 for (i__ = 1; i__ <= n; ++i__) {
01038 ipiv += i__;
01039 do {
01040 ++ia;
01041 ++ind;
01042 u[ind] = s[ia];
01043 } while (ind < ipiv);
01044 ia = ia + n - i__;
01045 }
01046
01047 ++u;
01048 return u;
01049 }
01050
01051
01052 float *TCL::trqsq(const float *q, const float *s, float *r__, int m)
01053 {
01054
01055
01056
01057
01058
01059
01060
01061
01062
01063 int indq, inds, imax, i__, j, k, l;
01064 int iq, ir, is, iqq;
01065 double sum;
01066
01067
01068 --r__; --s; --q;
01069
01070
01071 imax = (m * m + m) / 2;
01072 vzero(&r__[1], imax);
01073 inds = 0;
01074 i__ = 0;
01075
01076 do {
01077 inds += i__;
01078 ir = 0;
01079 indq = 0;
01080 j = 0;
01081
01082 do {
01083 indq += j;
01084 is = inds;
01085 iq = indq;
01086 sum = (float)0.;
01087 k = 0;
01088
01089 do {
01090 if (k > i__) is += k;
01091 else ++is;
01092
01093 if (k > j) iq += k;
01094 else ++iq;
01095
01096 sum += s[is] * q[iq];
01097 ++k;
01098 } while (k < m);
01099 iqq = inds;
01100 l = 0;
01101
01102 do {
01103 ++ir;
01104 if (l > i__) iqq += l;
01105 else ++iqq;
01106 r__[ir] += q[iqq] * sum;
01107 ++l;
01108 } while (l <= j);
01109 ++j;
01110 } while (j < m);
01111 ++i__;
01112 } while (i__ < m);
01113
01114 ++r__;
01115 return r__;
01116 }
01117
01118
01119 float *TCL::trsa(const float *s, const float *a, float *b, int m, int n)
01120 {
01121
01122
01123
01124
01125
01126
01127
01128
01129
01130 int inds, i__, j, k, ia, ib, is;
01131 double sum;
01132
01133
01134 --b; --a; --s;
01135
01136
01137 inds = 0;
01138 ib = 0;
01139 i__ = 0;
01140
01141 do {
01142 inds += i__;
01143
01144 for (j = 1; j <= n; ++j) {
01145 ia = j;
01146 is = inds;
01147 sum = 0.;
01148 k = 0;
01149
01150 do {
01151 if (k > i__) is += k;
01152 else ++is;
01153 sum += s[is] * a[ia];
01154 ia += n;
01155 ++k;
01156 } while (k < m);
01157 ++ib;
01158 b[ib] = sum;
01159 }
01160 ++i__;
01161 } while (i__ < m);
01162
01163 ++b;
01164 return b;
01165 }
01166
01167
01168 float *TCL::trsinv(const float *g, float *gi, int n)
01169 {
01170
01171
01172
01173
01174
01175
01176
01177
01178
01179
01180 trchlu(g, gi, n);
01181 trinv(gi, gi, n);
01182 return trsmul(gi, gi, n);
01183 }
01184
01185
01186 float *TCL::trsmlu(const float *u, float *s, int n)
01187 {
01188
01189
01190
01191
01192
01193
01194
01195
01196
01197
01198 int lhor, lver, i__, k, l, ind;
01199 double sum;
01200
01201
01202 --s; --u;
01203
01204
01205 ind = (n * n + n) / 2;
01206
01207 for (i__ = 1; i__ <= n; ++i__) {
01208 lver = ind;
01209
01210 for (k = i__; k <= n; ++k,--ind) {
01211 lhor = ind; sum = 0.;
01212 for (l = k; l <= n; ++l,--lver,--lhor)
01213 sum += u[lver] * u[lhor];
01214 s[ind] = sum;
01215 }
01216 }
01217 ++s;
01218 return s;
01219 }
01220
01221
01222 float *TCL::trsmul(const float *g, float *gi, int n)
01223 {
01224
01225
01226
01227
01228
01229
01230
01231
01232
01233
01234 int lhor, lver, lpiv, i__, j, k, ind;
01235 double sum;
01236
01237
01238 --gi; --g;
01239
01240
01241 ind = 1;
01242 lpiv = 0;
01243 for (i__ = 1; i__ <= n; ++i__) {
01244 lpiv += i__;
01245 for (j = 1; j <= i__; ++j,++ind) {
01246 lver = lpiv;
01247 lhor = ind;
01248 sum = 0.;
01249 for (k = i__; k <= n; lhor += k,lver += k,++k)
01250 sum += g[lver] * g[lhor];
01251 gi[ind] = sum;
01252 }
01253 }
01254 ++gi;
01255 return gi;
01256 }
01257
01258
01259 float *TCL::trupck(const float *u, float *s, int m)
01260 {
01261
01262
01263
01264
01265
01266
01267
01268
01269
01270
01271 int i__, im, is, iu, iv, ih, m2;
01272
01273
01274 --s; --u;
01275
01276
01277 m2 = m * m;
01278 is = m2;
01279 iu = (m2 + m) / 2;
01280 i__ = m - 1;
01281
01282 do {
01283 im = i__ * m;
01284 do {
01285 s[is] = u[iu];
01286 --is;
01287 --iu;
01288 } while (is > im);
01289 is = is - m + i__;
01290 --i__;
01291 } while (i__ >= 0);
01292
01293 is = 1;
01294 do {
01295 iv = is;
01296 ih = is;
01297 while (1) {
01298 iv += m;
01299 ++ih;
01300 if (iv > m2) break;
01301 s[ih] = s[iv];
01302 }
01303 is = is + m + 1;
01304 } while (is < m2);
01305
01306 ++s;
01307 return s;
01308 }
01309
01310
01311
01312 float *TCL::trsat(const float *s, const float *a, float *b, int m, int n)
01313 {
01314
01315
01316
01317
01318
01319
01320
01321 int inds, i__, j, k, ia, ib, is;
01322 double sum;
01323
01324
01325
01326
01327
01328
01329
01330 --b; --a; --s;
01331
01332
01333 inds = 0;
01334 ib = 0;
01335 i__ = 0;
01336
01337 do {
01338 inds += i__;
01339 ia = 0;
01340
01341 for (j = 1; j <= n; ++j) {
01342 is = inds;
01343 sum = 0.;
01344 k = 0;
01345
01346 do {
01347 if (k > i__) is += k;
01348 else ++is;
01349 ++ia;
01350 sum += s[is] * a[ia];
01351 ++k;
01352 } while (k < m);
01353 ++ib;
01354 b[ib] = sum;
01355 }
01356 ++i__;
01357 } while (i__ < m);
01358
01359 ++b;
01360 return b;
01361 }
01362
01363
01364
01365
01366
01367 double *TCL::trata(const double *a, double *r__, int m, int n)
01368 {
01369
01370
01371
01372
01373
01374
01375
01376 int i__, j, ia, mn, ir, iat;
01377 double sum;
01378
01379
01380
01381
01382
01383
01384
01385 --r__; --a;
01386
01387
01388 mn = m * n;
01389 ir = 0;
01390
01391 for (i__ = 1; i__ <= m; ++i__) {
01392
01393 for (j = 1; j <= i__; ++j) {
01394 ia = i__;
01395 iat = j;
01396
01397 sum = (double)0.;
01398 do {
01399 sum += a[ia] * a[iat];
01400 ia += m;
01401 iat += m;
01402 } while (ia <= mn);
01403 ++ir;
01404 r__[ir] = sum;
01405 }
01406 }
01407
01408 return 0;
01409 }
01410
01411
01412
01413 double *TCL::trats(const double *a, const double *s, double *b, int m, int n)
01414 {
01415
01416
01417
01418
01419
01420
01421 int inds, i__, j, k, ia, ib, is;
01422 double sum;
01423
01424
01425
01426
01427
01428
01429 --b; --s; --a;
01430
01431
01432 ib = 0; inds = 0; i__ = 0;
01433
01434 do {
01435 inds += i__;
01436 ib = i__ + 1;
01437
01438 for (j = 1; j <= m; ++j) {
01439 ia = j;
01440 is = inds;
01441 sum = (double)0.;
01442 k = 0;
01443
01444 do {
01445 if (k > i__) is += k;
01446 else ++is;
01447 sum += a[ia] * s[is];
01448 ia += m;
01449 ++k;
01450 } while (k < n);
01451
01452 b[ib] = sum;
01453 ib += n;
01454 }
01455 ++i__;
01456 } while (i__ < n);
01457
01458 return 0;
01459 }
01460
01461
01462
01463 double *TCL::tratsa(const double *a, const double *s, double *r__, int m, int n)
01464 {
01465
01466
01467
01468
01469
01470
01471 int imax, i__, j, k;
01472 int ia, ir, is, iaa, ind;
01473 double sum;
01474
01475
01476
01477
01478
01479
01480 --r__; --s; --a;
01481
01482
01483 imax = (m * m + m) / 2;
01484 vzero(&r__[1], imax);
01485 ind = 0;
01486 i__ = 0;
01487
01488 do {
01489 ind += i__;
01490 ir = 0;
01491
01492 for (j = 1; j <= m; ++j) {
01493 is = ind;
01494 ia = j;
01495 sum = (double)0.;
01496 k = 0;
01497
01498 do {
01499 if (k > i__) is += k;
01500 else ++is;
01501 sum += s[is] * a[ia];
01502 ia += m;
01503 ++k;
01504 } while (k < n);
01505 iaa = i__ * m;
01506
01507 for (k = 1; k <= j; ++k) {
01508 ++iaa;
01509 ++ir;
01510 r__[ir] += sum * a[iaa];
01511 }
01512 }
01513 ++i__;
01514 } while (i__ < n);
01515
01516 return 0;
01517 }
01518
01519
01520 double *TCL::trchlu(const double *a, double *b, int n)
01521 {
01522
01523
01524
01525
01526
01527
01528
01529 int ipiv, kpiv, i__, j;
01530 double r__, dc;
01531 int id, kd;
01532 double sum;
01533
01534
01535
01536
01537
01538
01539
01540 --b; --a;
01541
01542
01543 ipiv = 0;
01544
01545 i__ = 0;
01546
01547 do {
01548 ++i__;
01549 ipiv += i__;
01550 kpiv = ipiv;
01551 r__ = a[ipiv];
01552
01553 for (j = i__; j <= n; ++j) {
01554 sum = 0.;
01555 if (i__ == 1) goto L40;
01556 if (r__ == 0.) goto L42;
01557 id = ipiv - i__ + 1;
01558 kd = kpiv - i__ + 1;
01559
01560 do {
01561 sum += b[kd] * b[id];
01562 ++kd; ++id;
01563 } while (id < ipiv);
01564
01565 L40:
01566 sum = a[kpiv] - sum;
01567 L42:
01568 if (j != i__) b[kpiv] = sum * r__;
01569 else {
01570 dc = TMath::Sqrt(sum);
01571 b[kpiv] = dc;
01572 if (r__ > 0.) r__ = (double)1. / dc;
01573 }
01574 kpiv += j;
01575 }
01576
01577 } while (i__ < n);
01578
01579 return 0;
01580 }
01581
01582
01583
01584 double *TCL::trchul(const double *a, double *b, int n)
01585 {
01586
01587
01588
01589
01590
01591
01592 int ipiv, kpiv, i__;
01593 double r__;
01594 int nTep;
01595 double dc;
01596 int id, kd;
01597 double sum;
01598
01599
01600
01601
01602
01603
01604
01605 --b; --a;
01606
01607
01608 kpiv = (n * n + n) / 2;
01609
01610 i__ = n;
01611 do {
01612 ipiv = kpiv;
01613 r__ = a[ipiv];
01614
01615 do {
01616 sum = 0.;
01617 if (i__ == n) goto L40;
01618 if (r__ == (double)0.) goto L42;
01619 id = ipiv;
01620 kd = kpiv;
01621 nTep = i__;
01622
01623 do {
01624 kd += nTep;
01625 id += nTep;
01626 ++nTep;
01627 sum += b[id] * b[kd];
01628 } while (nTep < n);
01629
01630 L40:
01631 sum = a[kpiv] - sum;
01632 L42:
01633 if (kpiv < ipiv) b[kpiv] = sum * r__;
01634 else {
01635 dc = TMath::Sqrt(sum);
01636 b[kpiv] = dc;
01637 if (r__ > (double)0.) r__ = (double)1. / dc;
01638 }
01639 --kpiv;
01640 } while (kpiv > ipiv - i__);
01641
01642 --i__;
01643 } while (i__ > 0);
01644
01645 return 0;
01646 }
01647
01648
01649 double *TCL::trinv(const double *t, double *s, int n)
01650 {
01651
01652
01653
01654
01655
01656
01657
01658
01659
01660 int lhor, ipiv, lver, j;
01661 double r__;
01662 int mx, ndTep, ind;
01663 double sum;
01664
01665
01666 --s; --t;
01667
01668
01669 mx = (n * n + n) / 2;
01670 ipiv = mx;
01671
01672 int i = n;
01673 do {
01674 r__ = 0.;
01675 if (t[ipiv] > 0.) r__ = (double)1. / t[ipiv];
01676 s[ipiv] = r__;
01677 ndTep = n;
01678 ind = mx - n + i;
01679
01680 while (ind != ipiv) {
01681 sum = 0.;
01682 if (r__ != 0.) {
01683 lhor = ipiv;
01684 lver = ind;
01685 j = i;
01686
01687 do {
01688 lhor += j;
01689 ++lver;
01690 sum += t[lhor] * s[lver];
01691 ++j;
01692 } while (lhor < ind);
01693 }
01694 s[ind] = -sum * r__;
01695 --ndTep;
01696 ind -= ndTep;
01697 }
01698
01699 ipiv -= i;
01700 --i;
01701 } while (i > 0);
01702
01703 return 0;
01704 }
01705
01706
01707 double *TCL::trla(const double *u, const double *a, double *b, int m, int n)
01708 {
01709
01710
01711
01712
01713
01714
01715
01716
01717
01718
01719 int ipiv, ia, ib, iu;
01720 double sum;
01721
01722
01723 --b; --a; --u;
01724
01725
01726 ib = m * n;
01727 ipiv = (m * m + m) / 2;
01728
01729 do {
01730 do {
01731 ia = ib;
01732 iu = ipiv;
01733
01734 sum = 0.;
01735 do {
01736 sum += a[ia] * u[iu];
01737 --iu;
01738 ia -= n;
01739 } while (ia > 0);
01740
01741 b[ib] = sum;
01742 --ib;
01743 } while (ia > 1 - n);
01744
01745 ipiv = iu;
01746 } while (iu > 0);
01747
01748 return 0;
01749 }
01750
01751
01752 double *TCL::trlta(const double *u, const double *a, double *b, int m, int n)
01753 {
01754
01755
01756
01757
01758
01759
01760
01761
01762
01763 int ipiv, mxpn, i__, nTep, ia, ib, iu, mx;
01764 double sum;
01765
01766
01767 --b; --a; --u;
01768
01769
01770 ipiv = 0;
01771 mx = m * n;
01772 mxpn = mx + n;
01773 ib = 0;
01774
01775 i__ = 0;
01776 do {
01777 ++i__;
01778 ipiv += i__;
01779
01780 do {
01781 iu = ipiv;
01782 nTep = i__;
01783 ++ib;
01784 ia = ib;
01785
01786 sum = 0.;
01787 do {
01788 sum += a[ia] * u[iu];
01789 ia += n;
01790 iu += nTep;
01791 ++nTep;
01792 } while (ia <= mx);
01793
01794 b[ib] = sum;
01795 } while (ia < mxpn);
01796
01797 } while (i__ < m);
01798
01799 return 0;
01800 }
01801
01802
01803 double *TCL::trpck(const double *s, double *u, int n)
01804 {
01805
01806
01807
01808
01809
01810
01811
01812
01813 int i__, ia, ind, ipiv;
01814
01815
01816 --u; --s;
01817
01818
01819 ia = 0;
01820 ind = 0;
01821 ipiv = 0;
01822
01823 for (i__ = 1; i__ <= n; ++i__) {
01824 ipiv += i__;
01825 do {
01826 ++ia;
01827 ++ind;
01828 u[ind] = s[ia];
01829 } while (ind < ipiv);
01830 ia = ia + n - i__;
01831 }
01832
01833 return 0;
01834 }
01835
01836
01837 double *TCL::trqsq(const double *q, const double *s, double *r__, int m)
01838 {
01839
01840
01841
01842
01843
01844
01845
01846
01847
01848 int indq, inds, imax, i__, j, k, l;
01849 int iq, ir, is, iqq;
01850 double sum;
01851
01852
01853 --r__; --s; --q;
01854
01855
01856 imax = (m * m + m) / 2;
01857 vzero(&r__[1], imax);
01858 inds = 0;
01859 i__ = 0;
01860
01861 do {
01862 inds += i__;
01863 ir = 0;
01864 indq = 0;
01865 j = 0;
01866
01867 do {
01868 indq += j;
01869 is = inds;
01870 iq = indq;
01871 sum = 0.;
01872 k = 0;
01873
01874 do {
01875 if (k > i__) is += k;
01876 else ++is;
01877
01878 if (k > j) iq += k;
01879 else ++iq;
01880
01881 sum += s[is] * q[iq];
01882 ++k;
01883 } while (k < m);
01884 iqq = inds;
01885 l = 0;
01886
01887 do {
01888 ++ir;
01889 if (l > i__) iqq += l;
01890 else ++iqq;
01891 r__[ir] += q[iqq] * sum;
01892 ++l;
01893 } while (l <= j);
01894 ++j;
01895 } while (j < m);
01896 ++i__;
01897 } while (i__ < m);
01898
01899 return 0;
01900 }
01901
01902
01903 double *TCL::trsa(const double *s, const double *a, double *b, int m, int n)
01904 {
01905
01906
01907
01908
01909
01910
01911
01912
01913
01914 int inds, i__, j, k, ia, ib, is;
01915 double sum;
01916
01917
01918 --b; --a; --s;
01919
01920
01921 inds = 0;
01922 ib = 0;
01923 i__ = 0;
01924
01925 do {
01926 inds += i__;
01927
01928 for (j = 1; j <= n; ++j) {
01929 ia = j;
01930 is = inds;
01931 sum = 0.;
01932 k = 0;
01933
01934 do {
01935 if (k > i__) is += k;
01936 else ++is;
01937 sum += s[is] * a[ia];
01938 ia += n;
01939 ++k;
01940 } while (k < m);
01941 ++ib;
01942 b[ib] = sum;
01943 }
01944 ++i__;
01945 } while (i__ < m);
01946
01947 return 0;
01948 }
01949
01950
01951 double *TCL::trsinv(const double *g, double *gi, int n)
01952 {
01953
01954
01955
01956
01957
01958
01959
01960
01961
01962
01963 trchlu(g, gi, n);
01964 trinv(gi, gi, n);
01965 trsmul(gi, gi, n);
01966
01967 return 0;
01968 }
01969
01970
01971 double *TCL::trsmlu(const double *u, double *s, int n)
01972 {
01973
01974
01975
01976
01977
01978
01979
01980
01981
01982
01983 int lhor, lver, i__, k, l, ind;
01984 double sum;
01985
01986
01987 --s; --u;
01988
01989
01990 ind = (n * n + n) / 2;
01991
01992 for (i__ = 1; i__ <= n; ++i__) {
01993 lver = ind;
01994
01995 for (k = i__; k <= n; ++k,--ind) {
01996 lhor = ind; sum = 0.;
01997 for (l = k; l <= n; ++l,--lver,--lhor)
01998 sum += u[lver] * u[lhor];
01999 s[ind] = sum;
02000 }
02001 }
02002
02003 return 0;
02004 }
02005
02006
02007 double *TCL::trsmul(const double *g, double *gi, int n)
02008 {
02009
02010
02011
02012
02013
02014
02015
02016
02017
02018
02019 int lhor, lver, lpiv, i__, j, k, ind;
02020 double sum;
02021
02022
02023 --gi; --g;
02024
02025
02026 ind = 1;
02027 lpiv = 0;
02028 for (i__ = 1; i__ <= n; ++i__) {
02029 lpiv += i__;
02030 for (j = 1; j <= i__; ++j,++ind) {
02031 lver = lpiv;
02032 lhor = ind;
02033 sum = 0.;
02034 for (k = i__; k <= n;lhor += k,lver += k,++k)
02035 sum += g[lver] * g[lhor];
02036 gi[ind] = sum;
02037 }
02038 }
02039
02040 return 0;
02041 }
02042
02043
02044 double *TCL::trupck(const double *u, double *s, int m)
02045 {
02046
02047
02048
02049
02050
02051
02052
02053
02054
02055
02056 int i__, im, is, iu, iv, ih, m2;
02057
02058
02059 --s; --u;
02060
02061
02062 m2 = m * m;
02063 is = m2;
02064 iu = (m2 + m) / 2;
02065 i__ = m - 1;
02066
02067 do {
02068 im = i__ * m;
02069 do {
02070 s[is] = u[iu];
02071 --is;
02072 --iu;
02073 } while (is > im);
02074 is = is - m + i__;
02075 --i__;
02076 } while (i__ >= 0);
02077
02078 is = 1;
02079 do {
02080 iv = is;
02081 ih = is;
02082 while (1) {
02083 iv += m;
02084 ++ih;
02085 if (iv > m2) break;
02086 s[ih] = s[iv];
02087 }
02088 is = is + m + 1;
02089 } while (is < m2);
02090
02091 return 0;
02092 }
02093
02094
02095 double *TCL::trsat(const double *s, const double *a, double *b, int m, int n)
02096 {
02097
02098
02099
02100
02101
02102
02103
02104
02105
02106
02107 int inds, i__, j, k, ia, ib, is;
02108 double sum;
02109
02110
02111 --b; --a; --s;
02112
02113
02114 inds = 0;
02115 ib = 0;
02116 i__ = 0;
02117
02118 do {
02119 inds += i__;
02120 ia = 0;
02121
02122 for (j = 1; j <= n; ++j) {
02123 is = inds;
02124 sum = 0.;
02125 k = 0;
02126
02127 do {
02128 if (k > i__) is += k;
02129 else ++is;
02130 ++ia;
02131 sum += s[is] * a[ia];
02132 ++k;
02133 } while (k < m);
02134 ++ib;
02135 b[ib] = sum;
02136 }
02137 ++i__;
02138 } while (i__ < m);
02139
02140 return 0;
02141 }
02142
02143
02144
02145
02146 float *TCL::trsequ(float *smx, int m, float *b, int n)
02147 {
02148
02149
02150
02151
02152
02153
02154
02155
02156
02157 float *mem = new float[(m*(m+1))/2+m];
02158 float *v = mem;
02159 float *s = v+m;
02160 if (!b) n=0;
02161 TCL::trpck (smx,s ,m);
02162 TCL::trsinv(s ,s, m);
02163
02164 for (int i=0;i<n;i++) {
02165 TCL::trsa (s ,b+i*m, v, m, 1);
02166 TCL::ucopy (v ,b+i*m, m);}
02167 TCL::trupck(s ,smx, m);
02168 delete [] mem;
02169 return b;
02170 }
02171
02172 double *TCL::trsequ(double *smx, int m, double *b, int n)
02173 {
02174
02175
02176
02177
02178
02179
02180
02181
02182
02183 double *mem = new double[(m*(m+1))/2+m];
02184 double *v = mem;
02185 double *s = v+m;
02186 if (!b) n=0;
02187 TCL::trpck (smx,s ,m);
02188 TCL::trsinv(s ,s, m);
02189
02190 for (int i=0;i<n;i++) {
02191 TCL::trsa (s ,b+i*m, v, m, 1);
02192 TCL::ucopy (v ,b+i*m, m);}
02193 TCL::trupck(s ,smx, m);
02194 delete [] mem;
02195 return b;
02196 }
02197