1 #include <iostream>
2 #include <cmath>
3 #include "HashMatrix.hpp"
4 
5 //#include "cholmod_function.h"
6 #include <complex>
7 enum FactorizationType {
8     FactorizationNO=0,
9     FactorizationCholesky=1,
10     FactorizationCrout=2,
11     FactorizationLU=3};
12 
13 template <class Z,class R>
14 class SkyLineMatrix {
15 public:
16     typedef HashMatrix<Z,R>  HMat;
17     Z n,m;
18     FactorizationType whichfac;
19     /*----------------------------------------------------------------
20      D[i] = A[ii]
21      L[k] = A[ij]  j < i avec:   pL[i]<= k < pL[i+1] et j = pL[i+1]-k
22      U[k] = A[ij]  i < j avec:   pU[j]<= k < pU[j+1] et i = pU[i+1]-k
23      remarque pL = pU generalement
24      si L = U => la matrice est symetrique
25      -------------------------------------------------------------------
26      */
27 
28     mutable R *L,*oL;  // lower  if oL == L => no delete
29     mutable R *U,*oU;  // upper   ..
30     mutable R *D,*oD;  // diagonal   ..
31     Z *pL,*opL; // profile L
32     Z *pU,*opU; // profile U
33     mutable  FactorizationType typefac;
34     int verb;
35     FactorizationType typesolver;
36     std::ostream& dump (std::ostream&) const ;
37     SkyLineMatrix(HMat *A,Z *p,int tf,int verbb);
38 
SkyLineMatrix(const SkyLineMatrix & A,bool copy=false,int vrb=verbosity)39     SkyLineMatrix(const SkyLineMatrix& A,bool copy=false,int vrb=verbosity)
40     : n(A.n),m(n),whichfac(A.whichfac),
41     L(A.L),oL(L),      U(A.U),oU(U),     D(A.D),oD(A.D),
42     pL(A.pL),opL(pL),  pU(A.pU),opU(pU),
43     typefac(A.typefac),verb(vrb)
44     { if(copy) docopy(); }
45 
SkyLineMatrix(Z NbOfDF,R * d,R * u,Z * pu,R * l,Z * pl,bool copy,int vrb=verbosity,FactorizationType tf=FactorizationNO)46     SkyLineMatrix(Z NbOfDF,R* d,
47                    R* u, Z * pu,
48                    R* l, Z * pl,
49                    bool copy,int vrb=verbosity,
50                    FactorizationType tf=FactorizationNO)
51     : n(NbOfDF),m(n),
52       L(l),oL(l),       U(u),oU(u),    D(d),oD(d),
53       pL(pl),opL(pl),   pU(pu),opU(pu),
54       typefac(tf),verb(vrb)
55     { if(copy) docopy(); }
56 
57 
~SkyLineMatrix()58     ~SkyLineMatrix()
59     {
60         if(D &&  D != oD) delete [] D;
61         if(L &&  L != oL) delete [] L;
62         if(U &&  U != oU && U !=L ) delete [] U;
63         if(pL &&  pL != opL  ) delete [] pL;
64         if(pU &&  pU != opU && pU != pL  ) delete [] pU;
65     }
66 
t(bool cpy=false) const67     const SkyLineMatrix t(bool cpy=false) const  {return SkyLineMatrix(this->n,D,L,pL,U,pU,cpy,verb,typefac);}
lt(bool cpy=false) const68     const SkyLineMatrix lt(bool cpy=false) const {return SkyLineMatrix(this->n,0,L,pL,0,0,cpy,verb);}
l(bool cpy=false) const69     const SkyLineMatrix l(bool cpy=false) const  {return SkyLineMatrix(this->n,0,0,0,L,pL,cpy,verb);}
d(bool cpy=false) const70     const SkyLineMatrix d(bool cpy=false) const  {return SkyLineMatrix(this->n,D,0,0,0,0,cpy,verb);}
ld(bool cpy=false) const71     const SkyLineMatrix ld(bool cpy=false) const {return SkyLineMatrix(this->n,D,0,0,L,pL,cpy,verb);}
ldt(bool cpy=false) const72     const SkyLineMatrix ldt(bool cpy=false) const {return SkyLineMatrix(this->n,D,L,pL,0,0,cpy,verb);}
du(bool cpy=false) const73     const SkyLineMatrix du(bool cpy=false) const {return SkyLineMatrix(this->n,D,U,pU,0,0,cpy,verb);}
u(bool cpy=false) const74     const SkyLineMatrix u(bool cpy=false) const  {return SkyLineMatrix(this->n,0,U,pU,0,0,cpy,verb);}
ut(bool cpy=false) const75     const SkyLineMatrix ut(bool cpy=false) const {return SkyLineMatrix(this->n,0,0,0,U,pU,cpy,verb);}
dut(bool cpy=false) const76     const SkyLineMatrix dut(bool cpy=false) const {return SkyLineMatrix(this->n,D,0,0,U,pU,cpy,verb);}
77 
78     Z size() const ;
79 
80     void cholesky(double = 1e-15) const ; //
81     void crout(double = 1e-15) const ; //
82     void LU(double = 1e-15)  const ; //
factorize(double eps=1e-15) const83     void factorize(double eps= 1e-15) const
84     {
85         if(whichfac==FactorizationLU) LU(eps);
86         else if (whichfac==FactorizationCholesky) cholesky(eps);
87         else if (whichfac==FactorizationCrout) crout(eps);
88         else MATERROR(1,"UNKNOW FACTORIZATION type SkyLineMatrix");
89     }
90 
91 
92 
SumpT(Z * pT,Z n)93    static void SumpT(Z *pT,Z n)
94     {
95         Z i=0, s=0, pTi=0;
96         for( i=0; i<n;++i)
97         {
98             pTi=pT[i];
99             pT[i]=s;
100             s+= pTi;
101         }
102         pT[n]=s;
103         if(verbosity > 1)
104             cout << " SumpT="<< s << endl;
105     }
106     R* solve(R*x,int trans=1) const;
newcpy(RR * p,size_t n)107     template <class RR> static RR* newcpy(RR *p,size_t n)
108     {
109         RR *c = new RR[n];
110         for(size_t i=0; i<n; ++i)
111             c[i]=p[i];
112         return c;
113     }
114 
115 private:
docopy()116     void docopy()
117     {
118         bool copy=true;
119         if(copy&& D ) D= newcpy(oD,n);
120         if(copy&& L ) L= newcpy(oL,pL[n]);
121         if(copy&& U ) L= newcpy(oU,pU[n]);
122         if(copy&& pL) pL= newcpy(opL,n+1);
123         if(opL==opU)  pU=pL;
124         else if(copy&& pU) pU= newcpy(opU,n+1);
125     }
126     // no copy
127     void operator=(const SkyLineMatrix & A);
128 };
129 
130 
131 template <class Z,class R>
SkyLineMatrix(HashMatrix<Z,R> * A,Z * p,int typfact,int verbb)132  SkyLineMatrix<Z,R>::SkyLineMatrix(HashMatrix<Z,R> *A,Z *p,int typfact,int verbb)
133 : n(A->n),m(n),whichfac((FactorizationType)typfact),verb(verbb),
134 L(0),oL(0),U(0),oU(0),D(0),oD(0),
135 pL(0),opL(0),pU(0),opU(0),
136 typefac(FactorizationNO)
137 {
138     bool sym=whichfac==FactorizationCrout || whichfac==FactorizationCholesky;
139    //  calcul de pL
140     //    A_p(i),p(j) = PAP'_ij
141     // p1[p[i]=i
142     //    i,j,aij   =>> p1(i),p1(j), ajj
143     std::vector<Z> p1(n);
144     for(Z i=0; i<n; ++i)
145     {
146          p[i]=i;//ZZZZZ
147         p1[p[i]]=i;// perm inver
148     }
149     pU=pL= new Z[n+1];
150     std::fill(pL,pL+n,0);
151     if(!sym) {
152         pU= new Z[n+1];
153         std::fill(pU,pU+n,0);
154     }
155 
156     for(int k=0; k<A->nnz;++k)
157     {
158         Z i=p1[A->i[k]],j=p1[A->j[k]];//    i,j,aij   =>> p1(i),p1(j), ajj
159         if(i>j) pL[i] = max(i-j,pL[i]);// i>j => L
160         if(i<j) pU[j] = max(j-i,pU[j]);// i < j => U
161     }
162 
163     if(pL !=  pU)
164     {
165          Z diff=0;
166         for(int i=0; i<n;++i)
167             diff += abs(pL[i]-pU[i]);
168         if(verb > 2 || verbosity >9)
169             cout << "  - SkyLineMatrix : diff pL/pU: "<< n<< " " << diff<< endl;
170         if(2*diff <= n)
171         {
172             // Skyline symetric near symetric
173             for(int i=0; i<n;++i)
174                 pL[i]= max(pL[i],pU[i]);
175             delete [] pU;
176             pU=pL;
177         }
178     }
179     SumpT(pL,n);
180     if(pU && pU != pL) SumpT(pU,n);
181     if(sym)
182     {
183         L = new R[pL[n]];
184         D = new R[n];
185         fill(L,L+pL[n],R());
186         fill(D,D+n,R());
187 
188         U = L;
189         for(int k=0; k<A->nnz;++k)
190         {
191             Z i=p[A->i[k]],j=p[A->j[k]];
192             if( i ==j)
193                 D[i]= A->aij[k];
194             else if( i > j )
195                 L[pL[i+1]+j-i]=A->aij[k];
196         }
197     }
198     else
199     {
200         L = new R[pL[n]];
201         D = new R[n];
202         U = new R[pU[n]];
203         fill(L,L+pL[n],R());
204         fill(D,D+n,R());
205         fill(U,U+pU[n],R());
206 
207         for(int k=0; k<A->nnz;++k)
208         {
209             Z i=p1[A->i[k]],j=p1[A->j[k]];
210             if( i ==j) D[i]= A->aij[k];
211             else if( i > j ) L[pL[i+1]+j-i]=A->aij[k];
212             else U[pU[j+1]-j+i]=A->aij[k];
213         }
214     }
215     if(verbosity>4 || verb)
216         cout << "  SkyLineMatrix: size pL/pU: "<< n<< " " << pL[n] << " " << pU[n] << " moy=" << pU[n]/(double) n << endl;
217 }
218 
219 template <class Z,class R>
cholesky(double eps) const220 void SkyLineMatrix<Z,R>::cholesky(double eps) const {
221     double eps2=eps*eps;
222     R  *ij , *ii  , *ik , *jk , xii;
223     Z i,j,k;
224     if (L != U) MATERROR(1,"cholesky Skyline matrix non symmetric");
225     U = 0; //
226     typefac = FactorizationCholesky;
227     if(verbosity>3 || verb >1 )
228         cout << "  -- SkyLineMatrix Factorize/Cholesky   " << endl;
229     if ( std::norm(D[0]) <= 1.0e-60)
230         MATERROR(2,"cholesky SkyLine pivot ");
231 
232         D[0] = sqrt(D[0]);
233     ij = L ; // pointeur sur le terme ij de la matrice avec j<i
234     for (i=1;i<this->n;i++) // boucle sur les lignes
235     { ii = L+pL[i+1]; // pointeur sur le terme fin de la ligne +1 =>  ij < ii;
236         xii = D[i] ;
237         for ( ; ij < ii ; ij++) // pour les j la ligne i
238         { j = i -(ii - ij);
239             k = max( j - (pL[j+1]-pL[j]) ,  i-(pL[i+1]-pL[i]) );
240             ik =  ii - (i - k);
241             jk =  L + pL[j+1] -(j - k);
242             k = j - k ;
243             R s= -*ij;
244 #ifdef WITHBLAS
245             s += blas_sdot(k,ik,1,jk,1);
246 #else
247             while(k--) s += *ik++ * *jk++;
248 #endif
249             *ij =  -s/D[j] ;
250             xii -= *ij * *ij ;
251         }
252         // cout << std::norm(xii) << " " << Max(eps2*std::norm(D[i]),1.0e-60) << " " << sqrt(xii) <<endl;
253         if ( std::norm(xii) <= max(eps2*std::norm(D[i]),1.0e-60))
254             MATERROR(3,"cholesky SkyLine pivot ");
255             D[i] = sqrt(xii);
256     }
257 }
258 template <class Z,class R>
crout(double eps) const259 void SkyLineMatrix<Z,R>::crout(double eps) const  {
260     R  *ij , *ii  , *ik , *jk , xii, *dkk;
261     Z i,j,k;
262     double eps2=eps*eps;
263     if (L != U) MATERROR(1,"Skyline matrix  non symmetric");
264     if(verbosity>3 || verb >1 )
265         cout << "  -- SkyLineMatrix Factorize/Crout   " << endl;
266     U = 0; //
267     typefac = FactorizationCrout;
268 
269     ij = L ; // pointeur sur le terme ij de la matrice avec j<i
270     for (i=1;i<this->n;i++) // boucle sur les lignes
271     { ii = L+pL[i+1]; // pointeur sur le terme fin de la ligne +1 =>  ij < ii;
272         xii = D[i] ;
273         for ( ; ij < ii ; ij++) // pour les j la ligne i
274         { j = i -(ii - ij);
275             k = max( j - (pL[j+1]-pL[j]) ,  i-(pL[i+1]-pL[i]) );
276             ik =  ii - (i - k);
277             jk =  L + pL[j+1] -(j - k);
278             dkk = D + k;
279             k = j - k ;
280             R s=-*ij;
281             while ( k-- ) s += *ik++ * *jk++ * *dkk++;
282             *ij = -s/ *dkk ; // k = j ici
283 
284             xii -= *ij * *ij * *dkk;
285         }
286         if (std::norm(xii) <= max(eps2*std::norm(D[i]),1.0e-60))
287         {
288             cout << " Crout: zero pivot (" << i << " )= " << abs(xii)<< " <= " << eps*abs(D[i])
289             << " eps = " << eps <<endl;
290             MATERROR(3,"Crout SkyLine pivot ");
291         }
292             D[i] = xii;
293     }
294 }
295 template <class Z,class R>
LU(double eps) const296 void SkyLineMatrix<Z,R>::LU(double eps) const  {
297     R s,uii;
298     double eps2=eps*eps;
299     Z i,j;
300     if (L == U && ( pL[this->n]  || pU[this->n] ) ) MATERROR(3,"matrix LU  symmetric");
301     if(verbosity>3 || verb >1 )
302         cout << "  -- SkyLineMatrix Factorize/LU SkyLine  " << endl;
303     typefac=FactorizationLU;
304 
305     for (i=1;i<this->n;i++) // boucle sur les sous matrice de rang i
306     {
307         // for L(i,j)  j=j0,i-1
308         Z j0 = i-(pL[i+1]-pL[i]);
309         for ( j = j0; j<i;j++)
310         {
311             Z k0 = max(j0,j-(pU[j+1]-pU[j]));
312             R *Lik = L + pL[i+1]-i+k0; // lower
313             R *Ukj = U + pU[j+1]-j+k0; // upper
314             s =0;
315 #ifdef WITHBLAS
316             s = blas_sdot(j-k0,Lik,1,Ukj,1);
317             Lik += j-k0;
318 #else
319             for (int k=k0;k<j;k++) // k < j < i ;
320                 s += *Lik++ * *Ukj++ ;     // a(i,k)*a(k,j);
321 #endif
322             *Lik -= s;
323             *Lik /= D[j]; //  k == j here
324         }
325         // for U(j,i) j=0,i-1
326         j0=i-pU[i+1]+pU[i];
327         for (j=j0;j<i;j++)
328         {
329             s = 0;
330             Z k0 = max(j0,j-pL[j+1]+pL[j]);
331             R *Ljk = L + pL[j+1]-j+k0;
332             R *Uki = U + pU[i+1]-i+k0;
333 #ifdef WITHBLAS
334             s = blas_sdot(j-k0,Ljk,1,Uki,1);
335             Uki += j-k0;
336 #else
337             for (int k=k0  ;k<j;k++)    //
338                 s +=  *Ljk++ * *Uki++ ;
339 #endif
340             *Uki -= s;  // k = j here
341         }
342         // for D (i,i) in last because we need L(i,k) and U(k,i) for k<j
343         Z k0 = i-min(pL[i+1]-pL[i],pU[i+1]-pU[i]);
344         R *Lik = L + pL[i+1]-i+k0; // lower
345         R *Uki = U + pU[i+1]-i+k0; // upper
346         s =0;
347 #ifdef WITHBLAS
348         s = blas_sdot(i-k0,Lik,1,Uki,1);
349 #else
350         for (Z k=k0;k<i;k++) // k < i < i ;
351             s += *Lik++ * *Uki++ ;     // a(i,k)*a(k,i);
352 #endif
353         // cout << " k0 " << k0 << " i = " << i << " " <<  s << endl;
354         uii = D[i] -s;
355 
356         if (std::norm(uii) <= max(eps2*std::norm(D[i]),1.0e-30))
357             MATERROR(3,"LU SkyLine pivot ");
358 
359         D[i] = uii;
360 
361     }
362 }
363 
364 // solve in place ...
365 template <class Z,class R>
solve(R * x,int trans) const366 R* SkyLineMatrix<Z,R>::solve(R*x,int trans) const
367 {
368 
369     // --------------------------------------------------------------------
370     //   si La diagonal D n'existe pas alors on suppose 1 dessus (cf crout)
371     // --------------------------------------------------------------------
372     R * v = x;
373 
374 
375     const R *ij ,*ii, *ik, *ki;
376     R *xk,*xi;
377     int i;
378     switch (this->typefac) {
379         case FactorizationNO:
380             if (this->U && this->L) {std::cerr << "APROGRAMMER (KN_<R><R>::operator/SkyLineMatrix)";
381                 MATERROR(10,"SkyLineMatrix solve no factorized ");}
382 
383             if ( this->U && !this->L )
384             { // matrice triangulaire superieure
385                if( verbosity> 5 || verb>2) cout << "  -- backward substitution  " << (this->D ? "DU" : "U") << endl;
386                 ki = this->U + this->pU[n];
387                 i = n;
388                 while ( i-- )
389                 { ii = this->U + this->pU[i];
390                     xi= xk  = v +  i ;
391                     if (this->D) *xi /= this->D[i];// pour crout ou LU
392                     while ( ki > ii)
393                         *--xk  -=  *--ki *  *xi ;
394                 }
395             }
396             else if  ( !this->U && this->L )
397             { // matrice triangulaire inferieure
398                 if( verbosity> 5 || verb>2) cout << "  -- forward substitution "  <<( this->D ? "LD" : "L" ) <<endl;
399                 ii = this->L;
400                 for (i=0; i<n; i++)
401                 { ij = ik = (this->L + this->pL[i+1]) ;  // ii =debut,ij=fin+1 de la ligne
402                     xk = v + i;
403                     R ss = v[i];
404                     while ( ik > ii)
405                         ss -= *--ik * *--xk ;
406                     if ( this->D) ss /= this->D[i];// pour crout ou LU
407                     v[i] = ss ;
408                     ii = ij;
409                 }
410             }
411             else if (this->D)
412             { // matrice diagonale
413                 if( verbosity> 5 || verb>2) cout << "  -- diagonal substitution D" <<endl;
414                 for (i=0;i<n;i++)
415                     v[i]=v[i]/this->D[i];
416             }
417             break;
418         case FactorizationCholesky:
419             //if(verbosity>9|| verb>5 ) cout << " Factorization Choslesky" << endl;
420            ld().solve(x);
421            ldt().solve(x);
422             break;
423         case FactorizationCrout:
424           //     if(verbosity>4 || verb>1) cout << " Factorization Crout" << endl;
425            this->l().solve(x);
426            this->d().solve(x);
427            this->lt().solve(x);
428            break;
429         case FactorizationLU:
430          //  if(verbosity>4|| verb>1) cout << "  LU" << endl;
431           if(trans)
432           {
433               this->dut().solve(x);
434               this->lt().solve(x);
435           }
436           else
437           {
438           this->l().solve(x);
439           this->du().solve(x);
440           }
441             break;
442         default:
443             std::cerr << "  (operator /=(SkyLineMatrix, Error unkown type of Factorization  =" << typefac<< std::endl;
444             MATERROR(1,"unkown type of Factorization");
445     }
446     return x;
447 }
448 
449 template <class Z,class R>
450 Z SkyLineMatrix<Z,R>::size() const {
451     Z s = sizeof(SkyLineMatrix<Z,R>);
452     if (D) s += this->n*sizeof(R);
453     if (pL) s += this->n*sizeof(int);
454     if (pU && (pU != pL)) s += this->n*sizeof(Z);
455     if (L) s += pL[this->n]*sizeof(int);
456     if (U && (U != L)) s += pU[this->n]*sizeof(Z);
457     return s;
458 }
459 
460 template <class Z,class R>
dump(std::ostream & f) const461 std::ostream&  SkyLineMatrix<Z,R>::dump (std::ostream& f) const
462 {f<< " matrix skyline " << this->n << '\t' << this->m << '\t' ;
463     f <<  "  this " << endl;
464     f << " pL = " << pL << " L ="  << L << endl
465     << " pU = " << pU << " U ="  << U << endl
466     << " D = " << D << endl;
467     if ( (pL == pU) &&  (U == L) )
468         if (pL && L)
469         {f << " skyline symmetric " <<endl;
470             Z i,j,k;
471             for (i = 0;i<this->n;i++)
472             { f << i << " {" << pL[i+1]-pL[i] << "}" <<'\t' ;
473                 for (k=pL[i];k<pL[i+1];k++)
474                 { j=i-(pL[i+1]-k);
475                     f << j << " " << L[k] << "; ";
476                 }
477                 f <<  i  << ":" << D[i] << endl  ;
478             }
479         }
480         else f << "Skyline: pointeur null " <<endl;
481         else
482         {
483             f << " Skyline  non symmetric " << endl;
484             Z i,k;
485             for (i = 0;i<this->n;i++)
486             {
487                 f << i ;
488                 if (pL && L)
489                 {
490                     f << " jO=" << i-pL[i+1]+pL[i] << " L= " <<'\t' ;
491                     for (k=pL[i];k<pL[i+1];k++)
492                     {
493                         f <<  " " << L[k] ;
494                     }
495                 }
496                 if (D)
497                     f  << " D= " << D[i]  << '\t' ;
498                 else
499                     f  << " D=0 => 1 ; ";
500                 if (pU && U)
501                 {
502                     f << " i0=" << i-pU[i+1]+pU[i] << " U= " <<'\t' ;
503                     for (k=pU[i];k<pU[i+1];k++)
504                         f << " " << U[k] ;
505 
506                 }
507                 f << endl;
508             }
509 
510         }
511     return f;
512 }
513 template <class Z,class R>
operator <<(std::ostream & f,const SkyLineMatrix<Z,R> & m)514 inline std::ostream& operator <<(std::ostream& f,const SkyLineMatrix<Z,R> & m) {return m.dump(f);}
515 
516 
517