ADMB Documentation  11.1.1913
 All Classes Files Functions Variables Typedefs Friends Defines
hs_sparse.cpp
Go to the documentation of this file.
00001 /*
00002  * $Id: hs_sparse.cpp 1756 2014-03-05 22:24:49Z johnoel $
00003  *
00004  * Author: David Fournier
00005  * Copyright (c) 2009-2012 ADMB foundation
00006  *
00007  * This code is inspired by the CSparse package written by Timothy A. Davis.
00008  * It has been extensively modified for compliance with ADMB.
00009  *
00010  * License: LGPL
00011  */
00012 
00013 #include <fvar.hpp>
00014 //#include <admodel.h>
00015 //#include <df1b2fun.h>
00016 //#include <adrndeff.h>
00017 #define XCONST const
00018 #include "hs.h"
00019 //pthread_mutex_t mutex_dfpool = PTHREAD_MUTEX_INITIALIZER;
00020 
00021 #define USE_ADJOINT_CODE
00022 void report_derivatives(const dvar_vector& x);
00023 
00024 static void xxx(int i){;}
00025 static void xxx(ivector& i){;}
00026 static void xxxv(ivector& x)
00027 {
00028   int mmin=x.indexmin();
00029   int mmax=x.indexmax();
00030   for (int i=mmin;i<=mmax;i++)
00031   {
00032     switch (x(i))
00033     {
00034       case 0:
00035         cout << "not used" << endl;
00036         break;
00037       case 1:
00038         xxx(x);
00039         break;
00040       default:
00041         xxx(x);
00042         break;
00043     }
00044   }
00045 }
00046 //int varchol(XCONST dvar_hs_smatrix &A, XCONST hs_symbolic &S,
00047 //dvar_hs_smatrix &L, laplace_approximation_calculator * );
00048 int varchol(XCONST dvar_hs_smatrix &A, XCONST hs_symbolic &S,
00049   dvar_hs_smatrix &L, dcompressed_triplet & sparse_triplet2);
00050 
00051 int tmpxchol(XCONST hs_smatrix &A, XCONST hs_symbolic &S, hs_smatrix &L,
00052   ivector & xcount);
00053 
00054 hs_smatrix cs_multiply(const hs_smatrix &A, const hs_smatrix  &B);
00055 int cholnew(XCONST hs_smatrix &A, XCONST hs_symbolic &S, hs_smatrix &L);
00056 
00057 // Utility function: p [0..n] = cumulative sum of c [0..n-1],
00058 // and then copy p [0..n-1] into c
00059  double cs_cumsum (ivector &p, ivector &c, int n)
00060 {
00061    int i, nz = 0 ;
00062     double nz2 = 0 ;
00063     for (i = 0 ; i < n ; i++)
00064     {
00065         p [i] = nz ;
00066         nz += c [i] ;
00067         nz2 += c [i] ;// also in double to avoid int overflow
00068         c [i] = p [i] ;// also copy p[0..n-1] back into c[0..n-1]
00069     }
00070     p [n] = nz ;
00071     return (nz2) ;                    // return sum (c [0..n-1])
00072 }
00073 
00074 // clear w
00075  int cs_wclear (int mark, int lemax, ivector &w, int n)
00076 {
00077     int k ;
00078     if (mark < 2 || (mark + lemax < 0))
00079     {
00080         for (k = 0 ; k < n ; k++) if (w [k] != 0) w [k] = 1 ;
00081         mark = 2 ;
00082     }
00083     return (mark) ;        // at this point, w [0..n-1] < mark holds
00084 }
00085 
00086 // Find C = A'
00087  void cs_transpose (XCONST hs_smatrix &_A, int values, hs_smatrix &C)
00088 {
00089     ADUNCONST(hs_smatrix,A)
00090     int p, q, j;
00091 
00092     int n = A.n ;
00093     int m = n;
00094     ivector & Ap=A.p;
00095     ivector & Ai=A.i;
00096     dvector & Ax=A.x;
00097 
00098     ivector & Cp=C.p;
00099     ivector & Ci=C.i;
00100     dvector & Cx=C.x;
00101 
00102     ivector w(0,n-1);                                      // workspace
00103     w.initialize();
00104 
00105     for (p = 0 ; p < Ap [n] ; p++)
00106       w [Ai [p]]++ ;                                          // row counts
00107     cs_cumsum (Cp, w, m) ;                              // row pointers
00108     for (j = 0 ; j < n ; j++)
00109     {
00110         for (p = Ap [j] ; p < Ap [j+1] ; p++)
00111         {
00112           Ci [q = w [Ai [p]]++] = j ; // place A(i,j) as entry C(j,i)
00113           Cx [q] = Ax [p] ;
00114         }
00115     }
00116     return;
00117 }
00118 
00119  void cs_transpose (XCONST dvar_hs_smatrix &_A, int values, dvar_hs_smatrix &C)
00120 {
00121     ADUNCONST(dvar_hs_smatrix,A)
00122     int p, q, j;
00123 
00124     int n = A.n ;
00125     int m = n;
00126     ivector & Ap=A.p;
00127     ivector & Ai=A.i;
00128     dvar_vector & Ax=A.x;
00129 
00130     ivector & Cp=C.p;
00131     ivector & Ci=C.i;
00132     dvar_vector & Cx=C.x;
00133 
00134     ivector w(0,n-1);                                      // workspace
00135     w.initialize();
00136 
00137     for (p = 0 ; p < Ap [n] ; p++)
00138       w [Ai [p]]++ ;                                          // row counts
00139     cs_cumsum (Cp, w, m) ;                              // row pointers
00140     for (j = 0 ; j < n ; j++)
00141     {
00142         for (p = Ap [j] ; p < Ap [j+1] ; p++)
00143         {
00144           Ci [q = w [Ai [p]]++] = j ; // place A(i,j) as entry C(j,i)
00145           Cx [q] = Ax [p] ;
00146         }
00147     }
00148     return;
00149 }
00150 
00151 
00152 //Sparse matrix XCONSTructor (compressed format) from dmatrix in triplet format
00153 dmatrix make_dmatrix(dcompressed_triplet& M,int n,int m)
00154 {
00155   dmatrix tmp(1,n,1,m);
00156   tmp.initialize();
00157   int nz = M.indexmax()- M.indexmin() + 1;
00158   for (int i=1;i<=nz;i++)
00159   {
00160     tmp(M(1,i),M(2,i))=M(i);
00161     if (M(1,i) != M(2,i))
00162     {
00163       tmp(M(2,i),M(1,i))=M(i);
00164     }
00165   }
00166   return tmp;
00167 }
00168 
00169 dvar_matrix make_sdvar_matrix(dvar_compressed_triplet& M)
00170 {
00171   int n=M.get_n();
00172   int m=M.get_m();
00173   dvar_matrix tmp(1,n,1,m);
00174   tmp.initialize();
00175   int nz = M.indexmax()- M.indexmin() + 1;
00176   for (int i=1;i<=nz;i++)
00177   {
00178     tmp(M(1,i),M(2,i))=M(i);
00179     if (M(1,i) != M(2,i))
00180     {
00181       tmp(M(2,i),M(1,i))=M(i);
00182     }
00183   }
00184   return tmp;
00185 }
00186 
00187 dvar_matrix make_dvar_matrix(dvar_compressed_triplet& M)
00188 {
00189   int n=M.get_n();
00190   int m=M.get_m();
00191   dvar_matrix tmp(1,n,1,m);
00192   tmp.initialize();
00193   int nz = M.indexmax()- M.indexmin() + 1;
00194   for (int i=1;i<=nz;i++)
00195   {
00196     tmp(M(1,i),M(2,i))=M(i);
00197   }
00198   return tmp;
00199 }
00200 
00201 
00202 dmatrix make_dmatrix(dcompressed_triplet& M)
00203 {
00204   int n=M.get_n();
00205   int m=M.get_m();
00206   dmatrix tmp(1,n,1,m);
00207   tmp.initialize();
00208   int nz = M.indexmax()- M.indexmin() + 1;
00209   for (int i=1;i<=nz;i++)
00210   {
00211     tmp(M(1,i),M(2,i))=M(i);
00212   }
00213   return tmp;
00214 }
00215 
00216 dmatrix make_sdmatrix(dcompressed_triplet& M)
00217 {
00218   int n=M.get_n();
00219   int m=M.get_m();
00220   dmatrix tmp(1,n,1,m);
00221   tmp.initialize();
00222   int nz = M.indexmax()- M.indexmin() + 1;
00223   for (int i=1;i<=nz;i++)
00224   {
00225     tmp(M(1,i),M(2,i))=M(i);
00226     if (M(1,i) != M(2,i))
00227     {
00228       tmp(M(2,i),M(1,i))=M(i);
00229     }
00230   }
00231   return tmp;
00232 }
00233 
00234 dvar_matrix make_dvar_matrix(dvar_compressed_triplet& M,int n,int m)
00235 {
00236   dvar_matrix tmp(1,n,1,m);
00237   int nz = M.indexmax()- M.indexmin() + 1;
00238   for (int i=1;i<=nz;i++)
00239   {
00240     tmp(M(1,i),M(2,i))=M(i);
00241     if (M(1,i) != M(2,i))
00242     {
00243       tmp(M(2,i),M(1,i))=M(i);
00244     }
00245   }
00246   return tmp;
00247 }
00248 
00249 
00250 hs_smatrix::hs_smatrix(int _n, XCONST dcompressed_triplet &_M)
00251 {
00252     ADUNCONST(dcompressed_triplet,M)
00253 
00254     n = _n;
00255     m = n;                // May remove m from code; only square matrices needed
00256 
00257     int nz = M.indexmax()- M.indexmin() + 1;
00258     nzmax = nz;
00259 
00260     int k;
00261 
00262     // Matrix in triplet format
00263     ivector Ti(1,nz);
00264     ivector tmp=(M.get_coords())(1);
00265     Ti=tmp-1;
00266     Ti.shift(0);
00267 
00268 
00269     ivector Tj(1,nz);
00270     ivector tmp1=(M.get_coords())(2);
00271     Tj=tmp1-1;
00272     Tj.shift(0);
00273 
00274     dvector Tx(1,nz);
00275     Tx = M.get_x();
00276     Tx.shift(0);
00277 
00278     if( min(Ti)<0 || max(Ti)>(n-1) || max(Tj)>(n-1) || min(Tj)<0)
00279       cout << "Error #2 in hs_smatrix::hs_smatrix" << endl;
00280 
00281     int lower_tri=0;
00282     for (k = 0 ; k < nz ; k++)
00283       lower_tri += Ti[k]>Tj[k];
00284     if(lower_tri)
00285       cout << "hs_smatrix::hs_smatrix: M must be upper triangular" << endl;
00286 
00287     // Matrix in compressed format
00288     p.allocate(0,n);
00289     p = 0;
00290     i.allocate(0,nzmax-1);
00291     x.allocate(0,nzmax-1);
00292     ivector & Cp=p;
00293     ivector & Ci=i;
00294     dvector & Cx=x;
00295 
00296     ivector w(0,n-1);                                        // get workspace
00297     w.initialize();
00298 
00299     // Does the compression
00300     int P;                                                // Originally p
00301     for (k = 0 ; k < nz ; k++) w [Tj [k]]++ ;                // column counts
00302     cs_cumsum(Cp, w, n) ;                                // column pointers
00303     for (k = 0 ; k < nz ; k++)
00304     {
00305         P = w [Tj [k]]++;
00306         Ci [P] = Ti [k] ;
00307         Cx [P] = Tx [k] ;
00308     }
00309 }
00310 
00311 hs_smatrix::hs_smatrix(const dcompressed_triplet &_M)
00312 {
00313     ADUNCONST(dcompressed_triplet,M)
00314 
00315     m = M.get_n();
00316     n = M.get_m();
00317 
00318     int nz = M.indexmax()- M.indexmin() + 1;
00319     nzmax = nz;
00320 
00321     int k;
00322 
00323     // Matrix in triplet format
00324     ivector Ti(1,nz);
00325     ivector tmp=(M.get_coords())(1);
00326     Ti=tmp-1;
00327     Ti.shift(0);
00328 
00329 
00330     ivector Tj(1,nz);
00331     ivector tmp1=(M.get_coords())(2);
00332     Tj=tmp1-1;
00333     Tj.shift(0);
00334 
00335     dvector Tx(1,nz);
00336     Tx = M.get_x();
00337     Tx.shift(0);
00338 
00339     if( min(Ti)<0 || max(Ti)>(n-1) || max(Tj)>(m-1) || min(Tj)<0)
00340       cout << "Error #2 in hs_smatrix::hs_smatrix" << endl;
00341 
00342     int lower_tri=0;
00343     for (k = 0 ; k < nz ; k++)
00344       lower_tri += Ti[k]>Tj[k];
00345     if(lower_tri)
00346       cout << "hs_smatrix::hs_smatrix: M must be upper triangular" << endl;
00347 
00348     // Matrix in compressed format
00349     p.allocate(0,n);
00350     p = 0;
00351     i.allocate(0,nzmax-1);
00352     x.allocate(0,nzmax-1);
00353     ivector & Cp=p;
00354     ivector & Ci=i;
00355     dvector & Cx=x;
00356 
00357     ivector w(0,m-1);                                        // get workspace
00358     w.initialize();
00359 
00360     // Does the compression
00361     int P;                                                // Originally p
00362     for (k = 0 ; k < nz ; k++) w [Tj [k]]++ ;                // column counts
00363     cs_cumsum(Cp, w, n) ;                                // column pointers
00364     for (k = 0 ; k < nz ; k++)
00365     {
00366         P = w [Tj [k]]++;
00367         Ci [P] = Ti [k];
00368         Cx [P] = Tx [k] ;
00369     }
00370 }
00371 
00372 dvar_hs_smatrix::dvar_hs_smatrix(int _n, XCONST dvar_compressed_triplet &_M)
00373 {
00374     ADUNCONST(dvar_compressed_triplet,M)
00375     n = _n;
00376     m = n;                // May remove m from code; only square matrices needed
00377 
00378     int nz = M.indexmax()- M.indexmin() + 1;
00379     nzmax = nz;
00380 
00381     int k;
00382 
00383     // Matrix in triplet format
00384     ivector Ti(1,nz);
00385     ivector tmp=(M.get_coords())(1);
00386     Ti=tmp-1;
00387     Ti.shift(0);
00388 
00389 
00390     ivector Tj(1,nz);
00391     ivector tmp1=(M.get_coords())(2);
00392     Tj=tmp1-1;
00393     Tj.shift(0);
00394 
00395     dvar_vector Tx(1,nz);
00396     Tx = M.get_x();
00397     Tx.shift(0);
00398 
00399     if( min(Ti)<0 || max(Ti)>(n-1) || max(Tj)>(n-1) || min(Tj)<0)
00400       cout << "Error #2 in dvar_hs_smatrix::dvar_hs_smatrix" << endl;
00401 
00402     int lower_tri=0;
00403     for (k = 0 ; k < nz ; k++)
00404       lower_tri += Ti[k]>Tj[k];
00405     if(lower_tri)
00406       cout << "dvar_hs_smatrix::dvar_hs_smatrix: M must be upper triangular"
00407            << endl;
00408 
00409     // Matrix in compressed format
00410     p.allocate(0,n);
00411     p = 0;
00412     i.allocate(0,nzmax-1);
00413     x.allocate(0,nzmax-1);
00414     ivector & Cp=p;
00415     ivector & Ci=i;
00416     dvar_vector & Cx=x;
00417 
00418     ivector w(0,n-1);                                        // get workspace
00419     w.initialize();
00420 
00421     // Does the compression
00422     int P;                                                // Originally p
00423     for (k = 0 ; k < nz ; k++) w [Tj [k]]++ ;                // column counts
00424     cs_cumsum(Cp, w, n) ;                                // column pointers
00425     for (k = 0 ; k < nz ; k++)
00426     {
00427         P = w [Tj [k]]++;
00428         Ci [P] = Ti [k];
00429         Cx [P] = Tx [k] ;
00430     }
00431 }
00432 
00433 hs_smatrix::hs_smatrix(int _n, int _nzmax)
00434 {
00435     nzmax = _nzmax;
00436     n = _n;
00437     m = _n;               // May get rid of m later; only square matrices needed
00438 
00439     p.allocate(0,n);
00440     p = 0;
00441     i.allocate(0,nzmax-1);
00442     i = 0;
00443     x.allocate(0,nzmax-1);
00444     x = 0.0;
00445 }
00446 
00447 dvar_hs_smatrix::dvar_hs_smatrix(int _n, int _nzmax)
00448 {
00449     nzmax = _nzmax;
00450     n = _n;
00451     m = _n;               // May get rid of m later; only square matrices needed
00452 
00453     p.allocate(0,n);
00454     p = 0;
00455     i.allocate(0,nzmax-1);
00456     i = 0;
00457     x.allocate(0,nzmax-1);
00458     x = 0.0;
00459 }
00460 
00461 // Convert cs to hs_smatrix
00462 hs_smatrix::hs_smatrix(XCONST cs *A)
00463 {
00464   int j;
00465     nzmax = A->nzmax;
00466     n = A->n;
00467     m = n;
00468 
00469     p.allocate(0,n);
00470     for (j = 0 ; j <= n ; j++)
00471       p[j] = A->p[j];
00472 
00473     i.allocate(0,nzmax-1);
00474     for (j = 0 ; j < nzmax ; j++)
00475       i[j] = A->i[j];
00476 
00477     x.allocate(0,nzmax-1);
00478     for (j = 0 ; j < nzmax ; j++)
00479       x[j] = A->x[j];
00480 }
00481 
00482 // Extends nzmax; new entries initialized to zero
00483 void hs_smatrix::reallocate(int _nzmax)
00484 {
00485     ivector tmp(0,nzmax-1);
00486     tmp=i;
00487     i.deallocate();
00488     i.allocate(0,_nzmax-1);
00489     i(nzmax,_nzmax-1) = 0;
00490     i(0,nzmax-1) = tmp;
00491 
00492     dvector tmpx(0,nzmax-1);
00493     tmpx=x;
00494     x.deallocate();
00495     x.allocate(0,_nzmax-1);
00496     x(nzmax,_nzmax-1) = 0.0;
00497     x(0,nzmax-1) = tmpx;
00498 
00499     nzmax = _nzmax;
00500 }
00501 
00502 void dvar_hs_smatrix::reallocate(int _nzmax)
00503 {
00504     ivector tmp(0,nzmax-1);
00505     tmp=i;
00506     i.deallocate();
00507     i.allocate(0,_nzmax-1);
00508     i(nzmax,_nzmax-1) = 0;
00509     i(0,nzmax-1) = tmp;
00510 
00511     dvar_vector tmpx(0,nzmax-1);
00512     tmpx=x;
00513     x.deallocate();
00514     x.allocate(0,_nzmax-1);
00515     x(nzmax,_nzmax-1) = 0.0;
00516     x(0,nzmax-1) = tmpx;
00517 
00518     nzmax = _nzmax;
00519 }
00520 
00521 dvar_hs_smatrix::dvar_hs_smatrix(XCONST dvar_hs_smatrix &A)
00522 {
00523     nzmax = A.nzmax;
00524     n = A.n;
00525     m = n;                // May get rid of m later; only square matrices needed
00526 
00527     p.allocate(0,n);
00528     i.allocate(0,nzmax-1);
00529     x.allocate(0,nzmax-1);
00530     p = A.p;
00531     i = A.i;
00532     x = A.x;
00533 }
00534 
00535 hs_smatrix::hs_smatrix(XCONST hs_smatrix &A)
00536 {
00537     nzmax = A.nzmax;
00538     n = A.n;
00539     m = n;                // May get rid of m later; only square matrices needed
00540 
00541     p.allocate(0,n);
00542     i.allocate(0,nzmax-1);
00543     x.allocate(0,nzmax-1);
00544     p = A.p;
00545     i = A.i;
00546     x = A.x;
00547 }
00548 
00549 hs_smatrix& hs_smatrix::operator=(XCONST hs_smatrix &A)
00550 {
00551     if((n != A.n)||(nzmax != A.nzmax))
00552       cout << "hs_smatrix&: lhs and rhs dimensions differ" << endl;
00553     else
00554     {
00555       p = A.p;
00556       i = A.i;
00557       x = A.x;
00558     }
00559 
00560     return *this;
00561 }
00562 
00563 dvar_hs_smatrix& dvar_hs_smatrix::operator=(XCONST dvar_hs_smatrix &A)
00564 {
00565     if((n != A.n)||(nzmax != A.nzmax))
00566       cout << "dvar_hs_smatrix&: lhs and rhs dimensions differ" << endl;
00567     else
00568     {
00569       p = A.p;
00570       i = A.i;
00571       x = A.x;
00572     }
00573 
00574     return *this;
00575 }
00576 
00577 // Allocate Cholesky factor
00578 hs_smatrix::hs_smatrix(XCONST hs_symbolic &S)
00579 {
00580     nzmax = S.cp[S.n];
00581     n = S.n;
00582     m = n;
00583 
00584     p.allocate(0,n);
00585     i.allocate(0,nzmax-1);
00586     x.allocate(0,nzmax-1);
00587 }
00588 
00589 dvar_hs_smatrix::dvar_hs_smatrix(XCONST hs_symbolic &S)
00590 {
00591     nzmax = S.cp[S.n];
00592     n = S.n;
00593     m = n;
00594 
00595     p.allocate(0,n);
00596     i.allocate(0,nzmax-1);
00597     x.allocate(0,nzmax-1);
00598 }
00599 
00600 // For determinant calculations: sum(log(diag(L)).
00601 double hs_smatrix::trace_log(int & sgn)
00602 {
00603   sgn=0;
00604   double ret = 0.0;
00605   for (int j = 0 ; j < n ; j++)
00606   {
00607     for (int k = p [j] ; k < p [j+1] ; k++)
00608     {
00609       if(j==i[k])
00610       {
00611         //cout << " k = "  << k << "  x[k] = " << x[k] << endl;
00612         if(x[k]>0.0)
00613           ret += log(x[k]);
00614         else if (x[k]<0.0)
00615         {
00616           ret += log(-x[k]);
00617           sgn++;
00618         }
00619           else
00620         {
00621               cerr << "!!!! trace_log: zero diagonal element " << endl;
00622           sgn=-1;
00623           break;
00624         }
00625       }
00626       if (sgn==-1) break;
00627     }
00628     if (sgn==-1) break;
00629   }
00630   sgn=sgn%2;
00631   return(ret);
00632 }
00633 
00634 dvariable dvar_hs_smatrix::trace_log(int & sgn)
00635 {
00636   sgn=0;
00637   dvariable ret = 0.0;
00638   for (int j = 0 ; j < n ; j++)
00639   {
00640     for (int k = p [j] ; k < p [j+1] ; k++)
00641     {
00642       if(j==i[k])
00643       {
00644         //cout << " k = "  << k << "  x[k] = " << x[k] << endl;
00645         if(x[k]>0.0)
00646           ret += log(x[k]);
00647         else if (x[k]<0.0)
00648         {
00649           ret += log(-x[k]);
00650           sgn++;
00651         }
00652           else
00653         {
00654               cerr << "!!!! trace_log: zero diagonal element " << endl;
00655           sgn=-1;
00656           break;
00657         }
00658       }
00659       if (sgn==-1) break;
00660     }
00661     if (sgn==-1) break;
00662   }
00663   sgn=sgn%2;
00664   return(ret);
00665 }
00666 dvariable ln_det(XCONST dvar_hs_smatrix& M)
00667 {
00668   int sgn;
00669   return ln_det(M,sgn);
00670 }
00671 
00672 
00673 dvariable ln_det(XCONST dvar_hs_smatrix& _M,int & sgn)
00674 {
00675   ADUNCONST(dvar_hs_smatrix,M)
00676   return M.trace_log(sgn);
00677 }
00678 
00679 double ln_det(XCONST hs_smatrix& _M)
00680 {
00681   ADUNCONST(hs_smatrix,M)
00682   int sgn;
00683   return M.trace_log(sgn);
00684 }
00685 double ln_det(XCONST hs_smatrix& _M,int & sgn)
00686 {
00687   ADUNCONST(hs_smatrix,M)
00688   return M.trace_log(sgn);
00689 }
00690 
00691 int hs_smatrix::print()
00692 {
00693     cout << "Sparse matrix in compressed form (i,x):" << endl;
00694     for (int j = 0 ; j < n ; j++)
00695     {
00696       cout << "    col " << j << " : locations " << p[j] << " to " << p[j+1]-1
00697            <<  endl;
00698       for (int P = p [j] ; P < p [j+1] ; P++)
00699         cout << i [P] << " " << x [P] << endl;
00700     }
00701     return(0);
00702 }
00703 
00704 int hs_smatrix::print_pattern()
00705 {
00706     cout << "Sparseness pattern:" << endl;
00707     for (int j = 0 ; j < n ; j++)
00708     {
00709       for (int k = 0 ; k < n ; k++)
00710       {
00711         int tmp=0;
00712         for (int kk = p[k] ; kk < p[k+1] ; kk++)
00713           tmp += (i[kk]==j);
00714         if(tmp)
00715           cout << "x";
00716         else
00717           cout << " ";
00718       }
00719       cout << endl;
00720     }
00721     return(0);
00722 }
00723 
00724 // Print matrix transpose with zeros
00725 int hs_smatrix::print_trans_zeros()
00726 {
00727     for(int k = 0 ; k < n ; k++)        // cols
00728     {
00729       int kk = p[k];
00730       for(int j = 0 ; j < n ; j++)        // rows
00731         if(i[kk]==j)
00732         {
00733           cout << x[kk] << "\t";
00734           kk++;
00735         }
00736         else
00737           cout << "0\t";
00738       cout << endl;
00739     }
00740     return(0);
00741 }
00742 
00743 // Find nonzero pattern of Cholesky L(k,1:k-1) using etree and triu(A(:,k))
00744 int cs_ereach (XCONST hs_smatrix &_A, int k, XCONST ivector &parent, ivector &s,
00745   ivector &w)
00746 {
00747     ADUNCONST(hs_smatrix,A)
00748     int i, p, n, len, top;
00749 
00750     n = A.n;
00751     top = n;
00752 
00753     ivector & Ap=A.p;
00754     ivector & Ai=A.i;
00755 
00756     CS_MARK (w, k) ;                    /* mark node k as visited */
00757     for (p = Ap [k] ; p < Ap [k+1] ; p++)
00758     {
00759         i = Ai [p] ;                    /* A(i,k) is nonzero */
00760         if (i > k) continue ;          /* only use upper triangular part of A */
00761         for (len = 0 ; !CS_MARKED (w,i) ; i = parent [i]) /* traverse up etree*/
00762         {
00763             s [len++] = i ;            /* L(k,i) is nonzero */
00764             CS_MARK (w, i) ;            /* mark i as visited */
00765         }
00766         while (len > 0) s [--top] = s [--len] ; /* push path onto stack */
00767     }
00768     for (p = top ; p < n ; p++) CS_MARK (w, s [p]) ;      /* unmark all nodes */
00769     CS_MARK (w, k) ;                    /* unmark node k */
00770     return (top) ;                  /* s [top..n-1] contains pattern of L(k,:)*/
00771 }
00772 
00773 int cs_ereach (XCONST dvar_hs_smatrix &_A, int k, XCONST ivector &parent,
00774   ivector &s, ivector &w)
00775 {
00776     ADUNCONST(dvar_hs_smatrix,A)
00777     int i, p, n, len, top;
00778 
00779     n = A.n;
00780     top = n;
00781 
00782     ivector & Ap=A.p;
00783     ivector & Ai=A.i;
00784 
00785     CS_MARK (w, k) ;                    /* mark node k as visited */
00786     for (p = Ap [k] ; p < Ap [k+1] ; p++)
00787     {
00788         i = Ai [p] ;                    /* A(i,k) is nonzero */
00789         if (i > k) continue ;          /* only use upper triangular part of A */
00790         for (len = 0 ; !CS_MARKED (w,i) ; i = parent [i]) /* traverse up etree*/
00791         {
00792             s [len++] = i ;            /* L(k,i) is nonzero */
00793             CS_MARK (w, i) ;            /* mark i as visited */
00794         }
00795         while (len > 0) s [--top] = s [--len] ; /* push path onto stack */
00796     }
00797     for (p = top ; p < n ; p++) CS_MARK (w, s [p]) ;      /* unmark all nodes */
00798     CS_MARK (w, k) ;                    /* unmark node k */
00799     return (top) ;                  /* s [top..n-1] contains pattern of L(k,:)*/
00800 }
00801 
00802 /* C = A(p,p) where A and C are symmetric the upper part stored; pinv not p */
00803 void hs_symperm(XCONST hs_smatrix &_A, XCONST ivector &pinv, hs_smatrix &C)
00804 {
00805     ADUNCONST(hs_smatrix,A)
00806     int i, j, p, q, i2, j2;
00807 
00808     int n = A.n ;
00809     ivector & Ap=A.p;
00810     ivector & Ai=A.i;
00811     dvector & Ax=A.x;
00812     ivector w(0,n-1);                        /* get workspace */
00813     w.initialize();
00814     ivector & Cp=C.p;
00815     ivector & Ci=C.i;
00816     dvector & Cx=C.x;
00817 
00818     for (j = 0 ; j < n ; j++)            /* count entries in each column of C */
00819     {
00820         j2 = (pinv[0]!=-1) ? pinv [j] : j ;/* column j of A is column j2 of C */
00821         for (p = Ap [j] ; p < Ap [j+1] ; p++)
00822         {
00823             i = Ai [p] ;
00824             if (i > j) continue ;        /* skip lower triangular part of A */
00825             i2 = (pinv[0]!=-1) ? pinv [i] : i ;  /* row i of A is row i2 of C */
00826             w [CS_MAX (i2, j2)]++ ;        /* column count of C */
00827         }
00828     }
00829     cs_cumsum (Cp, w, n) ;                /* compute column pointers of C */
00830     for (j = 0 ; j < n ; j++)
00831     {
00832         j2 = (pinv[0]!=-1) ? pinv [j] : j ;/* column j of A is column j2 of C */
00833         for (p = Ap [j] ; p < Ap [j+1] ; p++)
00834         {
00835             i = Ai [p] ;
00836             if (i > j) continue ;        /* skip lower triangular part of A*/
00837             i2 = (pinv[0]!=-1) ? pinv [i] : i ;  /* row i of A is row i2 of C */
00838 
00839             q = w [CS_MAX (i2, j2)]++;
00840             Ci [q] = CS_MIN (i2, j2) ;
00841             Cx [q] = Ax [p] ;
00842         }
00843     }
00844 }
00845 void hs_symperm(XCONST dvar_hs_smatrix &_A, XCONST ivector &pinv,
00846   dvar_hs_smatrix &C)
00847 {
00848     ADUNCONST(dvar_hs_smatrix,A)
00849     int i, j, p, q, i2, j2;
00850 
00851     int n = A.n ;
00852     ivector & Ap=A.p;
00853     ivector & Ai=A.i;
00854     dvar_vector & Ax=A.x;
00855     ivector w(0,n-1);                        /* get workspace */
00856     w.initialize();
00857     ivector & Cp=C.p;
00858     ivector & Ci=C.i;
00859     dvar_vector & Cx=C.x;
00860 
00861     for (j = 0 ; j < n ; j++)            /* count entries in each column of C */
00862     {
00863         j2 = (pinv[0]!=-1) ? pinv [j] : j ;/* column j of A is column j2 of C */
00864         for (p = Ap [j] ; p < Ap [j+1] ; p++)
00865         {
00866             i = Ai [p] ;
00867             if (i > j) continue ;        /* skip lower triangular part of A */
00868             i2 = (pinv[0]!=-1) ? pinv [i] : i ;  /* row i of A is row i2 of C */
00869             w [CS_MAX (i2, j2)]++ ;        /* column count of C */
00870         }
00871     }
00872     cs_cumsum (Cp, w, n) ;                /* compute column pointers of C */
00873     for (j = 0 ; j < n ; j++)
00874     {
00875         j2 = (pinv[0]!=-1) ? pinv [j] : j ;/* column j of A is column j2 of C */
00876         for (p = Ap [j] ; p < Ap [j+1] ; p++)
00877         {
00878             i = Ai [p] ;
00879             if (i > j) continue ;        /* skip lower triangular part of A*/
00880             i2 = (pinv[0]!=-1) ? pinv [i] : i ;  /* row i of A is row i2 of C */
00881 
00882             q = w [CS_MAX (i2, j2)]++;
00883             Ci [q] = CS_MIN (i2, j2) ;
00884             Cx [q] = Ax [p] ;
00885         }
00886     }
00887 }
00888 
00889 void myacc(int & p,int Lpi1,int ci,const ivector & Li,
00890   dvector & x,const dvector & Lx,const double & lki)
00891 {
00892   for (p = Lpi1 ; p < ci ; p++)
00893   {
00894       x[Li[p]] -= Lx[p]*lki ;
00895   }
00896 }
00897 
00898 // Numeric Cholesky factorization (L is factor). Return 1 on success 0 otherwise
00899 int chol(XCONST hs_smatrix& A,
00900          XCONST hs_symbolic& T,
00901          hs_smatrix& L)
00902 {
00903     //ADUNCONST(hs_symbolic,S)
00904     hs_symbolic& S = (hs_symbolic&)T;
00905     double d, lki;
00906     int top, i, p, k, n;
00907 
00908     n = A.n ;
00909 
00910     ivector c(0,n-1);                              // int workspace
00911     ivector s(0,n-1);                                   // int workspace
00912     dvector x(0,n-1) ;                        // double workspace
00913 
00914     ivector & cp=S.cp;
00915     ivector & pinv=S.pinv;
00916     ivector & parent=S.parent;
00917 
00918     hs_smatrix C(A);
00919     C = A;
00920     if(S.pinv[0]!=-1)
00921       hs_symperm(A,pinv,C);
00922 
00923     ivector & Cp=C.p;
00924     ivector & Ci=C.i;
00925     dvector & Cx=C.x;
00926 
00927     ivector & Lp=L.p;
00928     ivector & Li=L.i;
00929     dvector & Lx=L.x;
00930 
00931     for (k = 0 ; k < n ; k++)
00932       Lp [k] = c [k] = cp [k] ;
00933 
00934     for (k = 0 ; k < n ; k++)            // compute L(:,k) for L*L' = C
00935     {
00936         // --- Nonzero pattern of L(k,:) ------------------------------------
00937         top = cs_ereach (C, k, parent, s, c) ;         // find pattern of L(k,:)
00938         x [k] = 0 ;                                    // x (0:k) is now zero
00939         for (p = Cp [k] ; p < Cp [k+1] ; p++)          // x = full(triu(C(:,k)))
00940         {
00941             if (Ci [p] <= k) x [Ci [p]] = Cx [p] ;
00942         }
00943         d = x [k] ;                        // d = C(k,k)
00944         x [k] = 0 ;                        // clear x for k+1st iteration
00945         // --- Triangular solve ---------------------------------------------
00946         for ( ; top < n ; top++)    // solve L(0:k-1,0:k-1) * x = C(:,k)
00947         {
00948             i = s [top] ;                // s [top..n-1] is pattern of L(k,:)
00949             lki = x [i] / Lx [Lp [i]] ; // L(k,i) = x (i) / L(i,i)
00950             x [i] = 0 ;                        // clear x for k+1st iteration
00951 
00952 
00953             int Lpi1=Lp[i]+1;
00954             int ci=c[i];
00955             for (p = Lpi1; p < ci ; p++)
00956             {
00957                 x [Li [p]] -= Lx [p] * lki ;
00958             }
00959 
00960             d -= lki * lki ;                // d = d - L(k,i)*L(k,i)
00961             p = c [i]++ ;
00962             Li [p] = k ;                // store L(k,i) in column i
00963             Lx [p] = lki ;
00964         }
00965         // --- Compute L(k,k) -----------------------------------------------
00966         if (d <= 0) return (0) ; // not pos def
00967         p = c [k]++ ;
00968         Li [p] = k ;                    // store L(k,k) = sqrt (d) in column k
00969         Lx [p] = sqrt (d) ;
00970     }
00971     Lp [n] = cp [n] ;                    // finalize L
00972     return (1);
00973 }
00974 // Numeric Cholesky factorization (L is factor).
00975 // Return 1 on success; 0 otherwise
00976 int tmpxchol1(XCONST hs_smatrix &A, XCONST hs_symbolic& T, hs_smatrix &L,
00977   ivector & nxcount)
00978 {
00979     //ADUNCONST(hs_symbolic,S)
00980     hs_symbolic& S = (hs_symbolic&)T;
00981     double d, lki;
00982     int top, i, p, k, n;
00983 
00984     n = A.n ;
00985 
00986     ivector c(0,n-1);                              /* int workspace */
00987     ivector s(0,n-1);                                   /* int workspace */
00988     //dvector x(0,n-1) ;                        /* double workspace */
00989     dmatrix x(0,n-1,1,nxcount) ;                        /* double workspace */
00990 
00991     ivector & cp=S.cp;
00992     ivector & pinv=S.pinv;
00993     ivector & parent=S.parent;
00994 
00995     hs_smatrix C(A);
00996     C = A;
00997     if(S.pinv[0]!=-1)
00998       hs_symperm(A,pinv,C);
00999 
01000     ivector & Cp=C.p;
01001     ivector & Ci=C.i;
01002     dvector & Cx=C.x;
01003 
01004     ivector & Lp=L.p;
01005     ivector & Li=L.i;
01006     dvector & Lx=L.x;
01007 
01008   // counters
01009     ivector Licount(Li.indexmin(),Li.indexmax());
01010     Licount.initialize();
01011     ivector Lxcount(Lx.indexmin(),Lx.indexmax());
01012     Lxcount.initialize();
01013     ivector xcount(x.indexmin(),x.indexmax());
01014     xcount.initialize();
01015 
01016     int dcount=0;
01017     int pcount=0;
01018     int icount=0;
01019     int lkicount=0;
01020 
01021     for (k = 0 ; k < n ; k++)
01022     {
01023       Lp [k] = c [k] = cp [k] ;
01024     }
01025 
01026     for (k = 0 ; k < n ; k++)            /* compute L(:,k) for L*L' = C */
01027     {
01028         /* --- Nonzero pattern of L(k,:) ------------------------------------ */
01029         top = cs_ereach (C, k, parent, s, c) ;      /* find pattern of L(k,:) */
01030         xcount[k]++;
01031         x (k,xcount(k)) = 0 ;                          /* x (0:k) is now zero */
01032         for (p = Cp [k] ; p < Cp [k+1] ; p++)       /* x = full(triu(C(:,k))) */
01033         {
01034           if (Ci [p] <= k)
01035           {
01036             xcount[Ci[p]]++;
01037             x(Ci[p],xcount(Ci[p])) = Cx[p] ;
01038           }
01039         }
01040         d = x(k,xcount[k]) ;                        /* d = C(k,k) */
01041         dcount++;
01042         xcount[k]++;
01043         x(k,xcount[k]) = 0 ;                   /* clear x for k+1st iteration */
01044         /* --- Triangular solve --------------------------------------------- */
01045         for ( ; top < n ; top++)    /* solve L(0:k-1,0:k-1) * x = C(:,k) */
01046         {
01047             i = s [top] ;                /* s [top..n-1] is pattern of L(k,:) */
01048             icount++;
01049             lki = x (i,xcount[i]) / Lx [Lp [i]] ; /* L(k,i) = x (i) / L(i,i) */
01050             lkicount++;
01051             xcount[i]++;
01052             x (i,xcount[i]) = 0.0 ;            /* clear x for k+1st iteration */
01053             for (p = Lp [i] + 1 ; p < c [i] ; p++)
01054             {
01055                 x [Li [p]] -= Lx [p] * lki ;
01056             }
01057             d -= lki * lki ;                /* d = d - L(k,i)*L(k,i) */
01058             p = c [i]++ ;
01059             pcount++;
01060             Li [p] = k ;                /* store L(k,i) in column i */
01061             Licount[p]++;
01062             if (Licount(p)>1)
01063             {
01064               cerr << "Error unhandled case in chol" << endl;
01065             }
01066             Lx [p] = lki ;
01067             Lxcount[p]++;
01068             if (Lxcount(p)>1)
01069             {
01070               cerr << "Error unhandled case in chol" << endl;
01071               ad_exit(1);
01072             }
01073         }
01074         /* --- Compute L(k,k) ----------------------------------------------- */
01075         if (d <= 0) return (0) ; /* not pos def */
01076         p = c [k]++ ;
01077         pcount++;
01078         Li [p] = k ;                   /* store L(k,k) = sqrt (d) in column k */
01079         Licount[p]++;
01080         if (Licount(p)>1)
01081         {
01082           cerr << "Error unhandled case in chol" << endl;
01083         }
01084         Lx [p] = sqrt (d) ;
01085         Lxcount[p]++;
01086         if (Lxcount(p)>1)
01087         {
01088           cerr << "Error unhandled case in chol" << endl;
01089           ad_exit(1);
01090         }
01091     }
01092     Lp [n] = cp [n] ;                    /* finalize L */
01093     xxx(pcount);
01094     xxx(dcount);
01095     xxx(icount);
01096     xxx(lkicount);
01097     xxxv(Licount);
01098     xxxv(Lxcount);
01099     xxxv(xcount);
01100     return (1);
01101 }
01102 
01103 /* x(p) = b, for dense vectors x and b; p=NULL denotes identity */
01104  dvector cs_ipvec(XCONST ivector &p, XCONST dvector &b)
01105 {
01106     if(p[0]==-1)                        // No permutation
01107       return(b);
01108     else
01109     {
01110       int n = p.indexmax()+1;
01111       dvector x(0,n-1);
01112       for (int k = 0 ; k < n ; k++)
01113         x [p [k]] = b [k] ;
01114       return (x) ;
01115     }
01116 }
01117 
01118 /* x = b(p), for dense vectors x and b; p=NULL denotes identity */
01119  dvector cs_pvec (XCONST ivector &p, XCONST dvector &b)
01120 {
01121     if(p[0]==-1)                        // No permutation
01122       return(b);
01123     else
01124     {
01125       int n = p.indexmax()+1;
01126       dvector x(0,n-1);
01127       for (int k = 0 ; k < n ; k++)
01128         x[k] = b[p[k]];
01129       return (x) ;
01130     }
01131 }
01132 
01133  dvar_vector cs_pvec (XCONST ivector &p, XCONST dvar_vector &b)
01134 {
01135     if(p[0]==-1)                        // No permutation
01136       return(b);
01137     else
01138     {
01139       int n = p.indexmax()+1;
01140       dvar_vector x(0,n-1);
01141       for (int k = 0 ; k < n ; k++)
01142         x[k] = b[p[k]];
01143       return (x) ;
01144     }
01145 }
01146 
01147 /* solve Lx=b where x and b are dense.  x=b on input, solution on output. */
01148  dvector cs_lsolve (XCONST hs_smatrix & LL, XCONST dvector &y)
01149 {
01150     //ADUNCONST(hs_smatrix,L)
01151     hs_smatrix& L = (hs_smatrix&)LL;
01152     int p, j, n;
01153 
01154     n = L.n;
01155     dvector x(0,n-1);
01156     x=y;
01157 
01158     ivector & Lp=L.p;
01159     ivector & Li=L.i;
01160     dvector & Lx=L.x;
01161 
01162     for (j = 0 ; j < n ; j++)
01163     {
01164         x [j] /= Lx [Lp [j]] ;
01165         for (p = Lp [j]+1 ; p < Lp [j+1] ; p++)
01166         {
01167             x [Li [p]] -= Lx [p] * x [j] ;
01168         }
01169     }
01170     return (x) ;
01171 }
01172 
01173  dvar_vector cs_lsolve (XCONST dvar_hs_smatrix & LL, XCONST dvar_vector &y)
01174 {
01175     //ADUNCONST(dvar_hs_smatrix,L)
01176     dvar_hs_smatrix& L = (dvar_hs_smatrix&)LL;
01177     int p, j, n;
01178 
01179     n = L.n;
01180     dvar_vector x(0,n-1);
01181     x=y;
01182 
01183     ivector & Lp=L.p;
01184     ivector & Li=L.i;
01185     dvar_vector & Lx=L.x;
01186 
01187     for (j = 0 ; j < n ; j++)
01188     {
01189         x [j] /= Lx [Lp [j]] ;
01190         for (p = Lp [j]+1 ; p < Lp [j+1] ; p++)
01191         {
01192             x [Li [p]] -= Lx [p] * x [j] ;
01193         }
01194     }
01195     return (x) ;
01196 }
01197 
01198  dvar_vector cs_lsolve (XCONST dvar_hs_smatrix & LL, XCONST dvector &y)
01199 {
01200     //ADUNCONST(dvar_hs_smatrix,L)
01201     dvar_hs_smatrix& L = (dvar_hs_smatrix&)LL;
01202     int p, j, n;
01203 
01204     n = L.n;
01205     dvar_vector x(0,n-1);
01206     x=y;
01207 
01208     ivector & Lp=L.p;
01209     ivector & Li=L.i;
01210     dvar_vector & Lx=L.x;
01211 
01212     for (j = 0 ; j < n ; j++)
01213     {
01214         x [j] /= Lx [Lp [j]] ;
01215         for (p = Lp [j]+1 ; p < Lp [j+1] ; p++)
01216         {
01217             x [Li [p]] -= Lx [p] * x [j] ;
01218         }
01219     }
01220     return (x) ;
01221 }
01222 
01223 /* solve L'x=b where x and b are dense.  x=b on input, solution on output. */
01224  dvector cs_ltsolve (XCONST hs_smatrix &LL, XCONST dvector &y)
01225 {
01226     //ADUNCONST(hs_smatrix,L)
01227     hs_smatrix& L = (hs_smatrix&)LL;
01228     int p, j, n;
01229 
01230     n = L.n;
01231     dvector x(0,n-1);
01232     x=y;
01233     ivector & Lp=L.p;
01234     ivector & Li=L.i;
01235     dvector & Lx=L.x;
01236 
01237     for (j = n-1 ; j >= 0 ; j--)
01238     {
01239         for (p = Lp [j]+1 ; p < Lp [j+1] ; p++)
01240         {
01241             x [j] -= Lx [p] * x [Li [p]] ;
01242         }
01243         x [j] /= Lx [Lp [j]] ;
01244     }
01245     return (x) ;
01246 }
01247 
01248  dvar_vector cs_ltsolve (XCONST dvar_hs_smatrix &LL, XCONST dvar_vector &y)
01249 {
01250     //ADUNCONST(dvar_hs_smatrix,L)
01251     dvar_hs_smatrix& L = (dvar_hs_smatrix&)LL;
01252     int p, j, n;
01253 
01254     n = L.n;
01255     dvar_vector x(0,n-1);
01256     x=y;
01257     ivector & Lp=L.p;
01258     ivector & Li=L.i;
01259     dvar_vector & Lx=L.x;
01260 
01261     for (j = n-1 ; j >= 0 ; j--)
01262     {
01263         for (p = Lp [j]+1 ; p < Lp [j+1] ; p++)
01264         {
01265             x [j] -= Lx [p] * x [Li [p]] ;
01266         }
01267         x [j] /= Lx [Lp [j]] ;
01268     }
01269     return (x) ;
01270 }
01271 
01272 // keep off-diagonal entries; drop diagonal entries
01273  int cs_diag(int i, int j, double aij, void *other) { return (i != j) ; }
01274 int cs_diag(int i, int j, const prevariable& aij, void *other)
01275   { return (i != j) ; }
01276 
01277 /* drop entries for which fkeep(A(i,j)) is false; return nz if OK, else -1 */
01278 int cs_fkeep (hs_smatrix &A, int (*fkeep) (int, int, double, void *),
01279   void *other)
01280 {
01281     int j, p, nz = 0 ;
01282 
01283     int n = A.n ;
01284     ivector & Ap=A.p;
01285     ivector & Ai=A.i;
01286     dvector & Ax=A.x;
01287 
01288     for (j = 0 ; j < n ; j++)
01289     {
01290         p = Ap [j] ;                        /* get current location of col j */
01291         Ap [j] = nz ;                       /* record new location of col j */
01292         for ( ; p < Ap [j+1] ; p++)
01293         {
01294             if (fkeep (Ai [p], j, Ax [p], other))
01295             {
01296                 Ax [nz] = Ax [p] ;  /* keep A(i,j) */
01297                 Ai [nz++] = Ai [p] ;
01298             }
01299         }
01300     }
01301     Ap [n] = nz ;                           /* finalize A */
01302     return (nz) ;
01303 }
01304 
01305 int cs_fkeep (dvar_hs_smatrix &A, int (*fkeep) (int, int, const prevariable&,
01306   void *), void *other)
01307 {
01308     int j, p, nz = 0 ;
01309 
01310     int n = A.n ;
01311     ivector & Ap=A.p;
01312     ivector & Ai=A.i;
01313     dvar_vector & Ax=A.x;
01314 
01315     for (j = 0 ; j < n ; j++)
01316     {
01317         p = Ap [j] ;                        /* get current location of col j */
01318         Ap [j] = nz ;                       /* record new location of col j */
01319         for ( ; p < Ap [j+1] ; p++)
01320         {
01321             if (fkeep (Ai [p], j, Ax [p], other))
01322             {
01323                 Ax [nz] = Ax [p] ;  /* keep A(i,j) */
01324                 Ai [nz++] = Ai [p] ;
01325             }
01326         }
01327     }
01328     Ap [n] = nz ;                           /* finalize A */
01329     return (nz) ;
01330 }
01331 
01332 /* x = x + beta * A(:,j), where x is a dense vector and A(:,j) is sparse */
01333 int cs_scatter(XCONST hs_smatrix &AA, int j, double beta, ivector &w,
01334   dvector &x, int mark, hs_smatrix &C, int nz)
01335 {
01336     //ADUNCONST(hs_smatrix,A)
01337     hs_smatrix& A = (hs_smatrix&)AA;
01338     int i, p;
01339     ivector & Ap=A.p;
01340     ivector & Ai=A.i;
01341     dvector & Ax=A.x;
01342     ivector & Ci=C.i;
01343     for (p = Ap [j] ; p < Ap [j+1] ; p++)
01344     {
01345         i = Ai [p] ;                            /* A(i,j) is nonzero */
01346         if (w [i] < mark)
01347         {
01348             w [i] = mark ;                      /* i is new entry in column j */
01349             Ci [nz++] = i ;                     /* add i to pattern of C(:,j) */
01350             x [i] = beta * Ax [p] ;      /* x(i) = beta*A(i,j) */
01351         }
01352         else x [i] += beta * Ax [p] ;    /* i exists in C(:,j) already */
01353     }
01354     return (nz) ;
01355 }
01356 
01357  //int cs_scatter(XCONST hs_smatrix &A, int j, double beta, ivector &w,
01358  //  dvector &x, int mark, hs_smatrix &C, int nz)
01359  //{
01360  //    int i, p;
01361  //    ivector & Ap=A.p;
01362  //    ivector & Ai=A.i;
01363  //    dvector & Ax=A.x;
01364  //    ivector & Ci=C.i;
01365  //    for (p = Ap [j] ; p < Ap [j+1] ; p++)
01366  //    {
01367  //        i = Ai [p] ;                            /* A(i,j) is nonzero */
01368  //        if (w [i] < mark)
01369  //        {
01370  //            w [i] = mark ;                   /* i is new entry in column j */
01371  //            Ci [nz++] = i ;                  /* add i to pattern of C(:,j) */
01372  //            x [i] = beta * Ax [p] ;      /* x(i) = beta*A(i,j) */
01373  //        }
01374  //        else x [i] += beta * Ax [p] ;    /* i exists in C(:,j) already */
01375  //    }
01376  //    return (nz) ;
01377  //}
01378 
01379 int cs_scatter(XCONST dvar_hs_smatrix &AA, int j, double beta, ivector &w,
01380   dvar_vector &x, int mark, dvar_hs_smatrix &C, int nz)
01381 {
01382     //ADUNCONST(dvar_hs_smatrix,A)
01383     dvar_hs_smatrix& A = (dvar_hs_smatrix&)AA;
01384     int i, p;
01385     ivector & Ap=A.p;
01386     ivector & Ai=A.i;
01387     dvar_vector & Ax=A.x;
01388     ivector & Ci=C.i;
01389     for (p = Ap [j] ; p < Ap [j+1] ; p++)
01390     {
01391         i = Ai [p] ;                            /* A(i,j) is nonzero */
01392         if (w [i] < mark)
01393         {
01394             w [i] = mark ;                      /* i is new entry in column j */
01395             Ci [nz++] = i ;                     /* add i to pattern of C(:,j) */
01396             x [i] = beta * Ax [p] ;      /* x(i) = beta*A(i,j) */
01397         }
01398         else x [i] += beta * Ax [p] ;    /* i exists in C(:,j) already */
01399     }
01400     return (nz) ;
01401 }
01402 
01403 /* C = alpha*A + beta*B */
01404 hs_smatrix cs_add(XCONST hs_smatrix &AA, XCONST hs_smatrix &BB, double alpha,
01405   double beta)
01406 {
01407     //ADUNCONST(hs_smatrix,A)
01408     //ADUNCONST(hs_smatrix,B)
01409     hs_smatrix& A = (hs_smatrix&)AA;
01410     hs_smatrix& B = (hs_smatrix&)BB;
01411     int p, j, nz = 0, anz,m, n, bnz;
01412 
01413     if (A.m != B.m || A.n != B.n)
01414     {
01415        cout << "!!!! Error in cs_add: A.m != B.m || A.n != B.n" << endl;
01416        exit(0);
01417     }
01418 
01419     m = A.m ; anz = A.p [A.n] ;
01420     n = B.n ;
01421 
01422     ivector & Bp=B.p;
01423     bnz = Bp [n] ;
01424     ivector w(0,m-1);                                             // Workspace
01425     w.initialize();
01426 
01427     // Always assumes values == 1
01428     dvector x(0,m-1);                                             // Workspace
01429     x.initialize();
01430 
01431     hs_smatrix C(n,anz + bnz);
01432     ivector & Cp=C.p;
01433     ivector & Ci=C.i;
01434     dvector & Cx=C.x;
01435 
01436     for (j = 0 ; j < n ; j++)
01437     {
01438         Cp [j] = nz ;                   /* column j of C starts here */
01439         nz = cs_scatter (A, j, alpha, w, x, j+1, C, nz) ;   /* alpha*A(:,j)*/
01440         nz = cs_scatter (B, j, beta, w, x, j+1, C, nz) ;    /* beta*B(:,j) */
01441         for (p = Cp [j] ; p < nz ; p++)
01442           Cx [p] = x [Ci [p]] ;
01443     }
01444     Cp [n] = nz ;                       /* finalize the last column of C */
01445     //cs_sprealloc (C, 0) ;               Ignoring this. Must be picked up
01446     return (C) ;
01447 }
01448 
01449 dvar_hs_smatrix cs_add(XCONST dvar_hs_smatrix &AA, XCONST dvar_hs_smatrix &BB,
01450   double alpha, double beta)
01451 {
01452     //ADUNCONST(dvar_hs_smatrix,A)
01453     //ADUNCONST(dvar_hs_smatrix,B)
01454     dvar_hs_smatrix& A = (dvar_hs_smatrix&)AA;
01455     dvar_hs_smatrix& B = (dvar_hs_smatrix&)BB;
01456     int p, j, nz = 0, anz,m, n, bnz;
01457 
01458     if (A.m != B.m || A.n != B.n)
01459     {
01460        cout << "!!!! Error in cs_add: A.m != B.m || A.n != B.n" << endl;
01461        exit(0);
01462     }
01463 
01464     m = A.m ; anz = A.p [A.n] ;
01465     n = B.n ;
01466 
01467     ivector & Bp=B.p;
01468     bnz = Bp [n] ;
01469     ivector w(0,m-1);                                             // Workspace
01470     w.initialize();
01471 
01472     // Always assumes values == 1
01473     dvar_vector x(0,m-1);                                           // Workspace
01474     x.initialize();
01475 
01476     dvar_hs_smatrix C(n,anz + bnz);
01477     ivector & Cp=C.p;
01478     ivector & Ci=C.i;
01479     dvar_vector & Cx=C.x;
01480 
01481     for (j = 0 ; j < n ; j++)
01482     {
01483         Cp [j] = nz ;                   /* column j of C starts here */
01484         nz = cs_scatter (A, j, alpha, w, x, j+1, C, nz) ;   /* alpha*A(:,j)*/
01485         nz = cs_scatter (B, j, beta, w, x, j+1, C, nz) ;    /* beta*B(:,j) */
01486         for (p = Cp [j] ; p < nz ; p++)
01487           Cx [p] = x [Ci [p]] ;
01488     }
01489     Cp [n] = nz ;                       /* finalize the last column of C */
01490     //cs_sprealloc (C, 0) ;               Ignoring this. Must be picked up
01491     return (C) ;
01492 }
01493 
01494 /* depth-first search and postorder of a tree rooted at node j */
01495 int cs_tdfs (int j, int k, ivector &head, XCONST ivector &next, ivector &post,
01496   ivector &stack)
01497 {
01498     int i, p, top = 0 ;
01499     stack [0] = j ;                 /* place j on the stack */
01500     while (top >= 0)                /* while (stack is not empty) */
01501     {
01502         p = stack [top] ;           /* p = top of stack */
01503         i = head [p] ;              /* i = youngest child of p */
01504         if (i == -1)
01505         {
01506             top-- ;                 /* p has no unordered children left */
01507             post [k++] = p ;        /* node p is the kth postordered node */
01508         }
01509         else
01510         {
01511             head [p] = next [i] ;   /* remove i from children of p */
01512             stack [++top] = i ;     /* start dfs on child node i */
01513         }
01514     }
01515     return (k) ;
01516 }
01517 
01518 /* p = amd(A+A') if symmetric is true, or amd(A'A) otherwise */
01519 ivector cs_amd (XCONST hs_smatrix &A)  /* Implements only order == 1: Chol*/
01520 {
01521     int d, dk, dext, lemax = 0, e, elenk, eln, i, j, k, k1,
01522         k2, k3, jlast, ln, dense, nzmax, mindeg = 0, nvi, nvj, nvk, mark, wnvi,
01523         ok, cnz, nel = 0, p, p1, p2, p3, p4, pj, pk, pk1, pk2, pn, q, t ;
01524     unsigned int h ;
01525     /* --- Construct matrix C ----------------------------------------------- */
01526 
01527     int n = A.n ;
01528 
01529     hs_smatrix AT(n,A.nzmax);
01530     cs_transpose(A,0,AT);
01531 
01532     dense = CS_MAX (16, 10 * sqrt ((double) n)) ;   /* find dense threshold */
01533     dense = CS_MIN (n-2, dense) ;
01534 
01535     hs_smatrix C = cs_add(A,AT);
01536     cs_fkeep (C, &cs_diag, NULL);        // drop diagonal entries
01537     cnz = C.p [n] ;
01538     ivector P(0,n);
01539 
01540     t = cnz + cnz/5 + 2*n ;                    // add elbow room to C
01541     C.reallocate(t);
01542     ivector & Cp=C.p;
01543 
01544     ivector len(0,n);
01545     len.initialize();
01546     ivector nv(0,n);
01547     ivector next(0,n);
01548     ivector head(0,n);
01549     head.initialize();
01550     ivector elen(0,n);
01551     ivector degree(0,n);
01552     ivector w(0,n);
01553     ivector hhead(0,n);
01554     ivector &last = P ;                        /* use P as workspace for last */
01555 
01556     /* --- Initialize quotient graph ---------------------------------------- */
01557     for (k = 0 ; k < n ; k++) len [k] = Cp [k+1] - Cp [k] ;
01558     len [n] = 0 ;
01559     nzmax = C.nzmax ;
01560     ivector & Ci=C.i;
01561     for (i = 0 ; i <= n ; i++)
01562     {
01563         head [i] = -1 ;                            /* degree list i is empty */
01564         last [i] = -1 ;
01565         next [i] = -1 ;
01566         hhead [i] = -1 ;                    /* hash list i is empty */
01567         nv [i] = 1 ;                            /* node i is just one node */
01568         w [i] = 1 ;                            /* node i is alive */
01569         elen [i] = 0 ;                            /* Ek of node i is empty */
01570         degree [i] = len [i] ;                    /* degree of node i */
01571     }
01572     mark = cs_wclear (0, 0, w, n) ;            /* clear w */
01573     elen [n] = -2 ;                            /* n is a dead element */
01574     Cp [n] = -1 ;                            /* n is a root of assembly tree */
01575     w [n] = 0 ;                                    /* n is a dead element */
01576     /* --- Initialize degree lists ------------------------------------------ */
01577     for (i = 0 ; i < n ; i++)
01578     {
01579         d = degree [i] ;
01580         if (d == 0)                            /* node i is empty */
01581         {
01582             elen [i] = -2 ;                    /* element i is dead */
01583             nel++ ;
01584             Cp [i] = -1 ;                    /* i is a root of assembly tree */
01585             w [i] = 0 ;
01586         }
01587         else if (d > dense)                    /* node i is dense */
01588         {
01589             nv [i] = 0 ;                    /* absorb i into element n */
01590             elen [i] = -1 ;                    /* node i is dead */
01591             nel++ ;
01592             Cp [i] = CS_FLIP (n) ;
01593             nv [n]++ ;
01594         }
01595         else
01596         {
01597             if (head [d] != -1) last [head [d]] = i ;
01598             next [i] = head [d] ;            /* put node i in degree list d */
01599             head [d] = i ;
01600         }
01601     }
01602     while (nel < n)                            /* while (selecting pivots) do */
01603     {
01604         /* --- Select node of minimum approximate degree -------------------- */
01605         for (k = -1 ; mindeg < n && (k = head [mindeg]) == -1 ; mindeg++) ;
01606         if (next [k] != -1) last [next [k]] = -1 ;
01607         head [mindeg] = next [k] ;            /* remove k from degree list */
01608         elenk = elen [k] ;                    /* elenk = |Ek| */
01609         nvk = nv [k] ;                            /* # of nodes k represents */
01610         nel += nvk ;                           /* nv[k] nodes of A eliminated */
01611         /* --- Garbage collection ------------------------------------------- */
01612         if (elenk > 0 && cnz + mindeg >= nzmax)
01613         {
01614             for (j = 0 ; j < n ; j++)
01615             {
01616                 if ((p = Cp [j]) >= 0)         /* j is a live node or element */
01617                 {
01618                     Cp [j] = Ci [p] ;           /* save first entry of object */
01619                     Ci [p] = CS_FLIP (j) ;  /* first entry is now CS_FLIP(j) */
01620                 }
01621             }
01622             for (q = 0, p = 0 ; p < cnz ; ) /* scan all of memory */
01623             {
01624                 if ((j = CS_FLIP (Ci [p++])) >= 0)  /* found object j */
01625                 {
01626                     Ci [q] = Cp [j] ;        /* restore first entry of object */
01627                     Cp [j] = q++ ;            /* new pointer to object j */
01628                     for (k3 = 0 ; k3 < len [j]-1 ; k3++) Ci [q++] = Ci [p++] ;
01629                 }
01630             }
01631             cnz = q ;                          /* Ci [cnz...nzmax-1] now free */
01632         }
01633         /* --- Construct new element ---------------------------------------- */
01634         dk = 0 ;
01635         nv [k] = -nvk ;                            /* flag k as in Lk */
01636         p = Cp [k] ;
01637         pk1 = (elenk == 0) ? p : cnz ;         /* do in place if elen[k] == 0 */
01638         pk2 = pk1 ;
01639         for (k1 = 1 ; k1 <= elenk + 1 ; k1++)
01640         {
01641             if (k1 > elenk)
01642             {
01643                 e = k ;                            /* search the nodes in k */
01644                 pj = p ;                    /* list of nodes starts at Ci[pj]*/
01645                 ln = len [k] - elenk ;        /* length of list of nodes in k */
01646             }
01647             else
01648             {
01649                 e = Ci [p++] ;                    /* search the nodes in e */
01650                 pj = Cp [e] ;
01651                 ln = len [e] ;                /* length of list of nodes in e */
01652             }
01653             for (k2 = 1 ; k2 <= ln ; k2++)
01654             {
01655                 i = Ci [pj++] ;
01656                 if ((nvi = nv [i]) <= 0) continue ; /* node i dead, or seen */
01657                 dk += nvi ;                   /* degree[Lk] += size of node i */
01658                 nv [i] = -nvi ;              /* negate nv[i] to denote i in Lk*/
01659                 Ci [pk2++] = i ;            /* place i in Lk */
01660                 if (next [i] != -1) last [next [i]] = last [i] ;
01661                 if (last [i] != -1)            /* remove i from degree list */
01662                 {
01663                     next [last [i]] = next [i] ;
01664                 }
01665                 else
01666                 {
01667                     head [degree [i]] = next [i] ;
01668                 }
01669             }
01670             if (e != k)
01671             {
01672                 Cp [e] = CS_FLIP (k) ;            /* absorb e into k */
01673                 w [e] = 0 ;                    /* e is now a dead element */
01674             }
01675         }
01676         if (elenk != 0) cnz = pk2 ;            /* Ci [cnz...nzmax] is free */
01677         degree [k] = dk ;                    /* external degree of k - |Lk\i| */
01678         Cp [k] = pk1 ;                      /* element k is in Ci[pk1..pk2-1] */
01679         len [k] = pk2 - pk1 ;
01680         elen [k] = -2 ;                            /* k is now an element */
01681         /* --- Find set differences ----------------------------------------- */
01682         mark = cs_wclear (mark, lemax, w, n) ;        /* clear w if necessary */
01683         for (pk = pk1 ; pk < pk2 ; pk++)    /* scan 1: find |Le\Lk| */
01684         {
01685             i = Ci [pk] ;
01686             if ((eln = elen [i]) <= 0) continue ;/* skip if elen[i] empty */
01687             nvi = -nv [i] ;                         /* nv [i] was negated */
01688             wnvi = mark - nvi ;
01689             for (p = Cp [i] ; p <= Cp [i] + eln - 1 ; p++)  /* scan Ei */
01690             {
01691                 e = Ci [p] ;
01692                 if (w [e] >= mark)
01693                 {
01694                     w [e] -= nvi ;            /* decrement |Le\Lk| */
01695                 }
01696                 else if (w [e] != 0)            /* ensure e is a live element */
01697                 {
01698                     w [e] = degree [e] + wnvi ;  /* 1st time e seen in scan 1 */
01699                 }
01700             }
01701         }
01702         /* --- Degree update ------------------------------------------------ */
01703         for (pk = pk1 ; pk < pk2 ; pk++)    /* scan2: degree update */
01704         {
01705             i = Ci [pk] ;                    /* consider node i in Lk */
01706             p1 = Cp [i] ;
01707             p2 = p1 + elen [i] - 1 ;
01708             pn = p1 ;
01709             for (h = 0, d = 0, p = p1 ; p <= p2 ; p++)    /* scan Ei */
01710             {
01711                 e = Ci [p] ;
01712                 if (w [e] != 0)                 /* e is an unabsorbed element */
01713                 {
01714                     dext = w [e] - mark ;   /* dext = |Le\Lk| */
01715                     if (dext > 0)
01716                     {
01717                         d += dext ;            /* sum up the set differences */
01718                         Ci [pn++] = e ;            /* keep e in Ei */
01719                         h += e ;            /* compute the hash of node i */
01720                     }
01721                     else
01722                     {
01723                         Cp [e] = CS_FLIP (k) ;     /* aggressive absorb. e->k */
01724                         w [e] = 0 ;                /* e is a dead element */
01725                     }
01726                 }
01727             }
01728             elen [i] = pn - p1 + 1 ;            /* elen[i] = |Ei| */
01729             p3 = pn ;
01730             p4 = p1 + len [i] ;
01731             for (p = p2 + 1 ; p < p4 ; p++) /* prune edges in Ai */
01732             {
01733                 j = Ci [p] ;
01734                 if ((nvj = nv [j]) <= 0) continue ; /* node j dead or in Lk */
01735                 d += nvj ;                    /* degree(i) += |j| */
01736                 Ci [pn++] = j ;                  /* place j in node list of i */
01737                 h += j ;                    /* compute hash for node i */
01738             }
01739             if (d == 0)                         /* check for mass elimination */
01740             {
01741                 Cp [i] = CS_FLIP (k) ;            /* absorb i into k */
01742                 nvi = -nv [i] ;
01743                 dk -= nvi ;                    /* |Lk| -= |i| */
01744                 nvk += nvi ;                    /* |k| += nv[i] */
01745                 nel += nvi ;
01746                 nv [i] = 0 ;
01747                 elen [i] = -1 ;                    /* node i is dead */
01748             }
01749             else
01750             {
01751                 degree [i] = CS_MIN (degree [i], d) ;     /* update degree(i) */
01752                 Ci [pn] = Ci [p3] ;            /* move first node to end */
01753                 Ci [p3] = Ci [p1] ;            /* move 1st el. to end of Ei */
01754                 Ci [p1] = k ;                /* add k as 1st element in of Ei */
01755                 len [i] = pn - p1 + 1 ;     /* new len of adj. list of node i */
01756                 h %= n ;                    /* finalize hash of i */
01757                 next [i] = hhead [h] ;            /* place i in hash bucket */
01758                 hhead [h] = i ;
01759                 last [i] = h ;                   /* save hash of i in last[i] */
01760             }
01761         }                                    /* scan2 is done */
01762         degree [k] = dk ;                    /* finalize |Lk| */
01763         lemax = CS_MAX (lemax, dk) ;
01764         mark = cs_wclear (mark+lemax, lemax, w, n) ;        /* clear w */
01765         /* --- Supernode detection ------------------------------------------ */
01766         for (pk = pk1 ; pk < pk2 ; pk++)
01767         {
01768             i = Ci [pk] ;
01769             if (nv [i] >= 0) continue ;                /* skip if i is dead */
01770             h = last [i] ;                      /* scan hash bucket of node i */
01771             i = hhead [h] ;
01772             hhead [h] = -1 ;                     /* hash bucket will be empty */
01773             for ( ; i != -1 && next [i] != -1 ; i = next [i], mark++)
01774             {
01775                 ln = len [i] ;
01776                 eln = elen [i] ;
01777                 for (p = Cp [i]+1 ; p <= Cp [i] + ln-1 ; p++) w [Ci [p]] = mark;
01778                 jlast = i ;
01779                 for (j = next [i] ; j != -1 ; )       /* compare i with all j */
01780                 {
01781                     ok = (len [j] == ln) && (elen [j] == eln) ;
01782                     for (p = Cp [j] + 1 ; ok && p <= Cp [j] + ln - 1 ; p++)
01783                     {
01784                         if (w [Ci [p]] != mark) ok = 0 ;    /* compare i and j*/
01785                     }
01786                     if (ok)                        /* i and j are identical */
01787                     {
01788                         Cp [j] = CS_FLIP (i) ;        /* absorb j into i */
01789                         nv [i] += nv [j] ;
01790                         nv [j] = 0 ;
01791                         elen [j] = -1 ;                /* node j is dead */
01792                         j = next [j] ;           /* delete j from hash bucket */
01793                         next [jlast] = j ;
01794                     }
01795                     else
01796                     {
01797                         jlast = j ;                /* j and i are different */
01798                         j = next [j] ;
01799                     }
01800                 }
01801             }
01802         }
01803         /* --- Finalize new element------------------------------------------ */
01804         for (p = pk1, pk = pk1 ; pk < pk2 ; pk++)   /* finalize Lk */
01805         {
01806             i = Ci [pk] ;
01807             if ((nvi = -nv [i]) <= 0) continue ;/* skip if i is dead */
01808             nv [i] = nvi ;                        /* restore nv[i] */
01809             d = degree [i] + dk - nvi ;         /* compute external degree(i) */
01810             d = CS_MIN (d, n - nel - nvi) ;
01811             if (head [d] != -1) last [head [d]] = i ;
01812             next [i] = head [d] ;                /* put i back in degree list */
01813             last [i] = -1 ;
01814             head [d] = i ;
01815             mindeg = CS_MIN (mindeg, d) ;        /* find new minimum degree */
01816             degree [i] = d ;
01817             Ci [p++] = i ;                        /* place i in Lk */
01818         }
01819         nv [k] = nvk ;                            /* # nodes absorbed into k */
01820         if ((len [k] = p-pk1) == 0)         /* length of adj list of element k*/
01821         {
01822             Cp [k] = -1 ;                    /* k is a root of the tree */
01823             w [k] = 0 ;                            /* k is now a dead element */
01824         }
01825         if (elenk != 0) cnz = p ;            /* free unused space in Lk */
01826     }
01827     /* --- Postordering ----------------------------------------------------- */
01828     for (i = 0 ; i < n ; i++) Cp [i] = CS_FLIP (Cp [i]) ;/* fix assembly tree */
01829     for (j = 0 ; j <= n ; j++) head [j] = -1 ;
01830     for (j = n ; j >= 0 ; j--)              /* place unordered nodes in lists */
01831     {
01832         if (nv [j] > 0) continue ;            /* skip if j is an element */
01833         next [j] = head [Cp [j]] ;           /* place j in list of its parent */
01834         head [Cp [j]] = j ;
01835     }
01836     for (e = n ; e >= 0 ; e--)                    /* place elements in lists */
01837     {
01838         if (nv [e] <= 0) continue ;            /* skip unless e is an element */
01839         if (Cp [e] != -1)
01840         {
01841             next [e] = head [Cp [e]] ;       /* place e in list of its parent */
01842             head [Cp [e]] = e ;
01843         }
01844     }
01845     for (k = 0, i = 0 ; i <= n ; i++)          /* postorder the assembly tree */
01846     {
01847         if (Cp [i] == -1) k = cs_tdfs (i, k, head, next, P, w) ;
01848     }
01849     return (P) ;
01850 }
01851 
01852 ivector cs_amd (XCONST dvar_hs_smatrix &A) /* Implements only order == 1: Chol*/
01853 {
01854     int d, dk, dext, lemax = 0, e, elenk, eln, i, j, k, k1,
01855         k2, k3, jlast, ln, dense, nzmax, mindeg = 0, nvi, nvj, nvk, mark, wnvi,
01856         ok, cnz, nel = 0, p, p1, p2, p3, p4, pj, pk, pk1, pk2, pn, q, t ;
01857     unsigned int h ;
01858     /* --- Construct matrix C ----------------------------------------------- */
01859 
01860     int n = A.n ;
01861 
01862     dvar_hs_smatrix AT(n,A.nzmax);
01863     cs_transpose(A,0,AT);
01864 
01865     dense = CS_MAX (16, 10 * sqrt ((double) n)) ;   /* find dense threshold */
01866     dense = CS_MIN (n-2, dense) ;
01867 
01868     dvar_hs_smatrix C = cs_add(A,AT);
01869     cs_fkeep (C, &cs_diag, NULL);        // drop diagonal entries
01870     cnz = C.p [n] ;
01871     ivector P(0,n);
01872 
01873     t = cnz + cnz/5 + 2*n ;                    // add elbow room to C
01874     C.reallocate(t);
01875     ivector & Cp=C.p;
01876 
01877     ivector len(0,n);
01878     len.initialize();
01879     ivector nv(0,n);
01880     ivector next(0,n);
01881     ivector head(0,n);
01882     head.initialize();
01883     ivector elen(0,n);
01884     ivector degree(0,n);
01885     ivector w(0,n);
01886     ivector hhead(0,n);
01887     ivector &last = P ;                        /* use P as workspace for last */
01888 
01889     /* --- Initialize quotient graph ---------------------------------------- */
01890     for (k = 0 ; k < n ; k++) len [k] = Cp [k+1] - Cp [k] ;
01891     len [n] = 0 ;
01892     nzmax = C.nzmax ;
01893     ivector & Ci=C.i;
01894     for (i = 0 ; i <= n ; i++)
01895     {
01896         head [i] = -1 ;                            /* degree list i is empty */
01897         last [i] = -1 ;
01898         next [i] = -1 ;
01899         hhead [i] = -1 ;                    /* hash list i is empty */
01900         nv [i] = 1 ;                            /* node i is just one node */
01901         w [i] = 1 ;                            /* node i is alive */
01902         elen [i] = 0 ;                            /* Ek of node i is empty */
01903         degree [i] = len [i] ;                    /* degree of node i */
01904     }
01905     mark = cs_wclear (0, 0, w, n) ;            /* clear w */
01906     elen [n] = -2 ;                            /* n is a dead element */
01907     Cp [n] = -1 ;                            /* n is a root of assembly tree */
01908     w [n] = 0 ;                                    /* n is a dead element */
01909     /* --- Initialize degree lists ------------------------------------------ */
01910     for (i = 0 ; i < n ; i++)
01911     {
01912         d = degree [i] ;
01913         if (d == 0)                            /* node i is empty */
01914         {
01915             elen [i] = -2 ;                    /* element i is dead */
01916             nel++ ;
01917             Cp [i] = -1 ;                    /* i is a root of assembly tree */
01918             w [i] = 0 ;
01919         }
01920         else if (d > dense)                    /* node i is dense */
01921         {
01922             nv [i] = 0 ;                    /* absorb i into element n */
01923             elen [i] = -1 ;                    /* node i is dead */
01924             nel++ ;
01925             Cp [i] = CS_FLIP (n) ;
01926             nv [n]++ ;
01927         }
01928         else
01929         {
01930             if (head [d] != -1) last [head [d]] = i ;
01931             next [i] = head [d] ;            /* put node i in degree list d */
01932             head [d] = i ;
01933         }
01934     }
01935     while (nel < n)                            /* while (selecting pivots) do */
01936     {
01937         /* --- Select node of minimum approximate degree -------------------- */
01938         for (k = -1 ; mindeg < n && (k = head [mindeg]) == -1 ; mindeg++) ;
01939         if (next [k] != -1) last [next [k]] = -1 ;
01940         head [mindeg] = next [k] ;            /* remove k from degree list */
01941         elenk = elen [k] ;                    /* elenk = |Ek| */
01942         nvk = nv [k] ;                            /* # of nodes k represents */
01943         nel += nvk ;                           /* nv[k] nodes of A eliminated */
01944         /* --- Garbage collection ------------------------------------------- */
01945         if (elenk > 0 && cnz + mindeg >= nzmax)
01946         {
01947             for (j = 0 ; j < n ; j++)
01948             {
01949                 if ((p = Cp [j]) >= 0)         /* j is a live node or element */
01950                 {
01951                     Cp [j] = Ci [p] ;           /* save first entry of object */
01952                     Ci [p] = CS_FLIP (j) ;  /* first entry is now CS_FLIP(j) */
01953                 }
01954             }
01955             for (q = 0, p = 0 ; p < cnz ; ) /* scan all of memory */
01956             {
01957                 if ((j = CS_FLIP (Ci [p++])) >= 0)  /* found object j */
01958                 {
01959                     Ci [q] = Cp [j] ;        /* restore first entry of object */
01960                     Cp [j] = q++ ;            /* new pointer to object j */
01961                     for (k3 = 0 ; k3 < len [j]-1 ; k3++) Ci [q++] = Ci [p++] ;
01962                 }
01963             }
01964             cnz = q ;                          /* Ci [cnz...nzmax-1] now free */
01965         }
01966         /* --- Construct new element ---------------------------------------- */
01967         dk = 0 ;
01968         nv [k] = -nvk ;                            /* flag k as in Lk */
01969         p = Cp [k] ;
01970         pk1 = (elenk == 0) ? p : cnz ;         /* do in place if elen[k] == 0 */
01971         pk2 = pk1 ;
01972         for (k1 = 1 ; k1 <= elenk + 1 ; k1++)
01973         {
01974             if (k1 > elenk)
01975             {
01976                 e = k ;                            /* search the nodes in k */
01977                 pj = p ;                    /* list of nodes starts at Ci[pj]*/
01978                 ln = len [k] - elenk ;        /* length of list of nodes in k */
01979             }
01980             else
01981             {
01982                 e = Ci [p++] ;                    /* search the nodes in e */
01983                 pj = Cp [e] ;
01984                 ln = len [e] ;                /* length of list of nodes in e */
01985             }
01986             for (k2 = 1 ; k2 <= ln ; k2++)
01987             {
01988                 i = Ci [pj++] ;
01989                 if ((nvi = nv [i]) <= 0) continue ; /* node i dead, or seen */
01990                 dk += nvi ;                   /* degree[Lk] += size of node i */
01991                 nv [i] = -nvi ;              /* negate nv[i] to denote i in Lk*/
01992                 Ci [pk2++] = i ;            /* place i in Lk */
01993                 if (next [i] != -1) last [next [i]] = last [i] ;
01994                 if (last [i] != -1)            /* remove i from degree list */
01995                 {
01996                     next [last [i]] = next [i] ;
01997                 }
01998                 else
01999                 {
02000                     head [degree [i]] = next [i] ;
02001                 }
02002             }
02003             if (e != k)
02004             {
02005                 Cp [e] = CS_FLIP (k) ;            /* absorb e into k */
02006                 w [e] = 0 ;                    /* e is now a dead element */
02007             }
02008         }
02009         if (elenk != 0) cnz = pk2 ;            /* Ci [cnz...nzmax] is free */
02010         degree [k] = dk ;                    /* external degree of k - |Lk\i| */
02011         Cp [k] = pk1 ;                      /* element k is in Ci[pk1..pk2-1] */
02012         len [k] = pk2 - pk1 ;
02013         elen [k] = -2 ;                            /* k is now an element */
02014         /* --- Find set differences ----------------------------------------- */
02015         mark = cs_wclear (mark, lemax, w, n) ;        /* clear w if necessary */
02016         for (pk = pk1 ; pk < pk2 ; pk++)    /* scan 1: find |Le\Lk| */
02017         {
02018             i = Ci [pk] ;
02019             if ((eln = elen [i]) <= 0) continue ;/* skip if elen[i] empty */
02020             nvi = -nv [i] ;                         /* nv [i] was negated */
02021             wnvi = mark - nvi ;
02022             for (p = Cp [i] ; p <= Cp [i] + eln - 1 ; p++)  /* scan Ei */
02023             {
02024                 e = Ci [p] ;
02025                 if (w [e] >= mark)
02026                 {
02027                     w [e] -= nvi ;            /* decrement |Le\Lk| */
02028                 }
02029                 else if (w [e] != 0)            /* ensure e is a live element */
02030                 {
02031                     w [e] = degree [e] + wnvi ;  /* 1st time e seen in scan 1 */
02032                 }
02033             }
02034         }
02035         /* --- Degree update ------------------------------------------------ */
02036         for (pk = pk1 ; pk < pk2 ; pk++)    /* scan2: degree update */
02037         {
02038             i = Ci [pk] ;                    /* consider node i in Lk */
02039             p1 = Cp [i] ;
02040             p2 = p1 + elen [i] - 1 ;
02041             pn = p1 ;
02042             for (h = 0, d = 0, p = p1 ; p <= p2 ; p++)    /* scan Ei */
02043             {
02044                 e = Ci [p] ;
02045                 if (w [e] != 0)                 /* e is an unabsorbed element */
02046                 {
02047                     dext = w [e] - mark ;   /* dext = |Le\Lk| */
02048                     if (dext > 0)
02049                     {
02050                         d += dext ;            /* sum up the set differences */
02051                         Ci [pn++] = e ;            /* keep e in Ei */
02052                         h += e ;            /* compute the hash of node i */
02053                     }
02054                     else
02055                     {
02056                         Cp [e] = CS_FLIP (k) ;     /* aggressive absorb. e->k */
02057                         w [e] = 0 ;                /* e is a dead element */
02058                     }
02059                 }
02060             }
02061             elen [i] = pn - p1 + 1 ;            /* elen[i] = |Ei| */
02062             p3 = pn ;
02063             p4 = p1 + len [i] ;
02064             for (p = p2 + 1 ; p < p4 ; p++) /* prune edges in Ai */
02065             {
02066                 j = Ci [p] ;
02067                 if ((nvj = nv [j]) <= 0) continue ; /* node j dead or in Lk */
02068                 d += nvj ;                    /* degree(i) += |j| */
02069                 Ci [pn++] = j ;                  /* place j in node list of i */
02070                 h += j ;                    /* compute hash for node i */
02071             }
02072             if (d == 0)                         /* check for mass elimination */
02073             {
02074                 Cp [i] = CS_FLIP (k) ;            /* absorb i into k */
02075                 nvi = -nv [i] ;
02076                 dk -= nvi ;                    /* |Lk| -= |i| */
02077                 nvk += nvi ;                    /* |k| += nv[i] */
02078                 nel += nvi ;
02079                 nv [i] = 0 ;
02080                 elen [i] = -1 ;                    /* node i is dead */
02081             }
02082             else
02083             {
02084                 degree [i] = CS_MIN (degree [i], d) ;     /* update degree(i) */
02085                 Ci [pn] = Ci [p3] ;            /* move first node to end */
02086                 Ci [p3] = Ci [p1] ;            /* move 1st el. to end of Ei */
02087                 Ci [p1] = k ;                /* add k as 1st element in of Ei */
02088                 len [i] = pn - p1 + 1 ;     /* new len of adj. list of node i */
02089                 h %= n ;                    /* finalize hash of i */
02090                 next [i] = hhead [h] ;            /* place i in hash bucket */
02091                 hhead [h] = i ;
02092                 last [i] = h ;                   /* save hash of i in last[i] */
02093             }
02094         }                                    /* scan2 is done */
02095         degree [k] = dk ;                    /* finalize |Lk| */
02096         lemax = CS_MAX (lemax, dk) ;
02097         mark = cs_wclear (mark+lemax, lemax, w, n) ;        /* clear w */
02098         /* --- Supernode detection ------------------------------------------ */
02099         for (pk = pk1 ; pk < pk2 ; pk++)
02100         {
02101             i = Ci [pk] ;
02102             if (nv [i] >= 0) continue ;                /* skip if i is dead */
02103             h = last [i] ;                      /* scan hash bucket of node i */
02104             i = hhead [h] ;
02105             hhead [h] = -1 ;                     /* hash bucket will be empty */
02106             for ( ; i != -1 && next [i] != -1 ; i = next [i], mark++)
02107             {
02108                 ln = len [i] ;
02109                 eln = elen [i] ;
02110                 for (p = Cp [i]+1 ; p <= Cp [i] + ln-1 ; p++) w [Ci [p]] = mark;
02111                 jlast = i ;
02112                 for (j = next [i] ; j != -1 ; )       /* compare i with all j */
02113                 {
02114                     ok = (len [j] == ln) && (elen [j] == eln) ;
02115                     for (p = Cp [j] + 1 ; ok && p <= Cp [j] + ln - 1 ; p++)
02116                     {
02117                         if (w [Ci [p]] != mark) ok = 0 ;    /* compare i and j*/
02118                     }
02119                     if (ok)                        /* i and j are identical */
02120                     {
02121                         Cp [j] = CS_FLIP (i) ;        /* absorb j into i */
02122                         nv [i] += nv [j] ;
02123                         nv [j] = 0 ;
02124                         elen [j] = -1 ;                /* node j is dead */
02125                         j = next [j] ;           /* delete j from hash bucket */
02126                         next [jlast] = j ;
02127                     }
02128                     else
02129                     {
02130                         jlast = j ;                /* j and i are different */
02131                         j = next [j] ;
02132                     }
02133                 }
02134             }
02135         }
02136         /* --- Finalize new element------------------------------------------ */
02137         for (p = pk1, pk = pk1 ; pk < pk2 ; pk++)   /* finalize Lk */
02138         {
02139             i = Ci [pk] ;
02140             if ((nvi = -nv [i]) <= 0) continue ;/* skip if i is dead */
02141             nv [i] = nvi ;                        /* restore nv[i] */
02142             d = degree [i] + dk - nvi ;         /* compute external degree(i) */
02143             d = CS_MIN (d, n - nel - nvi) ;
02144             if (head [d] != -1) last [head [d]] = i ;
02145             next [i] = head [d] ;                /* put i back in degree list */
02146             last [i] = -1 ;
02147             head [d] = i ;
02148             mindeg = CS_MIN (mindeg, d) ;        /* find new minimum degree */
02149             degree [i] = d ;
02150             Ci [p++] = i ;                        /* place i in Lk */
02151         }
02152         nv [k] = nvk ;                            /* # nodes absorbed into k */
02153         if ((len [k] = p-pk1) == 0)         /* length of adj list of element k*/
02154         {
02155             Cp [k] = -1 ;                    /* k is a root of the tree */
02156             w [k] = 0 ;                            /* k is now a dead element */
02157         }
02158         if (elenk != 0) cnz = p ;            /* free unused space in Lk */
02159     }
02160     /* --- Postordering ----------------------------------------------------- */
02161     for (i = 0 ; i < n ; i++) Cp [i] = CS_FLIP (Cp [i]) ;/* fix assembly tree */
02162     for (j = 0 ; j <= n ; j++) head [j] = -1 ;
02163     for (j = n ; j >= 0 ; j--)              /* place unordered nodes in lists */
02164     {
02165         if (nv [j] > 0) continue ;            /* skip if j is an element */
02166         next [j] = head [Cp [j]] ;           /* place j in list of its parent */
02167         head [Cp [j]] = j ;
02168     }
02169     for (e = n ; e >= 0 ; e--)                    /* place elements in lists */
02170     {
02171         if (nv [e] <= 0) continue ;            /* skip unless e is an element */
02172         if (Cp [e] != -1)
02173         {
02174             next [e] = head [Cp [e]] ;       /* place e in list of its parent */
02175             head [Cp [e]] = e ;
02176         }
02177     }
02178     for (k = 0, i = 0 ; i <= n ; i++)          /* postorder the assembly tree */
02179     {
02180         if (Cp [i] == -1) k = cs_tdfs (i, k, head, next, P, w) ;
02181     }
02182     return (P) ;
02183 }
02184 
02185 /* compute the etree of A (using triu(A), or A'A without forming A'A */
02186 ivector cs_etree (XCONST hs_smatrix &_A)
02187 {
02188     ADUNCONST(hs_smatrix,A)
02189     int i, k, p, inext;
02190 
02191     int n = A.n ;
02192     ivector & Ap=A.p;
02193     ivector & Ai=A.i;
02194 
02195     ivector parent(0,n-1);
02196     parent.initialize();
02197     ivector w(0,n-1);                   /* get workspace */
02198     w.initialize();
02199     ivector &ancestor = w ;
02200     for (k = 0 ; k < n ; k++)
02201     {
02202         parent [k] = -1 ;                   /* node k has no parent yet */
02203         ancestor [k] = -1 ;                 /* nor does k have an ancestor */
02204         for (p = Ap [k] ; p < Ap [k+1] ; p++)
02205         {
02206             i = Ai [p] ;
02207             for ( ; i != -1 && i < k ; i = inext)   /* traverse from i to k */
02208             {
02209                 inext = ancestor [i] ;              /* inext = ancestor of i */
02210                 ancestor [i] = k ;                  /* path compression */
02211                 if (inext == -1) parent [i] = k ;   /* no anc., parent is k */
02212             }
02213         }
02214     }
02215     return (parent) ;
02216 }
02217 
02218 ivector cs_etree (XCONST dvar_hs_smatrix &_A)
02219 {
02220     ADUNCONST(dvar_hs_smatrix,A)
02221     int i, k, p, inext;
02222 
02223     int n = A.n ;
02224     ivector & Ap=A.p;
02225     ivector & Ai=A.i;
02226 
02227     ivector parent(0,n-1);
02228     parent.initialize();
02229     ivector w(0,n-1);                   /* get workspace */
02230     w.initialize();
02231     ivector &ancestor = w ;
02232     for (k = 0 ; k < n ; k++)
02233     {
02234         parent [k] = -1 ;                   /* node k has no parent yet */
02235         ancestor [k] = -1 ;                 /* nor does k have an ancestor */
02236         for (p = Ap [k] ; p < Ap [k+1] ; p++)
02237         {
02238             i = Ai [p] ;
02239             for ( ; i != -1 && i < k ; i = inext)   /* traverse from i to k */
02240             {
02241                 inext = ancestor [i] ;              /* inext = ancestor of i */
02242                 ancestor [i] = k ;                  /* path compression */
02243                 if (inext == -1) parent [i] = k ;   /* no anc., parent is k */
02244             }
02245         }
02246     }
02247     return (parent) ;
02248 }
02249 
02250 /* post order a forest */
02251 ivector cs_post (XCONST ivector &parent, int n)
02252 {
02253     int j, k = 0;
02254 
02255     ivector post(0,n-1);
02256     post.initialize();
02257     ivector head(0,n-1);
02258     ivector next(0,n-1);
02259     next.initialize();
02260     ivector stack(0,n-1);
02261     stack.initialize();
02262 
02263     for (j = 0 ; j < n ; j++) head [j] = -1 ;           /* empty linked lists */
02264     for (j = n-1 ; j >= 0 ; j--)            /* traverse nodes in reverse order*/
02265     {
02266         if (parent [j] == -1) continue ;    /* j is a root */
02267         next [j] = head [parent [j]] ;      /* add j to list of its parent */
02268         head [parent [j]] = j ;
02269     }
02270     for (j = 0 ; j < n ; j++)
02271     {
02272         if (parent [j] != -1) continue ;    /* skip j if it is not a root */
02273         k = cs_tdfs (j, k, head, next, post, stack) ;
02274     }
02275     return (post) ;
02276 }
02277 
02278 
02279 /* consider A(i,j), node j in ith row subtree and return lca(jprev,j) */
02280 int cs_leaf (int i, int j, XCONST ivector &first, ivector &maxfirst,
02281   ivector &prevleaf, ivector &ancestor, int *jleaf)
02282 {
02283     int q, s, sparent, jprev ;
02284     *jleaf = 0 ;
02285     if (i <= j || first [j] <= maxfirst [i]) return (-1) ;  /* j not a leaf */
02286     maxfirst [i] = first [j] ;      /* update max first[j] seen so far */
02287     jprev = prevleaf [i] ;          /* jprev = previous leaf of ith subtree */
02288     prevleaf [i] = j ;
02289     *jleaf = (jprev == -1) ? 1: 2 ; /* j is first or subsequent leaf */
02290     if (*jleaf == 1) return (i) ;   /* if 1st leaf, q = root of ith subtree */
02291     for (q = jprev ; q != ancestor [q] ; q = ancestor [q]) ;
02292     for (s = jprev ; s != q ; s = sparent)
02293     {
02294         sparent = ancestor [s] ;    /* path compression */
02295         ancestor [s] = q ;
02296     }
02297     return (q) ;                    /* q = least common ancester (jprev,j) */
02298 }
02299 
02300 /* column counts of LL'=A or LL'=A'A, given parent & post ordering */
02301 ivector cs_counts (XCONST hs_smatrix &A, XCONST ivector &parent,
02302   XCONST ivector &post)
02303 {
02304     int i, j, k, J, p, q, jleaf;
02305 
02306     int n = A.n ;
02307     ivector delta(0,n-1);
02308     delta.initialize();
02309     ivector& colcount = delta;
02310 
02311     hs_smatrix AT(n,A.nzmax);
02312     cs_transpose (A,0,AT) ;                                /* AT = A' */
02313 
02314     ivector ancestor(0,n-1);
02315     ancestor = -1;
02316     ivector maxfirst(0,n-1);
02317     maxfirst = -1;
02318     ivector prevleaf(0,n-1);
02319     prevleaf = -1;
02320     ivector first(0,n-1);
02321     first = -1;
02322 
02323     for (k = 0 ; k < n ; k++)                        /* find first [j] */
02324     {
02325         j = post [k] ;
02326         delta [j] = (first [j] == -1) ? 1 : 0 ;  /* delta[j]=1 if j is a leaf */
02327         for ( ; j != -1 && first [j] == -1 ; j = parent [j]) first [j] = k ;
02328     }
02329 
02330     ivector & ATp=AT.p;
02331     ivector & ATi=AT.i;
02332 
02333     for (i = 0 ; i < n ; i++) ancestor [i] = i ; /* each node in its own set */
02334     for (k = 0 ; k < n ; k++)
02335     {
02336         j = post [k] ;              /* j is the kth node in postordered etree */
02337         if (parent [j] != -1) delta [parent [j]]-- ;       /* j is not a root */
02338         for (J = j ; J != -1 ; J = -1)        /* J=j for LL'=A case */
02339         {
02340             for (p = ATp [J] ; p < ATp [J+1] ; p++)
02341             {
02342                 i = ATi [p] ;
02343                 q = cs_leaf (i, j, first, maxfirst, prevleaf, ancestor, &jleaf);
02344                 if (jleaf >= 1) delta [j]++ ;   /* A(i,j) is in skeleton */
02345                 if (jleaf == 2) delta [q]-- ;     /* account for overlap in q */
02346             }
02347         }
02348         if (parent [j] != -1) ancestor [j] = parent [j] ;
02349     }
02350     for (j = 0 ; j < n ; j++)                /* sum up delta's of each child */
02351     {
02352         if (parent [j] != -1) colcount [parent [j]] += colcount [j] ;
02353     }
02354     return (colcount) ;
02355 }
02356 
02357 ivector cs_counts (XCONST dvar_hs_smatrix &A, XCONST ivector &parent,
02358   XCONST ivector &post)
02359 {
02360     int i, j, k, J, p, q, jleaf;
02361 
02362     int n = A.n ;
02363     ivector delta(0,n-1);
02364     delta.initialize();
02365     ivector& colcount = delta;
02366 
02367     dvar_hs_smatrix AT(n,A.nzmax);
02368     cs_transpose (A,0,AT) ;                                /* AT = A' */
02369 
02370     ivector ancestor(0,n-1);
02371     ancestor = -1;
02372     ivector maxfirst(0,n-1);
02373     maxfirst = -1;
02374     ivector prevleaf(0,n-1);
02375     prevleaf = -1;
02376     ivector first(0,n-1);
02377     first = -1;
02378 
02379     for (k = 0 ; k < n ; k++)                        /* find first [j] */
02380     {
02381         j = post [k] ;
02382         delta [j] = (first [j] == -1) ? 1 : 0 ;  /* delta[j]=1 if j is a leaf */
02383         for ( ; j != -1 && first [j] == -1 ; j = parent [j]) first [j] = k ;
02384     }
02385 
02386     ivector & ATp=AT.p;
02387     ivector & ATi=AT.i;
02388 
02389     for (i = 0 ; i < n ; i++) ancestor [i] = i ; /* each node in its own set */
02390     for (k = 0 ; k < n ; k++)
02391     {
02392         j = post [k] ;              /* j is the kth node in postordered etree */
02393         if (parent [j] != -1) delta [parent [j]]-- ;       /* j is not a root */
02394         for (J = j ; J != -1 ; J = -1)        /* J=j for LL'=A case */
02395         {
02396             for (p = ATp [J] ; p < ATp [J+1] ; p++)
02397             {
02398                 i = ATi [p] ;
02399                 q = cs_leaf (i, j, first, maxfirst, prevleaf, ancestor, &jleaf);
02400                 if (jleaf >= 1) delta [j]++ ;   /* A(i,j) is in skeleton */
02401                 if (jleaf == 2) delta [q]-- ;     /* account for overlap in q */
02402             }
02403         }
02404         if (parent [j] != -1) ancestor [j] = parent [j] ;
02405     }
02406     for (j = 0 ; j < n ; j++)                /* sum up delta's of each child */
02407     {
02408         if (parent [j] != -1) colcount [parent [j]] += colcount [j] ;
02409     }
02410     return (colcount) ;
02411 }
02412 
02413 /* pinv = p', or p = pinv' */
02414 ivector cs_pinv (XCONST ivector &p, int n)
02415 {
02416     int k ;
02417     ivector pinv(0,n-1);
02418     pinv.initialize();
02419     for (k = 0 ; k < n ; k++) pinv [p [k]] = k ;/* invert the permutation */
02420     return (pinv) ;                             /* return result */
02421 }
02422 
02423 /* Constructor that does symbolic Cholesky  */
02424  //hs_symbolic::hs_symbolic(int _n, XCONST dmatrix &T, int order)
02425  //{
02426  //
02427  //    if (order != 0 && order != 1 )
02428  //    {
02429  //        cout << "!!! hs_symbolic: only order = 0,1 allowed" << endl;
02430  //        exit(0);
02431  //    }
02432  //
02433  //    hs_smatrix A(_n,T);
02434  //    n = _n;
02435  //
02436  //    // Allocate symbolic structure
02437  //    pinv.allocate(0,n-1);
02438  //    parent.allocate(0,n-1);
02439  //    cp.allocate(0,n);
02440  //    cp = 0;
02441  //
02442  //    hs_smatrix C(A);
02443  //    C = A;
02444  //
02445  //    if(order==0)
02446  //    {
02447  //      pinv(0) = -1;
02448  //    }
02449  //    else
02450  //    {
02451  //      ivector P = cs_amd (A) ;          /* P = amd(A+A'), or natural */
02452  //      pinv = cs_pinv (P, n) ;           /* find inverse permutation */
02453  //      hs_symperm(A,pinv,C);
02454  //    }
02455  //
02456  //    parent = cs_etree (C) ;                     /* find etree of C */
02457  //    ivector post = cs_post (parent, n) ;  /* postorder the etree */
02458  //    /* find column counts of chol(C) */
02459  //    ivector c = cs_counts (C, parent, post) ;
02460  //    lnz = cs_cumsum (cp, c, n) ;         /* find column pointers for L */
02461  //
02462  //}
02463  //
02464 hs_symbolic::hs_symbolic(void)
02465 {
02466     n = 0;
02467 
02468     // Allocate symbolic structure
02469     pinv.allocate();
02470     parent.allocate();
02471     cp.allocate();
02472 }
02473 
02474 hs_symbolic::hs_symbolic(XCONST dcompressed_triplet &_T, int order)
02475 {
02476   ADUNCONST(dcompressed_triplet,T)
02477   int _n=T.get_n();
02478 
02479     if (order != 0 && order != 1 )
02480     {
02481         cout << "!!! hs_symbolic: only order = 0,1 allowed" << endl;
02482         exit(0);
02483     }
02484 
02485     hs_smatrix A(_n,T);
02486     n = _n;
02487 
02488     // Allocate symbolic structure
02489     pinv.allocate(0,n-1);
02490     parent.allocate(0,n-1);
02491     cp.allocate(0,n);
02492     cp = 0;
02493 
02494     hs_smatrix C(A);
02495     C = A;
02496 
02497     if(order==0)
02498     {
02499       pinv(0) = -1;
02500     }
02501     else
02502     {
02503       ivector P = cs_amd (A) ;          /* P = amd(A+A'), or natural */
02504       pinv = cs_pinv (P, n) ;           /* find inverse permutation */
02505       hs_symperm(A,pinv,C);
02506     }
02507 
02508     parent = cs_etree (C) ;                     /* find etree of C */
02509     ivector post = cs_post (parent, n) ;  /* postorder the etree */
02510    ivector c = cs_counts (C, parent, post) ; /* find column counts of chol(C) */
02511     lnz = cs_cumsum (cp, c, n) ;         /* find column pointers for L */
02512 }
02513 
02514 hs_symbolic::hs_symbolic(XCONST dvar_compressed_triplet &_T, int order)
02515 {
02516   ADUNCONST(dvar_compressed_triplet,T)
02517   int _n=T.get_n();
02518 
02519     if (order != 0 && order != 1 )
02520     {
02521         cout << "!!! hs_symbolic: only order = 0,1 allowed" << endl;
02522         exit(0);
02523     }
02524 
02525     dvar_hs_smatrix A(_n,T);
02526     n = _n;
02527 
02528     // Allocate symbolic structure
02529     pinv.allocate(0,n-1);
02530     parent.allocate(0,n-1);
02531     cp.allocate(0,n);
02532     cp = 0;
02533 
02534     dvar_hs_smatrix C(A);
02535     C = A;
02536 
02537     if(order==0)
02538     {
02539       pinv(0) = -1;
02540     }
02541     else
02542     {
02543       ivector P = cs_amd (A) ;          /* P = amd(A+A'), or natural */
02544       pinv = cs_pinv (P, n) ;           /* find inverse permutation */
02545       hs_symperm(A,pinv,C);
02546     }
02547 
02548     parent = cs_etree (C) ;                     /* find etree of C */
02549     ivector post = cs_post (parent, n) ;  /* postorder the etree */
02550    ivector c = cs_counts (C, parent, post) ; /* find column counts of chol(C) */
02551     lnz = cs_cumsum (cp, c, n) ;         /* find column pointers for L */
02552 }
02553 dvar_compressed_triplet::dvar_compressed_triplet(int mmin,int mmax,int _n,
02554   int _m)
02555 {
02556   allocate(mmin,mmax,_n,_m);
02557 }
02558 
02559 void dcompressed_triplet::initialize(void)
02560 {
02561   x.initialize();
02562 }
02563 
02564 dcompressed_triplet::dcompressed_triplet(int mmin,int mmax,int _n,int _m)
02565 {
02566   allocate(mmin,mmax,_n,_m);
02567 }
02568 
02569 
02570 void dvar_compressed_triplet::allocate(int mmin,int mmax,int _n,int _m)
02571 {
02572   n=_n;
02573   m=_m;
02574   coords.allocate(1,2,mmin,mmax);
02575   x.allocate(mmin,mmax);
02576 }
02577 
02578 void dvar_compressed_triplet::deallocate(void)
02579 {
02580   n=-1;
02581   m=-1;
02582   coords.deallocate();
02583   x.deallocate();
02584 }
02585 
02586 void dcompressed_triplet::allocate(int mmin,int mmax,int _n,int _m)
02587 {
02588   n=_n;
02589   m=_m;
02590   coords.allocate(1,2,mmin,mmax);
02591   x.allocate(mmin,mmax);
02592 }
02593 
02594 void dcompressed_triplet::deallocate(void)
02595 {
02596   n=-1;
02597   m=-1;
02598   coords.deallocate();
02599   x.deallocate();
02600 }
02601 
02602 
02603 istream & operator >> (istream & is,dcompressed_triplet & M)
02604 {
02605   int mmin=M.indexmin();
02606   int mmax=M.indexmax();
02607   for (int i=mmin;i<=mmax;i++)
02608   {
02609     is >> M(i,1);
02610     is >> M(i,2);
02611     is >> M(i);
02612   }
02613   return is;
02614 }
02615 
02616 istream & operator >> (istream & is,dvar_compressed_triplet & M)
02617 {
02618   int mmin=M.indexmin();
02619   int mmax=M.indexmax();
02620   for (int i=mmin;i<=mmax;i++)
02621   {
02622     is >> M(i,1);
02623     is >> M(i,2);
02624     is >> M(i);
02625   }
02626   return is;
02627 }
02628 
02629 hs_smatrix * return_choleski_decomp(dcompressed_triplet & st)
02630 {
02631   //ADUNCONST(dmatrix,st)
02632   int n=st.get_n();
02633 
02634   hs_smatrix HS(n,st);  // Convert triplet to working format
02635 
02636   hs_symbolic S(st,1);         // Fill reducing row-col permutation
02637   hs_smatrix * PL = new hs_smatrix(S);              // Allocates cholesky factor
02638 
02639   chol(HS,S,*PL);                  // Does numeric factorization
02640 
02641   PL->set_symbolic(S);
02642 
02643   return PL;
02644 }
02645 
02646 dvar_hs_smatrix * return_choleski_decomp(dvar_compressed_triplet & st)
02647 {
02648   //ADUNCONST(dmatrix,st)
02649   int n=st.get_n();
02650 
02651   dvar_hs_smatrix HS(n,st);  // Convert triplet to working format
02652 
02653   hs_symbolic S(st,1);         // Fill reducing row-col permutation
02654   dvar_hs_smatrix * PL = new dvar_hs_smatrix(S);    // Allocates cholesky factor
02655 
02656   chol(HS,S,*PL);                  // Does numeric factorization
02657 
02658   PL->set_symbolic(S);
02659 
02660   return PL;
02661 }
02662 
02663 dvector return_choleski_decomp_solve(dcompressed_triplet & st,dvector& eps)
02664 {
02665   //ADUNCONST(dmatrix,st)
02666   int n=st.get_n();
02667 
02668   hs_smatrix HS(n,st);  // Convert triplet to working format
02669 
02670   hs_symbolic S(st,1);         // Fill reducing row-col permutation
02671   hs_smatrix L(S);              // Allocates cholesky factor
02672 
02673   chol(HS,S,L);                  // Does numeric factorization
02674 
02675   dvector x(0,n-1);
02676   eps.shift(0);
02677   x = cs_ipvec(S.pinv, eps);
02678   eps.shift(1);
02679   x = cs_lsolve(L,x);
02680   //x = cs_ltsolve(L,x);
02681   x = cs_pvec(S.pinv,x);
02682   x.shift(1);
02683   return x;
02684 }
02685 
02686 dvector return_choleski_factor_solve(hs_smatrix * PL,dvector& eps)
02687 {
02688   //ADUNCONST(dmatrix,st)
02689   hs_smatrix& L= *PL;
02690   int n=L.m;
02691   hs_symbolic & S = L.sym;
02692   dvector x(0,n-1);
02693   eps.shift(0);
02694   x = cs_ipvec(S.pinv, eps);
02695   eps.shift(1);
02696   x = cs_lsolve(L,x);
02697   //x = cs_ltsolve(L,x);
02698   x = cs_pvec(S.pinv,x);
02699   x.shift(1);
02700   return x;
02701 }
02702 
02703 dvar_vector return_choleski_factor_solve(dvar_hs_smatrix * PL,dvector& eps)
02704 {
02705   //ADUNCONST(dmatrix,st)
02706   dvar_hs_smatrix& L= *PL;
02707   int n=L.m;
02708   hs_symbolic & S = L.sym;
02709   dvar_vector x(0,n-1);
02710   eps.shift(0);
02711   x = cs_ipvec(S.pinv, eps);
02712   eps.shift(1);
02713   x = cs_lsolve(L,x);
02714   //x = cs_ltsolve(L,x);
02715   x = cs_pvec(S.pinv,x);
02716   x.shift(1);
02717   return x;
02718 }
02719 
02720 
02721 dvector solve(dcompressed_triplet & st,dmatrix & Hess,
02722   dvector& grad)
02723 {
02724   //ADUNCONST(dmatrix,st)
02725   int nz=st.indexmax();
02726   int n=Hess.indexmax();
02727   // fill up compressed triplet with nonzero entries of the Hessian
02728   for (int i=1;i<=nz;i++)
02729   {
02730     st(i)=Hess(st(1,i),st(2,i));
02731   }
02732 
02733   hs_smatrix HS(n,st);  // Convert triplet to working format
02734 
02735   hs_symbolic S(st,1);         // Fill reducing row-col permutation
02736   hs_smatrix L(S);              // Allocates cholesky factor
02737 
02738   chol(HS,S,L);                  // Does numeric factorization
02739 
02740   dvector x(0,n-1);
02741   grad.shift(0);
02742   x = cs_ipvec(S.pinv, grad);
02743   grad.shift(1);
02744   x = cs_lsolve(L,x);
02745   x = cs_ltsolve(L,x);
02746   x = cs_pvec(S.pinv,x);
02747   x.shift(1);
02748   return x;
02749 }
02750 
02751 dvector solve(dcompressed_triplet & st,dmatrix & Hess,
02752   dvector& grad,hs_symbolic& S)
02753 {
02754   //ADUNCONST(dmatrix,st)
02755   int nz=st.indexmax();
02756   int n=Hess.indexmax();
02757   // fill up compressed triplet with nonzero entries of the Hessian
02758   for (int i=1;i<=nz;i++)
02759   {
02760     st(i)=Hess(st(1,i),st(2,i));
02761   }
02762 
02763   hs_smatrix HS(n,st);  // Convert triplet to working format
02764 
02765   //hs_symbolic S(st,1);         // Fill reducing row-col permutation
02766   hs_smatrix L(S);              // Allocates cholesky factor
02767   //hs_smatrix L1(S);              // Allocates cholesky factor
02768 
02769   //chol(HS,S,L);                  // Does numeric factorization
02770   ivector nxcount;
02771   chol(HS,S,L);                  // Does numeric factorization
02772   //tmpxchol(HS,S,L,nxcount);                  // Does numeric factorization
02773 
02774   //cout << norm2(L.get_x()-L1.get_x()) << endl;
02775   dvector x(0,n-1);
02776   grad.shift(0);
02777   x = cs_ipvec(S.pinv, grad);
02778   grad.shift(1);
02779   x = cs_lsolve(L,x);
02780   x = cs_ltsolve(L,x);
02781   x = cs_pvec(S.pinv,x);
02782   x.shift(1);
02783   return x;
02784 }
02785 
02786 dvector solve(const dcompressed_triplet & _st,const dvector& _grad,
02787   const hs_symbolic& S)
02788   {
02789     ADUNCONST(dcompressed_triplet,st)
02790     ADUNCONST(dvector,grad)
02791     int n=st.get_n();
02792     //int n=Hess.indexmax();
02793     // fill up compressed triplet with nonzero entries of the Hessian
02794 
02795     hs_smatrix HS(n,st);  // Convert triplet to working format
02796 
02797     hs_smatrix L(S);              // Allocates cholesky factor
02798 
02799     ivector nxcount;
02800     chol(HS,S,L);                  // Does numeric factorization
02801 
02802     dvector x(0,n-1);
02803     grad.shift(0);
02804     x = cs_ipvec(S.pinv, grad);
02805     grad.shift(1);
02806     x = cs_lsolve(L,x);
02807     x = cs_ltsolve(L,x);
02808     x = cs_pvec(S.pinv,x);
02809     x.shift(1);
02810     return x;
02811   }
02812 
02813 dvector solve(const dcompressed_triplet & _st,const dvector& _grad,
02814   const hs_symbolic& S,int& ierr)
02815   {
02816     ADUNCONST(dcompressed_triplet,st)
02817     ADUNCONST(dvector,grad)
02818     int n=st.get_n();
02819     //int n=Hess.indexmax();
02820     // fill up compressed triplet with nonzero entries of the Hessian
02821 
02822     hs_smatrix HS(n,st);  // Convert triplet to working format
02823 
02824     hs_smatrix L(S);              // Allocates cholesky factor
02825 
02826     ivector nxcount;
02827     ierr=chol(HS,S,L);     // 0 error 1 OK        Does numeric factorization
02828 
02829     dvector x(0,n-1);
02830   if (ierr)
02831   {
02832     grad.shift(0);
02833     x = cs_ipvec(S.pinv, grad);
02834     grad.shift(1);
02835     x = cs_lsolve(L,x);
02836     x = cs_ltsolve(L,x);
02837     x = cs_pvec(S.pinv,x);
02838     x.shift(1);
02839   }
02840   else
02841   {
02842     x.initialize();
02843     x.shift(1);
02844   }
02845   return x;
02846 }
02847 
02848 int allocated(const dcompressed_triplet & _st)
02849 {
02850   ADUNCONST(dcompressed_triplet,st)
02851   return allocated(st.get_coords());
02852 }
02853 int allocated(const dvar_compressed_triplet & _st)
02854 {
02855   ADUNCONST(dvar_compressed_triplet,st)
02856   return allocated(st.get_coords());
02857 }
02858 dvariable ln_det(dvar_compressed_triplet& VM)
02859 {
02860   int n=VM.get_n();
02861   dvar_hs_smatrix H(n,VM);
02862   hs_symbolic S(VM,1);         // Fill reducing row-col permutation
02863   dvar_hs_smatrix L(S);              // Allocates cholesky factor
02864   int ierr=chol(H,S,L);                  // Does numeric factorization
02865   if (ierr==0)
02866   {
02867     cerr << "Error matrix not positrive definite in chol" << endl;
02868     ad_exit(1);
02869   }
02870   return 2.0*ln_det(L);
02871   //return L.x(0);
02872 }
02873 
02874 
02875 dvariable ln_det(dvar_compressed_triplet& VM,hs_symbolic& S)
02876 {
02877   int n=VM.get_n();
02878   dvar_hs_smatrix H(n,VM);
02879   //hs_symbolic S(VM,1);         // Fill reducing row-col permutation
02880   dvar_hs_smatrix L(S);              // Allocates cholesky factor
02881   int ierr=chol(H,S,L);                  // Does numeric factorization
02882   if (ierr==0)
02883   {
02884     cerr << "Error matrix not positive definite in chol" << endl;
02885     ad_exit(1);
02886   }
02887   return 2.0*ln_det(L);
02888   //return L.x(0);
02889 }
02890 
02891 
02892  int check_flag=0;
02893 
02894 dvariable ln_det(dvar_compressed_triplet& VM,hs_symbolic& S,
02895   dcompressed_triplet& s)
02896 {
02897   RETURN_ARRAYS_INCREMENT();
02898   int n=VM.get_n();
02899   dvar_hs_smatrix H(n,VM);
02900   //hs_symbolic S(VM,1);         // Fill reducing row-col permutation
02901   dvar_hs_smatrix L(S);              // Allocates cholesky factor
02902   int ierr = 0;
02903   if (check_flag==0)
02904   {
02905     ierr=varchol(H,S,L,s);
02906   }
02907   else
02908   {
02909     ierr=chol(H,S,L);
02910   }
02911   if (ierr==0)
02912   {
02913     cerr << "Error matrix not positive definite in chol" << endl;
02914     ad_exit(1);
02915   }
02916   //set_gradstack_flag("AAC");
02917   dvariable tmp= 2.0*ln_det(L);
02918   RETURN_ARRAYS_DECREMENT();
02919   return tmp;
02920   //return L.x(0);
02921 }
02922 
02923 
02924 double ln_det(const dcompressed_triplet& VVM,
02925               const hs_symbolic& T)
02926 {
02927   //ADUNCONST(dcompressed_triplet,VM)
02928   //ADUNCONST(hs_symbolic,S)
02929   dcompressed_triplet& VM = (dcompressed_triplet&)VVM;
02930   hs_symbolic& S = (hs_symbolic&)T;
02931   int n=VM.get_n();
02932   hs_smatrix H(n,VM);
02933   //hs_symbolic S(VM,1);         // Fill reducing row-col permutation
02934   hs_smatrix L(S);              // Allocates cholesky factor
02935   int ierr=chol(H,S,L);                  // Does numeric factorization
02936   if (ierr==0)
02937   {
02938     cerr << "Error matrix not positrive definite in chol" << endl;
02939     ad_exit(1);
02940   }
02941   return 2.0*ln_det(L);
02942   //return L.x(0);
02943 }
02944 
02945 
02946 double ln_det(const dcompressed_triplet& VVM)
02947 {
02948   //ADUNCONST(dcompressed_triplet,VM)
02949   dcompressed_triplet& VM = (dcompressed_triplet&)VVM;
02950   int n=VM.get_n();
02951   hs_smatrix H(n,VM);
02952   hs_symbolic S(VM,1);         // Fill reducing row-col permutation
02953   hs_smatrix L(S);              // Allocates cholesky factor
02954   int ierr=chol(H,S,L);                  // Does numeric factorization
02955   if (ierr==0)
02956   {
02957     cerr << "Error matrix not positrive definite in chol" << endl;
02958     ad_exit(1);
02959   }
02960   return 2.0*ln_det(L);
02961   //return L.x(0);
02962 }
02963 
02964 
02965 int cholnew(XCONST hs_smatrix &AA, XCONST hs_symbolic &T, hs_smatrix &LL)
02966 {
02967     //ADUNCONST(hs_symbolic,S)
02968     //ADUNCONST(hs_smatrix,L)
02969     //ADUNCONST(hs_smatrix,A)
02970     hs_symbolic& S = (hs_symbolic&)T;
02971     hs_smatrix& A = (hs_smatrix&)AA;
02972     hs_smatrix& L = (hs_smatrix&)LL;
02973     double d, lki;
02974     int top, i, p, k, n;
02975 
02976     n = A.n ;
02977 
02978     ivector c(0,n-1);                              /* int workspace */
02979     ivector s(0,n-1);                                   /* int workspace */
02980     dvector x(0,n-1) ;                        /* double workspace */
02981     double xcount=0.0;                        /* double workspace */
02982 
02983     ivector & cp=S.cp;
02984     ivector & pinv=S.pinv;
02985     ivector & parent=S.parent;
02986 
02987     hs_smatrix C(A);
02988     C = A;
02989     if(S.pinv[0]!=-1)
02990       hs_symperm(A,pinv,C);
02991 
02992     ivector & Cp=C.p;
02993     ivector & Ci=C.i;
02994     dvector & Cx=C.x;
02995 
02996     ivector & Lp=L.p;
02997     ivector & Li=L.i;
02998     dvector & Lx=L.x;
02999 
03000     for (k = 0 ; k < n ; k++)
03001       Lp [k] = c [k] = cp [k] ;
03002 
03003     for (k = 0 ; k < n ; k++)            /* compute L(:,k) for L*L' = C */
03004     {
03005         /* --- Nonzero pattern of L(k,:) ------------------------------------ */
03006         top = cs_ereach (C, k, parent, s, c) ;      /* find pattern of L(k,:) */
03007         x [k] = 0 ;                                    /* x (0:k) is now zero */
03008         for (p = Cp [k] ; p < Cp [k+1] ; p++)       /* x = full(triu(C(:,k))) */
03009         {
03010             if (Ci [p] <= k) x [Ci [p]] = Cx [p] ;
03011         }
03012         d = x [k] ;                        /* d = C(k,k) */
03013         x [k] = 0 ;                        /* clear x for k+1st iteration */
03014         /* --- Triangular solve --------------------------------------------- */
03015         for ( ; top < n ; top++)    /* solve L(0:k-1,0:k-1) * x = C(:,k) */
03016         {
03017             i = s [top] ;                /* s [top..n-1] is pattern of L(k,:) */
03018             lki = x [i] / Lx [Lp [i]] ; /* L(k,i) = x (i) / L(i,i) */
03019             xcount++;
03020             x [i] = 0 ;                        /* clear x for k+1st iteration */
03021 
03022 
03023             int Lpi1=Lp[i]+1;
03024             int ci=c[i];
03025             if (Lpi1<ci)
03026             {
03027               myacc(p,Lpi1,ci,Li,x,Lx,lki);
03028             }
03029             /*
03030             for (p = Lp [i] + 1 ; p < c [i] ; p++)
03031             {
03032                 x [Li [p]] -= Lx [p] * lki ;
03033             }
03034             */
03035 
03036             d -= lki * lki ;                /* d = d - L(k,i)*L(k,i) */
03037             p = c [i]++ ;
03038             Li [p] = k ;                /* store L(k,i) in column i */
03039             Lx [p] = lki ;
03040         }
03041         /* --- Compute L(k,k) ----------------------------------------------- */
03042         if (d <= 0) return (0) ; /* not pos def */
03043         p = c [k]++ ;
03044         Li [p] = k ;                   /* store L(k,k) = sqrt (d) in column k */
03045         Lx [p] = sqrt (d) ;
03046     }
03047     Lp [n] = cp [n] ;                    /* finalize L */
03048     return (1) ;
03049 }
03050 
03051 static void dfcholeski_sparse(void);
03052 
03053 int varchol(XCONST dvar_hs_smatrix &AA, XCONST hs_symbolic &T,
03054   dvar_hs_smatrix &LL, dcompressed_triplet & sparse_triplet2)
03055  //laplace_approximation_calculator * lapprox)
03056 {
03057   RETURN_ARRAYS_INCREMENT(); //Need this statement because the function
03058   //ADUNCONST(hs_symbolic,S)
03059   //ADUNCONST(dvar_hs_smatrix,L)
03060   //ADUNCONST(dvar_hs_smatrix,A)
03061     hs_symbolic& S = (hs_symbolic&)T;
03062     dvar_hs_smatrix& A = (dvar_hs_smatrix&)AA;
03063     dvar_hs_smatrix& L = (dvar_hs_smatrix&)LL;
03064   int icount=0;
03065   double lki;
03066   double d;
03067   int top, i, p, k, n;
03068   n = A.n ;
03069   ivector c(0,n-1);                              /* int workspace */
03070   ivector s(0,n-1);                                   /* int workspace */
03071   dvector x(0,n-1) ;                        /* double workspace */
03072 
03073   ivector & cp=S.cp;
03074   ivector & pinv=S.pinv;
03075   ivector & parent=S.parent;
03076 
03077   dvar_hs_smatrix C(A);
03078   C = A;
03079   if(S.pinv[0]!=-1)
03080     hs_symperm(A,pinv,C);
03081 
03082   ivector & Cp=C.p;
03083   ivector & Ci=C.i;
03084   dvector Cx=value(C.x);
03085 
03086   ivector & Lp=L.p;
03087   ivector & Li=L.i;
03088   dvector Lx=value(L.x);
03089   int txcount=0;
03090   int lkicount=0;
03091   int tccount=0;
03092 
03093   for (k = 0 ; k < n ; k++)
03094   {
03095     Lp [k] = c [k] = cp [k] ;
03096   }
03097 
03098   for (k = 0 ; k < n ; k++)
03099   {
03100     top = cs_ereach (C, k, parent, s, c) ;
03101     x [k] = 0 ;
03102     for (p = Cp [k] ; p < Cp [k+1] ; p++)
03103     {
03104       if (Ci[p] <= k) x [Ci[p]] = Cx[p] ;
03105     }
03106     d = x[k] ;
03107     x[k] = 0.0;
03108     for ( ; top < n ; top++)
03109     {
03110       i = s[top] ;
03111       lki = x[i] / Lx[Lp[i]] ;
03112       txcount++;
03113       icount++;   // count the number of times lki is overwritten
03114       lkicount++;   // count the number of times lki is overwritten
03115       x [i] = 0 ;
03116       for (p = Lp [i] + 1 ; p < c [i] ; p++)
03117       {
03118         x[Li[p]] -= Lx[p] * lki ;
03119       }
03120       d -= lki * lki ;
03121       p = c [i]++ ;
03122       tccount++;
03123       Li [p] = k ;
03124       Lx [p] = lki ;
03125     }
03126     if (d <= 0) return (0) ;
03127     p = c [k]++ ;
03128     tccount++;
03129     Li [p] = k ;
03130     Lx [p] = sqrt (d) ;
03131   }
03132   Lp [n] = cp [n] ;
03133   xxx(txcount);
03134   int mmin=Lx.indexmin();
03135   int mmax=Lx.indexmax();
03136   for (int ii=mmin;ii<=mmax;ii++)
03137   {
03138     value(L.x(ii))=Lx(ii);
03139   }
03140   int nxcount=txcount;
03141   int nccount=tccount;
03142   int nlkicount=lkicount;
03143   save_identifier_string("ty");
03144 
03145   save_int_value(nxcount);
03146   save_int_value(nlkicount);
03147   save_int_value(nccount);
03148 
03149   save_identifier_string("tu");
03150   C.x.save_dvar_vector_position();
03151   save_identifier_string("wy");
03152   L.x.save_dvar_vector_position();
03153   save_identifier_string("iy");
03154   save_ad_pointer(&S);
03155   save_ad_pointer(&sparse_triplet2);
03156   save_identifier_string("dg");
03157   gradient_structure::GRAD_STACK1->
03158       set_gradient_stack(dfcholeski_sparse);
03159   RETURN_ARRAYS_DECREMENT(); //Need this statement because the function
03160   return (1) ;
03161 }
03162 
03163 static void dfcholeski_sparse(void)
03164 {
03165   verify_identifier_string("dg");
03166   dcompressed_triplet * sparse_triplet2  =
03167     ( dcompressed_triplet *) restore_ad_pointer();
03168   hs_symbolic & S  =
03169     *( hs_symbolic * ) restore_ad_pointer();
03170   verify_identifier_string("iy");
03171   dvar_vector_position dpos=restore_dvar_vector_position();
03172   verify_identifier_string("wy");
03173   dvar_vector_position cpos=restore_dvar_vector_position();
03174   verify_identifier_string("tu");
03175 
03176   int nccount=restore_int_value();
03177   int nlkicount=restore_int_value();
03178   int nxcount=restore_int_value();
03179 
03180   verify_identifier_string("ty");
03181 
03182     dvector  dfLx=restore_dvar_vector_derivatives(dpos);
03183     hs_smatrix A(sparse_triplet2->get_n(),
03184       *(sparse_triplet2) );
03185 
03186     //hs_symbolic& S=*(lapprox->sparse_symbolic2);
03187     double d;
03188     double lki=0;
03189     double dfd=0.0;
03190     double dflki=0.0;
03191     int top, i, p, k, n;
03192     int p2;
03193     n = A.n ;
03194 
03195     ivector cold(0,n-1);                              /* int workspace */
03196     ivector c(0,n-1);                              /* int workspace */
03197     imatrix ssave(0,n-1);                              /* int workspace */
03198     ivector s(0,n-1);                                   /* int workspace */
03199     dvector x(0,n-1) ;                        /* double workspace */
03200     dvector dfx(0,n-1) ;                        /* double workspace */
03201     dfx.initialize();
03202 
03203     ivector & cp=S.cp;
03204     ivector & pinv=S.pinv;
03205     ivector & parent=S.parent;
03206 
03207     hs_smatrix C(A);
03208     C = A;
03209     if(S.pinv[0]!=-1)
03210       hs_symperm(A,pinv,C);
03211 
03212     hs_smatrix L(S);              // Allocates cholesky factor
03213 
03214     ivector & Cp=C.p;
03215     ivector & Ci=C.i;
03216     dvector & Cx=C.x;
03217     dvector dfCx(C.x.indexmin(),Cx.indexmax());
03218     dfCx.initialize();
03219 
03220     ivector & Lp=L.p;
03221     ivector & Li=L.i;
03222     dvector & Lx=L.x;
03223 
03224   // counters
03225     int tccount=0;
03226     int txcount=0;
03227     ivector ccount(c.indexmin(),c.indexmax());
03228     ccount.initialize();
03229     ivector Licount(Li.indexmin(),Li.indexmax());
03230     Licount.initialize();
03231     ivector Lxcount(Lx.indexmin(),Lx.indexmax());
03232     Lxcount.initialize();
03233     ivector xcount(x.indexmin(),x.indexmax());
03234     xcount.initialize();
03235 
03236     int pcount=0;
03237     int icount=0;
03238     int lkicount=0;
03239 
03240     int p1=0;
03241 
03242     dvector xsave(0,nxcount);
03243     dvector csave(0,nccount);
03244     dvector lkisave(0,nlkicount);
03245 
03246     tccount=0;
03247     txcount=0;
03248     pcount=0;
03249     ccount.initialize();
03250     Licount.initialize();
03251     Lxcount.initialize();
03252     xcount.initialize();
03253     lkicount=0;
03254 
03255     // do it again -- this oulod be the frist time in the adjoint code
03256     for (k = 0 ; k < n ; k++)
03257     {
03258       Lp [k] = c [k] = cp [k] ;
03259       //ccount[k]++;
03260       //tccount++;
03261     }
03262     ivector Top(0,n);
03263 
03264     for (k = 0 ; k < n ; k++)            /* compute L(:,k) for L*L' = C */
03265     {
03266         /* --- Nonzero pattern of L(k,:) ------------------------------------ */
03267         Top(k) = cs_ereach (C, k, parent, s, c) ;
03268 
03269         //ssave(k)=s;
03270         if (allocated(ssave(k)))
03271         {
03272           cerr << "This can't happen" << endl;
03273           ad_exit(1);
03274         }
03275         else
03276         {
03277           ssave(k).allocate(Top(k),n-1);
03278         }
03279         ssave(k)=s(Top(k),n-1);
03280         x [k] = 0 ;                                    /* x (0:k) is now zero */
03281         //xcount[k]++;
03282         for (p = Cp [k] ; p < Cp [k+1] ; p++)       /* x = full(triu(C(:,k))) */
03283         {
03284           if (Ci [p] <= k)
03285           {
03286             x[Ci[p]] = Cx[p] ;
03287             xcount[Ci[p]]++;
03288           }
03289         }
03290         d = x [k] ;                        /* d = C(k,k) */
03291         //dcount++;
03292         x [k] = 0 ;                        /* clear x for k+1st iteration */
03293         //xcount[k]++;
03294         /* --- Triangular solve --------------------------------------------- */
03295         top=Top(k);
03296         for ( ; top < n ; top++)    /* solve L(0:k-1,0:k-1) * x = C(:,k) */
03297         {
03298             i = s [top] ;
03299             icount++;
03300             lkisave(lkicount++)=lki;
03301             lki = x [i] / Lx [Lp [i]] ; /* L(k,i) = x (i) / L(i,i) */
03302             xsave(txcount++)=x(i);
03303             x [i] = 0 ;                        /* clear x for k+1st iteration */
03304             for (p1 = Lp [i] + 1 ; p1 < c [i] ; p1++)
03305             {
03306                x [Li [p1]] -= Lx [p1] * lki ;
03307             }
03308             d -= lki * lki ;                /* d = d - L(k,i)*L(k,i) */
03309             csave(tccount++)=c[i];
03310             p2 = c [i] ;
03311             //ofs << ttc++ << " " << p2 << " 2" << endl;
03312             c[i]++;
03313             ccount[i]++;
03314             Li [p2] = k ;                /* store L(k,i) in column i */
03315             Licount[p2]++;
03316             if (Licount(p2)>1)
03317             {
03318               cerr << "Error unhandled case in chol" << endl;
03319             }
03320             Lx [p2] = lki ;
03321             Lxcount[p2]++;
03322             if (Lxcount(p2)>1)
03323             {
03324               cerr << "Error unhandled case in chol" << endl;
03325               ad_exit(1);
03326             }
03327         }
03328         /* --- Compute L(k,k) ----------------------------------------------- */
03329         if (d <= 0) return ; /* not pos def */
03330         csave(tccount++)=c[k];
03331         p = c [k];
03332         //ofs << ttc++ << " " << p << " 1"<< endl;
03333         c[k]++;
03334         //ccount[k]++;
03335         pcount++;
03336         Li [p] = k ;                   /* store L(k,k) = sqrt (d) in column k */
03337         Licount[p]++;
03338         if (Licount(p)>1)
03339         {
03340           cerr << "Error unhandled case in chol" << endl;
03341         }
03342         Lx [p] = sqrt (d) ;
03343         Lxcount[p]++;
03344         if (Lxcount(p)>1)
03345         {
03346           cerr << "Error unhandled case in chol" << endl;
03347           ad_exit(1);
03348         }
03349     }
03350     Lp [n] = cp [n] ;                    /* finalize L */
03351 
03352 
03353     // now the real adjoint code
03354 
03355     for (k = n-1 ; k >=0 ; k--)
03356     {
03357       c[k]=csave(--tccount);
03358       p=c[k];
03359       s(ssave(k).indexmin(),ssave(k).indexmax())=ssave(k);
03360       //if (k==3)
03361        // cout << "HERE2" << endl;
03362       // Lx [p] = sqrt (d) ;
03363       //ofs << --ttc << " " << p << " 1" << endl;
03364 
03365       dfd+=dfLx(p)/(2.0*Lx(p));
03366       dfLx(p)=0.0;
03367 
03368       //c[k]=csave(--tccount);
03369       //p=c[k];
03370 
03371       for (top=n-1 ; top >=Top[k] ; top--)
03372       {
03373         i=s(top);
03374         //Lx [p] = lki ;
03375         c[i]=csave(--tccount);
03376         p2=c[i];
03377         //ofs << --ttc << " " << p2 << " 2" << endl;
03378         dflki+=dfLx[p2];
03379         dfLx[p2]=0.0;
03380         //d -= lki * lki ;                /* d = d - L(k,i)*L(k,i) */
03381         dflki-=2.0*dfd*lki;
03382         for (p1 = Lp [i] + 1 ; p1 < c [i] ; p1++)
03383         {
03384           //x [Li [p1]] -= Lx [p1] * lki ;
03385           dflki-=dfx(Li(p1))*Lx(p1);
03386           dfLx(p1)-=dfx(Li(p1))*lki;
03387         }
03388         dfx[i]=0.0;
03389         // maybe not here
03390         x(i)=xsave(--txcount);
03391         // lki = x[i] / Lx[Lp[i]] ;
03392         dfx(i)+=dflki/Lx(Lp(i));
03393         dfLx(Lp(i))-=dflki*x(i)/square(Lx(Lp(i)));
03394         // but here
03395         dflki=0.0;
03396         lki=lkisave(--lkicount);
03397       }
03398       // x[k]=0.0;
03399       dfx[k]=0.0;
03400       //d = x [k] ;                        /* d = C(k,k) */
03401       dfx(k)+=dfd;
03402       dfd=0.0;
03403       for (p1 = Cp [k+1]-1 ; p1 >= Cp [k] ; p1--)
03404       {
03405         if (Ci [p1] <= k)
03406         {
03407           //x[Ci[p1]] = Cx[p1] ;
03408           dfCx[p1]+=dfx[Ci[p1]];
03409           dfx[Ci[p1]]=0.0;
03410         }
03411       }
03412       //x [k] = 0 ;                        /* clear x for k+1st iteration */
03413       dfx(k)=0.0;
03414     }
03415 
03416     dfCx.save_dvector_derivatives(cpos);
03417 
03418     return  ;
03419 }
03420 
03421 int chol(XCONST dvar_hs_smatrix &AA, XCONST hs_symbolic &T,
03422   dvar_hs_smatrix &LL)
03423 {
03424   //ADUNCONST(hs_symbolic,S)
03425   //ADUNCONST(dvar_hs_smatrix,L)
03426   //ADUNCONST(dvar_hs_smatrix,A)
03427   dvar_hs_smatrix& A = (dvar_hs_smatrix&)AA;
03428   hs_symbolic& S = (hs_symbolic&)T;
03429   dvar_hs_smatrix& L = (dvar_hs_smatrix&)LL;
03430   int icount=0;
03431   dvariable lki;
03432   dvariable d;
03433   int top, i, p, k, n;
03434   int p1,p2;
03435   n = A.n ;
03436   ivector c(0,n-1);                              /* int workspace */
03437   ivector s(0,n-1);                                   /* int workspace */
03438   dvar_vector x(0,n-1) ;                        /* double workspace */
03439 
03440   ivector & cp=S.cp;
03441   ivector & pinv=S.pinv;
03442   ivector & parent=S.parent;
03443 
03444   dvar_hs_smatrix C(A);
03445   C = A;
03446   if(S.pinv[0]!=-1)
03447     hs_symperm(A,pinv,C);
03448 
03449   //dvar_hs_smatrix & E = C;
03450 
03451   ivector & Cp=C.p;
03452   ivector & Ci=C.i;
03453   dvar_vector & Cx=C.x;
03454 
03455   ivector & Lp=L.p;
03456   ivector & Li=L.i;
03457   dvar_vector & Lx=L.x;
03458 
03459   for (k = 0 ; k < n ; k++)
03460   {
03461     Lp [k] = c [k] = cp [k] ;
03462   }
03463 
03464   for (k = 0 ; k < n ; k++)
03465   {
03466     top = cs_ereach (C, k, parent, s, c) ;
03467     x [k] = 0 ;
03468     for (p = Cp [k] ; p < Cp [k+1] ; p++)
03469     {
03470       if (Ci[p] <= k) x [Ci[p]] = Cx[p] ;
03471     }
03472     d = x[k] ;
03473     x[k] = 0.0;
03474     for ( ; top < n ; top++)
03475     {
03476       i = s[top] ;
03477       lki = x[i] / Lx[Lp[i]] ;
03478       icount++;   // count the number of times lki is overwritten
03479       x [i] = 0 ;
03480       for (p1 = Lp [i] + 1 ; p1 < c [i] ; p1++)
03481       {
03482         x[Li[p1]] -= Lx[p1] * lki ;
03483       }
03484       d -= lki * lki ;
03485       p2 = c[i]++ ;
03486       Li [p2] = k ;
03487       Lx [p2] = lki ;
03488     }
03489     if (d <= 0) return (0) ;
03490     p = c[k]++ ;
03491     Li [p] = k ;
03492     Lx [p] = sqrt (d) ;
03493   }
03494   Lp [n] = cp [n] ;
03495   xxx(icount);
03496   return (1) ;
03497 }
03498  //class hs_symbolic    // Info for symbolic cholesky
03499  //{
03500  //    public:
03501  //
03502  //    int n ;     // Dimension of underlying pos. def. matrix
03503  //    ivector pinv ;     // inverse row perm. for QR, fill red. perm for Chol
03504  //    ivector parent ;   // elimination tree for Cholesky and QR
03505  //    ivector cp ;       // column pointers for Cholesky, row counts for QR
03506  //    double lnz ;    // # entries in L for LU or Cholesky; in V for QR
03507  //
03508  //    hs_symbolic(int, XCONST css *);
03509  //    hs_symbolic(int n, XCONST dmatrix &T, int order);
03510  //    hs_symbolic(XCONST dcompressed_triplet &T, int order);
03511  //    hs_symbolic(XCONST dvar_compressed_triplet &T, int order);
03512  //    int is_null();
03513  //    int cmp(hs_symbolic &S);
03514  //    hs_symbolic(void);
03515  //};
03516 
03517 void hs_smatrix::set_symbolic(hs_symbolic& s)
03518 {
03519   sym.n=s.n;
03520   sym.pinv.allocate(s.pinv.indexmin(),s.pinv.indexmax());
03521   sym.pinv=s.pinv;
03522   sym.parent.allocate(s.parent.indexmin(),s.parent.indexmax());
03523   sym.parent=s.parent;
03524   sym.cp.allocate( s.cp.indexmin(),s.cp.indexmax());
03525   sym.cp=s.cp;
03526   sym.lnz=s.lnz;
03527 }
03528 
03529 void dvar_hs_smatrix::set_symbolic(hs_symbolic& s)
03530 {
03531   sym.n=s.n;
03532   sym.pinv.allocate(s.pinv.indexmin(),s.pinv.indexmax());
03533   sym.pinv=s.pinv;
03534   sym.parent.allocate(s.parent.indexmin(),s.parent.indexmax());
03535   sym.parent=s.parent;
03536   sym.cp.allocate( s.cp.indexmin(),s.cp.indexmax());
03537   sym.cp=s.cp;
03538   sym.lnz=s.lnz;
03539 }
03540 
03541 void report_dvar_vector_derivatives(void)
03542 {
03543   verify_identifier_string("jr");
03544   /*dvar_vector_position dpos=*/restore_dvar_vector_position();
03545   //dvector  dfLx=restore_dvar_vector_derivatives(dpos);
03546   verify_identifier_string("jx");
03547 }
03548 
03549 
03550 void report_derivatives(const dvar_vector& x)
03551 {
03552   save_identifier_string("jx");
03553   x.save_dvar_vector_position();
03554   gradient_structure::GRAD_STACK1->
03555       set_gradient_stack(report_dvar_vector_derivatives);
03556   save_identifier_string("jr");
03557 }
03558 
03559 void get_inverse_sparse_hessian(dcompressed_triplet & st, hs_symbolic& S,
03560   uostream& ofs1,ofstream& ofs,int usize,int xsize,dvector& u)
03561 {
03562   int n=st.get_n();
03563   hs_smatrix HS(n,st);
03564   hs_smatrix L(S);
03565   chol(HS,S,L);
03566   dvector gr(0,n-1);
03567   dvector x(0,n-1);
03568   gr.initialize();
03569   ofs1 << usize << xsize;
03570   int i;
03571   for (i=1;i<=n;i++)
03572   {
03573     gr(i-1)=1.0;
03574     x = cs_ipvec(S.pinv, gr);
03575     gr(i-1)=0.0;
03576     x = cs_lsolve(L,x);
03577     x = cs_ltsolve(L,x);
03578     x = cs_pvec(S.pinv,x);
03579     ofs << setprecision(5) << setscientific()
03580         << setw(14) << u(i) << " " << sqrt(x(i-1)) << endl;;
03581     x.shift(1);
03582     ofs1 << x;
03583     x.shift(0);
03584   }
03585 }
03586 //#include "cs.h"
03587 /* C = A*B */
03588 hs_smatrix cs_multiply(const hs_smatrix &AA, const hs_smatrix  &BB)
03589 {
03590     //ADUNCONST(hs_smatrix,A)
03591     //ADUNCONST(hs_smatrix,B)
03592     hs_smatrix& A = (hs_smatrix&)AA;
03593     hs_smatrix& B = (hs_smatrix&)BB;
03594     int p, j, nz = 0, anz,  m, n, bnz;
03595     //hs_smatrix *pC ;
03596     //hs_smatrix C(n,anz + bnz);
03597     //hs_smatrix& C=*pC ;
03598 
03599      //  if (!CS_CSC (A) || !CS_CSC (B)) return (NULL) ;      /* check inputs */
03600        if (A.n != B.m) return (NULL) ;
03601        m = A.m ; anz = A.p[A.n] ;
03602        n = B.n ; ivector & Bp = B.p ; ivector & Bi = B.i ;
03603        dvector & Bx = B.x ; bnz = Bp[n] ;
03604        //w = cs_calloc (m, sizeof (int)) ;                   /* get workspace */
03605        ivector w(0,m);                    /* get workspace */
03606        //values = (A.x != NULL) && (Bx != NULL) ;
03607        //x = values ? cs_malloc (m, sizeof (double)) : NULL ;/* get workspace */
03608        dvector x(0,m);  /* get workspace */
03609        hs_smatrix C(n,anz + bnz) ;        /* allocate result */
03610        //if (!C || !w || (values && !x)) return (cs_done (C, w, x, 0)) ;
03611        ivector & Cp = C.p ;
03612        for (j = 0 ; j < n ; j++)
03613        {
03614            C.reallocate(2*(C.nzmax)+m);
03615 
03616            //if (nz + m > C.nzmax && !cs_sprealloc (C, 2*(C.nzmax)+m))
03617            //{
03618            //    return (cs_done (C, w, x, 0)) ;             /* out of memory */
03619            //}
03620            ivector& Ci = C.i ;
03621            dvector& Cx = C.x ;       /* C->i and C->x may be reallocated */
03622            Cp [j] = nz ;                   /* column j of C starts here */
03623            for (p = Bp [j] ; p < Bp [j+1] ; p++)
03624            {
03625                nz = cs_scatter (A, Bi [p], Bx[p], w, x, j+1, C, nz) ;
03626            }
03627            for (p = Cp [j] ; p < nz ; p++) Cx [p] = x [Ci [p]] ;
03628        }
03629        Cp [n] = nz ;                       /* finalize the last column of C */
03630 
03631     return C;
03632       //cs_sprealloc (C, 0) ;               /* remove extra space from C */
03633    //return (cs_done (C, w, x, 1)) ;     /* success; free workspace, return C */
03634 }
03635 
03636 hs_smatrix operator * (const hs_smatrix &A, const hs_smatrix  &B)
03637 {
03638   return cs_multiply(A,B);
03639 }
03640 dcompressed_triplet make_dcompressed_triplet(const dmatrix & M)
03641 {
03642   int mmin=M.indexmin();
03643   int mmax=M.indexmax();
03644   int n=mmax-mmin+1;
03645   int jmin=M(mmin).indexmin();
03646   int jmax=M(mmax).indexmax();
03647   int m=jmax-jmin+1;
03648   int ii=0;
03649   int i,j;
03650   for (i=mmin;i<=mmax;i++)
03651   {
03652     int jmin=M(i).indexmin();
03653     int jmax=M(i).indexmax();
03654     for (j=jmin;j<=jmax;j++)
03655     {
03656       if (M(i,j) !=0) ii++;
03657     }
03658   }
03659   dcompressed_triplet dct(1,ii,n,m);
03660   ii=0;
03661   for (i=mmin;i<=mmax;i++)
03662   {
03663     int jmin=M(i).indexmin();
03664     int jmax=M(i).indexmax();
03665     for (j=jmin;j<=jmax;j++)
03666     {
03667       if (M(i,j) !=0)
03668       {
03669         ii++;
03670         dct(ii)=M(i,j);
03671         dct(1,ii)=i;
03672         dct(2,ii)=j;
03673       }
03674     }
03675   }
03676   return dct;
03677 }
03678 /*
03679 extern "C"  {
03680   void ad_boundf(int i)
03681   {
03682     // so we can stop here
03683     exit(i);
03684   }
03685 }
03686 */
03687 
03688 hs_smatrix make_hs_smatrix(const dmatrix & M)
03689 {
03690   return hs_smatrix(make_dcompressed_triplet(M));
03691 }
03692 
03693 ostream& operator << (const ostream& _ofs,const hs_smatrix& M)
03694 {
03695   ADUNCONST(ostream,ofs)
03696   ofs << "nrows " << M.m << " ncols " << M.n  << " nzmax " << M.nzmax
03697       << endl;
03698   ofs << "p = " << M.p << endl;
03699   ofs << "i = " << M.i << endl;
03700   ofs << "x = " << M.x << endl;
03701   return ofs;
03702 }
03703 
03704 dmatrix make_dmatrix(const hs_smatrix& M)
03705 {
03706   int n=M.m;
03707   int m=M.n;
03708   dmatrix tmp(1,n,1,m);
03709   tmp.initialize();
03710   int ii=0;
03711   for (int j=1;j<=m;j++)
03712   {
03713     for (int i=M.p(j-1);i<M.p(j);i++)
03714     {
03715       tmp(M.i(ii)+1,j)=M.x(ii);
03716       ii++;
03717     }
03718   }
03719   return tmp;
03720 }
03721 
03722 
03723  //
03724  //main()
03725  //{
03726  //
03727  //  ad_exit=&ad_boundf;
03728  //  int i,j;
03729  //  int n=20;
03730  //  int n2=n*n;
03731  //  double alpha=0.3;
03732  //  double beta=0.4;
03733  // /*
03734  //  dmatrix X(1,6,1,5);
03735  //  X.initialize();
03736  //  X(1,1)=1.;
03737  //  X(2,1)=2.;
03738  //  X(6,1)=3.;
03739  //  X(1,3)=4.;
03740  //  X(2,3)=5.;
03741  //  X(6,3)=6.;
03742  //  X(5,4)=7.;
03743  //  dcompressed_triplet dct1=make_dcompressed_triplet(X);
03744  //  hs_smatrix Z0=hs_smatrix(dct1);
03745  //  cout << Z0 << endl;
03746  //  cout << X << endl;
03747  //  cout << norm2(X-make_dmatrix(Z0)) << endl;
03748  //  cout << make_dmatrix(dct1) << endl;
03749  //  X.initialize();
03750  //  X(1,1)=1.;
03751  //  X(2,2)=2.;
03752  //  X(3,3)=3.;
03753  //  X(3,1)=5.;
03754  //  X(3,2)=9.;
03755  //  X(3,4)=7.;
03756  //  dcompressed_triplet dct2=make_dcompressed_triplet(X);
03757  //  hs_smatrix Z2=hs_smatrix(dct2);
03758  //  cout << X << endl;
03759  //  cout << make_dmatrix(dct2) << endl;
03760  // */
03761  //
03762  //  dmatrix M(1,n2,1,n2);
03763  //  M(1,1)=1;
03764  //  M(1,2)=beta;
03765  //  for (i=2;i<n2;i++)
03766  //  {
03767  //    M(i,i-1)=alpha;
03768  //    M(i,i)=1;
03769  //    M(i,i+1)=beta;
03770  //  }
03771  //  M(n2,n2-1)=alpha;
03772  //  M(n2,n2)=1;
03773  //  //dcompressed_triplet dct=make_dcompressed_triplet(M);
03774  //  hs_smatrix SM=make_hs_smatrix(M);
03775  //  //cout << norm2(make_dmatrix(dct)-M) << endl;
03776  //
03777  //  dmatrix L(1,n2,1,n2);
03778  //  L.initialize();
03779  //  int ii=1;
03780  //  for (i=1;i<=n;i++)
03781  //  {
03782  //    for (j=1;j<=n;j++)
03783  //    {
03784  //       L(ii,(j-1)*n+i)=1;
03785  //       ii++;
03786  //    }
03787  //  }
03788  //  hs_smatrix SL=make_hs_smatrix(L);
03789  //  dmatrix Y=make_dmatrix(SM*SL);
03790  //
03791  //  cout << norm2(Y-M*L) << endl;
03792  //  exit(2);
03793  //  //cout << L << endl;
03794  //
03795  //  //cout <<  M << endl;
03796  //  //cout << trans(L) * M * L << endl;
03797  //  dmatrix N= M * trans(L) * M * L;
03798  //  dmatrix N2=N*trans(N);
03799  //  ii=0;
03800  //  for (i=1;i<=n2;i++)
03801  //  {
03802  //    for (j=1;j<=n2;j++)
03803  //    {
03804  //      if (fabs(N(i,j))>1.e-8) ii++;
03805  //    }
03806  //  }
03807  //  cout << "N num no zero " << ii << " percentage full "
03808  //       << ii/double(n2*n2) << endl;
03809  //  ii=0;
03810  //  for (i=1;i<=n2;i++)
03811  //  {
03812  //    for (j=1;j<=n2;j++)
03813  //    {
03814  //      if (fabs(N2(i,j))>1.e-8) ii++;
03815  //    }
03816  //  }
03817  //  cout << "N*trans(N) num no zero " << ii << " percentage full "
03818  //       << ii/double(n2*n2) << endl;
03819  //  //cout << setfixed() << setprecision(2) << setw(5) << N << endl;
03820  //}
03821  //