2 void dCopy(
int,
double*,
double*);
4 static void SolFwdSnode(chfac *sf,
10 int i,t,sze,*ls,*subg=sf->subg,
11 *ujbeg=sf->ujbeg,*uhead=sf->uhead,
13 double xi,*l1,*diag=sf->diag,*uval=sf->uval;
32 static void SolBward(
int nrow,
61 x[i-1] -= (uval[fir[i-1]]*x[i]+x1)/diag[i-1];
79 void ChlSolveForwardPrivate(chfac *sf,
82 int k,s,t,sze,f,l,itemp,*ls,
83 *subg=sf->subg,*ujsze=sf->ujsze,*usub=sf->usub,
84 *ujbeg=sf->ujbeg,*uhead=sf->uhead;
85 double rtemp1,rtemp2,rtemp3,rtemp4,
86 rtemp5,rtemp6,rtemp7,rtemp8,
87 *l1,*l3,*l2,*l4,*l5,*l6,*l7,*l8,
90 for(s=0; s<sf->nsnds; ++s) {
94 SolFwdSnode(sf,s,0,l-f,x);
97 ls = usub+ujbeg[f]+itemp;
103 l1 = uval+uhead[k+0]+itemp-(k+0);
104 l2 = uval+uhead[k+1]+itemp-(k+1);
105 l3 = uval+uhead[k+2]+itemp-(k+2);
106 l4 = uval+uhead[k+3]+itemp-(k+3);
107 l5 = uval+uhead[k+4]+itemp-(k+4);
108 l6 = uval+uhead[k+5]+itemp-(k+5);
109 l7 = uval+uhead[k+6]+itemp-(k+6);
110 l8 = uval+uhead[k+7]+itemp-(k+7);
122 x[ls[t]] -= rtemp1*l1[t]
133 l1 = uval+uhead[k+0]+itemp-(k+0);
134 l2 = uval+uhead[k+1]+itemp-(k+1);
135 l3 = uval+uhead[k+2]+itemp-(k+2);
136 l4 = uval+uhead[k+3]+itemp-(k+3);
144 x[ls[t]] -= rtemp1*l1[t]
151 l1 = uval+uhead[k+0]+itemp-(k+0);
152 l2 = uval+uhead[k+1]+itemp-(k+1);
158 x[ls[t]] -= rtemp1*l1[t]
164 l1 = uval+uhead[k+0]+itemp-(k+0);
169 x[ls[t]] -= rtemp1*l1[t];
175 void ChlSolveBackwardPrivate(chfac *sf,
181 int i,s,t,sze,f,l,*ls,
182 *subg=sf->subg,*ujsze=sf->ujsze,*usub=sf->usub,
183 *ujbeg=sf->ujbeg,*uhead=sf->uhead;
184 double x1,x2,*l1,*l2,rtemp1,
185 *diag=sf->diag,*uval=sf->uval;
195 SolBward(l-f,diag+f,uval,uhead+f,b+f);
207 l1 = uval+uhead[i-1]+1;
208 l2 = uval+uhead[i ]+0;
213 for(t=0; t<sze; ++t) {
220 b[i] = x[i ] - x2 / diag[i];
221 b[i-1] = x[i-1] - (x1 + uval[uhead[i-1]]*b[i]) / diag[i-1];
234 b[i] = x[i] - x1/diag[i];
242 void ChlSolveForward2(chfac *sf,
246 double *sqrtdiag=sf->sqrtdiag;
247 ChlSolveForwardPrivate(sf,b);
248 for(i=0; i<nrow; ++i){
249 x[i] = b[i]*sqrtdiag[i];
253 void ChlSolveBackward2(chfac *sf,
257 double *sqrtdiag=sf->sqrtdiag;
258 for(i=0; i<nrow; ++i){
259 x[i] = b[i]/(sqrtdiag[i]);
261 ChlSolveBackwardPrivate(sf,x,b);
262 memcpy(x,b,nrow*
sizeof(
double));
266 void ChlSolveForward(chfac *sf,
269 int i,nrow=sf->nrow,*perm=sf->perm;
270 double *w=sf->rw,*sqrtdiag=sf->sqrtdiag;
271 for(i=0; i<nrow; ++i)
273 ChlSolveForwardPrivate(sf,w);
274 for(i=0; i<nrow; ++i){
275 x[i] = w[i]*sqrtdiag[i];
280 void ChlSolveBackward(chfac *sf,
283 int i,nrow=sf->nrow,*invp=sf->invp;
284 double *w=sf->rw,*sqrtdiag=sf->sqrtdiag;
285 for(i=0; i<nrow; ++i){
286 x[i] = b[i]/sqrtdiag[i];
288 ChlSolveBackwardPrivate(sf,x,w);
289 for(i=0; i<nrow; ++i)
294 void ChlSolve(chfac *sf,
297 int i,nrow=sf->nrow,*perm=sf->perm,*invp=sf->invp;
303 for(i=0; i<nrow; ++i)
306 ChlSolveForwardPrivate(sf,x);
307 ChlSolveBackwardPrivate(sf,x,rw);
309 for(i=0; i<nrow; ++i)
315 void ForwSubst(chfac *sf,
319 int i,k,s,t,sze,f,l,itemp,*ls,
320 *subg=sf->subg,*ujsze=sf->ujsze,*usub=sf->usub,
321 *ujbeg=sf->ujbeg,*uhead=sf->uhead;
322 double rtemp1,rtemp2,rtemp3,rtemp4,
323 rtemp5,rtemp6,rtemp7,rtemp8,
324 *l1,*l3,*l2,*l4,*l5,*l6,*l7,*l8,
325 *diag=sf->diag,*uval=sf->uval;
327 for(i=0; i<sf->nrow; ++i)
328 x[i] = b[sf->perm[i]];
330 for(s=0; s<sf->nsnds; ++s) {
334 SolFwdSnode(sf,s,0,l-f,x);
337 ls = usub+ujbeg[f]+itemp;
338 sze = ujsze[f]-itemp;
343 l1 = uval+uhead[k+0]+itemp-(k+0);
344 l2 = uval+uhead[k+1]+itemp-(k+1);
345 l3 = uval+uhead[k+2]+itemp-(k+2);
346 l4 = uval+uhead[k+3]+itemp-(k+3);
347 l5 = uval+uhead[k+4]+itemp-(k+4);
348 l6 = uval+uhead[k+5]+itemp-(k+5);
349 l7 = uval+uhead[k+6]+itemp-(k+6);
350 l8 = uval+uhead[k+7]+itemp-(k+7);
362 x[ls[t]] -= rtemp1*l1[t]
373 l1 = uval+uhead[k+0]+itemp-(k+0);
374 l2 = uval+uhead[k+1]+itemp-(k+1);
375 l3 = uval+uhead[k+2]+itemp-(k+2);
376 l4 = uval+uhead[k+3]+itemp-(k+3);
384 x[ls[t]] -= rtemp1*l1[t]
391 l1 = uval+uhead[k+0]+itemp-(k+0);
392 l2 = uval+uhead[k+1]+itemp-(k+1);
398 x[ls[t]] -= rtemp1*l1[t]
404 l1 = uval+uhead[k+0]+itemp-(k+0);
409 x[ls[t]] -= rtemp1*l1[t];
413 for (i=0; i<sf->nrow; i++){
414 x[i] = x[i] * sqrt( fabs(diag[i]) );
421 static void mulSnod(chfac *sf,
428 int i,t,sze,*ls,*subg,*ujbeg,*uhead,*usub;
429 double xi,*l1,*diag,*uval;
452 void GetUhat(chfac *sf,
457 int i,k,n,s,t,sze,f,l,itemp,*ls,
458 *subg,*ujsze,*usub,*ujbeg,*uhead;
459 double rtemp1,rtemp2,rtemp3,rtemp4,
460 rtemp5,rtemp6,rtemp7,rtemp8,
461 *l1,*l3,*l2,*l4,*l5,*l6,*l7,*l8,
473 for (i=0; i<n; i++) {
475 x[i]=b[i]/sqrt(diag[i]);
476 else x[i]=b[i]/sqrt(-diag[i]);
480 for (s=0; s<sf->nsnds; s++) {
484 mulSnod(sf,s,0,l-f,x,b);
487 ls =usub+ujbeg[f]+itemp;
493 l1 =uval+uhead[k+0]+itemp-(k+0);
494 l2 =uval+uhead[k+1]+itemp-(k+1);
495 l3 =uval+uhead[k+2]+itemp-(k+2);
496 l4 =uval+uhead[k+3]+itemp-(k+3);
497 l5 =uval+uhead[k+4]+itemp-(k+4);
498 l6 =uval+uhead[k+5]+itemp-(k+5);
499 l7 =uval+uhead[k+6]+itemp-(k+6);
500 l8 =uval+uhead[k+7]+itemp-(k+7);
512 b[ls[t]]+= rtemp1*l1[t]
524 l1 =uval+uhead[k+0]+itemp-(k+0);
525 l2 =uval+uhead[k+1]+itemp-(k+1);
526 l3 =uval+uhead[k+2]+itemp-(k+2);
527 l4 =uval+uhead[k+3]+itemp-(k+3);
535 b[ls[t]]+= rtemp1*l1[t]
542 l1 =uval+uhead[k+0]+itemp-(k+0);
543 l2 =uval+uhead[k+1]+itemp-(k+1);
549 b[ls[t]]+= rtemp1*l1[t]
555 l1 =uval+uhead[k+0]+itemp-(k+0);
560 b[ls[t]]+=rtemp1*l1[t];
565 x[ sf->invp[i] ]=b[i];
571 int MatSolve4(chfac*sf,
double *b,
double *x,
int n){
573 memcpy(x,b,n*
sizeof(
double));
579 int Mat4GetDiagonal(chfac*sf,
double *b,
int n){
581 int i,*invp=sf->invp;
582 double *diag=sf->diag;
584 for (i=0; i<n; i++, invp++){
590 int Mat4SetDiagonal(chfac*sf,
double *b,
int n){
592 int i,*invp=sf->invp;
593 double *diag=sf->diag;
600 int Mat4AddDiagonal(chfac*sf,
double *b,
int n){
602 int i,*invp=sf->invp;
603 double *diag=sf->diag;
610 int MatAddDiagonalElement(chfac*sf,
int row,
double dd){
613 double *diag=sf->diag;
619 int MatMult4(chfac *sf,
double *x,
double *y,
int n){
621 int i,j,*invp=sf->invp,*perm=sf->perm;
622 int *usub=sf->usub,*ujbeg=sf->ujbeg,*uhead=sf->uhead, *ujsze=sf->ujsze;
624 double dd,*sval,*diag=sf->diag,*uval=sf->uval;
627 y[i] = diag[invp[i]] * x[i];
631 iptr=usub + ujbeg[i];
632 sval=uval + uhead[i];
634 for (j=0; j<ujsze[i]; j++){
636 if (fabs(dd)> 1e-15){
647 static void setXYind2(
int nnz,
655 for(i=0; i<nnz; ++i) {
656 x[i]=y[ invp[ s[i] ] ];
661 static void setColi(chfac* cl,
665 setXYind2(cl->ujsze[i],ai,
666 cl->uval+cl->uhead[i],
667 cl->usub+cl->ujbeg[i],
671 static void setXYind2add(
int nnz,
680 for(i=0; i<nnz; ++i) {
681 x[i]+=dd*y[ invp[ s[i] ] ];
686 static void setColi2(chfac* cl,
690 setXYind2add(cl->ujsze[i],dd,ai,
691 cl->uval+cl->uhead[i],
692 cl->usub+cl->ujbeg[i],
697 static void getXYind2(
int nnz,
705 for(i=0; i<nnz; ++i) {
706 y[ invp[s[i]] ]=x[i];
711 int Mat4View(chfac *sf){
716 for (j=0;j<n;++j) v[j]=0.0;
717 getXYind2(sf->ujsze[i],v,
718 sf->uval+sf->uhead[i],
719 sf->usub+sf->ujbeg[i],
721 v[i]=sf->diag[sf->invp[i]];
722 printf(
"Row %d, ",i);
724 if (v[j]!=0) printf(
" %d: %4.4e ",j,v[j]);
733 int MatZeroEntries4(chfac *sf){
737 memset((
void*)(sf->diag),0,n*
sizeof(
double));
738 memset((
void*)(rw),0,n*
sizeof(
double));
749 int MatSetColumn4(chfac *cl,
double *val,
int col){
751 int pcol=cl->invp[col];
753 cl->diag[pcol]=val[col];
755 setColi(cl,pcol,val);
760 int MatAddColumn4(chfac *cl,
double dd,
double *val,
int col){
762 int pcol=cl->invp[col];
764 cl->diag[pcol]+=dd*val[col];
766 setColi2(cl,pcol,dd,val);
772 int MatSetValue4(chfac *cl,
int row,
int col,
double val,
int setmode){
775 double* x=cl->uval+cl->uhead[col];
776 int* s=cl->usub+cl->ujbeg[col];
777 int nnz=cl->ujsze[col];
778 int insertmode=1,addmode=2;
780 if (row<0 || col<0 || row>=cl->n || col>=cl->n){
781 printf(
"CHol set Value error: Row: %d, COl: %d \n",row,col);
785 if (setmode==insertmode&&row==col){
786 cl->diag[cl->invp[col]]=val;
787 }
else if (setmode==addmode&&row==col) {
788 cl->diag[cl->invp[col]]+=val;
789 }
else if (setmode==insertmode){
790 for(i=0; i<nnz; ++i) {
795 }
else if (setmode==addmode){
796 for(i=0; i<nnz; ++i) {
809 int Mat4DiagonalShift(chfac*sf,
double shift){
811 double *diag=sf->diag;
818 int Mat4LogDet(chfac*sf,
double *dd){
820 double *diag=sf->diag,ddd=0;
822 if (diag[i]<=0)
return 1;