ADMB Documentation  11.1x.2711
 All Classes Files Functions Variables Typedefs Friends Defines
hs_sparse.cpp
Go to the documentation of this file.
00001 /*
00002  * $Id: hs_sparse.cpp 2661 2014-11-15 22:23:22Z 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 
01522 ivector cs_amd (XCONST hs_smatrix &A)  /* Implements only order == 1: Chol*/
01523 {
01524     int d, dk, dext, lemax = 0, e, elenk, eln, i, j, k, k1,
01525         k2, k3, jlast, ln, dense, nzmax, mindeg = 0, nvi, nvj, nvk, mark, wnvi,
01526         ok, cnz, nel = 0, p, p1, p2, p3, p4, pj, pk, pk1, pk2, pn, q, t ;
01527     unsigned int h ;
01528     /* --- Construct matrix C ----------------------------------------------- */
01529 
01530     int n = A.n ;
01531 
01532     hs_smatrix AT(n,A.nzmax);
01533     cs_transpose(A,0,AT);
01534 
01535     /* find dense threshold */
01536     dense = CS_MAX (16, (int)(10.0 * sqrt((double)n)));
01537     dense = CS_MIN (n-2, dense) ;
01538 
01539     hs_smatrix C = cs_add(A,AT);
01540     cs_fkeep (C, &cs_diag, NULL);        // drop diagonal entries
01541     cnz = C.p [n] ;
01542     ivector P(0,n);
01543 
01544     t = cnz + cnz/5 + 2*n ;                    // add elbow room to C
01545     C.reallocate(t);
01546     ivector & Cp=C.p;
01547 
01548     ivector len(0,n);
01549     len.initialize();
01550     ivector nv(0,n);
01551     ivector next(0,n);
01552     ivector head(0,n);
01553     head.initialize();
01554     ivector elen(0,n);
01555     ivector degree(0,n);
01556     ivector w(0,n);
01557     ivector hhead(0,n);
01558     ivector &last = P ;                        /* use P as workspace for last */
01559 
01560     /* --- Initialize quotient graph ---------------------------------------- */
01561     for (k = 0 ; k < n ; k++) len [k] = Cp [k+1] - Cp [k] ;
01562     len [n] = 0 ;
01563     nzmax = C.nzmax ;
01564     ivector & Ci=C.i;
01565     for (i = 0 ; i <= n ; i++)
01566     {
01567         head [i] = -1 ;                            /* degree list i is empty */
01568         last [i] = -1 ;
01569         next [i] = -1 ;
01570         hhead [i] = -1 ;                    /* hash list i is empty */
01571         nv [i] = 1 ;                            /* node i is just one node */
01572         w [i] = 1 ;                            /* node i is alive */
01573         elen [i] = 0 ;                            /* Ek of node i is empty */
01574         degree [i] = len [i] ;                    /* degree of node i */
01575     }
01576     mark = cs_wclear (0, 0, w, n) ;            /* clear w */
01577     elen [n] = -2 ;                            /* n is a dead element */
01578     Cp [n] = -1 ;                            /* n is a root of assembly tree */
01579     w [n] = 0 ;                                    /* n is a dead element */
01580     /* --- Initialize degree lists ------------------------------------------ */
01581     for (i = 0 ; i < n ; i++)
01582     {
01583         d = degree [i] ;
01584         if (d == 0)                            /* node i is empty */
01585         {
01586             elen [i] = -2 ;                    /* element i is dead */
01587             nel++ ;
01588             Cp [i] = -1 ;                    /* i is a root of assembly tree */
01589             w [i] = 0 ;
01590         }
01591         else if (d > dense)                    /* node i is dense */
01592         {
01593             nv [i] = 0 ;                    /* absorb i into element n */
01594             elen [i] = -1 ;                    /* node i is dead */
01595             nel++ ;
01596             Cp [i] = CS_FLIP (n) ;
01597             nv [n]++ ;
01598         }
01599         else
01600         {
01601             if (head [d] != -1) last [head [d]] = i ;
01602             next [i] = head [d] ;            /* put node i in degree list d */
01603             head [d] = i ;
01604         }
01605     }
01606     while (nel < n)                            /* while (selecting pivots) do */
01607     {
01608         /* --- Select node of minimum approximate degree -------------------- */
01609         for (k = -1 ; mindeg < n && (k = head [mindeg]) == -1 ; mindeg++) ;
01610         if (next [k] != -1) last [next [k]] = -1 ;
01611         head [mindeg] = next [k] ;            /* remove k from degree list */
01612         elenk = elen [k] ;                    /* elenk = |Ek| */
01613         nvk = nv [k] ;                            /* # of nodes k represents */
01614         nel += nvk ;                           /* nv[k] nodes of A eliminated */
01615         /* --- Garbage collection ------------------------------------------- */
01616         if (elenk > 0 && cnz + mindeg >= nzmax)
01617         {
01618             for (j = 0 ; j < n ; j++)
01619             {
01620                 if ((p = Cp [j]) >= 0)         /* j is a live node or element */
01621                 {
01622                     Cp [j] = Ci [p] ;           /* save first entry of object */
01623                     Ci [p] = CS_FLIP (j) ;  /* first entry is now CS_FLIP(j) */
01624                 }
01625             }
01626             for (q = 0, p = 0 ; p < cnz ; ) /* scan all of memory */
01627             {
01628                 if ((j = CS_FLIP (Ci [p++])) >= 0)  /* found object j */
01629                 {
01630                     Ci [q] = Cp [j] ;        /* restore first entry of object */
01631                     Cp [j] = q++ ;            /* new pointer to object j */
01632                     for (k3 = 0 ; k3 < len [j]-1 ; k3++) Ci [q++] = Ci [p++] ;
01633                 }
01634             }
01635             cnz = q ;                          /* Ci [cnz...nzmax-1] now free */
01636         }
01637         /* --- Construct new element ---------------------------------------- */
01638         dk = 0 ;
01639         nv [k] = -nvk ;                            /* flag k as in Lk */
01640         p = Cp [k] ;
01641         pk1 = (elenk == 0) ? p : cnz ;         /* do in place if elen[k] == 0 */
01642         pk2 = pk1 ;
01643         for (k1 = 1 ; k1 <= elenk + 1 ; k1++)
01644         {
01645             if (k1 > elenk)
01646             {
01647                 e = k ;                            /* search the nodes in k */
01648                 pj = p ;                    /* list of nodes starts at Ci[pj]*/
01649                 ln = len [k] - elenk ;        /* length of list of nodes in k */
01650             }
01651             else
01652             {
01653                 e = Ci [p++] ;                    /* search the nodes in e */
01654                 pj = Cp [e] ;
01655                 ln = len [e] ;                /* length of list of nodes in e */
01656             }
01657             for (k2 = 1 ; k2 <= ln ; k2++)
01658             {
01659                 i = Ci [pj++] ;
01660                 if ((nvi = nv [i]) <= 0) continue ; /* node i dead, or seen */
01661                 dk += nvi ;                   /* degree[Lk] += size of node i */
01662                 nv [i] = -nvi ;              /* negate nv[i] to denote i in Lk*/
01663                 Ci [pk2++] = i ;            /* place i in Lk */
01664                 if (next [i] != -1) last [next [i]] = last [i] ;
01665                 if (last [i] != -1)            /* remove i from degree list */
01666                 {
01667                     next [last [i]] = next [i] ;
01668                 }
01669                 else
01670                 {
01671                     head [degree [i]] = next [i] ;
01672                 }
01673             }
01674             if (e != k)
01675             {
01676                 Cp [e] = CS_FLIP (k) ;            /* absorb e into k */
01677                 w [e] = 0 ;                    /* e is now a dead element */
01678             }
01679         }
01680         if (elenk != 0) cnz = pk2 ;            /* Ci [cnz...nzmax] is free */
01681         degree [k] = dk ;                    /* external degree of k - |Lk\i| */
01682         Cp [k] = pk1 ;                      /* element k is in Ci[pk1..pk2-1] */
01683         len [k] = pk2 - pk1 ;
01684         elen [k] = -2 ;                            /* k is now an element */
01685         /* --- Find set differences ----------------------------------------- */
01686         mark = cs_wclear (mark, lemax, w, n) ;        /* clear w if necessary */
01687         for (pk = pk1 ; pk < pk2 ; pk++)    /* scan 1: find |Le\Lk| */
01688         {
01689             i = Ci [pk] ;
01690             if ((eln = elen [i]) <= 0) continue ;/* skip if elen[i] empty */
01691             nvi = -nv [i] ;                         /* nv [i] was negated */
01692             wnvi = mark - nvi ;
01693             for (p = Cp [i] ; p <= Cp [i] + eln - 1 ; p++)  /* scan Ei */
01694             {
01695                 e = Ci [p] ;
01696                 if (w [e] >= mark)
01697                 {
01698                     w [e] -= nvi ;            /* decrement |Le\Lk| */
01699                 }
01700                 else if (w [e] != 0)            /* ensure e is a live element */
01701                 {
01702                     w [e] = degree [e] + wnvi ;  /* 1st time e seen in scan 1 */
01703                 }
01704             }
01705         }
01706         /* --- Degree update ------------------------------------------------ */
01707         for (pk = pk1 ; pk < pk2 ; pk++)    /* scan2: degree update */
01708         {
01709             i = Ci [pk] ;                    /* consider node i in Lk */
01710             p1 = Cp [i] ;
01711             p2 = p1 + elen [i] - 1 ;
01712             pn = p1 ;
01713             for (h = 0, d = 0, p = p1 ; p <= p2 ; p++)    /* scan Ei */
01714             {
01715                 e = Ci [p] ;
01716                 if (w [e] != 0)                 /* e is an unabsorbed element */
01717                 {
01718                     dext = w [e] - mark ;   /* dext = |Le\Lk| */
01719                     if (dext > 0)
01720                     {
01721                         d += dext ;            /* sum up the set differences */
01722                         Ci [pn++] = e ;            /* keep e in Ei */
01723                         h += e ;            /* compute the hash of node i */
01724                     }
01725                     else
01726                     {
01727                         Cp [e] = CS_FLIP (k) ;     /* aggressive absorb. e->k */
01728                         w [e] = 0 ;                /* e is a dead element */
01729                     }
01730                 }
01731             }
01732             elen [i] = pn - p1 + 1 ;            /* elen[i] = |Ei| */
01733             p3 = pn ;
01734             p4 = p1 + len [i] ;
01735             for (p = p2 + 1 ; p < p4 ; p++) /* prune edges in Ai */
01736             {
01737                 j = Ci [p] ;
01738                 if ((nvj = nv [j]) <= 0) continue ; /* node j dead or in Lk */
01739                 d += nvj ;                    /* degree(i) += |j| */
01740                 Ci [pn++] = j ;                  /* place j in node list of i */
01741                 h += j ;                    /* compute hash for node i */
01742             }
01743             if (d == 0)                         /* check for mass elimination */
01744             {
01745                 Cp [i] = CS_FLIP (k) ;            /* absorb i into k */
01746                 nvi = -nv [i] ;
01747                 dk -= nvi ;                    /* |Lk| -= |i| */
01748                 nvk += nvi ;                    /* |k| += nv[i] */
01749                 nel += nvi ;
01750                 nv [i] = 0 ;
01751                 elen [i] = -1 ;                    /* node i is dead */
01752             }
01753             else
01754             {
01755                 degree [i] = CS_MIN (degree [i], d) ;     /* update degree(i) */
01756                 Ci [pn] = Ci [p3] ;            /* move first node to end */
01757                 Ci [p3] = Ci [p1] ;            /* move 1st el. to end of Ei */
01758                 Ci [p1] = k ;                /* add k as 1st element in of Ei */
01759                 len [i] = pn - p1 + 1 ;     /* new len of adj. list of node i */
01760                 h %= n ;                    /* finalize hash of i */
01761                 next [i] = hhead [h] ;            /* place i in hash bucket */
01762                 hhead [h] = i ;
01763                 last [i] = h ;                   /* save hash of i in last[i] */
01764             }
01765         }                                    /* scan2 is done */
01766         degree [k] = dk ;                    /* finalize |Lk| */
01767         lemax = CS_MAX (lemax, dk) ;
01768         mark = cs_wclear (mark+lemax, lemax, w, n) ;        /* clear w */
01769         /* --- Supernode detection ------------------------------------------ */
01770         for (pk = pk1 ; pk < pk2 ; pk++)
01771         {
01772             i = Ci [pk] ;
01773             if (nv [i] >= 0) continue ;                /* skip if i is dead */
01774             h = last [i] ;                      /* scan hash bucket of node i */
01775             i = hhead [h] ;
01776             hhead [h] = -1 ;                     /* hash bucket will be empty */
01777             for ( ; i != -1 && next [i] != -1 ; i = next [i], mark++)
01778             {
01779                 ln = len [i] ;
01780                 eln = elen [i] ;
01781                 for (p = Cp [i]+1 ; p <= Cp [i] + ln-1 ; p++) w [Ci [p]] = mark;
01782                 jlast = i ;
01783                 for (j = next [i] ; j != -1 ; )       /* compare i with all j */
01784                 {
01785                     ok = (len [j] == ln) && (elen [j] == eln) ;
01786                     for (p = Cp [j] + 1 ; ok && p <= Cp [j] + ln - 1 ; p++)
01787                     {
01788                         if (w [Ci [p]] != mark) ok = 0 ;    /* compare i and j*/
01789                     }
01790                     if (ok)                        /* i and j are identical */
01791                     {
01792                         Cp [j] = CS_FLIP (i) ;        /* absorb j into i */
01793                         nv [i] += nv [j] ;
01794                         nv [j] = 0 ;
01795                         elen [j] = -1 ;                /* node j is dead */
01796                         j = next [j] ;           /* delete j from hash bucket */
01797                         next [jlast] = j ;
01798                     }
01799                     else
01800                     {
01801                         jlast = j ;                /* j and i are different */
01802                         j = next [j] ;
01803                     }
01804                 }
01805             }
01806         }
01807         /* --- Finalize new element------------------------------------------ */
01808         for (p = pk1, pk = pk1 ; pk < pk2 ; pk++)   /* finalize Lk */
01809         {
01810             i = Ci [pk] ;
01811             if ((nvi = -nv [i]) <= 0) continue ;/* skip if i is dead */
01812             nv [i] = nvi ;                        /* restore nv[i] */
01813             d = degree [i] + dk - nvi ;         /* compute external degree(i) */
01814             d = CS_MIN (d, n - nel - nvi) ;
01815             if (head [d] != -1) last [head [d]] = i ;
01816             next [i] = head [d] ;                /* put i back in degree list */
01817             last [i] = -1 ;
01818             head [d] = i ;
01819             mindeg = CS_MIN (mindeg, d) ;        /* find new minimum degree */
01820             degree [i] = d ;
01821             Ci [p++] = i ;                        /* place i in Lk */
01822         }
01823         nv [k] = nvk ;                            /* # nodes absorbed into k */
01824         if ((len [k] = p-pk1) == 0)         /* length of adj list of element k*/
01825         {
01826             Cp [k] = -1 ;                    /* k is a root of the tree */
01827             w [k] = 0 ;                            /* k is now a dead element */
01828         }
01829         if (elenk != 0) cnz = p ;            /* free unused space in Lk */
01830     }
01831     /* --- Postordering ----------------------------------------------------- */
01832     for (i = 0 ; i < n ; i++) Cp [i] = CS_FLIP (Cp [i]) ;/* fix assembly tree */
01833     for (j = 0 ; j <= n ; j++) head [j] = -1 ;
01834     for (j = n ; j >= 0 ; j--)              /* place unordered nodes in lists */
01835     {
01836         if (nv [j] > 0) continue ;            /* skip if j is an element */
01837         next [j] = head [Cp [j]] ;           /* place j in list of its parent */
01838         head [Cp [j]] = j ;
01839     }
01840     for (e = n ; e >= 0 ; e--)                    /* place elements in lists */
01841     {
01842         if (nv [e] <= 0) continue ;            /* skip unless e is an element */
01843         if (Cp [e] != -1)
01844         {
01845             next [e] = head [Cp [e]] ;       /* place e in list of its parent */
01846             head [Cp [e]] = e ;
01847         }
01848     }
01849     for (k = 0, i = 0 ; i <= n ; i++)          /* postorder the assembly tree */
01850     {
01851         if (Cp [i] == -1) k = cs_tdfs (i, k, head, next, P, w) ;
01852     }
01853     return (P) ;
01854 }
01855 
01859 ivector cs_amd (XCONST dvar_hs_smatrix &A) /* Implements only order == 1: Chol*/
01860 {
01861     int d, dk, dext, lemax = 0, e, elenk, eln, i, j, k, k1,
01862         k2, k3, jlast, ln, dense, nzmax, mindeg = 0, nvi, nvj, nvk, mark, wnvi,
01863         ok, cnz, nel = 0, p, p1, p2, p3, p4, pj, pk, pk1, pk2, pn, q, t ;
01864     unsigned int h ;
01865     /* --- Construct matrix C ----------------------------------------------- */
01866 
01867     int n = A.n ;
01868 
01869     dvar_hs_smatrix AT(n,A.nzmax);
01870     cs_transpose(A,0,AT);
01871     /* find dense threshold */
01872     dense = CS_MAX (16, (int)(10.0 * sqrt((double)n)));
01873     dense = CS_MIN (n-2, dense) ;
01874 
01875     dvar_hs_smatrix C = cs_add(A,AT);
01876     cs_fkeep (C, &cs_diag, NULL);        // drop diagonal entries
01877     cnz = C.p [n] ;
01878     ivector P(0,n);
01879 
01880     t = cnz + cnz/5 + 2*n ;                    // add elbow room to C
01881     C.reallocate(t);
01882     ivector & Cp=C.p;
01883 
01884     ivector len(0,n);
01885     len.initialize();
01886     ivector nv(0,n);
01887     ivector next(0,n);
01888     ivector head(0,n);
01889     head.initialize();
01890     ivector elen(0,n);
01891     ivector degree(0,n);
01892     ivector w(0,n);
01893     ivector hhead(0,n);
01894     ivector &last = P ;                        /* use P as workspace for last */
01895 
01896     /* --- Initialize quotient graph ---------------------------------------- */
01897     for (k = 0 ; k < n ; k++) len [k] = Cp [k+1] - Cp [k] ;
01898     len [n] = 0 ;
01899     nzmax = C.nzmax ;
01900     ivector & Ci=C.i;
01901     for (i = 0 ; i <= n ; i++)
01902     {
01903         head [i] = -1 ;                            /* degree list i is empty */
01904         last [i] = -1 ;
01905         next [i] = -1 ;
01906         hhead [i] = -1 ;                    /* hash list i is empty */
01907         nv [i] = 1 ;                            /* node i is just one node */
01908         w [i] = 1 ;                            /* node i is alive */
01909         elen [i] = 0 ;                            /* Ek of node i is empty */
01910         degree [i] = len [i] ;                    /* degree of node i */
01911     }
01912     mark = cs_wclear (0, 0, w, n) ;            /* clear w */
01913     elen [n] = -2 ;                            /* n is a dead element */
01914     Cp [n] = -1 ;                            /* n is a root of assembly tree */
01915     w [n] = 0 ;                                    /* n is a dead element */
01916     /* --- Initialize degree lists ------------------------------------------ */
01917     for (i = 0 ; i < n ; i++)
01918     {
01919         d = degree [i] ;
01920         if (d == 0)                            /* node i is empty */
01921         {
01922             elen [i] = -2 ;                    /* element i is dead */
01923             nel++ ;
01924             Cp [i] = -1 ;                    /* i is a root of assembly tree */
01925             w [i] = 0 ;
01926         }
01927         else if (d > dense)                    /* node i is dense */
01928         {
01929             nv [i] = 0 ;                    /* absorb i into element n */
01930             elen [i] = -1 ;                    /* node i is dead */
01931             nel++ ;
01932             Cp [i] = CS_FLIP (n) ;
01933             nv [n]++ ;
01934         }
01935         else
01936         {
01937             if (head [d] != -1) last [head [d]] = i ;
01938             next [i] = head [d] ;            /* put node i in degree list d */
01939             head [d] = i ;
01940         }
01941     }
01942     while (nel < n)                            /* while (selecting pivots) do */
01943     {
01944         /* --- Select node of minimum approximate degree -------------------- */
01945         for (k = -1 ; mindeg < n && (k = head [mindeg]) == -1 ; mindeg++) ;
01946         if (next [k] != -1) last [next [k]] = -1 ;
01947         head [mindeg] = next [k] ;            /* remove k from degree list */
01948         elenk = elen [k] ;                    /* elenk = |Ek| */
01949         nvk = nv [k] ;                            /* # of nodes k represents */
01950         nel += nvk ;                           /* nv[k] nodes of A eliminated */
01951         /* --- Garbage collection ------------------------------------------- */
01952         if (elenk > 0 && cnz + mindeg >= nzmax)
01953         {
01954             for (j = 0 ; j < n ; j++)
01955             {
01956                 if ((p = Cp [j]) >= 0)         /* j is a live node or element */
01957                 {
01958                     Cp [j] = Ci [p] ;           /* save first entry of object */
01959                     Ci [p] = CS_FLIP (j) ;  /* first entry is now CS_FLIP(j) */
01960                 }
01961             }
01962             for (q = 0, p = 0 ; p < cnz ; ) /* scan all of memory */
01963             {
01964                 if ((j = CS_FLIP (Ci [p++])) >= 0)  /* found object j */
01965                 {
01966                     Ci [q] = Cp [j] ;        /* restore first entry of object */
01967                     Cp [j] = q++ ;            /* new pointer to object j */
01968                     for (k3 = 0 ; k3 < len [j]-1 ; k3++) Ci [q++] = Ci [p++] ;
01969                 }
01970             }
01971             cnz = q ;                          /* Ci [cnz...nzmax-1] now free */
01972         }
01973         /* --- Construct new element ---------------------------------------- */
01974         dk = 0 ;
01975         nv [k] = -nvk ;                            /* flag k as in Lk */
01976         p = Cp [k] ;
01977         pk1 = (elenk == 0) ? p : cnz ;         /* do in place if elen[k] == 0 */
01978         pk2 = pk1 ;
01979         for (k1 = 1 ; k1 <= elenk + 1 ; k1++)
01980         {
01981             if (k1 > elenk)
01982             {
01983                 e = k ;                            /* search the nodes in k */
01984                 pj = p ;                    /* list of nodes starts at Ci[pj]*/
01985                 ln = len [k] - elenk ;        /* length of list of nodes in k */
01986             }
01987             else
01988             {
01989                 e = Ci [p++] ;                    /* search the nodes in e */
01990                 pj = Cp [e] ;
01991                 ln = len [e] ;                /* length of list of nodes in e */
01992             }
01993             for (k2 = 1 ; k2 <= ln ; k2++)
01994             {
01995                 i = Ci [pj++] ;
01996                 if ((nvi = nv [i]) <= 0) continue ; /* node i dead, or seen */
01997                 dk += nvi ;                   /* degree[Lk] += size of node i */
01998                 nv [i] = -nvi ;              /* negate nv[i] to denote i in Lk*/
01999                 Ci [pk2++] = i ;            /* place i in Lk */
02000                 if (next [i] != -1) last [next [i]] = last [i] ;
02001                 if (last [i] != -1)            /* remove i from degree list */
02002                 {
02003                     next [last [i]] = next [i] ;
02004                 }
02005                 else
02006                 {
02007                     head [degree [i]] = next [i] ;
02008                 }
02009             }
02010             if (e != k)
02011             {
02012                 Cp [e] = CS_FLIP (k) ;            /* absorb e into k */
02013                 w [e] = 0 ;                    /* e is now a dead element */
02014             }
02015         }
02016         if (elenk != 0) cnz = pk2 ;            /* Ci [cnz...nzmax] is free */
02017         degree [k] = dk ;                    /* external degree of k - |Lk\i| */
02018         Cp [k] = pk1 ;                      /* element k is in Ci[pk1..pk2-1] */
02019         len [k] = pk2 - pk1 ;
02020         elen [k] = -2 ;                            /* k is now an element */
02021         /* --- Find set differences ----------------------------------------- */
02022         mark = cs_wclear (mark, lemax, w, n) ;        /* clear w if necessary */
02023         for (pk = pk1 ; pk < pk2 ; pk++)    /* scan 1: find |Le\Lk| */
02024         {
02025             i = Ci [pk] ;
02026             if ((eln = elen [i]) <= 0) continue ;/* skip if elen[i] empty */
02027             nvi = -nv [i] ;                         /* nv [i] was negated */
02028             wnvi = mark - nvi ;
02029             for (p = Cp [i] ; p <= Cp [i] + eln - 1 ; p++)  /* scan Ei */
02030             {
02031                 e = Ci [p] ;
02032                 if (w [e] >= mark)
02033                 {
02034                     w [e] -= nvi ;            /* decrement |Le\Lk| */
02035                 }
02036                 else if (w [e] != 0)            /* ensure e is a live element */
02037                 {
02038                     w [e] = degree [e] + wnvi ;  /* 1st time e seen in scan 1 */
02039                 }
02040             }
02041         }
02042         /* --- Degree update ------------------------------------------------ */
02043         for (pk = pk1 ; pk < pk2 ; pk++)    /* scan2: degree update */
02044         {
02045             i = Ci [pk] ;                    /* consider node i in Lk */
02046             p1 = Cp [i] ;
02047             p2 = p1 + elen [i] - 1 ;
02048             pn = p1 ;
02049             for (h = 0, d = 0, p = p1 ; p <= p2 ; p++)    /* scan Ei */
02050             {
02051                 e = Ci [p] ;
02052                 if (w [e] != 0)                 /* e is an unabsorbed element */
02053                 {
02054                     dext = w [e] - mark ;   /* dext = |Le\Lk| */
02055                     if (dext > 0)
02056                     {
02057                         d += dext ;            /* sum up the set differences */
02058                         Ci [pn++] = e ;            /* keep e in Ei */
02059                         h += e ;            /* compute the hash of node i */
02060                     }
02061                     else
02062                     {
02063                         Cp [e] = CS_FLIP (k) ;     /* aggressive absorb. e->k */
02064                         w [e] = 0 ;                /* e is a dead element */
02065                     }
02066                 }
02067             }
02068             elen [i] = pn - p1 + 1 ;            /* elen[i] = |Ei| */
02069             p3 = pn ;
02070             p4 = p1 + len [i] ;
02071             for (p = p2 + 1 ; p < p4 ; p++) /* prune edges in Ai */
02072             {
02073                 j = Ci [p] ;
02074                 if ((nvj = nv [j]) <= 0) continue ; /* node j dead or in Lk */
02075                 d += nvj ;                    /* degree(i) += |j| */
02076                 Ci [pn++] = j ;                  /* place j in node list of i */
02077                 h += j ;                    /* compute hash for node i */
02078             }
02079             if (d == 0)                         /* check for mass elimination */
02080             {
02081                 Cp [i] = CS_FLIP (k) ;            /* absorb i into k */
02082                 nvi = -nv [i] ;
02083                 dk -= nvi ;                    /* |Lk| -= |i| */
02084                 nvk += nvi ;                    /* |k| += nv[i] */
02085                 nel += nvi ;
02086                 nv [i] = 0 ;
02087                 elen [i] = -1 ;                    /* node i is dead */
02088             }
02089             else
02090             {
02091                 degree [i] = CS_MIN (degree [i], d) ;     /* update degree(i) */
02092                 Ci [pn] = Ci [p3] ;            /* move first node to end */
02093                 Ci [p3] = Ci [p1] ;            /* move 1st el. to end of Ei */
02094                 Ci [p1] = k ;                /* add k as 1st element in of Ei */
02095                 len [i] = pn - p1 + 1 ;     /* new len of adj. list of node i */
02096                 h %= n ;                    /* finalize hash of i */
02097                 next [i] = hhead [h] ;            /* place i in hash bucket */
02098                 hhead [h] = i ;
02099                 last [i] = h ;                   /* save hash of i in last[i] */
02100             }
02101         }                                    /* scan2 is done */
02102         degree [k] = dk ;                    /* finalize |Lk| */
02103         lemax = CS_MAX (lemax, dk) ;
02104         mark = cs_wclear (mark+lemax, lemax, w, n) ;        /* clear w */
02105         /* --- Supernode detection ------------------------------------------ */
02106         for (pk = pk1 ; pk < pk2 ; pk++)
02107         {
02108             i = Ci [pk] ;
02109             if (nv [i] >= 0) continue ;                /* skip if i is dead */
02110             h = last [i] ;                      /* scan hash bucket of node i */
02111             i = hhead [h] ;
02112             hhead [h] = -1 ;                     /* hash bucket will be empty */
02113             for ( ; i != -1 && next [i] != -1 ; i = next [i], mark++)
02114             {
02115                 ln = len [i] ;
02116                 eln = elen [i] ;
02117                 for (p = Cp [i]+1 ; p <= Cp [i] + ln-1 ; p++) w [Ci [p]] = mark;
02118                 jlast = i ;
02119                 for (j = next [i] ; j != -1 ; )       /* compare i with all j */
02120                 {
02121                     ok = (len [j] == ln) && (elen [j] == eln) ;
02122                     for (p = Cp [j] + 1 ; ok && p <= Cp [j] + ln - 1 ; p++)
02123                     {
02124                         if (w [Ci [p]] != mark) ok = 0 ;    /* compare i and j*/
02125                     }
02126                     if (ok)                        /* i and j are identical */
02127                     {
02128                         Cp [j] = CS_FLIP (i) ;        /* absorb j into i */
02129                         nv [i] += nv [j] ;
02130                         nv [j] = 0 ;
02131                         elen [j] = -1 ;                /* node j is dead */
02132                         j = next [j] ;           /* delete j from hash bucket */
02133                         next [jlast] = j ;
02134                     }
02135                     else
02136                     {
02137                         jlast = j ;                /* j and i are different */
02138                         j = next [j] ;
02139                     }
02140                 }
02141             }
02142         }
02143         /* --- Finalize new element------------------------------------------ */
02144         for (p = pk1, pk = pk1 ; pk < pk2 ; pk++)   /* finalize Lk */
02145         {
02146             i = Ci [pk] ;
02147             if ((nvi = -nv [i]) <= 0) continue ;/* skip if i is dead */
02148             nv [i] = nvi ;                        /* restore nv[i] */
02149             d = degree [i] + dk - nvi ;         /* compute external degree(i) */
02150             d = CS_MIN (d, n - nel - nvi) ;
02151             if (head [d] != -1) last [head [d]] = i ;
02152             next [i] = head [d] ;                /* put i back in degree list */
02153             last [i] = -1 ;
02154             head [d] = i ;
02155             mindeg = CS_MIN (mindeg, d) ;        /* find new minimum degree */
02156             degree [i] = d ;
02157             Ci [p++] = i ;                        /* place i in Lk */
02158         }
02159         nv [k] = nvk ;                            /* # nodes absorbed into k */
02160         if ((len [k] = p-pk1) == 0)         /* length of adj list of element k*/
02161         {
02162             Cp [k] = -1 ;                    /* k is a root of the tree */
02163             w [k] = 0 ;                            /* k is now a dead element */
02164         }
02165         if (elenk != 0) cnz = p ;            /* free unused space in Lk */
02166     }
02167     /* --- Postordering ----------------------------------------------------- */
02168     for (i = 0 ; i < n ; i++) Cp [i] = CS_FLIP (Cp [i]) ;/* fix assembly tree */
02169     for (j = 0 ; j <= n ; j++) head [j] = -1 ;
02170     for (j = n ; j >= 0 ; j--)              /* place unordered nodes in lists */
02171     {
02172         if (nv [j] > 0) continue ;            /* skip if j is an element */
02173         next [j] = head [Cp [j]] ;           /* place j in list of its parent */
02174         head [Cp [j]] = j ;
02175     }
02176     for (e = n ; e >= 0 ; e--)                    /* place elements in lists */
02177     {
02178         if (nv [e] <= 0) continue ;            /* skip unless e is an element */
02179         if (Cp [e] != -1)
02180         {
02181             next [e] = head [Cp [e]] ;       /* place e in list of its parent */
02182             head [Cp [e]] = e ;
02183         }
02184     }
02185     for (k = 0, i = 0 ; i <= n ; i++)          /* postorder the assembly tree */
02186     {
02187         if (Cp [i] == -1) k = cs_tdfs (i, k, head, next, P, w) ;
02188     }
02189     return (P) ;
02190 }
02191 
02192 /* compute the etree of A (using triu(A), or A'A without forming A'A */
02193 ivector cs_etree (XCONST hs_smatrix &_A)
02194 {
02195     ADUNCONST(hs_smatrix,A)
02196     int i, k, p, inext;
02197 
02198     int n = A.n ;
02199     ivector & Ap=A.p;
02200     ivector & Ai=A.i;
02201 
02202     ivector parent(0,n-1);
02203     parent.initialize();
02204     ivector w(0,n-1);                   /* get workspace */
02205     w.initialize();
02206     ivector &ancestor = w ;
02207     for (k = 0 ; k < n ; k++)
02208     {
02209         parent [k] = -1 ;                   /* node k has no parent yet */
02210         ancestor [k] = -1 ;                 /* nor does k have an ancestor */
02211         for (p = Ap [k] ; p < Ap [k+1] ; p++)
02212         {
02213             i = Ai [p] ;
02214             for ( ; i != -1 && i < k ; i = inext)   /* traverse from i to k */
02215             {
02216                 inext = ancestor [i] ;              /* inext = ancestor of i */
02217                 ancestor [i] = k ;                  /* path compression */
02218                 if (inext == -1) parent [i] = k ;   /* no anc., parent is k */
02219             }
02220         }
02221     }
02222     return (parent) ;
02223 }
02224 
02225 ivector cs_etree (XCONST dvar_hs_smatrix &_A)
02226 {
02227     ADUNCONST(dvar_hs_smatrix,A)
02228     int i, k, p, inext;
02229 
02230     int n = A.n ;
02231     ivector & Ap=A.p;
02232     ivector & Ai=A.i;
02233 
02234     ivector parent(0,n-1);
02235     parent.initialize();
02236     ivector w(0,n-1);                   /* get workspace */
02237     w.initialize();
02238     ivector &ancestor = w ;
02239     for (k = 0 ; k < n ; k++)
02240     {
02241         parent [k] = -1 ;                   /* node k has no parent yet */
02242         ancestor [k] = -1 ;                 /* nor does k have an ancestor */
02243         for (p = Ap [k] ; p < Ap [k+1] ; p++)
02244         {
02245             i = Ai [p] ;
02246             for ( ; i != -1 && i < k ; i = inext)   /* traverse from i to k */
02247             {
02248                 inext = ancestor [i] ;              /* inext = ancestor of i */
02249                 ancestor [i] = k ;                  /* path compression */
02250                 if (inext == -1) parent [i] = k ;   /* no anc., parent is k */
02251             }
02252         }
02253     }
02254     return (parent) ;
02255 }
02256 
02257 /* post order a forest */
02258 ivector cs_post (XCONST ivector &parent, int n)
02259 {
02260     int j, k = 0;
02261 
02262     ivector post(0,n-1);
02263     post.initialize();
02264     ivector head(0,n-1);
02265     ivector next(0,n-1);
02266     next.initialize();
02267     ivector stack(0,n-1);
02268     stack.initialize();
02269 
02270     for (j = 0 ; j < n ; j++) head [j] = -1 ;           /* empty linked lists */
02271     for (j = n-1 ; j >= 0 ; j--)            /* traverse nodes in reverse order*/
02272     {
02273         if (parent [j] == -1) continue ;    /* j is a root */
02274         next [j] = head [parent [j]] ;      /* add j to list of its parent */
02275         head [parent [j]] = j ;
02276     }
02277     for (j = 0 ; j < n ; j++)
02278     {
02279         if (parent [j] != -1) continue ;    /* skip j if it is not a root */
02280         k = cs_tdfs (j, k, head, next, post, stack) ;
02281     }
02282     return (post) ;
02283 }
02284 
02285 
02286 /* consider A(i,j), node j in ith row subtree and return lca(jprev,j) */
02287 int cs_leaf (int i, int j, XCONST ivector &first, ivector &maxfirst,
02288   ivector &prevleaf, ivector &ancestor, int *jleaf)
02289 {
02290     int q, s, sparent, jprev ;
02291     *jleaf = 0 ;
02292     if (i <= j || first [j] <= maxfirst [i]) return (-1) ;  /* j not a leaf */
02293     maxfirst [i] = first [j] ;      /* update max first[j] seen so far */
02294     jprev = prevleaf [i] ;          /* jprev = previous leaf of ith subtree */
02295     prevleaf [i] = j ;
02296     *jleaf = (jprev == -1) ? 1: 2 ; /* j is first or subsequent leaf */
02297     if (*jleaf == 1) return (i) ;   /* if 1st leaf, q = root of ith subtree */
02298     for (q = jprev ; q != ancestor [q] ; q = ancestor [q]) ;
02299     for (s = jprev ; s != q ; s = sparent)
02300     {
02301         sparent = ancestor [s] ;    /* path compression */
02302         ancestor [s] = q ;
02303     }
02304     return (q) ;                    /* q = least common ancester (jprev,j) */
02305 }
02306 
02307 /* column counts of LL'=A or LL'=A'A, given parent & post ordering */
02308 ivector cs_counts (XCONST hs_smatrix &A, XCONST ivector &parent,
02309   XCONST ivector &post)
02310 {
02311     int i, j, k, J, p, q, jleaf;
02312 
02313     int n = A.n ;
02314     ivector delta(0,n-1);
02315     delta.initialize();
02316     ivector& colcount = delta;
02317 
02318     hs_smatrix AT(n,A.nzmax);
02319     cs_transpose (A,0,AT) ;                                /* AT = A' */
02320 
02321     ivector ancestor(0,n-1);
02322     ancestor = -1;
02323     ivector maxfirst(0,n-1);
02324     maxfirst = -1;
02325     ivector prevleaf(0,n-1);
02326     prevleaf = -1;
02327     ivector first(0,n-1);
02328     first = -1;
02329 
02330     for (k = 0 ; k < n ; k++)                        /* find first [j] */
02331     {
02332         j = post [k] ;
02333         delta [j] = (first [j] == -1) ? 1 : 0 ;  /* delta[j]=1 if j is a leaf */
02334         for ( ; j != -1 && first [j] == -1 ; j = parent [j]) first [j] = k ;
02335     }
02336 
02337     ivector & ATp=AT.p;
02338     ivector & ATi=AT.i;
02339 
02340     for (i = 0 ; i < n ; i++) ancestor [i] = i ; /* each node in its own set */
02341     for (k = 0 ; k < n ; k++)
02342     {
02343         j = post [k] ;              /* j is the kth node in postordered etree */
02344         if (parent [j] != -1) delta [parent [j]]-- ;       /* j is not a root */
02345         for (J = j ; J != -1 ; J = -1)        /* J=j for LL'=A case */
02346         {
02347             for (p = ATp [J] ; p < ATp [J+1] ; p++)
02348             {
02349                 i = ATi [p] ;
02350                 q = cs_leaf (i, j, first, maxfirst, prevleaf, ancestor, &jleaf);
02351                 if (jleaf >= 1) delta [j]++ ;   /* A(i,j) is in skeleton */
02352                 if (jleaf == 2) delta [q]-- ;     /* account for overlap in q */
02353             }
02354         }
02355         if (parent [j] != -1) ancestor [j] = parent [j] ;
02356     }
02357     for (j = 0 ; j < n ; j++)                /* sum up delta's of each child */
02358     {
02359         if (parent [j] != -1) colcount [parent [j]] += colcount [j] ;
02360     }
02361     return (colcount) ;
02362 }
02363 
02364 ivector cs_counts (XCONST dvar_hs_smatrix &A, XCONST ivector &parent,
02365   XCONST ivector &post)
02366 {
02367     int i, j, k, J, p, q, jleaf;
02368 
02369     int n = A.n ;
02370     ivector delta(0,n-1);
02371     delta.initialize();
02372     ivector& colcount = delta;
02373 
02374     dvar_hs_smatrix AT(n,A.nzmax);
02375     cs_transpose (A,0,AT) ;                                /* AT = A' */
02376 
02377     ivector ancestor(0,n-1);
02378     ancestor = -1;
02379     ivector maxfirst(0,n-1);
02380     maxfirst = -1;
02381     ivector prevleaf(0,n-1);
02382     prevleaf = -1;
02383     ivector first(0,n-1);
02384     first = -1;
02385 
02386     for (k = 0 ; k < n ; k++)                        /* find first [j] */
02387     {
02388         j = post [k] ;
02389         delta [j] = (first [j] == -1) ? 1 : 0 ;  /* delta[j]=1 if j is a leaf */
02390         for ( ; j != -1 && first [j] == -1 ; j = parent [j]) first [j] = k ;
02391     }
02392 
02393     ivector & ATp=AT.p;
02394     ivector & ATi=AT.i;
02395 
02396     for (i = 0 ; i < n ; i++) ancestor [i] = i ; /* each node in its own set */
02397     for (k = 0 ; k < n ; k++)
02398     {
02399         j = post [k] ;              /* j is the kth node in postordered etree */
02400         if (parent [j] != -1) delta [parent [j]]-- ;       /* j is not a root */
02401         for (J = j ; J != -1 ; J = -1)        /* J=j for LL'=A case */
02402         {
02403             for (p = ATp [J] ; p < ATp [J+1] ; p++)
02404             {
02405                 i = ATi [p] ;
02406                 q = cs_leaf (i, j, first, maxfirst, prevleaf, ancestor, &jleaf);
02407                 if (jleaf >= 1) delta [j]++ ;   /* A(i,j) is in skeleton */
02408                 if (jleaf == 2) delta [q]-- ;     /* account for overlap in q */
02409             }
02410         }
02411         if (parent [j] != -1) ancestor [j] = parent [j] ;
02412     }
02413     for (j = 0 ; j < n ; j++)                /* sum up delta's of each child */
02414     {
02415         if (parent [j] != -1) colcount [parent [j]] += colcount [j] ;
02416     }
02417     return (colcount) ;
02418 }
02419 
02420 /* pinv = p', or p = pinv' */
02421 ivector cs_pinv (XCONST ivector &p, int n)
02422 {
02423     int k ;
02424     ivector pinv(0,n-1);
02425     pinv.initialize();
02426     for (k = 0 ; k < n ; k++) pinv [p [k]] = k ;/* invert the permutation */
02427     return (pinv) ;                             /* return result */
02428 }
02429 
02430 /* Constructor that does symbolic Cholesky  */
02431  //hs_symbolic::hs_symbolic(int _n, XCONST dmatrix &T, int order)
02432  //{
02433  //
02434  //    if (order != 0 && order != 1 )
02435  //    {
02436  //        cout << "!!! hs_symbolic: only order = 0,1 allowed" << endl;
02437  //        exit(0);
02438  //    }
02439  //
02440  //    hs_smatrix A(_n,T);
02441  //    n = _n;
02442  //
02443  //    // Allocate symbolic structure
02444  //    pinv.allocate(0,n-1);
02445  //    parent.allocate(0,n-1);
02446  //    cp.allocate(0,n);
02447  //    cp = 0;
02448  //
02449  //    hs_smatrix C(A);
02450  //    C = A;
02451  //
02452  //    if(order==0)
02453  //    {
02454  //      pinv(0) = -1;
02455  //    }
02456  //    else
02457  //    {
02458  //      ivector P = cs_amd (A) ;          /* P = amd(A+A'), or natural */
02459  //      pinv = cs_pinv (P, n) ;           /* find inverse permutation */
02460  //      hs_symperm(A,pinv,C);
02461  //    }
02462  //
02463  //    parent = cs_etree (C) ;                     /* find etree of C */
02464  //    ivector post = cs_post (parent, n) ;  /* postorder the etree */
02465  //    /* find column counts of chol(C) */
02466  //    ivector c = cs_counts (C, parent, post) ;
02467  //    lnz = cs_cumsum (cp, c, n) ;         /* find column pointers for L */
02468  //
02469  //}
02470  //
02471 hs_symbolic::hs_symbolic(void)
02472 {
02473     n = 0;
02474 
02475     // Allocate symbolic structure
02476     pinv.allocate();
02477     parent.allocate();
02478     cp.allocate();
02479 }
02480 
02481 hs_symbolic::hs_symbolic(XCONST dcompressed_triplet &_T, int order)
02482 {
02483   ADUNCONST(dcompressed_triplet,T)
02484   int _n=T.get_n();
02485 
02486     if (order != 0 && order != 1 )
02487     {
02488         cout << "!!! hs_symbolic: only order = 0,1 allowed" << endl;
02489         exit(0);
02490     }
02491 
02492     hs_smatrix A(_n,T);
02493     n = _n;
02494 
02495     // Allocate symbolic structure
02496     pinv.allocate(0,n-1);
02497     parent.allocate(0,n-1);
02498     cp.allocate(0,n);
02499     cp = 0;
02500 
02501     hs_smatrix C(A);
02502     C = A;
02503 
02504     if(order==0)
02505     {
02506       pinv(0) = -1;
02507     }
02508     else
02509     {
02510       ivector P = cs_amd (A) ;          /* P = amd(A+A'), or natural */
02511       pinv = cs_pinv (P, n) ;           /* find inverse permutation */
02512       hs_symperm(A,pinv,C);
02513     }
02514 
02515     parent = cs_etree (C) ;                     /* find etree of C */
02516     ivector post = cs_post (parent, n) ;  /* postorder the etree */
02517    ivector c = cs_counts (C, parent, post) ; /* find column counts of chol(C) */
02518     lnz = cs_cumsum (cp, c, n) ;         /* find column pointers for L */
02519 }
02520 
02521 hs_symbolic::hs_symbolic(XCONST dvar_compressed_triplet &_T, int order)
02522 {
02523   ADUNCONST(dvar_compressed_triplet,T)
02524   int _n=T.get_n();
02525 
02526     if (order != 0 && order != 1 )
02527     {
02528         cout << "!!! hs_symbolic: only order = 0,1 allowed" << endl;
02529         exit(0);
02530     }
02531 
02532     dvar_hs_smatrix A(_n,T);
02533     n = _n;
02534 
02535     // Allocate symbolic structure
02536     pinv.allocate(0,n-1);
02537     parent.allocate(0,n-1);
02538     cp.allocate(0,n);
02539     cp = 0;
02540 
02541     dvar_hs_smatrix C(A);
02542     C = A;
02543 
02544     if(order==0)
02545     {
02546       pinv(0) = -1;
02547     }
02548     else
02549     {
02550       ivector P = cs_amd (A) ;          /* P = amd(A+A'), or natural */
02551       pinv = cs_pinv (P, n) ;           /* find inverse permutation */
02552       hs_symperm(A,pinv,C);
02553     }
02554 
02555     parent = cs_etree (C) ;                     /* find etree of C */
02556     ivector post = cs_post (parent, n) ;  /* postorder the etree */
02557    ivector c = cs_counts (C, parent, post) ; /* find column counts of chol(C) */
02558     lnz = cs_cumsum (cp, c, n) ;         /* find column pointers for L */
02559 }
02560 dvar_compressed_triplet::dvar_compressed_triplet(int mmin,int mmax,int _n,
02561   int _m)
02562 {
02563   allocate(mmin,mmax,_n,_m);
02564 }
02565 
02566 void dcompressed_triplet::initialize(void)
02567 {
02568   x.initialize();
02569 }
02570 
02571 dcompressed_triplet::dcompressed_triplet(int mmin,int mmax,int _n,int _m)
02572 {
02573   allocate(mmin,mmax,_n,_m);
02574 }
02575 
02576 
02577 void dvar_compressed_triplet::allocate(int mmin,int mmax,int _n,int _m)
02578 {
02579   n=_n;
02580   m=_m;
02581   coords.allocate(1,2,mmin,mmax);
02582   x.allocate(mmin,mmax);
02583 }
02584 
02585 void dvar_compressed_triplet::deallocate(void)
02586 {
02587   n=-1;
02588   m=-1;
02589   coords.deallocate();
02590   x.deallocate();
02591 }
02592 
02593 void dcompressed_triplet::allocate(int mmin,int mmax,int _n,int _m)
02594 {
02595   n=_n;
02596   m=_m;
02597   coords.allocate(1,2,mmin,mmax);
02598   x.allocate(mmin,mmax);
02599 }
02600 
02601 void dcompressed_triplet::deallocate(void)
02602 {
02603   n=-1;
02604   m=-1;
02605   coords.deallocate();
02606   x.deallocate();
02607 }
02608 
02609 
02610 istream & operator >> (istream & is,dcompressed_triplet & M)
02611 {
02612   int mmin=M.indexmin();
02613   int mmax=M.indexmax();
02614   for (int i=mmin;i<=mmax;i++)
02615   {
02616     is >> M(i,1);
02617     is >> M(i,2);
02618     is >> M(i);
02619   }
02620   return is;
02621 }
02622 
02623 istream & operator >> (istream & is,dvar_compressed_triplet & M)
02624 {
02625   int mmin=M.indexmin();
02626   int mmax=M.indexmax();
02627   for (int i=mmin;i<=mmax;i++)
02628   {
02629     is >> M(i,1);
02630     is >> M(i,2);
02631     is >> M(i);
02632   }
02633   return is;
02634 }
02635 
02636 hs_smatrix * return_choleski_decomp(dcompressed_triplet & st)
02637 {
02638   //ADUNCONST(dmatrix,st)
02639   int n=st.get_n();
02640 
02641   hs_smatrix HS(n,st);  // Convert triplet to working format
02642 
02643   hs_symbolic S(st,1);         // Fill reducing row-col permutation
02644   hs_smatrix * PL = new hs_smatrix(S);              // Allocates cholesky factor
02645 
02646   chol(HS,S,*PL);                  // Does numeric factorization
02647 
02648   PL->set_symbolic(S);
02649 
02650   return PL;
02651 }
02652 
02653 dvar_hs_smatrix * return_choleski_decomp(dvar_compressed_triplet & st)
02654 {
02655   //ADUNCONST(dmatrix,st)
02656   int n=st.get_n();
02657 
02658   dvar_hs_smatrix HS(n,st);  // Convert triplet to working format
02659 
02660   hs_symbolic S(st,1);         // Fill reducing row-col permutation
02661   dvar_hs_smatrix * PL = new dvar_hs_smatrix(S);    // Allocates cholesky factor
02662 
02663   chol(HS,S,*PL);                  // Does numeric factorization
02664 
02665   PL->set_symbolic(S);
02666 
02667   return PL;
02668 }
02669 
02670 dvector return_choleski_decomp_solve(dcompressed_triplet & st,dvector& eps)
02671 {
02672   //ADUNCONST(dmatrix,st)
02673   int n=st.get_n();
02674 
02675   hs_smatrix HS(n,st);  // Convert triplet to working format
02676 
02677   hs_symbolic S(st,1);         // Fill reducing row-col permutation
02678   hs_smatrix L(S);              // Allocates cholesky factor
02679 
02680   chol(HS,S,L);                  // Does numeric factorization
02681 
02682   dvector x(0,n-1);
02683   eps.shift(0);
02684   x = cs_ipvec(S.pinv, eps);
02685   eps.shift(1);
02686   x = cs_lsolve(L,x);
02687   //x = cs_ltsolve(L,x);
02688   x = cs_pvec(S.pinv,x);
02689   x.shift(1);
02690   return x;
02691 }
02692 
02693 dvector return_choleski_factor_solve(hs_smatrix * PL,dvector& eps)
02694 {
02695   //ADUNCONST(dmatrix,st)
02696   hs_smatrix& L= *PL;
02697   int n=L.m;
02698   hs_symbolic & S = L.sym;
02699   dvector x(0,n-1);
02700   eps.shift(0);
02701   x = cs_ipvec(S.pinv, eps);
02702   eps.shift(1);
02703   x = cs_lsolve(L,x);
02704   //x = cs_ltsolve(L,x);
02705   x = cs_pvec(S.pinv,x);
02706   x.shift(1);
02707   return x;
02708 }
02709 
02710 dvar_vector return_choleski_factor_solve(dvar_hs_smatrix * PL,dvector& eps)
02711 {
02712   //ADUNCONST(dmatrix,st)
02713   dvar_hs_smatrix& L= *PL;
02714   int n=L.m;
02715   hs_symbolic & S = L.sym;
02716   dvar_vector x(0,n-1);
02717   eps.shift(0);
02718   x = cs_ipvec(S.pinv, eps);
02719   eps.shift(1);
02720   x = cs_lsolve(L,x);
02721   //x = cs_ltsolve(L,x);
02722   x = cs_pvec(S.pinv,x);
02723   x.shift(1);
02724   return x;
02725 }
02726 
02727 
02728 dvector solve(dcompressed_triplet & st,dmatrix & Hess,
02729   dvector& grad)
02730 {
02731   //ADUNCONST(dmatrix,st)
02732   int nz=st.indexmax();
02733   int n=Hess.indexmax();
02734   // fill up compressed triplet with nonzero entries of the Hessian
02735   for (int i=1;i<=nz;i++)
02736   {
02737     st(i)=Hess(st(1,i),st(2,i));
02738   }
02739 
02740   hs_smatrix HS(n,st);  // Convert triplet to working format
02741 
02742   hs_symbolic S(st,1);         // Fill reducing row-col permutation
02743   hs_smatrix L(S);              // Allocates cholesky factor
02744 
02745   chol(HS,S,L);                  // Does numeric factorization
02746 
02747   dvector x(0,n-1);
02748   grad.shift(0);
02749   x = cs_ipvec(S.pinv, grad);
02750   grad.shift(1);
02751   x = cs_lsolve(L,x);
02752   x = cs_ltsolve(L,x);
02753   x = cs_pvec(S.pinv,x);
02754   x.shift(1);
02755   return x;
02756 }
02757 
02758 dvector solve(dcompressed_triplet & st,dmatrix & Hess,
02759   dvector& grad,hs_symbolic& S)
02760 {
02761   //ADUNCONST(dmatrix,st)
02762   int nz=st.indexmax();
02763   int n=Hess.indexmax();
02764   // fill up compressed triplet with nonzero entries of the Hessian
02765   for (int i=1;i<=nz;i++)
02766   {
02767     st(i)=Hess(st(1,i),st(2,i));
02768   }
02769 
02770   hs_smatrix HS(n,st);  // Convert triplet to working format
02771 
02772   //hs_symbolic S(st,1);         // Fill reducing row-col permutation
02773   hs_smatrix L(S);              // Allocates cholesky factor
02774   //hs_smatrix L1(S);              // Allocates cholesky factor
02775 
02776   //chol(HS,S,L);                  // Does numeric factorization
02777   ivector nxcount;
02778   chol(HS,S,L);                  // Does numeric factorization
02779   //tmpxchol(HS,S,L,nxcount);                  // Does numeric factorization
02780 
02781   //cout << norm2(L.get_x()-L1.get_x()) << endl;
02782   dvector x(0,n-1);
02783   grad.shift(0);
02784   x = cs_ipvec(S.pinv, grad);
02785   grad.shift(1);
02786   x = cs_lsolve(L,x);
02787   x = cs_ltsolve(L,x);
02788   x = cs_pvec(S.pinv,x);
02789   x.shift(1);
02790   return x;
02791 }
02792 
02793 dvector solve(const dcompressed_triplet & _st,const dvector& _grad,
02794   const hs_symbolic& S)
02795   {
02796     ADUNCONST(dcompressed_triplet,st)
02797     ADUNCONST(dvector,grad)
02798     int n=st.get_n();
02799     //int n=Hess.indexmax();
02800     // fill up compressed triplet with nonzero entries of the Hessian
02801 
02802     hs_smatrix HS(n,st);  // Convert triplet to working format
02803 
02804     hs_smatrix L(S);              // Allocates cholesky factor
02805 
02806     ivector nxcount;
02807     chol(HS,S,L);                  // Does numeric factorization
02808 
02809     dvector x(0,n-1);
02810     grad.shift(0);
02811     x = cs_ipvec(S.pinv, grad);
02812     grad.shift(1);
02813     x = cs_lsolve(L,x);
02814     x = cs_ltsolve(L,x);
02815     x = cs_pvec(S.pinv,x);
02816     x.shift(1);
02817     return x;
02818   }
02819 
02820 dvector solve(const dcompressed_triplet & _st,const dvector& _grad,
02821   const hs_symbolic& S,int& ierr)
02822   {
02823     ADUNCONST(dcompressed_triplet,st)
02824     ADUNCONST(dvector,grad)
02825     int n=st.get_n();
02826     //int n=Hess.indexmax();
02827     // fill up compressed triplet with nonzero entries of the Hessian
02828 
02829     hs_smatrix HS(n,st);  // Convert triplet to working format
02830 
02831     hs_smatrix L(S);              // Allocates cholesky factor
02832 
02833     ivector nxcount;
02834     ierr=chol(HS,S,L);     // 0 error 1 OK        Does numeric factorization
02835 
02836     dvector x(0,n-1);
02837   if (ierr)
02838   {
02839     grad.shift(0);
02840     x = cs_ipvec(S.pinv, grad);
02841     grad.shift(1);
02842     x = cs_lsolve(L,x);
02843     x = cs_ltsolve(L,x);
02844     x = cs_pvec(S.pinv,x);
02845     x.shift(1);
02846   }
02847   else
02848   {
02849     x.initialize();
02850     x.shift(1);
02851   }
02852   return x;
02853 }
02854 
02855 int allocated(const dcompressed_triplet & _st)
02856 {
02857   ADUNCONST(dcompressed_triplet,st)
02858   return allocated(st.get_coords());
02859 }
02860 int allocated(const dvar_compressed_triplet & _st)
02861 {
02862   ADUNCONST(dvar_compressed_triplet,st)
02863   return allocated(st.get_coords());
02864 }
02865 dvariable ln_det(dvar_compressed_triplet& VM)
02866 {
02867   int n=VM.get_n();
02868   dvar_hs_smatrix H(n,VM);
02869   hs_symbolic S(VM,1);         // Fill reducing row-col permutation
02870   dvar_hs_smatrix L(S);              // Allocates cholesky factor
02871   int ierr=chol(H,S,L);                  // Does numeric factorization
02872   if (ierr==0)
02873   {
02874     cerr << "Error matrix not positrive definite in chol" << endl;
02875     ad_exit(1);
02876   }
02877   return 2.0*ln_det(L);
02878   //return L.x(0);
02879 }
02880 
02881 
02882 dvariable ln_det(dvar_compressed_triplet& VM,hs_symbolic& S)
02883 {
02884   int n=VM.get_n();
02885   dvar_hs_smatrix H(n,VM);
02886   //hs_symbolic S(VM,1);         // Fill reducing row-col permutation
02887   dvar_hs_smatrix L(S);              // Allocates cholesky factor
02888   int ierr=chol(H,S,L);                  // Does numeric factorization
02889   if (ierr==0)
02890   {
02891     cerr << "Error matrix not positive definite in chol" << endl;
02892     ad_exit(1);
02893   }
02894   return 2.0*ln_det(L);
02895   //return L.x(0);
02896 }
02897 
02898 
02899  int check_flag=0;
02900 
02901 dvariable ln_det(dvar_compressed_triplet& VM,hs_symbolic& S,
02902   dcompressed_triplet& s)
02903 {
02904   RETURN_ARRAYS_INCREMENT();
02905   int n=VM.get_n();
02906   dvar_hs_smatrix H(n,VM);
02907   //hs_symbolic S(VM,1);         // Fill reducing row-col permutation
02908   dvar_hs_smatrix L(S);              // Allocates cholesky factor
02909   int ierr = 0;
02910   if (check_flag==0)
02911   {
02912     ierr=varchol(H,S,L,s);
02913   }
02914   else
02915   {
02916     ierr=chol(H,S,L);
02917   }
02918   if (ierr==0)
02919   {
02920     cerr << "Error matrix not positive definite in chol" << endl;
02921     ad_exit(1);
02922   }
02923   //set_gradstack_flag("AAC");
02924   dvariable tmp= 2.0*ln_det(L);
02925   RETURN_ARRAYS_DECREMENT();
02926   return tmp;
02927   //return L.x(0);
02928 }
02929 
02930 
02931 double ln_det(const dcompressed_triplet& VVM,
02932               const hs_symbolic& T)
02933 {
02934   //ADUNCONST(dcompressed_triplet,VM)
02935   //ADUNCONST(hs_symbolic,S)
02936   dcompressed_triplet& VM = (dcompressed_triplet&)VVM;
02937   hs_symbolic& S = (hs_symbolic&)T;
02938   int n=VM.get_n();
02939   hs_smatrix H(n,VM);
02940   //hs_symbolic S(VM,1);         // Fill reducing row-col permutation
02941   hs_smatrix L(S);              // Allocates cholesky factor
02942   int ierr=chol(H,S,L);                  // Does numeric factorization
02943   if (ierr==0)
02944   {
02945     cerr << "Error matrix not positrive definite in chol" << endl;
02946     ad_exit(1);
02947   }
02948   return 2.0*ln_det(L);
02949   //return L.x(0);
02950 }
02951 
02952 
02953 double ln_det(const dcompressed_triplet& VVM)
02954 {
02955   //ADUNCONST(dcompressed_triplet,VM)
02956   dcompressed_triplet& VM = (dcompressed_triplet&)VVM;
02957   int n=VM.get_n();
02958   hs_smatrix H(n,VM);
02959   hs_symbolic S(VM,1);         // Fill reducing row-col permutation
02960   hs_smatrix L(S);              // Allocates cholesky factor
02961   int ierr=chol(H,S,L);                  // Does numeric factorization
02962   if (ierr==0)
02963   {
02964     cerr << "Error matrix not positrive definite in chol" << endl;
02965     ad_exit(1);
02966   }
02967   return 2.0*ln_det(L);
02968   //return L.x(0);
02969 }
02970 
02971 
02972 int cholnew(XCONST hs_smatrix &AA, XCONST hs_symbolic &T, hs_smatrix &LL)
02973 {
02974     //ADUNCONST(hs_symbolic,S)
02975     //ADUNCONST(hs_smatrix,L)
02976     //ADUNCONST(hs_smatrix,A)
02977     hs_symbolic& S = (hs_symbolic&)T;
02978     hs_smatrix& A = (hs_smatrix&)AA;
02979     hs_smatrix& L = (hs_smatrix&)LL;
02980     double d, lki;
02981     int top, i, p, k, n;
02982 
02983     n = A.n ;
02984 
02985     ivector c(0,n-1);                              /* int workspace */
02986     ivector s(0,n-1);                                   /* int workspace */
02987     dvector x(0,n-1) ;                        /* double workspace */
02988     double xcount=0.0;                        /* double workspace */
02989 
02990     ivector & cp=S.cp;
02991     ivector & pinv=S.pinv;
02992     ivector & parent=S.parent;
02993 
02994     hs_smatrix C(A);
02995     C = A;
02996     if(S.pinv[0]!=-1)
02997       hs_symperm(A,pinv,C);
02998 
02999     ivector & Cp=C.p;
03000     ivector & Ci=C.i;
03001     dvector & Cx=C.x;
03002 
03003     ivector & Lp=L.p;
03004     ivector & Li=L.i;
03005     dvector & Lx=L.x;
03006 
03007     for (k = 0 ; k < n ; k++)
03008       Lp [k] = c [k] = cp [k] ;
03009 
03010     for (k = 0 ; k < n ; k++)            /* compute L(:,k) for L*L' = C */
03011     {
03012         /* --- Nonzero pattern of L(k,:) ------------------------------------ */
03013         top = cs_ereach (C, k, parent, s, c) ;      /* find pattern of L(k,:) */
03014         x [k] = 0 ;                                    /* x (0:k) is now zero */
03015         for (p = Cp [k] ; p < Cp [k+1] ; p++)       /* x = full(triu(C(:,k))) */
03016         {
03017             if (Ci [p] <= k) x [Ci [p]] = Cx [p] ;
03018         }
03019         d = x [k] ;                        /* d = C(k,k) */
03020         x [k] = 0 ;                        /* clear x for k+1st iteration */
03021         /* --- Triangular solve --------------------------------------------- */
03022         for ( ; top < n ; top++)    /* solve L(0:k-1,0:k-1) * x = C(:,k) */
03023         {
03024             i = s [top] ;                /* s [top..n-1] is pattern of L(k,:) */
03025             lki = x [i] / Lx [Lp [i]] ; /* L(k,i) = x (i) / L(i,i) */
03026             xcount++;
03027             x [i] = 0 ;                        /* clear x for k+1st iteration */
03028 
03029 
03030             int Lpi1=Lp[i]+1;
03031             int ci=c[i];
03032             if (Lpi1<ci)
03033             {
03034               myacc(p,Lpi1,ci,Li,x,Lx,lki);
03035             }
03036             /*
03037             for (p = Lp [i] + 1 ; p < c [i] ; p++)
03038             {
03039                 x [Li [p]] -= Lx [p] * lki ;
03040             }
03041             */
03042 
03043             d -= lki * lki ;                /* d = d - L(k,i)*L(k,i) */
03044             p = c [i]++ ;
03045             Li [p] = k ;                /* store L(k,i) in column i */
03046             Lx [p] = lki ;
03047         }
03048         /* --- Compute L(k,k) ----------------------------------------------- */
03049         if (d <= 0) return (0) ; /* not pos def */
03050         p = c [k]++ ;
03051         Li [p] = k ;                   /* store L(k,k) = sqrt (d) in column k */
03052         Lx [p] = sqrt (d) ;
03053     }
03054     Lp [n] = cp [n] ;                    /* finalize L */
03055     return (1) ;
03056 }
03057 
03058 static void dfcholeski_sparse(void);
03059 
03060 int varchol(XCONST dvar_hs_smatrix &AA, XCONST hs_symbolic &T,
03061   dvar_hs_smatrix &LL, dcompressed_triplet & sparse_triplet2)
03062  //laplace_approximation_calculator * lapprox)
03063 {
03064   RETURN_ARRAYS_INCREMENT(); //Need this statement because the function
03065   //ADUNCONST(hs_symbolic,S)
03066   //ADUNCONST(dvar_hs_smatrix,L)
03067   //ADUNCONST(dvar_hs_smatrix,A)
03068     hs_symbolic& S = (hs_symbolic&)T;
03069     dvar_hs_smatrix& A = (dvar_hs_smatrix&)AA;
03070     dvar_hs_smatrix& L = (dvar_hs_smatrix&)LL;
03071   int icount=0;
03072   double lki;
03073   double d;
03074   int top, i, p, k, n;
03075   n = A.n ;
03076   ivector c(0,n-1);                              /* int workspace */
03077   ivector s(0,n-1);                                   /* int workspace */
03078   dvector x(0,n-1) ;                        /* double workspace */
03079 
03080   ivector & cp=S.cp;
03081   ivector & pinv=S.pinv;
03082   ivector & parent=S.parent;
03083 
03084   dvar_hs_smatrix C(A);
03085   C = A;
03086   if(S.pinv[0]!=-1)
03087     hs_symperm(A,pinv,C);
03088 
03089   ivector & Cp=C.p;
03090   ivector & Ci=C.i;
03091   dvector Cx=value(C.x);
03092 
03093   ivector & Lp=L.p;
03094   ivector & Li=L.i;
03095   dvector Lx=value(L.x);
03096   int txcount=0;
03097   int lkicount=0;
03098   int tccount=0;
03099 
03100   for (k = 0 ; k < n ; k++)
03101   {
03102     Lp [k] = c [k] = cp [k] ;
03103   }
03104 
03105   for (k = 0 ; k < n ; k++)
03106   {
03107     top = cs_ereach (C, k, parent, s, c) ;
03108     x [k] = 0 ;
03109     for (p = Cp [k] ; p < Cp [k+1] ; p++)
03110     {
03111       if (Ci[p] <= k) x [Ci[p]] = Cx[p] ;
03112     }
03113     d = x[k] ;
03114     x[k] = 0.0;
03115     for ( ; top < n ; top++)
03116     {
03117       i = s[top] ;
03118       lki = x[i] / Lx[Lp[i]] ;
03119       txcount++;
03120       icount++;   // count the number of times lki is overwritten
03121       lkicount++;   // count the number of times lki is overwritten
03122       x [i] = 0 ;
03123       for (p = Lp [i] + 1 ; p < c [i] ; p++)
03124       {
03125         x[Li[p]] -= Lx[p] * lki ;
03126       }
03127       d -= lki * lki ;
03128       p = c [i]++ ;
03129       tccount++;
03130       Li [p] = k ;
03131       Lx [p] = lki ;
03132     }
03133     if (d <= 0) return (0) ;
03134     p = c [k]++ ;
03135     tccount++;
03136     Li [p] = k ;
03137     Lx [p] = sqrt (d) ;
03138   }
03139   Lp [n] = cp [n] ;
03140   xxx(txcount);
03141   int mmin=Lx.indexmin();
03142   int mmax=Lx.indexmax();
03143   for (int ii=mmin;ii<=mmax;ii++)
03144   {
03145     value(L.x(ii))=Lx(ii);
03146   }
03147   int nxcount=txcount;
03148   int nccount=tccount;
03149   int nlkicount=lkicount;
03150   save_identifier_string("ty");
03151 
03152   save_int_value(nxcount);
03153   save_int_value(nlkicount);
03154   save_int_value(nccount);
03155 
03156   save_identifier_string("tu");
03157   C.x.save_dvar_vector_position();
03158   save_identifier_string("wy");
03159   L.x.save_dvar_vector_position();
03160   save_identifier_string("iy");
03161   save_ad_pointer(&S);
03162   save_ad_pointer(&sparse_triplet2);
03163   save_identifier_string("dg");
03164   gradient_structure::GRAD_STACK1->
03165       set_gradient_stack(dfcholeski_sparse);
03166   RETURN_ARRAYS_DECREMENT(); //Need this statement because the function
03167   return (1) ;
03168 }
03169 
03170 static void dfcholeski_sparse(void)
03171 {
03172   verify_identifier_string("dg");
03173   dcompressed_triplet * sparse_triplet2  =
03174     ( dcompressed_triplet *) restore_ad_pointer();
03175   hs_symbolic & S  =
03176     *( hs_symbolic * ) restore_ad_pointer();
03177   verify_identifier_string("iy");
03178   dvar_vector_position dpos=restore_dvar_vector_position();
03179   verify_identifier_string("wy");
03180   dvar_vector_position cpos=restore_dvar_vector_position();
03181   verify_identifier_string("tu");
03182 
03183   int nccount=restore_int_value();
03184   int nlkicount=restore_int_value();
03185   int nxcount=restore_int_value();
03186 
03187   verify_identifier_string("ty");
03188 
03189     dvector  dfLx=restore_dvar_vector_derivatives(dpos);
03190     hs_smatrix A(sparse_triplet2->get_n(),
03191       *(sparse_triplet2) );
03192 
03193     //hs_symbolic& S=*(lapprox->sparse_symbolic2);
03194     double d;
03195     double lki=0;
03196     double dfd=0.0;
03197     double dflki=0.0;
03198     int top, i, p, k, n;
03199     int p2;
03200     n = A.n ;
03201 
03202     ivector cold(0,n-1);                              /* int workspace */
03203     ivector c(0,n-1);                              /* int workspace */
03204     imatrix ssave(0,n-1);                              /* int workspace */
03205     ivector s(0,n-1);                                   /* int workspace */
03206     dvector x(0,n-1) ;                        /* double workspace */
03207     dvector dfx(0,n-1) ;                        /* double workspace */
03208     dfx.initialize();
03209 
03210     ivector & cp=S.cp;
03211     ivector & pinv=S.pinv;
03212     ivector & parent=S.parent;
03213 
03214     hs_smatrix C(A);
03215     C = A;
03216     if(S.pinv[0]!=-1)
03217       hs_symperm(A,pinv,C);
03218 
03219     hs_smatrix L(S);              // Allocates cholesky factor
03220 
03221     ivector & Cp=C.p;
03222     ivector & Ci=C.i;
03223     dvector & Cx=C.x;
03224     dvector dfCx(C.x.indexmin(),Cx.indexmax());
03225     dfCx.initialize();
03226 
03227     ivector & Lp=L.p;
03228     ivector & Li=L.i;
03229     dvector & Lx=L.x;
03230 
03231   // counters
03232     int tccount=0;
03233     int txcount=0;
03234     ivector ccount(c.indexmin(),c.indexmax());
03235     ccount.initialize();
03236     ivector Licount(Li.indexmin(),Li.indexmax());
03237     Licount.initialize();
03238     ivector Lxcount(Lx.indexmin(),Lx.indexmax());
03239     Lxcount.initialize();
03240     ivector xcount(x.indexmin(),x.indexmax());
03241     xcount.initialize();
03242 
03243     int pcount=0;
03244     int icount=0;
03245     int lkicount=0;
03246 
03247     int p1=0;
03248 
03249     dvector xsave(0,nxcount);
03250     ivector csave(0,nccount);
03251     dvector lkisave(0,nlkicount);
03252 
03253     tccount=0;
03254     txcount=0;
03255     pcount=0;
03256     ccount.initialize();
03257     Licount.initialize();
03258     Lxcount.initialize();
03259     xcount.initialize();
03260     lkicount=0;
03261 
03262     // do it again -- this oulod be the frist time in the adjoint code
03263     for (k = 0 ; k < n ; k++)
03264     {
03265       Lp [k] = c [k] = cp [k] ;
03266       //ccount[k]++;
03267       //tccount++;
03268     }
03269     ivector Top(0,n);
03270 
03271     for (k = 0 ; k < n ; k++)            /* compute L(:,k) for L*L' = C */
03272     {
03273         /* --- Nonzero pattern of L(k,:) ------------------------------------ */
03274         Top(k) = cs_ereach (C, k, parent, s, c) ;
03275 
03276         //ssave(k)=s;
03277         if (allocated(ssave(k)))
03278         {
03279           cerr << "This can't happen" << endl;
03280           ad_exit(1);
03281         }
03282         else
03283         {
03284           ssave(k).allocate(Top(k),n-1);
03285         }
03286         ssave(k)=s(Top(k),n-1);
03287         x [k] = 0 ;                                    /* x (0:k) is now zero */
03288         //xcount[k]++;
03289         for (p = Cp [k] ; p < Cp [k+1] ; p++)       /* x = full(triu(C(:,k))) */
03290         {
03291           if (Ci [p] <= k)
03292           {
03293             x[Ci[p]] = Cx[p] ;
03294             xcount[Ci[p]]++;
03295           }
03296         }
03297         d = x [k] ;                        /* d = C(k,k) */
03298         //dcount++;
03299         x [k] = 0 ;                        /* clear x for k+1st iteration */
03300         //xcount[k]++;
03301         /* --- Triangular solve --------------------------------------------- */
03302         top=Top(k);
03303         for ( ; top < n ; top++)    /* solve L(0:k-1,0:k-1) * x = C(:,k) */
03304         {
03305             i = s [top] ;
03306             icount++;
03307             lkisave(lkicount++)=lki;
03308             lki = x [i] / Lx [Lp [i]] ; /* L(k,i) = x (i) / L(i,i) */
03309             xsave(txcount++)=x(i);
03310             x [i] = 0 ;                        /* clear x for k+1st iteration */
03311             for (p1 = Lp [i] + 1 ; p1 < c [i] ; p1++)
03312             {
03313                x [Li [p1]] -= Lx [p1] * lki ;
03314             }
03315             d -= lki * lki ;                /* d = d - L(k,i)*L(k,i) */
03316             csave(tccount++)=c[i];
03317             p2 = c [i] ;
03318             //ofs << ttc++ << " " << p2 << " 2" << endl;
03319             c[i]++;
03320             ccount[i]++;
03321             Li [p2] = k ;                /* store L(k,i) in column i */
03322             Licount[p2]++;
03323             if (Licount(p2)>1)
03324             {
03325               cerr << "Error unhandled case in chol" << endl;
03326             }
03327             Lx [p2] = lki ;
03328             Lxcount[p2]++;
03329             if (Lxcount(p2)>1)
03330             {
03331               cerr << "Error unhandled case in chol" << endl;
03332               ad_exit(1);
03333             }
03334         }
03335         /* --- Compute L(k,k) ----------------------------------------------- */
03336         if (d <= 0) return ; /* not pos def */
03337         csave(tccount++)=c[k];
03338         p = c [k];
03339         //ofs << ttc++ << " " << p << " 1"<< endl;
03340         c[k]++;
03341         //ccount[k]++;
03342         pcount++;
03343         Li [p] = k ;                   /* store L(k,k) = sqrt (d) in column k */
03344         Licount[p]++;
03345         if (Licount(p)>1)
03346         {
03347           cerr << "Error unhandled case in chol" << endl;
03348         }
03349         Lx [p] = sqrt (d) ;
03350         Lxcount[p]++;
03351         if (Lxcount(p)>1)
03352         {
03353           cerr << "Error unhandled case in chol" << endl;
03354           ad_exit(1);
03355         }
03356     }
03357     Lp [n] = cp [n] ;                    /* finalize L */
03358 
03359 
03360     // now the real adjoint code
03361 
03362     for (k = n-1 ; k >=0 ; k--)
03363     {
03364       c[k]=csave(--tccount);
03365       p=c[k];
03366       s(ssave(k).indexmin(),ssave(k).indexmax())=ssave(k);
03367       //if (k==3)
03368        // cout << "HERE2" << endl;
03369       // Lx [p] = sqrt (d) ;
03370       //ofs << --ttc << " " << p << " 1" << endl;
03371 
03372       dfd+=dfLx(p)/(2.0*Lx(p));
03373       dfLx(p)=0.0;
03374 
03375       //c[k]=csave(--tccount);
03376       //p=c[k];
03377 
03378       for (top=n-1 ; top >=Top[k] ; top--)
03379       {
03380         i=s(top);
03381         //Lx [p] = lki ;
03382         c[i]=csave(--tccount);
03383         p2=c[i];
03384         //ofs << --ttc << " " << p2 << " 2" << endl;
03385         dflki+=dfLx[p2];
03386         dfLx[p2]=0.0;
03387         //d -= lki * lki ;                /* d = d - L(k,i)*L(k,i) */
03388         dflki-=2.0*dfd*lki;
03389         for (p1 = Lp [i] + 1 ; p1 < c [i] ; p1++)
03390         {
03391           //x [Li [p1]] -= Lx [p1] * lki ;
03392           dflki-=dfx(Li(p1))*Lx(p1);
03393           dfLx(p1)-=dfx(Li(p1))*lki;
03394         }
03395         dfx[i]=0.0;
03396         // maybe not here
03397         x(i)=xsave(--txcount);
03398         // lki = x[i] / Lx[Lp[i]] ;
03399         dfx(i)+=dflki/Lx(Lp(i));
03400         dfLx(Lp(i))-=dflki*x(i)/square(Lx(Lp(i)));
03401         // but here
03402         dflki=0.0;
03403         lki=lkisave(--lkicount);
03404       }
03405       // x[k]=0.0;
03406       dfx[k]=0.0;
03407       //d = x [k] ;                        /* d = C(k,k) */
03408       dfx(k)+=dfd;
03409       dfd=0.0;
03410       for (p1 = Cp [k+1]-1 ; p1 >= Cp [k] ; p1--)
03411       {
03412         if (Ci [p1] <= k)
03413         {
03414           //x[Ci[p1]] = Cx[p1] ;
03415           dfCx[p1]+=dfx[Ci[p1]];
03416           dfx[Ci[p1]]=0.0;
03417         }
03418       }
03419       //x [k] = 0 ;                        /* clear x for k+1st iteration */
03420       dfx(k)=0.0;
03421     }
03422 
03423     dfCx.save_dvector_derivatives(cpos);
03424 
03425     return  ;
03426 }
03427 
03428 int chol(XCONST dvar_hs_smatrix &AA, XCONST hs_symbolic &T,
03429   dvar_hs_smatrix &LL)
03430 {
03431   //ADUNCONST(hs_symbolic,S)
03432   //ADUNCONST(dvar_hs_smatrix,L)
03433   //ADUNCONST(dvar_hs_smatrix,A)
03434   dvar_hs_smatrix& A = (dvar_hs_smatrix&)AA;
03435   hs_symbolic& S = (hs_symbolic&)T;
03436   dvar_hs_smatrix& L = (dvar_hs_smatrix&)LL;
03437   int icount=0;
03438   dvariable lki;
03439   dvariable d;
03440   int top, i, p, k, n;
03441   int p1,p2;
03442   n = A.n ;
03443   ivector c(0,n-1);                              /* int workspace */
03444   ivector s(0,n-1);                                   /* int workspace */
03445   dvar_vector x(0,n-1) ;                        /* double workspace */
03446 
03447   ivector & cp=S.cp;
03448   ivector & pinv=S.pinv;
03449   ivector & parent=S.parent;
03450 
03451   dvar_hs_smatrix C(A);
03452   C = A;
03453   if(S.pinv[0]!=-1)
03454     hs_symperm(A,pinv,C);
03455 
03456   //dvar_hs_smatrix & E = C;
03457 
03458   ivector & Cp=C.p;
03459   ivector & Ci=C.i;
03460   dvar_vector & Cx=C.x;
03461 
03462   ivector & Lp=L.p;
03463   ivector & Li=L.i;
03464   dvar_vector & Lx=L.x;
03465 
03466   for (k = 0 ; k < n ; k++)
03467   {
03468     Lp [k] = c [k] = cp [k] ;
03469   }
03470 
03471   for (k = 0 ; k < n ; k++)
03472   {
03473     top = cs_ereach (C, k, parent, s, c) ;
03474     x [k] = 0 ;
03475     for (p = Cp [k] ; p < Cp [k+1] ; p++)
03476     {
03477       if (Ci[p] <= k) x [Ci[p]] = Cx[p] ;
03478     }
03479     d = x[k] ;
03480     x[k] = 0.0;
03481     for ( ; top < n ; top++)
03482     {
03483       i = s[top] ;
03484       lki = x[i] / Lx[Lp[i]] ;
03485       icount++;   // count the number of times lki is overwritten
03486       x [i] = 0 ;
03487       for (p1 = Lp [i] + 1 ; p1 < c [i] ; p1++)
03488       {
03489         x[Li[p1]] -= Lx[p1] * lki ;
03490       }
03491       d -= lki * lki ;
03492       p2 = c[i]++ ;
03493       Li [p2] = k ;
03494       Lx [p2] = lki ;
03495     }
03496     if (d <= 0) return (0) ;
03497     p = c[k]++ ;
03498     Li [p] = k ;
03499     Lx [p] = sqrt (d) ;
03500   }
03501   Lp [n] = cp [n] ;
03502   xxx(icount);
03503   return (1) ;
03504 }
03505  //class hs_symbolic    // Info for symbolic cholesky
03506  //{
03507  //    public:
03508  //
03509  //    int n ;     // Dimension of underlying pos. def. matrix
03510  //    ivector pinv ;     // inverse row perm. for QR, fill red. perm for Chol
03511  //    ivector parent ;   // elimination tree for Cholesky and QR
03512  //    ivector cp ;       // column pointers for Cholesky, row counts for QR
03513  //    double lnz ;    // # entries in L for LU or Cholesky; in V for QR
03514  //
03515  //    hs_symbolic(int, XCONST css *);
03516  //    hs_symbolic(int n, XCONST dmatrix &T, int order);
03517  //    hs_symbolic(XCONST dcompressed_triplet &T, int order);
03518  //    hs_symbolic(XCONST dvar_compressed_triplet &T, int order);
03519  //    int is_null();
03520  //    int cmp(hs_symbolic &S);
03521  //    hs_symbolic(void);
03522  //};
03523 
03524 void hs_smatrix::set_symbolic(hs_symbolic& s)
03525 {
03526   sym.n=s.n;
03527   sym.pinv.allocate(s.pinv.indexmin(),s.pinv.indexmax());
03528   sym.pinv=s.pinv;
03529   sym.parent.allocate(s.parent.indexmin(),s.parent.indexmax());
03530   sym.parent=s.parent;
03531   sym.cp.allocate( s.cp.indexmin(),s.cp.indexmax());
03532   sym.cp=s.cp;
03533   sym.lnz=s.lnz;
03534 }
03535 
03536 void dvar_hs_smatrix::set_symbolic(hs_symbolic& s)
03537 {
03538   sym.n=s.n;
03539   sym.pinv.allocate(s.pinv.indexmin(),s.pinv.indexmax());
03540   sym.pinv=s.pinv;
03541   sym.parent.allocate(s.parent.indexmin(),s.parent.indexmax());
03542   sym.parent=s.parent;
03543   sym.cp.allocate( s.cp.indexmin(),s.cp.indexmax());
03544   sym.cp=s.cp;
03545   sym.lnz=s.lnz;
03546 }
03547 
03548 void report_dvar_vector_derivatives(void)
03549 {
03550   verify_identifier_string("jr");
03551   /*dvar_vector_position dpos=*/restore_dvar_vector_position();
03552   //dvector  dfLx=restore_dvar_vector_derivatives(dpos);
03553   verify_identifier_string("jx");
03554 }
03555 
03556 
03557 void report_derivatives(const dvar_vector& x)
03558 {
03559   save_identifier_string("jx");
03560   x.save_dvar_vector_position();
03561   gradient_structure::GRAD_STACK1->
03562       set_gradient_stack(report_dvar_vector_derivatives);
03563   save_identifier_string("jr");
03564 }
03565 
03566 void get_inverse_sparse_hessian(dcompressed_triplet & st, hs_symbolic& S,
03567   uostream& ofs1,ofstream& ofs,int usize,int xsize,dvector& u)
03568 {
03569   int n=st.get_n();
03570   hs_smatrix HS(n,st);
03571   hs_smatrix L(S);
03572   chol(HS,S,L);
03573   dvector gr(0,n-1);
03574   dvector x(0,n-1);
03575   gr.initialize();
03576   ofs1 << usize << xsize;
03577   int i;
03578   for (i=1;i<=n;i++)
03579   {
03580     gr(i-1)=1.0;
03581     x = cs_ipvec(S.pinv, gr);
03582     gr(i-1)=0.0;
03583     x = cs_lsolve(L,x);
03584     x = cs_ltsolve(L,x);
03585     x = cs_pvec(S.pinv,x);
03586     ofs << setprecision(5) << setscientific()
03587         << setw(14) << u(i) << " " << sqrt(x(i-1)) << endl;;
03588     x.shift(1);
03589     ofs1 << x;
03590     x.shift(0);
03591   }
03592 }
03593 //#include "cs.h"
03594 /* C = A*B */
03595 hs_smatrix cs_multiply(const hs_smatrix &AA, const hs_smatrix  &BB)
03596 {
03597     //ADUNCONST(hs_smatrix,A)
03598     //ADUNCONST(hs_smatrix,B)
03599     hs_smatrix& A = (hs_smatrix&)AA;
03600     hs_smatrix& B = (hs_smatrix&)BB;
03601     int p, j, nz = 0, anz,  m, n, bnz;
03602     //hs_smatrix *pC ;
03603     //hs_smatrix C(n,anz + bnz);
03604     //hs_smatrix& C=*pC ;
03605 
03606      //  if (!CS_CSC (A) || !CS_CSC (B)) return (NULL) ;      /* check inputs */
03607        if (A.n != B.m) return (NULL) ;
03608        m = A.m ; anz = A.p[A.n] ;
03609        n = B.n ; ivector & Bp = B.p ; ivector & Bi = B.i ;
03610        dvector & Bx = B.x ; bnz = Bp[n] ;
03611        //w = cs_calloc (m, sizeof (int)) ;                   /* get workspace */
03612        ivector w(0,m);                    /* get workspace */
03613        //values = (A.x != NULL) && (Bx != NULL) ;
03614        //x = values ? cs_malloc (m, sizeof (double)) : NULL ;/* get workspace */
03615        dvector x(0,m);  /* get workspace */
03616        hs_smatrix C(n,anz + bnz) ;        /* allocate result */
03617        //if (!C || !w || (values && !x)) return (cs_done (C, w, x, 0)) ;
03618        ivector & Cp = C.p ;
03619        for (j = 0 ; j < n ; j++)
03620        {
03621            C.reallocate(2*(C.nzmax)+m);
03622 
03623            //if (nz + m > C.nzmax && !cs_sprealloc (C, 2*(C.nzmax)+m))
03624            //{
03625            //    return (cs_done (C, w, x, 0)) ;             /* out of memory */
03626            //}
03627            ivector& Ci = C.i ;
03628            dvector& Cx = C.x ;       /* C->i and C->x may be reallocated */
03629            Cp [j] = nz ;                   /* column j of C starts here */
03630            for (p = Bp [j] ; p < Bp [j+1] ; p++)
03631            {
03632                nz = cs_scatter (A, Bi [p], Bx[p], w, x, j+1, C, nz) ;
03633            }
03634            for (p = Cp [j] ; p < nz ; p++) Cx [p] = x [Ci [p]] ;
03635        }
03636        Cp [n] = nz ;                       /* finalize the last column of C */
03637 
03638     return C;
03639       //cs_sprealloc (C, 0) ;               /* remove extra space from C */
03640    //return (cs_done (C, w, x, 1)) ;     /* success; free workspace, return C */
03641 }
03642 
03643 hs_smatrix operator * (const hs_smatrix &A, const hs_smatrix  &B)
03644 {
03645   return cs_multiply(A,B);
03646 }
03647 dcompressed_triplet make_dcompressed_triplet(const dmatrix & M)
03648 {
03649   int mmin=M.indexmin();
03650   int mmax=M.indexmax();
03651   int n=mmax-mmin+1;
03652   int _jmin=M(mmin).indexmin();
03653   int _jmax=M(mmax).indexmax();
03654   int m=_jmax-_jmin+1;
03655   int ii=0;
03656   for (int i=mmin;i<=mmax;i++)
03657   {
03658     int jmin=M(i).indexmin();
03659     int jmax=M(i).indexmax();
03660     for (int j=jmin;j<=jmax;j++)
03661     {
03662       if (M(i,j) !=0) ii++;
03663     }
03664   }
03665   dcompressed_triplet dct(1,ii,n,m);
03666   ii=0;
03667   for (int i=mmin;i<=mmax;i++)
03668   {
03669     int jmin=M(i).indexmin();
03670     int jmax=M(i).indexmax();
03671     for (int j=jmin;j<=jmax;j++)
03672     {
03673       if (M(i,j) !=0)
03674       {
03675         ii++;
03676         dct(ii)=M(i,j);
03677         dct(1,ii)=i;
03678         dct(2,ii)=j;
03679       }
03680     }
03681   }
03682   return dct;
03683 }
03684 /*
03685 extern "C"  {
03686   void ad_boundf(int i)
03687   {
03688     // so we can stop here
03689     exit(i);
03690   }
03691 }
03692 */
03693 
03694 hs_smatrix make_hs_smatrix(const dmatrix & M)
03695 {
03696   return hs_smatrix(make_dcompressed_triplet(M));
03697 }
03698 
03699 ostream& operator << (const ostream& _ofs,const hs_smatrix& M)
03700 {
03701   ADUNCONST(ostream,ofs)
03702   ofs << "nrows " << M.m << " ncols " << M.n  << " nzmax " << M.nzmax
03703       << endl;
03704   ofs << "p = " << M.p << endl;
03705   ofs << "i = " << M.i << endl;
03706   ofs << "x = " << M.x << endl;
03707   return ofs;
03708 }
03709 
03710 dmatrix make_dmatrix(const hs_smatrix& M)
03711 {
03712   int n=M.m;
03713   int m=M.n;
03714   dmatrix tmp(1,n,1,m);
03715   tmp.initialize();
03716   int ii=0;
03717   for (int j=1;j<=m;j++)
03718   {
03719     for (int i=M.p(j-1);i<M.p(j);i++)
03720     {
03721       tmp(M.i(ii)+1,j)=M.x(ii);
03722       ii++;
03723     }
03724   }
03725   return tmp;
03726 }
03727 
03728 
03729  //
03730  //main()
03731  //{
03732  //
03733  //  ad_exit=&ad_boundf;
03734  //  int i,j;
03735  //  int n=20;
03736  //  int n2=n*n;
03737  //  double alpha=0.3;
03738  //  double beta=0.4;
03739  // /*
03740  //  dmatrix X(1,6,1,5);
03741  //  X.initialize();
03742  //  X(1,1)=1.;
03743  //  X(2,1)=2.;
03744  //  X(6,1)=3.;
03745  //  X(1,3)=4.;
03746  //  X(2,3)=5.;
03747  //  X(6,3)=6.;
03748  //  X(5,4)=7.;
03749  //  dcompressed_triplet dct1=make_dcompressed_triplet(X);
03750  //  hs_smatrix Z0=hs_smatrix(dct1);
03751  //  cout << Z0 << endl;
03752  //  cout << X << endl;
03753  //  cout << norm2(X-make_dmatrix(Z0)) << endl;
03754  //  cout << make_dmatrix(dct1) << endl;
03755  //  X.initialize();
03756  //  X(1,1)=1.;
03757  //  X(2,2)=2.;
03758  //  X(3,3)=3.;
03759  //  X(3,1)=5.;
03760  //  X(3,2)=9.;
03761  //  X(3,4)=7.;
03762  //  dcompressed_triplet dct2=make_dcompressed_triplet(X);
03763  //  hs_smatrix Z2=hs_smatrix(dct2);
03764  //  cout << X << endl;
03765  //  cout << make_dmatrix(dct2) << endl;
03766  // */
03767  //
03768  //  dmatrix M(1,n2,1,n2);
03769  //  M(1,1)=1;
03770  //  M(1,2)=beta;
03771  //  for (i=2;i<n2;i++)
03772  //  {
03773  //    M(i,i-1)=alpha;
03774  //    M(i,i)=1;
03775  //    M(i,i+1)=beta;
03776  //  }
03777  //  M(n2,n2-1)=alpha;
03778  //  M(n2,n2)=1;
03779  //  //dcompressed_triplet dct=make_dcompressed_triplet(M);
03780  //  hs_smatrix SM=make_hs_smatrix(M);
03781  //  //cout << norm2(make_dmatrix(dct)-M) << endl;
03782  //
03783  //  dmatrix L(1,n2,1,n2);
03784  //  L.initialize();
03785  //  int ii=1;
03786  //  for (i=1;i<=n;i++)
03787  //  {
03788  //    for (j=1;j<=n;j++)
03789  //    {
03790  //       L(ii,(j-1)*n+i)=1;
03791  //       ii++;
03792  //    }
03793  //  }
03794  //  hs_smatrix SL=make_hs_smatrix(L);
03795  //  dmatrix Y=make_dmatrix(SM*SL);
03796  //
03797  //  cout << norm2(Y-M*L) << endl;
03798  //  exit(2);
03799  //  //cout << L << endl;
03800  //
03801  //  //cout <<  M << endl;
03802  //  //cout << trans(L) * M * L << endl;
03803  //  dmatrix N= M * trans(L) * M * L;
03804  //  dmatrix N2=N*trans(N);
03805  //  ii=0;
03806  //  for (i=1;i<=n2;i++)
03807  //  {
03808  //    for (j=1;j<=n2;j++)
03809  //    {
03810  //      if (fabs(N(i,j))>1.e-8) ii++;
03811  //    }
03812  //  }
03813  //  cout << "N num no zero " << ii << " percentage full "
03814  //       << ii/double(n2*n2) << endl;
03815  //  ii=0;
03816  //  for (i=1;i<=n2;i++)
03817  //  {
03818  //    for (j=1;j<=n2;j++)
03819  //    {
03820  //      if (fabs(N2(i,j))>1.e-8) ii++;
03821  //    }
03822  //  }
03823  //  cout << "N*trans(N) num no zero " << ii << " percentage full "
03824  //       << ii/double(n2*n2) << endl;
03825  //  //cout << setfixed() << setprecision(2) << setw(5) << N << endl;
03826  //}
03827  //