00001
00002
00003 #include "DenseMatrixArithmeticsNN.h"
00004
00005 DSS_NAMESPASE_BEGIN
00006
00007 long DenseMatrixArithmetics::zero_pivots = 0;
00008
00009 DenseMatrixArithmetics* DenseMatrixArithmetics::NewArithmetics(long block_size)
00010 {
00011 if (block_size == 1)
00012 return new DenseMatrixArithmetics1x1(1);
00013 if (block_size == 2)
00014 return new DenseMatrixArithmetics2x2(2);
00015 if (block_size == 3)
00016 return new DenseMatrixArithmetics3x3(3);
00017 if (block_size == 4)
00018 return new DenseMatrixArithmetics4x4(4);
00019 if (block_size == 5)
00020 return new DenseMatrixArithmetics5x5(5);
00021 if (block_size == 6)
00022 return new DenseMatrixArithmetics6x6(6);
00023 else
00024 return new DenseMatrixArithmetics(block_size);
00025 }
00026
00027
00028
00029 DenseMatrixArithmetics::DenseMatrixArithmetics(long bn)
00030 {
00031 this->bn = bn;
00032 bnbn = bn*bn;
00033 bn1 = bn+1;
00034 p = new double[bn];
00035 DenseMatrixArithmetics::zero_pivots = 0;
00036 prefered_decomposition = eLDL_decomposition;
00037 eMT = &MT;
00038 }
00039
00040 DenseMatrixArithmetics::~DenseMatrixArithmetics()
00041 {
00042 if (p) delete [] p; p = NULL;
00043 }
00044
00045
00046 void DenseMatrixArithmetics::AddMultBlockByVectorSym(double* B,double* x, double* b, long bi_bn, long bj_bn)
00047 {
00048 for (long i=0; i<bn; i++)
00049 for (long j=0; j<bn; j++)
00050 {
00051 double DataIJ = B[i+bn*j];
00052 b[bi_bn+i] += DataIJ*x[bj_bn+j];
00053 b[bj_bn+j] += DataIJ*x[bi_bn+i];
00054 }
00055 }
00056
00057
00058 void DenseMatrixArithmetics::SubMultBlockByVector(double* B,double* x, double* b)
00059 {
00060 for (long i=0; i<bn; i++)
00061 for (long j=0; j<bn; j++)
00062 b[i] -= B[i+bn*j]*x[j];
00063 }
00064
00065
00066 void DenseMatrixArithmetics::SubMultTBlockByVector(double* B,double* x, double* b)
00067 {
00068 for (long i=0; i<bn; i++)
00069 for (long j=0; j<bn; j++)
00070 b[i] -= B[j+bn*i]*x[j];
00071
00072 }
00073
00074
00075 void DenseMatrixArithmetics::MultDiagonalBlockByVector(double* B,double* x, double* b)
00076 {
00077 for (long i=0; i<bn; i++)
00078 {
00079 b[i] += B[i+bn*i]*x[i];
00080 for (long j=i+1; j<bn; j++)
00081 {
00082 double DataIJ = B[i+bn*j];
00083 b[i] += DataIJ*x[j];
00084 b[j] += DataIJ*x[i];
00085 }
00086 }
00087 }
00088
00089
00090
00091 void DenseMatrixArithmetics::SubATBproduct(double* pC,double* pA,double* pB)
00092 {
00093 long i,j,k;
00094 for (j=0; j<bn; j++,pA -= bnbn,pB+=bn)
00095 for (i=0; i<bn; i++,pC++,pB -= bn)
00096 {
00097 tmp=0;
00098 for (k=0; k<bn; k++,pA++,pB++)
00099 tmp += *pA * (*pB);
00100 *pC -= tmp;
00101 }
00102 }
00103
00104
00105
00106
00107
00108
00109
00110 void DenseMatrixArithmetics::Cholesky_Decomposition(double* pC,double* &p)
00111 {
00112 double* pi,*pj,*a = pC;
00113 long i,j,k,n = bn;
00114 double sum;
00115 if (p==NULL)
00116 p = new double[bn];
00117
00118 for (i=0;i<n;i++) {
00119 for (j=i;j<n;j++) {
00120
00121
00122 for (k=i-1,pi=a+i+k*n,pj=a+j+k*n,sum=a[i+n*j];k>=0;k--,pi-=n,pj-=n)
00123 sum -= *pi * *pj;
00124 if (i == j)
00125 {
00126 if (sum > 0.0)
00127 p[i]=sqrt(sum);
00128 else
00129 {
00130 p[i] = 1.0;
00131 DenseMatrixArithmetics::zero_pivots++;
00132 }
00133
00134 }
00135 else
00136 a[j+n*i]=sum/p[i];
00137 }
00138 }
00139 }
00140
00141
00142
00143
00144
00145
00146 void DenseMatrixArithmetics::Cholesky_Solve(double *pC,double* p,double* b,double* x)
00147 {
00148 double* a = pC;
00149 long i,k,n = bn;
00150 double sum;
00151
00152 for (i=0;i<n;i++)
00153 {
00154 for (sum=b[i],k=i-1;k>=0;k--) sum -= a[i+n*k]*x[k];
00155 x[i]=sum/p[i];
00156 }
00157 for (i=n-1;i>=0;i--)
00158 {
00159 for (sum=x[i],k=i+1;k<n;k++) sum -= a[k+n*i]*x[k];
00160 x[i]=sum/p[i];
00161 }
00162 }
00163
00164 void DenseMatrixArithmetics::Cholesky_Linv(double* pC,double* p)
00165 {
00166 double* a = pC;
00167 long i,j,k,n = bn;
00168 double sum;
00169 for (i=0;i<n;i++)
00170 {
00171 a[i+n*i]=1.0/p[i];
00172 for (j=i+1;j<n;j++) {
00173 sum=0.0;
00174 for (k=i;k<j;k++) sum -= a[j+n*k]*a[k+n*i];
00175 a[j+n*i]=sum/p[j];
00176 }
00177 }
00178 }
00179
00180 void DenseMatrixArithmetics::ComputeInversionByCholesky(double *C,double* Inv)
00181 {
00182 Cholesky_Decomposition(C,this->p);
00183 Cholesky_Linv(C,this->p);
00184
00185 double* a = C;
00186 long i,j,k,n = bn;
00187 double sum;
00188
00189 for (i=0;i<n;i++) {
00190 for (j=i;j<n;j++) {
00191 sum = 0;
00192 for (k=j; k<n; k++) sum += a[k+n*i]*a[k+n*j];
00193 Inv[j+n*i] = Inv[i+n*j] = sum;
00194 }
00195 }
00196 }
00197
00198
00199
00200
00201
00202
00203
00204 void DenseMatrixArithmetics::LL_Decomposition(double* pC)
00205 {
00206 double* pi,*pj,*a = pC;
00207 long i,j,k,n = bn;
00208 double sum;
00209
00210 for (i=0;i<n;i++) {
00211 for (j=i;j<n;j++) {
00212
00213
00214 for (k=i-1,pi=a+i+k*n,pj=a+j+k*n,sum=a[i+n*j];k>=0;k--,pi-=n,pj-=n)
00215 sum -= *pi * *pj;
00216 if (i == j)
00217 {
00218 if (sum > 0.0)
00219 a[bn*i]=sqrt(sum);
00220 else
00221 {
00222 this->MT.Write("Matrix is not positive definite.");
00223 a[bn*i] = 1.0;
00224 DenseMatrixArithmetics::zero_pivots++;
00225 }
00226 }
00227 else
00228 a[j+n*i]=sum/a[bn*i];
00229 }
00230 }
00231 }
00232
00233
00234
00235
00236
00237 void DenseMatrixArithmetics::LL_Solve(double *pC,double* b,double* x)
00238 {
00239 double* a = pC;
00240 long i,k,n = bn;
00241 double sum;
00242
00243 for (i=0;i<n;i++)
00244 {
00245 for (sum=b[i],k=i-1;k>=0;k--) sum -= a[i+n*k]*x[k];
00246 x[i]=sum/a[bn*i];
00247 }
00248 for (i=n-1;i>=0;i--)
00249 {
00250 for (sum=x[i],k=i+1;k<n;k++) sum -= a[k+n*i]*x[k];
00251 x[i]=sum/a[bn*i];
00252 }
00253 }
00254
00255
00256
00257
00258
00259 void DenseMatrixArithmetics::L_BlockSolve(double *C,double* B)
00260 {
00261 double* a = C;
00262 int i,j,n=bn,k;
00263 double sum;
00264 double *b = B;
00265
00266 for (j=0;j<n;j++)
00267 {
00268 for (i=0;i<n;i++)
00269 {
00270 for (sum=b[i],k=i-1;k>=0;k--) sum -= a[i+n*k]*b[k];
00271 b[i]=sum/a[bn*i];
00272 }
00273 b += n;
00274 }
00275 }
00276
00277 void DenseMatrixArithmetics::SubstSolveL(double *a,double* x)
00278 {
00279 long i,k,n = bn;
00280 double sum;
00281
00282 for (i=0;i<n;i++)
00283 {
00284 for (sum=x[i],k=i-1;k>=0;k--) sum -= a[i+n*k]*x[k];
00285 x[i]=sum/a[bn*i];
00286 }
00287 }
00288
00289
00290 void DenseMatrixArithmetics::SubstSolveLT(double *a,double* x)
00291 {
00292 long i,k,n = bn;
00293 double sum;
00294
00295 for (i=n-1;i>=0;i--)
00296 {
00297 for (sum=x[i],k=i+1;k<n;k++) sum -= a[k+n*i]*x[k];
00298 x[i]=sum/a[bn*i];
00299 }
00300 }
00301
00302
00303 void DenseMatrixArithmetics::LU_Decomposition(double* C)
00304 {
00305 double* a = C;
00306
00307
00308 long n = bn;
00309 double TINY = 1e-10;
00310 long i,j,k;
00311 double dum,sum;
00312
00313 for (j=0;j<n;j++)
00314 {
00315 for (i=0;i<j;i++)
00316 {
00317 sum=a[i+n*j];
00318 for (k=0;k<i;k++) sum -= a[i+n*k]*a[k+n*j];
00319 a[i+n*j]=sum;
00320 }
00321
00322 for (i=j;i<n;i++)
00323 {
00324 sum=a[i+n*j];
00325 for (k=0;k<j;k++)
00326 sum -= a[i+n*k]*a[k+n*j];
00327 a[i+n*j]=sum;
00328 }
00329 if (a[j+n*j] == 0.0)
00330 {
00331 a[j+n*j]=TINY;
00332 DenseMatrixArithmetics::zero_pivots++;
00333 }
00334 else
00335 a[j+n*j]=1.0/a[j+n*j];
00336
00337 if (j != n)
00338 {
00339 dum=a[j+n*j];
00340 for (i=j+1;i<n;i++) a[i+n*j] *= dum;
00341 }
00342 }
00343 }
00344
00345 void DenseMatrixArithmetics::LU_Solve(double* C,double* b)
00346 {
00347 double* a = C;
00348
00349 long i,ii=-1,j,n=bn;
00350 double sum;
00351 for (i=0;i<n;i++)
00352 {
00353
00354 sum=b[i];
00355 if (ii!=-1)
00356 for (j=ii;j<=i-1;j++) sum -= a[i+n*j]*b[j];
00357 else if (sum!=0.0) ii=i;
00358
00359 b[i]=sum;
00360 }
00361 for (i=n-1;i>=0;i--)
00362 {
00363 sum=b[i];
00364 for (j=i+1;j<n;j++) sum -= a[i+n*j]*b[j];
00365 b[i]=sum*a[i+n*i];
00366 }
00367 }
00368
00369
00370
00371 void DenseMatrixArithmetics::ULT_BlockSolve(double* C,double* B)
00372 {
00373
00374 double* a = C;
00375 int i,j,n=bn,k;
00376 double sum;
00377
00378 for (k=0;k<n;k++)
00379 {
00380 for (j=0;j<n;j++)
00381 {
00382 sum=B[k*n+j];
00383 for (i=0;i<j;i++) sum -= a[n*j+i]*B[k*n+i];
00384 B[k*n+j]=sum*a[j+n*j];
00385 }
00386 for (j=n-2;j>=0;j--)
00387 {
00388 sum=0;
00389 for (i=j+1;i<n;i++) sum += a[n*j+i]*B[k*n+i];
00390 B[k*n+j]-=sum;
00391 }
00392 }
00393 }
00394
00395
00396 void DenseMatrixArithmetics::LDL_Decomposition(double* A)
00397 {
00398 long i,j,k;
00399 double* Ai,*Aj,*Aii,*Ajj = A;
00400 double* A0j=A;
00401 long n=bn;
00402
00403 for (j=0; j<n; j++,Ajj += bn1,A0j+=n)
00404 {
00405
00406 for (i=1; i<j; i++)
00407 {
00408 double sum = 0;
00409 for (k=0,Ai=A+n*i,Aj=A0j; k<i; k++)
00410 sum += *(Ai++) * (*(Aj++));
00411 *Aj -= sum;
00412 }
00413
00414
00415 for (Aj=A0j,Aii=A; Aii<Ajj; Aj++,Aii+=bn1)
00416 {
00417 double A_ij = *Aj;
00418 *Aj *= *Aii;
00419 *Ajj -= (*Aj)*A_ij;
00420 }
00421
00422 if (fabs(*Ajj)<=eMT->min_pivot)
00423 {
00424 if (eMT->stabil_pivot==0.0)
00425 {
00426 eMT->act_row = eMT->act_block+j;
00427 if (!eMT->CallUnstableDialog())
00428 return;
00429 }
00430
00431 DenseMatrixArithmetics::zero_pivots++;
00432 *Ajj = eMT->stabil_pivot;
00433 }
00434 *Ajj = 1.0/(*Ajj);
00435 }
00436 }
00437
00438 void DenseMatrixArithmetics::LDL_Solve(double* A,double* b,const long startIndex)
00439 {
00440 long i,j,n=bn;
00441 double tmp;
00442
00443 for (j=startIndex; j<n-1; j++)
00444 {
00445 tmp = b[j];
00446 for (i=j+1;i<n;i++)
00447 b[i] -= A[j+i*n]*tmp;
00448 }
00449 for (j=startIndex; j<n; j++)
00450 b[j] *= A[j+j*n];
00451
00452 for (j=n-1; j>0; j--)
00453 {
00454 tmp = b[j];
00455 for (i=0;i<j;i++)
00456 b[i] -= A[i+j*n]*tmp;
00457 }
00458
00459 }
00460
00461
00462
00463 void DenseMatrixArithmetics::SubstSolveBlock(double* A,double* B)
00464 {
00465 double* b = B;
00466 for (int j=0; j<bn; j++,b+=bn)
00467 SubstSolve(A,b);
00468 }
00469
00470 void DenseMatrixArithmetics::FactorizeBlock(double* A)
00471 {
00472 switch (prefered_decomposition)
00473 {
00474 case eLDL_decomposition: LDL_Decomposition(A); break;
00475 case eLL_decomposition: LL_Decomposition(A); break;
00476 }
00477 }
00478
00479
00480 void DenseMatrixArithmetics::SubstSolve(double* A,double* b)
00481 {
00482 switch (prefered_decomposition)
00483 {
00484 case eLDL_decomposition: LDL_Solve(A,b); break;
00485 case eLL_decomposition: LL_Solve(A,b,b); break;
00486 }
00487 }
00488
00489 void DenseMatrixArithmetics::ComputeInversionByLDL(double *C,double* Inv)
00490 {
00491 long j;
00492 LDL_Decomposition(C);
00493 memset(Inv,0,bn*bn*sizeof(double));
00494 for(j=0;j<bn;j++,Inv+=bn)
00495 {
00496 Inv[j] = 1.0;
00497 LDL_Solve(C,Inv,0);
00498 }
00499 }
00500
00501 void DenseMatrixArithmetics::GetInversion(double* C,double* blockA)
00502 {
00503 ComputeInversionByLDL(C,blockA);
00504 }
00505
00506 bool DenseMatrixArithmetics::GaussElimination(double* C,double* blockA)
00507 {
00508 *blockA = 1/ (*C);
00509 return true;
00510
00511 }
00512
00513
00514
00515 void DenseMatrixArithmetics1x1::SubATBproduct(double* pC,double* pA,double* pB)
00516 {
00517 *pC -= (*pA)*(*pB);
00518 }
00519
00520 void DenseMatrixArithmetics1x1::GetInversion(double* pC,double* pA)
00521 {
00522 *pA = 1.0/(*pC);
00523 }
00524
00525 void DenseMatrixArithmetics1x1::FactorizeBlock(double* A)
00526 {
00527 if (*A==0.0) {
00528 DenseMatrixArithmetics::zero_pivots++; *A = 1.0;
00529 } else {
00530 *A = 1.0/(*A);
00531 }
00532 }
00533 void DenseMatrixArithmetics1x1::SubstSolveBlock(double* A,double* B)
00534 {
00535 *B *= *A;
00536 }
00537 void DenseMatrixArithmetics1x1::SubstSolve(double* A,double* b)
00538 {
00539 *b *= *A;
00540 }
00541
00542
00543
00544 void DenseMatrixArithmetics2x2::SubATBproduct(double* pC,double* pa,double* pb)
00545 {
00546 pC[0] -= pa[0]*pb[0] + pa[1]*pb[1];
00547 pC[1] -= pa[2]*pb[0] + pa[3]*pb[1];
00548 pC[2] -= pa[0]*pb[2] + pa[1]*pb[3];
00549 pC[3] -= pa[2]*pb[2] + pa[3]*pb[3];
00550 }
00551
00552 void DenseMatrixArithmetics2x2::GetInversion(double* C,double* pA)
00553 {
00554 double D = C[0]*C[3]-C[1]*C[1];
00555 pA[0] = C[3]/D;
00556 pA[1] = pA[2] = -C[1]/D;
00557 pA[3] = C[0]/D;
00558 }
00559
00560
00561
00562 void DenseMatrixArithmetics3x3::SubATBproduct(double* pC,double* pa,double* pb)
00563 {
00564 pC[0] -= pa[0]*pb[0] + pa[1]*pb[1] + pa[2]*pb[2];
00565 pC[1] -= pa[3]*pb[0] + pa[4]*pb[1] + pa[5]*pb[2];
00566 pC[2] -= pa[6]*pb[0] + pa[7]*pb[1] + pa[8]*pb[2];
00567
00568 pC[3] -= pa[0]*pb[3] + pa[1]*pb[4] + pa[2]*pb[5];
00569 pC[4] -= pa[3]*pb[3] + pa[4]*pb[4] + pa[5]*pb[5];
00570 pC[5] -= pa[6]*pb[3] + pa[7]*pb[4] + pa[8]*pb[5];
00571
00572 pC[6] -= pa[0]*pb[6] + pa[1]*pb[7] + pa[2]*pb[8];
00573 pC[7] -= pa[3]*pb[6] + pa[4]*pb[7] + pa[5]*pb[8];
00574 pC[8] -= pa[6]*pb[6] + pa[7]*pb[7] + pa[8]*pb[8];
00575 }
00576
00577
00578
00579 void DenseMatrixArithmetics4x4::SubATBproduct(double* pC,double* pa,double* pb)
00580 {
00581 pC[0] -= pa[ 0]*pb[ 0] + pa[ 1]*pb[ 1] + pa[ 2]*pb[ 2] + pa[ 3]*pb[ 3];
00582 pC[1] -= pa[ 4]*pb[ 0] + pa[ 5]*pb[ 1] + pa[ 6]*pb[ 2] + pa[ 7]*pb[ 3];
00583 pC[2] -= pa[ 8]*pb[ 0] + pa[ 9]*pb[ 1] + pa[10]*pb[ 2] + pa[11]*pb[ 3];
00584 pC[3] -= pa[12]*pb[ 0] + pa[13]*pb[ 1] + pa[14]*pb[ 2] + pa[15]*pb[ 3];
00585
00586 pC[4] -= pa[ 0]*pb[ 4] + pa[ 1]*pb[ 5] + pa[ 2]*pb[ 6] + pa[ 3]*pb[ 7];
00587 pC[5] -= pa[ 4]*pb[ 4] + pa[ 5]*pb[ 5] + pa[ 6]*pb[ 6] + pa[ 7]*pb[ 7];
00588 pC[6] -= pa[ 8]*pb[ 4] + pa[ 9]*pb[ 5] + pa[10]*pb[ 6] + pa[11]*pb[ 7];
00589 pC[7] -= pa[12]*pb[ 4] + pa[13]*pb[ 5] + pa[14]*pb[ 6] + pa[15]*pb[ 7];
00590
00591 pC[8] -= pa[ 0]*pb[ 8] + pa[ 1]*pb[ 9] + pa[ 2]*pb[10] + pa[ 3]*pb[11];
00592 pC[9] -= pa[ 4]*pb[ 8] + pa[ 5]*pb[ 9] + pa[ 6]*pb[10] + pa[ 7]*pb[11];
00593 pC[10] -= pa[ 8]*pb[ 8] + pa[ 9]*pb[ 9] + pa[10]*pb[10] + pa[11]*pb[11];
00594 pC[11] -= pa[12]*pb[ 8] + pa[13]*pb[ 9] + pa[14]*pb[10] + pa[15]*pb[11];
00595
00596 pC[12] -= pa[ 0]*pb[12] + pa[ 1]*pb[13] + pa[ 2]*pb[14] + pa[ 3]*pb[15];
00597 pC[13] -= pa[ 4]*pb[12] + pa[ 5]*pb[13] + pa[ 6]*pb[14] + pa[ 7]*pb[15];
00598 pC[14] -= pa[ 8]*pb[12] + pa[ 9]*pb[13] + pa[10]*pb[14] + pa[11]*pb[15];
00599 pC[15] -= pa[12]*pb[12] + pa[13]*pb[13] + pa[14]*pb[14] + pa[15]*pb[15];
00600 }
00601
00602
00603
00604 void DenseMatrixArithmetics5x5::SubATBproduct(double* pC,double* pA,double* pB)
00605 {
00606 long i,j;
00607 for (j=0; j<bn; j++,pB += bn,pA-=bnbn)
00608 for (i=0; i<bn; i++,pC++,pA += bn)
00609 *pC -= pA[0]*pB[0]+pA[1]*pB[1]+pA[2]*pB[2]+pA[3]*pB[3]+pA[4]*pB[4];
00610 }
00611
00612
00613
00614 void DenseMatrixArithmetics6x6::SubATBproduct(double* pC,double* pA,double* pB)
00615 {
00616 long i,j;
00617 for (j=0; j<bn; j++,pB += bn,pA-=bnbn)
00618 for (i=0; i<bn; i++,pC++,pA += bn)
00619 *pC -= pA[0]*pB[0]+pA[1]*pB[1]+pA[2]*pB[2]+pA[3]*pB[3]+pA[4]*pB[4]+pA[5]*pB[5];
00620 }
00621
00622
00623
00624 void DenseMatrixArithmetics_Fake::SubATBproduct(double*C,double* blockA,double* blockB)
00625 {
00626 *C = *blockA * *blockB;
00627
00628 }
00629
00630 DSS_NAMESPASE_END
00631