00001 #include <stdlib.h>
00002
00003 long max(long i,long j)
00004 {
00005 if (i>j) return i;
00006 else return j;
00007 }
00008
00009
00010 long min(long i,long j)
00011 {
00012 if (j>i) return i;
00013 else return j;
00014 }
00015
00016 long bandwidth (long *adr,long n)
00017
00018
00019
00020
00021
00022
00023
00024
00025 {
00026 long i,j,k;
00027
00028 j=0;
00029 for (i=0;i<n;i++){
00030 k=adr[i+1]-adr[i];
00031 if (k>j) j=k;
00032 }
00033 return j;
00034 }
00035
00036 void eliminuj_4i_rev(double *a,long n,long s)
00037 {
00038 double sum1,sum2,sum3,sum4,x;
00039 long i,j,k;
00040
00041 for(j=0;j<n;j++)
00042 {
00043 sum1=sum2=0.0;
00044 for(k=0;k<(j-1);k+=2)
00045 {
00046 sum1+=(a[j*s+k]*a[j*s+k]);
00047 sum2+=(a[j*s+k+1]*a[j*s+k+1]);
00048 }
00049 if (j&1) sum1+=(a[j*s+j-1]*a[j*s+j-1]);
00050
00051 a[j*s+j]=sqrt(a[j*s+j]-sum1-sum2);
00052 if(j&1)
00053 {
00054 for(i=j+1;i<(n-3);i+=4)
00055 {
00056 sum1=sum2=sum3=sum4=0.0;
00057 for(k=0;k<j;k++)
00058 {
00059 x=a[j*s+k];
00060 sum1+=a[i*s+k]*x;
00061 sum2+=a[(i+1)*s+k]*x;
00062 sum3+=a[(i+2)*s+k]*x;
00063 sum4+=a[(i+3)*s+k]*x;
00064 }
00065 x=a[j*s+j];
00066 a[i*s+j]=(a[i*s+j]-sum1)/x;
00067 a[(i+1)*s+j]=(a[(i+1)*s+j]-sum2)/x;
00068 a[(i+2)*s+j]=(a[(i+2)*s+j]-sum3)/x;
00069 a[(i+3)*s+j]=(a[(i+3)*s+j]-sum4)/x;
00070 }
00071 for(;i<n;i++)
00072 {
00073 sum1=0.0;
00074 for(k=0;k<j;k++)
00075 {
00076 sum1+=a[i*s+k]*a[j*s+k];
00077 }
00078 a[i*s+j]=(a[i*s+j]-sum1)/a[j*s+j];
00079 }
00080 }
00081 else
00082 {
00083 for(i=n-1;i>(j+3);i-=4)
00084 {
00085 sum1=sum2=sum3=sum4=0.0;
00086 for(k=0;k<j;k++)
00087 {
00088 x=a[j*s+k];
00089 sum1+=a[i*s+k]*x;
00090 sum2+=a[(i-1)*s+k]*x;
00091 sum3+=a[(i-2)*s+k]*x;
00092 sum4+=a[(i-3)*s+k]*x;
00093 }
00094 x=a[j*s+j];
00095 a[i*s+j]=(a[i*s+j]-sum1)/x;
00096 a[(i-1)*s+j]=(a[(i-1)*s+j]-sum2)/x;
00097 a[(i-2)*s+j]=(a[(i-2)*s+j]-sum3)/x;
00098 a[(i-3)*s+j]=(a[(i-3)*s+j]-sum4)/x;
00099 }
00100 for(;i>j;i--)
00101 {
00102 sum1=0.0;
00103 for(k=0;k<j;k++)
00104 {
00105 sum1+=a[i*s+k]*a[j*s+k];
00106 }
00107 a[i*s+j]=(a[i*s+j]-sum1)/a[j*s+j];
00108 }
00109 }
00110 }
00111 }
00112
00113
00114 void faze1_opt_sp(double *a,double *b,long n,long s,long n2)
00115 {
00116 long i,j,k;
00117 double *sumy,x,sum1,sum2,sum3,sum4;
00118 sumy = new double [n];
00119 for(i=0;i<n;i++)
00120 {
00121 for(j=0;j<(n-3);j+=4)
00122 {
00123 sum1=sum2=sum3=sum4=0.0;
00124 for(k=0;k<j;k++)
00125 {
00126 x=sumy[k];
00127 sum1+=x*a[j*s+k];
00128 sum2+=x*a[(j+1)*s+k];
00129 sum3+=x*a[(j+2)*s+k];
00130 sum4+=x*a[(j+3)*s+k];
00131 }
00132 x=b[i*s+j]-sum1;
00133 x=sumy[j]=x/a[j*s+j];
00134
00135 sum2+=x*a[(j+1)*s+j];
00136 sum3+=x*a[(j+2)*s+j];
00137 sum4+=x*a[(j+3)*s+j];
00138 x=b[i*s+j+1]-sum2;
00139 x=sumy[j+1]=x/a[(j+1)*s+(j+1)];
00140
00141 sum3+=x*a[(j+2)*s+j+1];
00142 sum4+=x*a[(j+3)*s+j+1];
00143 x=b[i*s+j+2]-sum3;
00144 sumy[j+2]=x/a[(j+2)*s+(j+2)];
00145
00146
00147 sum4+=sumy[j+2]*a[(j+3)*s+j+2];
00148 x=b[i*s+j+3]-sum4;
00149 sumy[j+3]=x/a[(j+3)*s+(j+3)];
00150 }
00151 for(;j<n;j++)
00152 {
00153 sum1=sum2=0.0;
00154 for(k=0;k<(j-1);k+=2)
00155 {
00156 sum1+=sumy[k]*a[j*s+k];
00157 sum2+=sumy[k+1]*a[j*s+k+1];
00158 }
00159 if (j&1) sum1+=sumy[j-1]*a[j*s+j-1];
00160 sumy[j]=(b[i*s+j]-sum1-sum2)/a[j*s+j];
00161 }
00162 for(k=0;k<n;k++) b[i*s+k]=sumy[k];
00163 }
00164 delete [] sumy;
00165 }
00166
00167
00168 void faze2_rev_sp(double *a,double *b,long n,long s,long n2)
00169 {
00170 double sum,x,sum1,sum2,sum3,sum4;
00171 long i,j,k;
00172 for(i=0;i<n2;i++)
00173 {
00174 if (i&1)
00175 {
00176 for(j=0;j<=(i-3);j+=4)
00177 {
00178 sum1=sum2=sum3=sum4=0.0;
00179 for(k=0;k<n;k++)
00180 {
00181
00182 x=b[i*s+k];
00183 sum1+=x*b[j*s+k];
00184 sum2+=x*b[(j+1)*s+k];
00185 sum3+=x*b[(j+2)*s+k];
00186 sum4+=x*b[(j+3)*s+k];
00187 }
00188 a[i*s+j]-=sum1;
00189 a[i*s+j+1]-=sum2;
00190 a[i*s+j+2]-=sum3;
00191 a[i*s+j+3]-=sum4;
00192 }
00193 for(;j<=i;j++)
00194 {
00195 sum=0.0;
00196 for(k=0;k<n;k++)
00197 {
00198
00199 sum+=b[i*s+k]*b[j*s+k];
00200 }
00201 a[i*s+j]-=sum;
00202 }
00203 }
00204 else
00205 {
00206 for(j=i;j>=3;j-=4)
00207 {
00208 sum1=sum2=sum3=sum4=0.0;
00209 for(k=0;k<n;k++)
00210 {
00211
00212 x=b[i*s+k];
00213 sum1+=x*b[j*s+k];
00214 sum2+=x*b[(j-1)*s+k];
00215 sum3+=x*b[(j-2)*s+k];
00216 sum4+=x*b[(j-3)*s+k];
00217 }
00218 a[i*s+j]-=sum1;
00219 a[i*s+j-1]-=sum2;
00220 a[i*s+j-2]-=sum3;
00221 a[i*s+j-3]-=sum4;
00222 }
00223 for(;j>=0;j--)
00224 {
00225 sum=0.0;
00226 for(k=0;k<n;k++)
00227 {
00228
00229 sum+=b[i*s+k]*b[j*s+k];
00230 }
00231 a[i*s+j]-=sum;
00232 }
00233 }
00234 }
00235 }
00236
00237
00238 void faze1(double *a,double *b,long n,long s,long n2)
00239 {
00240 long i,j,k;
00241 double sum1;
00242 for(i=0;i<n;i++)
00243 {
00244 for(j=0;j<n;j++)
00245 {
00246 sum1=0.0;
00247 for(k=0;k<j;k++) sum1+=b[i*s+k]*a[j*s+k];
00248 b[i*s+j]=(b[i*s+j]-sum1)/a[j*s+j];
00249 }
00250 }
00251 }
00252
00253
00254 void faze2(double *a,double *b,long n,long s,long n2)
00255 {
00256 double sum1;
00257 long i,j,k;
00258 for(i=0;i<n2;i++)
00259 {
00260 for(j=0;j<=i;j++)
00261 {
00262 sum1=0.0;
00263 for(k=0;k<n;k++) sum1+=b[i*s+k]*b[j*s+k];
00264 a[i*s+j]-=sum1;
00265 }
00266 }
00267 }
00268
00269
00270 void faze1_block(double *a,double *b,long n,long n2,long block)
00271 {
00272 long i,j,j2,step1,k;
00273 double *sumy,x,sum1,sum2,sum3,sum4;
00274 sumy=new double [n];
00275 step1=(n+block-1)/block;
00276 for(j2=0;j2<n;j2+=step1)
00277 {
00278
00279 for(i=0;i<n2;i++)
00280 {
00281 if (j2)
00282 {
00283 for(k=0;k<j2;k++) sumy[k]=b[i*n+k];
00284 }
00285 for(j=j2;j<min(n,j2+step1)-3;j+=4)
00286 {
00287 sum1=sum2=sum3=sum4=0.0;
00288 for(k=0;k<j;k++)
00289 {
00290 x=sumy[k];
00291 sum1+=x*a[j*n+k];
00292 sum2+=x*a[(j+1)*n+k];
00293 sum3+=x*a[(j+2)*n+k];
00294 sum4+=x*a[(j+3)*n+k];
00295 }
00296 x=b[i*n+j]-sum1;
00297 x=sumy[j]=x/a[j*n+j];
00298
00299 sum2+=x*a[(j+1)*n+j];
00300 sum3+=x*a[(j+2)*n+j];
00301 sum4+=x*a[(j+3)*n+j];
00302 x=b[i*n+j+1]-sum2;
00303 x=sumy[j+1]=x/a[(j+1)*n+(j+1)];
00304
00305 sum3+=x*a[(j+2)*n+j+1];
00306 sum4+=x*a[(j+3)*n+j+1];
00307 x=b[i*n+j+2]-sum3;
00308 sumy[j+2]=x/a[(j+2)*n+(j+2)];
00309
00310 sum4+=sumy[j+2]*a[(j+3)*n+j+2];
00311 x=b[i*n+j+3]-sum4;
00312 sumy[j+3]=x/a[(j+3)*n+(j+3)];
00313 }
00314 for(;j<min(n,j2+step1);j++)
00315 {
00316 sum1=sum2=0.0;
00317 for(k=0;k<(j-1);k+=2)
00318 {
00319 sum1+=sumy[k]*a[j*n+k];
00320 sum2+=sumy[k+1]*a[j*n+k+1];
00321 }
00322 if (j&1) sum1+=sumy[j-1]*a[j*n+j-1];
00323 sumy[j]=(b[i*n+j]-sum1-sum2)/a[j*n+j];
00324 }
00325 for(k=j2;k<min(n,j2+step1);k++) b[i*n+k]=sumy[k];
00326 }
00327 }
00328 delete [] sumy;
00329 }
00330
00331
00332 void faze2_block(double *a,double *b,long n,long n2,long block)
00333 {
00334 double sum,x,sum1,sum2,sum3,sum4;
00335 long i,j,j2,k,step2;
00336
00337 step2=(n+block-1)/block;
00338 for(j2=0;j2<n;j2+=step2)
00339 {
00340 for(i=0;i<n2;i++)
00341 {
00342 for(j=j2;j<=min((i-3),j2+step2-1);j+=4)
00343 {
00344 sum1=sum2=sum3=sum4=0.0;
00345 for(k=0;k<n;k++)
00346 {
00347
00348 x=b[i*n+k];
00349 sum1+=x*b[j*n+k];
00350 sum2+=x*b[(j+1)*n+k];
00351 sum3+=x*b[(j+2)*n+k];
00352 sum4+=x*b[(j+3)*n+k];
00353 }
00354 a[i*n+j]-=sum1;
00355 a[i*n+j+1]-=sum2;
00356 a[i*n+j+2]-=sum3;
00357 a[i*n+j+3]-=sum4;
00358 }
00359 for(;j<=min(i,j2+step2-1);j++)
00360 {
00361 sum=0.0;
00362 for(k=0;k<n;k++)
00363 {
00364
00365 sum+=b[i*n+k]*b[j*n+k];
00366 }
00367 a[i*n+j]-=sum;
00368 }
00369 }
00370 }
00371 }
00372
00373
00374 void napln_a(long i1,long i2,long band,double *pole,double *a,long *adr)
00375 {
00376 long i,j,a1,a2,kolik;
00377 double *x;
00378 for(i=i1;i<i2;i++)
00379 {
00380 a1=adr[i];
00381 a2=adr[i+1];
00382 kolik=min(a2-a1,i+1-i1);
00383 x=pole+a1;
00384 for(j=0;j<kolik;j++) {a[(i-i1)*band+(i-i1)-j]=*x; x++; }
00385 for(j=kolik;j<=(i-i1);j++) a[(i-i1)*band+(i-i1)-j]=0.0;
00386 }
00387
00388 }
00389
00390
00391 void uloz_a(long i1,long i2,long band,double *pole,double *a,long *adr)
00392 {
00393 long i,j,a1,a2,kolik;
00394 double *x;
00395 for(i=i1;i<i2;i++)
00396 {
00397 a1=adr[i];
00398 a2=adr[i+1];
00399 kolik=min(a2-a1,i+1-i1);
00400 x=pole+adr[i];
00401 for(j=0;j<kolik;j++) { *x=a[(i-i1)*band+(i-i1)-j]; x++; }
00402 }
00403
00404 }
00405
00406
00407 void napln_b(long i1,long i2,long band,double *pole,double *b,long *adr)
00408 {
00409 long i,j,a1,a2,kolik;
00410 double *x;
00411 for(i=i1;i<i2;i++)
00412 {
00413 a1=adr[i];
00414 a2=adr[i+1];
00415 kolik=i+1-i1;
00416 x=pole+adr[i]+kolik;
00417 for(j=0;j<((a2-a1)-kolik);j++) { b[(i-i1)*band+(band-1)-j]=*x; x++; }
00418 for(j=((a2-a1)-kolik);j<band;j++) b[(i-i1)*band+(band-1)-j]=0.0;
00419 }
00420
00421 }
00422
00423
00424 void uloz_b(long i1,long i2,long band,double *pole,double *b,long *adr)
00425 {
00426 long i,j,a1,a2,kolik;
00427 double *x;
00428 for(i=i1;i<i2;i++)
00429 {
00430 a1=adr[i];
00431 a2=adr[i+1];
00432
00433 kolik=i+1-i1;
00434 x=pole+adr[i]+kolik;
00435 for(j=0;j<((a2-a1)-kolik);j++) { *x=b[(i-i1)*band+(band-1)-j]; x++; }
00436 }
00437
00438 }
00439
00440
00441 void blokove(double *pole, long *adr, long n, long band)
00442 {
00443 long i,i1,i2;
00444 double *a,*b;
00445
00446 printf("Band = %li \n",band);
00447 a=new double [band*band];
00448 b=new double [band*band];
00449
00450 i=0;
00451
00452 i1=i2=band;
00453 napln_a(i,i1,band,pole,a,adr);
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465 eliminuj_4i_rev(a,band,band);
00466
00467
00468
00469
00470
00471
00472
00473
00474 while(i1<n)
00475 {
00476 i2=min(i1+band,n);
00477
00478
00479
00480 napln_b(i1,i2,i1-i,pole,b,adr);
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491 faze1_block(a,b,i1-i,i2-i1,4);
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502 uloz_a(i,i1,i1-i,pole,a,adr);
00503
00504
00505 napln_a(i1,i2,i2-i1,pole,a,adr);
00506
00507
00508
00509
00510
00511
00512
00513
00514
00515
00516 faze2(a,b,i1-i,i2-i1,4);
00517
00518
00519
00520
00521
00522
00523
00524
00525
00526 uloz_b(i1,i2,i1-i,pole,b,adr);
00527
00528 eliminuj_4i_rev(a,i2-i1,i2-i1);
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538 i=i1;
00539 i1=i2;
00540 }
00541
00542 uloz_a(i,n,i1-i,pole,a,adr);
00543
00544 delete [] a;
00545 delete [] b;
00546 }
00547
00548
00549 void ldl_sky2 (double *a,double *x,double *y,long *adr,long n,long tc)
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571 {
00572 long i,k,ac,ac1,ack,ack1;
00573 double s,g;
00574
00575 if (tc==1 || tc==2)
00576 {
00577 i=bandwidth(adr,n);
00578 blokove(a,adr,n,i);
00579 }
00580 if (tc==1 || tc==3){
00581
00582
00583
00584
00585 for (k=1;k<n;k++){
00586
00587 ack=adr[k]+1; ack1=adr[k+1];
00588 ac1=k-1; s=0.0;
00589 for (i=ack;i<ack1;i++){
00590 s+=a[i]*y[ac1];
00591 ac1--;
00592 }
00593 y[k]-=s;
00594 }
00595
00596 for (k=0;k<n;k++){
00597 ac=adr[k]; y[k]/=a[ac];
00598 }
00599
00600 for (k=n-1;k>-1;k--){
00601
00602 ack=adr[k]+1; ack1=adr[k+1];
00603 x[k]=y[k]; g=x[k];
00604 ac=k-1;
00605 for (i=ack;i<ack1;i++){
00606 y[ac]-=a[i]*g;
00607 ac--;
00608 }
00609 }
00610 }
00611 }
00612