DSDP
|
00001 #include "numchol.h" 00002 void dCopy(int,double*,double*); 00003 00004 static void SolFwdSnode(chfac *sf, 00005 int snde, 00006 int f, 00007 int l, 00008 double x[]) 00009 { 00010 int i,t,sze,*ls,*subg=sf->subg, 00011 *ujbeg=sf->ujbeg,*uhead=sf->uhead, 00012 *usub=sf->usub; 00013 double xi,*l1,*diag=sf->diag,*uval=sf->uval; 00014 00015 f += subg[snde]; 00016 l += subg[snde]; 00017 00018 for(i=f; i<l; ++i) 00019 { 00020 x[i] /= diag[i]; 00021 xi = x[i]; 00022 00023 ls = usub+ujbeg[i]; 00024 l1 = uval+uhead[i]; 00025 sze = l-i-1; 00026 00027 for(t=0; t<sze; ++t) 00028 x[ls[t]] -= l1[t]*xi; 00029 } 00030 } /* SolFwdSnode */ 00031 00032 static void SolBward(int nrow, 00033 double diag[], 00034 double uval[], 00035 int fir[], 00036 double x[]) 00037 { 00038 int i,t,sze; 00039 double x1,x2,rtemp, 00040 *x0,*l1,*l2; 00041 00042 for(i=nrow; i;) { 00043 for(; i>1; --i) { 00044 -- i; 00045 l1 = uval+fir[i-1]+1; 00046 l2 = uval+fir[i ]+0; 00047 sze = nrow-i-1; 00048 x1 = 0.0; 00049 x2 = 0.0; 00050 x0 = x+1+i; 00051 00052 for(t=0; t<sze; ++t) 00053 { 00054 rtemp = x0[t]; 00055 00056 x1 += l1[t]*rtemp; 00057 x2 += l2[t]*rtemp; 00058 } 00059 00060 x[i] -= x2/diag[i]; 00061 x[i-1] -= (uval[fir[i-1]]*x[i]+x1)/diag[i-1]; 00062 } 00063 00064 for(; i;) { 00065 -- i; 00066 l1 = uval+fir[i]; 00067 sze = nrow-i-1; 00068 x1 = 0.0; 00069 x0 = x+1+i; 00070 00071 for(t=0; t<sze; ++t) 00072 x1 += l1[t]*x0[t]; 00073 00074 x[i] -= x1/diag[i]; 00075 } 00076 } 00077 } /* SolBward */ 00078 00079 void ChlSolveForwardPrivate(chfac *sf, 00080 double x[]) 00081 { 00082 int k,s,t,sze,f,l,itemp,*ls, 00083 *subg=sf->subg,*ujsze=sf->ujsze,*usub=sf->usub, 00084 *ujbeg=sf->ujbeg,*uhead=sf->uhead; 00085 double rtemp1,rtemp2,rtemp3,rtemp4, 00086 rtemp5,rtemp6,rtemp7,rtemp8, 00087 *l1,*l3,*l2,*l4,*l5,*l6,*l7,*l8, 00088 *uval=sf->uval; 00089 00090 for(s=0; s<sf->nsnds; ++s) { 00091 f = subg[s]; 00092 l = subg[s+1]; 00093 00094 SolFwdSnode(sf,s,0,l-f,x); 00095 00096 itemp = l-f-1; 00097 ls = usub+ujbeg[f]+itemp; 00098 sze = ujsze[f]-itemp; 00099 k = f; 00100 00101 itemp = l-1; 00102 for(; k+7<l; k+=8) { 00103 l1 = uval+uhead[k+0]+itemp-(k+0); 00104 l2 = uval+uhead[k+1]+itemp-(k+1); 00105 l3 = uval+uhead[k+2]+itemp-(k+2); 00106 l4 = uval+uhead[k+3]+itemp-(k+3); 00107 l5 = uval+uhead[k+4]+itemp-(k+4); 00108 l6 = uval+uhead[k+5]+itemp-(k+5); 00109 l7 = uval+uhead[k+6]+itemp-(k+6); 00110 l8 = uval+uhead[k+7]+itemp-(k+7); 00111 00112 rtemp1 = x[k+0]; 00113 rtemp2 = x[k+1]; 00114 rtemp3 = x[k+2]; 00115 rtemp4 = x[k+3]; 00116 rtemp5 = x[k+4]; 00117 rtemp6 = x[k+5]; 00118 rtemp7 = x[k+6]; 00119 rtemp8 = x[k+7]; 00120 00121 for(t=0; t<sze; ++t) 00122 x[ls[t]] -= rtemp1*l1[t] 00123 + rtemp2*l2[t] 00124 + rtemp3*l3[t] 00125 + rtemp4*l4[t] 00126 + rtemp5*l5[t] 00127 + rtemp6*l6[t] 00128 + rtemp7*l7[t] 00129 + rtemp8*l8[t]; 00130 } 00131 00132 for(; k+3<l; k+=4) { 00133 l1 = uval+uhead[k+0]+itemp-(k+0); 00134 l2 = uval+uhead[k+1]+itemp-(k+1); 00135 l3 = uval+uhead[k+2]+itemp-(k+2); 00136 l4 = uval+uhead[k+3]+itemp-(k+3); 00137 00138 rtemp1 = x[k+0]; 00139 rtemp2 = x[k+1]; 00140 rtemp3 = x[k+2]; 00141 rtemp4 = x[k+3]; 00142 00143 for(t=0; t<sze; ++t) 00144 x[ls[t]] -= rtemp1*l1[t] 00145 + rtemp2*l2[t] 00146 + rtemp3*l3[t] 00147 + rtemp4*l4[t]; 00148 } 00149 00150 for(; k+1<l; k+=2) { 00151 l1 = uval+uhead[k+0]+itemp-(k+0); 00152 l2 = uval+uhead[k+1]+itemp-(k+1); 00153 00154 rtemp1 = x[k+0]; 00155 rtemp2 = x[k+1]; 00156 00157 for(t=0; t<sze; ++t) 00158 x[ls[t]] -= rtemp1*l1[t] 00159 + rtemp2*l2[t]; 00160 } 00161 00162 00163 for(; k<l; ++k) { 00164 l1 = uval+uhead[k+0]+itemp-(k+0); 00165 00166 rtemp1 = x[k+0]; 00167 00168 for(t=0; t<sze; ++t) 00169 x[ls[t]] -= rtemp1*l1[t]; 00170 } 00171 } 00172 00173 } 00174 00175 void ChlSolveBackwardPrivate(chfac *sf, 00176 double x[], 00177 double b[]) 00178 { 00179 /* Note: x: input, or left hand side b: output, or solution */ 00180 00181 int i,s,t,sze,f,l,*ls, 00182 *subg=sf->subg,*ujsze=sf->ujsze,*usub=sf->usub, 00183 *ujbeg=sf->ujbeg,*uhead=sf->uhead; 00184 double x1,x2,*l1,*l2,rtemp1, 00185 *diag=sf->diag,*uval=sf->uval; 00186 00187 00188 if (sf->nsnds) { 00189 s = sf->nsnds - 1; 00190 f = subg[s]; 00191 l = subg[s+1]; 00192 00193 dCopy(l-f,x+f,b+f); 00194 00195 SolBward(l-f,diag+f,uval,uhead+f,b+f); 00196 00197 s = sf->nsnds-1; 00198 00199 for(; s>=1; --s) { 00200 f = subg[s-1]; 00201 l = subg[s]; 00202 i = l; 00203 00204 for(; i>1+f; --i) { 00205 -- i; 00206 ls = usub+ujbeg[i]; 00207 l1 = uval+uhead[i-1]+1; 00208 l2 = uval+uhead[i ]+0; 00209 sze = ujsze[i]; 00210 x1 = 0.0; 00211 x2 = 0.0; 00212 00213 for(t=0; t<sze; ++t) { 00214 rtemp1 = b[ls[t]]; 00215 00216 x1 += l1[t]*rtemp1; 00217 x2 += l2[t]*rtemp1; 00218 } 00219 00220 b[i] = x[i ] - x2 / diag[i]; 00221 b[i-1] = x[i-1] - (x1 + uval[uhead[i-1]]*b[i]) / diag[i-1]; 00222 } 00223 00224 for(; i>f;) { 00225 -- i; 00226 l1 = uval+uhead[i]; 00227 ls = usub+ujbeg[i]; 00228 sze = ujsze[i]; 00229 x1 = 0.0; 00230 00231 for(t=0; t<sze; ++t) 00232 x1+= l1[t]*b[ls[t]]; 00233 00234 b[i] = x[i] - x1/diag[i]; 00235 } 00236 } 00237 } 00238 00239 } 00240 00241 /* Everything right for permuted system */ 00242 void ChlSolveForward2(chfac *sf, 00243 double b[], 00244 double x[]){ 00245 int i,nrow=sf->nrow; 00246 double *sqrtdiag=sf->sqrtdiag; 00247 ChlSolveForwardPrivate(sf,b); 00248 for(i=0; i<nrow; ++i){ 00249 x[i] = b[i]*sqrtdiag[i]; /* x[i] = b[i]*sqrt(sf->diag[i]); */ 00250 } 00251 } 00252 00253 void ChlSolveBackward2(chfac *sf, 00254 double b[], 00255 double x[]){ 00256 int i,nrow=sf->nrow; 00257 double *sqrtdiag=sf->sqrtdiag; 00258 for(i=0; i<nrow; ++i){ 00259 x[i] = b[i]/(sqrtdiag[i]); /* x[i] = b[i]/sqrt(sf->diag[i]) ; */ 00260 } 00261 ChlSolveBackwardPrivate(sf,x,b); 00262 memcpy(x,b,nrow*sizeof(double)); 00263 } 00264 00265 /* These routines together will solve an equation correctly */ 00266 void ChlSolveForward(chfac *sf, 00267 double b[], 00268 double x[]){ 00269 int i,nrow=sf->nrow,*perm=sf->perm; 00270 double *w=sf->rw,*sqrtdiag=sf->sqrtdiag; 00271 for(i=0; i<nrow; ++i) 00272 w[i] = b[perm[i]]; 00273 ChlSolveForwardPrivate(sf,w); 00274 for(i=0; i<nrow; ++i){ 00275 x[i] = w[i]*sqrtdiag[i]; /* x[i] = w[i]*sqrt(sf->diag[i]); */ 00276 } 00277 00278 } 00279 00280 void ChlSolveBackward(chfac *sf, 00281 double b[], 00282 double x[]){ 00283 int i,nrow=sf->nrow,*invp=sf->invp; 00284 double *w=sf->rw,*sqrtdiag=sf->sqrtdiag; 00285 for(i=0; i<nrow; ++i){ 00286 x[i] = b[i]/sqrtdiag[i]; 00287 } 00288 ChlSolveBackwardPrivate(sf,x,w); 00289 for(i=0; i<nrow; ++i) 00290 x[i] = w[invp[i]]; 00291 00292 } 00293 00294 void ChlSolve(chfac *sf, 00295 double b[], 00296 double x[]){ 00297 int i,nrow=sf->nrow,*perm=sf->perm,*invp=sf->invp; 00298 double *rw=sf->rw; 00299 /* 00300 ChlSolveForward(sf,b,w,x); 00301 ChlSolveBackward(sf,w,x,b); 00302 */ 00303 for(i=0; i<nrow; ++i) 00304 x[i] = b[perm[i]]; 00305 00306 ChlSolveForwardPrivate(sf,x); 00307 ChlSolveBackwardPrivate(sf,x,rw); 00308 00309 for(i=0; i<nrow; ++i) 00310 x[i] = rw[invp[i]]; /* x[i] = b[i]; */ 00311 return; 00312 } 00313 00314 00315 void ForwSubst(chfac *sf, 00316 double b[], 00317 double x[]) 00318 { 00319 int i,k,s,t,sze,f,l,itemp,*ls, 00320 *subg=sf->subg,*ujsze=sf->ujsze,*usub=sf->usub, 00321 *ujbeg=sf->ujbeg,*uhead=sf->uhead; 00322 double rtemp1,rtemp2,rtemp3,rtemp4, 00323 rtemp5,rtemp6,rtemp7,rtemp8, 00324 *l1,*l3,*l2,*l4,*l5,*l6,*l7,*l8, 00325 *diag=sf->diag,*uval=sf->uval; 00326 00327 for(i=0; i<sf->nrow; ++i) 00328 x[i] = b[sf->perm[i]]; 00329 00330 for(s=0; s<sf->nsnds; ++s) { 00331 f = subg[s]; 00332 l = subg[s+1]; 00333 00334 SolFwdSnode(sf,s,0,l-f,x); 00335 00336 itemp = l-f-1; 00337 ls = usub+ujbeg[f]+itemp; 00338 sze = ujsze[f]-itemp; 00339 k = f; 00340 00341 itemp = l-1; 00342 for(; k+7<l; k+=8) { 00343 l1 = uval+uhead[k+0]+itemp-(k+0); 00344 l2 = uval+uhead[k+1]+itemp-(k+1); 00345 l3 = uval+uhead[k+2]+itemp-(k+2); 00346 l4 = uval+uhead[k+3]+itemp-(k+3); 00347 l5 = uval+uhead[k+4]+itemp-(k+4); 00348 l6 = uval+uhead[k+5]+itemp-(k+5); 00349 l7 = uval+uhead[k+6]+itemp-(k+6); 00350 l8 = uval+uhead[k+7]+itemp-(k+7); 00351 00352 rtemp1 = x[k+0]; 00353 rtemp2 = x[k+1]; 00354 rtemp3 = x[k+2]; 00355 rtemp4 = x[k+3]; 00356 rtemp5 = x[k+4]; 00357 rtemp6 = x[k+5]; 00358 rtemp7 = x[k+6]; 00359 rtemp8 = x[k+7]; 00360 00361 for(t=0; t<sze; ++t) 00362 x[ls[t]] -= rtemp1*l1[t] 00363 + rtemp2*l2[t] 00364 + rtemp3*l3[t] 00365 + rtemp4*l4[t] 00366 + rtemp5*l5[t] 00367 + rtemp6*l6[t] 00368 + rtemp7*l7[t] 00369 + rtemp8*l8[t]; 00370 } 00371 00372 for(; k+3<l; k+=4) { 00373 l1 = uval+uhead[k+0]+itemp-(k+0); 00374 l2 = uval+uhead[k+1]+itemp-(k+1); 00375 l3 = uval+uhead[k+2]+itemp-(k+2); 00376 l4 = uval+uhead[k+3]+itemp-(k+3); 00377 00378 rtemp1 = x[k+0]; 00379 rtemp2 = x[k+1]; 00380 rtemp3 = x[k+2]; 00381 rtemp4 = x[k+3]; 00382 00383 for(t=0; t<sze; ++t) 00384 x[ls[t]] -= rtemp1*l1[t] 00385 + rtemp2*l2[t] 00386 + rtemp3*l3[t] 00387 + rtemp4*l4[t]; 00388 } 00389 00390 for(; k+1<l; k+=2) { 00391 l1 = uval+uhead[k+0]+itemp-(k+0); 00392 l2 = uval+uhead[k+1]+itemp-(k+1); 00393 00394 rtemp1 = x[k+0]; 00395 rtemp2 = x[k+1]; 00396 00397 for(t=0; t<sze; ++t) 00398 x[ls[t]] -= rtemp1*l1[t] 00399 + rtemp2*l2[t]; 00400 } 00401 00402 00403 for(; k<l; ++k) { 00404 l1 = uval+uhead[k+0]+itemp-(k+0); 00405 00406 rtemp1 = x[k+0]; 00407 00408 for(t=0; t<sze; ++t) 00409 x[ls[t]] -= rtemp1*l1[t]; 00410 } 00411 } 00412 00413 for (i=0; i<sf->nrow; i++){ 00414 x[i] = x[i] * sqrt( fabs(diag[i]) ); 00415 } 00416 00417 } /* ForwSubst */ 00418 00419 00420 00421 static void mulSnod(chfac *sf, 00422 int snde, 00423 int f, 00424 int l, 00425 double *b, 00426 double *x) 00427 { 00428 int i,t,sze,*ls,*subg,*ujbeg,*uhead,*usub; 00429 double xi,*l1,*diag,*uval; 00430 00431 subg =sf->subg; 00432 ujbeg=sf->ujbeg; 00433 uhead=sf->uhead; 00434 usub =sf->usub; 00435 diag =sf->diag; 00436 uval =sf->uval; 00437 00438 f += subg[snde]; 00439 l += subg[snde]; 00440 00441 for(i=f; i<l; ++i) { 00442 xi =b[i]; 00443 ls =usub+ujbeg[i]; 00444 l1 =uval+uhead[i]; 00445 sze =l-i-1; 00446 x[i]+=xi*diag[i]; 00447 for(t=0; t<sze; ++t) 00448 x[ls[t]]+=l1[t]*xi; 00449 } 00450 } /* mulSnod */ 00451 00452 void GetUhat(chfac *sf, 00453 double *b, 00454 double *x) 00455 /* If S = L L^T, then b = L x */ 00456 { 00457 int i,k,n,s,t,sze,f,l,itemp,*ls, 00458 *subg,*ujsze,*usub,*ujbeg,*uhead; 00459 double rtemp1,rtemp2,rtemp3,rtemp4, 00460 rtemp5,rtemp6,rtemp7,rtemp8, 00461 *l1,*l3,*l2,*l4,*l5,*l6,*l7,*l8, 00462 *diag,*uval; 00463 00464 n =sf->nrow; 00465 subg =sf->subg; 00466 ujsze=sf->ujsze; 00467 usub =sf->usub; 00468 ujbeg=sf->ujbeg; 00469 uhead=sf->uhead; 00470 diag =sf->diag; 00471 uval =sf->uval; 00472 00473 for (i=0; i<n; i++) { 00474 if (diag[i]>0) 00475 x[i]=b[i]/sqrt(diag[i]); 00476 else x[i]=b[i]/sqrt(-diag[i]); 00477 b[i]=0.0; 00478 } 00479 00480 for (s=0; s<sf->nsnds; s++) { 00481 f=subg[s]; 00482 l=subg[s+1]; 00483 00484 mulSnod(sf,s,0,l-f,x,b); 00485 00486 itemp=l-f-1; 00487 ls =usub+ujbeg[f]+itemp; 00488 sze =ujsze[f]-itemp; 00489 k =f; 00490 00491 itemp=l-1; 00492 for(; k+7<l; k+=8) { 00493 l1 =uval+uhead[k+0]+itemp-(k+0); 00494 l2 =uval+uhead[k+1]+itemp-(k+1); 00495 l3 =uval+uhead[k+2]+itemp-(k+2); 00496 l4 =uval+uhead[k+3]+itemp-(k+3); 00497 l5 =uval+uhead[k+4]+itemp-(k+4); 00498 l6 =uval+uhead[k+5]+itemp-(k+5); 00499 l7 =uval+uhead[k+6]+itemp-(k+6); 00500 l8 =uval+uhead[k+7]+itemp-(k+7); 00501 00502 rtemp1=x[k+0]; 00503 rtemp2=x[k+1]; 00504 rtemp3=x[k+2]; 00505 rtemp4=x[k+3]; 00506 rtemp5=x[k+4]; 00507 rtemp6=x[k+5]; 00508 rtemp7=x[k+6]; 00509 rtemp8=x[k+7]; 00510 00511 for(t=0; t<sze; ++t) 00512 b[ls[t]]+= rtemp1*l1[t] 00513 +rtemp2*l2[t] 00514 +rtemp3*l3[t] 00515 +rtemp4*l4[t] 00516 +rtemp5*l5[t] 00517 +rtemp6*l6[t] 00518 +rtemp7*l7[t] 00519 +rtemp8*l8[t]; 00520 } 00521 00522 00523 for(; k+3<l; k+=4) { 00524 l1 =uval+uhead[k+0]+itemp-(k+0); 00525 l2 =uval+uhead[k+1]+itemp-(k+1); 00526 l3 =uval+uhead[k+2]+itemp-(k+2); 00527 l4 =uval+uhead[k+3]+itemp-(k+3); 00528 00529 rtemp1=x[k+0]; 00530 rtemp2=x[k+1]; 00531 rtemp3=x[k+2]; 00532 rtemp4=x[k+3]; 00533 00534 for(t=0; t<sze; ++t) 00535 b[ls[t]]+= rtemp1*l1[t] 00536 +rtemp2*l2[t] 00537 +rtemp3*l3[t] 00538 +rtemp4*l4[t]; 00539 } 00540 00541 for(; k+1<l; k+=2) { 00542 l1 =uval+uhead[k+0]+itemp-(k+0); 00543 l2 =uval+uhead[k+1]+itemp-(k+1); 00544 00545 rtemp1=x[k+0]; 00546 rtemp2=x[k+1]; 00547 00548 for(t=0; t<sze; ++t) 00549 b[ls[t]]+= rtemp1*l1[t] 00550 +rtemp2*l2[t]; 00551 } 00552 00553 00554 for(; k<l; ++k) { 00555 l1 =uval+uhead[k+0]+itemp-(k+0); 00556 00557 rtemp1=x[k+0]; 00558 00559 for(t=0; t<sze; ++t) 00560 b[ls[t]]+=rtemp1*l1[t]; 00561 } 00562 } 00563 00564 for (i=0; i<n; i++) 00565 x[ sf->invp[i] ]=b[i]; 00566 } /* GetUhat */ 00567 00568 00569 00570 00571 int MatSolve4(chfac*sf, double *b, double *x,int n){ 00572 00573 memcpy(x,b,n*sizeof(double)); 00574 ChlSolve(sf, b, x); 00575 00576 return 0; 00577 } 00578 00579 int Mat4GetDiagonal(chfac*sf, double *b,int n){ 00580 00581 int i,*invp=sf->invp; 00582 double *diag=sf->diag; 00583 00584 for (i=0; i<n; i++, invp++){ 00585 b[i]=diag[*invp]; 00586 } 00587 return 0; 00588 } 00589 00590 int Mat4SetDiagonal(chfac*sf, double *b,int n){ 00591 00592 int i,*invp=sf->invp; 00593 double *diag=sf->diag; 00594 for (i=0; i<n; i++){ 00595 diag[invp[i]]=b[i]; 00596 } 00597 return 0; 00598 } 00599 00600 int Mat4AddDiagonal(chfac*sf, double *b,int n){ 00601 00602 int i,*invp=sf->invp; 00603 double *diag=sf->diag; 00604 for (i=0; i<n; i++){ 00605 diag[invp[i]]+=b[i]; 00606 } 00607 return 0; 00608 } 00609 00610 int MatAddDiagonalElement(chfac*sf, int row, double dd){ 00611 00612 int *invp=sf->invp; 00613 double *diag=sf->diag; 00614 diag[invp[row]]+=dd; 00615 return 0; 00616 } 00617 00618 00619 int MatMult4(chfac *sf, double *x, double *y, int n){ 00620 00621 int i,j,*invp=sf->invp,*perm=sf->perm; 00622 int *usub=sf->usub,*ujbeg=sf->ujbeg,*uhead=sf->uhead, *ujsze=sf->ujsze; 00623 int *iptr,k1,k2; 00624 double dd,*sval,*diag=sf->diag,*uval=sf->uval; 00625 00626 for (i=0; i<n; i++){ 00627 y[i] = diag[invp[i]] * x[i]; 00628 } 00629 for (i=0; i<n; ++i){ 00630 00631 iptr=usub + ujbeg[i]; 00632 sval=uval + uhead[i]; 00633 k1=perm[i]; 00634 for (j=0; j<ujsze[i]; j++){ 00635 dd=sval[j]; 00636 if (fabs(dd)> 1e-15){ 00637 k2=perm[iptr[j]]; 00638 y[k1] += dd * x[k2]; 00639 y[k2] += dd * x[k1]; 00640 } 00641 } 00642 } 00643 return 0; 00644 } 00645 00646 00647 static void setXYind2(int nnz, 00648 double* y, 00649 double* x, 00650 int* s, 00651 int* invp) 00652 { 00653 int i; 00654 00655 for(i=0; i<nnz; ++i) { 00656 x[i]=y[ invp[ s[i] ] ]; 00657 y[ invp[s[i]] ]=0.0; 00658 } 00659 } /* setXYind */ 00660 00661 static void setColi(chfac* cl, 00662 int i, 00663 double* ai) 00664 { 00665 setXYind2(cl->ujsze[i],ai, 00666 cl->uval+cl->uhead[i], 00667 cl->usub+cl->ujbeg[i], 00668 cl->perm); 00669 } /* setColi */ 00670 00671 static void setXYind2add(int nnz, 00672 double dd, 00673 double* y, 00674 double* x, 00675 int* s, 00676 int* invp) 00677 { 00678 int i; 00679 00680 for(i=0; i<nnz; ++i) { 00681 x[i]+=dd*y[ invp[ s[i] ] ]; 00682 y[ invp[s[i]] ]=0.0; 00683 } 00684 } /* setXYind */ 00685 00686 static void setColi2(chfac* cl, 00687 int i,double dd, 00688 double* ai) 00689 { 00690 setXYind2add(cl->ujsze[i],dd,ai, 00691 cl->uval+cl->uhead[i], 00692 cl->usub+cl->ujbeg[i], 00693 cl->perm); 00694 } /* setColi */ 00695 00696 00697 static void getXYind2(int nnz, 00698 double* y, 00699 double* x, 00700 int* s, 00701 int* invp) 00702 { 00703 int i; 00704 00705 for(i=0; i<nnz; ++i) { 00706 y[ invp[s[i]] ]=x[i]; 00707 } 00708 } /* setXYind */ 00709 00710 00711 int Mat4View(chfac *sf){ 00712 00713 int i,j,n=sf->nrow; 00714 double *v=sf->rw; 00715 for (i=0; i<n; i++){ 00716 for (j=0;j<n;++j) v[j]=0.0; 00717 getXYind2(sf->ujsze[i],v, 00718 sf->uval+sf->uhead[i], 00719 sf->usub+sf->ujbeg[i], 00720 sf->perm); 00721 v[i]=sf->diag[sf->invp[i]]; 00722 printf("Row %d, ",i); 00723 for (j=0;j<n;j++){ 00724 if (v[j]!=0) printf(" %d: %4.4e ",j,v[j]); 00725 } 00726 printf("\n"); 00727 } 00728 00729 return 0; 00730 00731 } 00732 00733 int MatZeroEntries4(chfac *sf){ 00734 00735 int i,n=sf->n; 00736 double *rw=sf->rw; 00737 memset((void*)(sf->diag),0,n*sizeof(double)); 00738 memset((void*)(rw),0,n*sizeof(double)); 00739 00740 for (i=0; i<n; i++){ 00741 setColi(sf,i,rw); 00742 } 00743 00744 return 0; 00745 } 00746 00747 00748 00749 int MatSetColumn4(chfac *cl, double *val, int col){ 00750 00751 int pcol=cl->invp[col]; 00752 00753 cl->diag[pcol]=val[col]; 00754 val[col]=0.0; 00755 setColi(cl,pcol,val); 00756 00757 return 0; 00758 } /* SetColumn */ 00759 00760 int MatAddColumn4(chfac *cl, double dd, double *val, int col){ 00761 00762 int pcol=cl->invp[col]; 00763 00764 cl->diag[pcol]+=dd*val[col]; 00765 val[col]=0.0; 00766 setColi2(cl,pcol,dd,val); 00767 00768 return 0; 00769 } /* SetColumn */ 00770 00771 00772 int MatSetValue4(chfac *cl, int row,int col,double val, int setmode){ 00773 00774 int i; 00775 double* x=cl->uval+cl->uhead[col]; 00776 int* s=cl->usub+cl->ujbeg[col]; 00777 int nnz=cl->ujsze[col]; 00778 int insertmode=1,addmode=2; 00779 00780 if (row<0 || col<0 || row>=cl->n || col>=cl->n){ 00781 printf("CHol set Value error: Row: %d, COl: %d \n",row,col); 00782 return 1; 00783 } 00784 00785 if (setmode==insertmode&&row==col){ 00786 cl->diag[cl->invp[col]]=val; 00787 } else if (setmode==addmode&&row==col) { 00788 cl->diag[cl->invp[col]]+=val; 00789 } else if (setmode==insertmode){ 00790 for(i=0; i<nnz; ++i) { 00791 if (s[i]==row){ 00792 x[i]=val; 00793 } 00794 } 00795 } else if (setmode==addmode){ 00796 for(i=0; i<nnz; ++i) { 00797 if (s[i]==row){ 00798 x[i]+=val; 00799 } 00800 } 00801 } else { 00802 return 1; 00803 } 00804 00805 return 0; 00806 } /* SetValue */ 00807 00808 00809 int Mat4DiagonalShift(chfac*sf, double shift){ 00810 int i,n=sf->nrow; 00811 double *diag=sf->diag; 00812 for (i=0; i<n; i++){ 00813 diag[i] += shift; 00814 } 00815 return 0; 00816 } 00817 00818 int Mat4LogDet(chfac*sf, double *dd){ 00819 int i,n=sf->nrow; 00820 double *diag=sf->diag,ddd=0; 00821 for (i=0; i<n; i++){ 00822 if (diag[i]<=0) return 1; 00823 ddd+=log(diag[i]); 00824 } 00825 *dd=ddd; 00826 return 0; 00827 } 00828