DSDP
sdpmatx.c
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