1 /* Copyright 2021 INRIA.
2 * Siconos is a program dedicated to modeling, simulation and control
3 * of non smooth dynamical systems.
4 * Siconos is a free software; you can redistribute it and/or modify
5 * it under the terms of the GNU Lesser General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
8 * Siconos is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU Lesser General Public License for more details.
12 *
13 * You should have received a copy of the GNU Lesser General Public License
14 * along with Siconos; if not, write to the Free Software
15 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
16 *
17 * Contact: Vincent ACARY, siconos-team@lists.gforge.inria.fr
18 */
19 #define _POSIX_C_SOURCE 200112L
20
21 #include "CSparseMatrix_internal.h"
22 #include <assert.h> // for assert
23 #include <cs.h> // for CS_INT, CS_ID, cs_dl_spalloc, cs_dl_free
24 #include <float.h> // for DBL_EPSILON
25 #include <math.h> // for fabs
26 #include <stdio.h> // for fprintf, sscanf, printf, NULL, fgets
27 #include <stdlib.h> // for realloc, exit, free, malloc, EXIT_FAILURE
28 #include <string.h> // for strtok_r, memcpy, strncmp
29 #include "SiconosCompat.h" // for SN_PTRDIFF_T_F
30 #include "numerics_verbose.h" // for CHECK_IO
31 #define LDL_LONG
32 #include "ldl.h"
33 #if defined(__cplusplus)
34 #undef restrict
35 #include <sys/cdefs.h> // for __restrict
36 #define restrict __restrict
37 #endif
38 /* #define DEBUG_NOCOLOR */
39 /* #define DEBUG_STDOUT */
40 /* #define DEBUG_MESSAGES */
41 #include "siconos_debug.h" // for DEBUG_PRINTF
42
43
44 #ifdef DEBUG_MESSAGES
45 #include "NumericsVector.h"
46 #endif
47
CSparseMatrix_get_value(const CSparseMatrix * A,CS_INT i,CS_INT j)48 double CSparseMatrix_get_value(const CSparseMatrix *A, CS_INT i, CS_INT j)
49 {
50 CS_INT * Ai = A->i;
51 CS_INT * Ap = A->p;
52 double * Ax = A->x;
53
54 for(CS_INT row = Ap[j]; row < Ap[j+1] ; row++)
55 {
56 if(i == Ai[row])
57 return Ax[row];
58 }
59 return 0.0;
60 }
61
CSparseMatrix_write_in_file_python(const CSparseMatrix * const m,FILE * file)62 void CSparseMatrix_write_in_file_python(const CSparseMatrix* const m, FILE* file)
63 {
64
65 fprintf(file, "m = %ld; \n", m->m);
66 fprintf(file, "n = %ld; \n", m->n);
67 fprintf(file, "data= [");
68 for(int i = 0; i < m->m; i++)
69 {
70 fprintf(file, "[");
71 for(int j = 0; j < m->n; j++)
72 {
73 fprintf(file, "%32.24e,\t ", CSparseMatrix_get_value((CSparseMatrix*) m,i,j));
74 }
75 fprintf(file, "],\n");
76 }
77 fprintf(file, "]");
78 }
79
80
81
82 /* y = alpha*A*x+beta*y */
CSparseMatrix_aaxpby(const double alpha,const CSparseMatrix * A,const double * restrict x,const double beta,double * restrict y)83 int CSparseMatrix_aaxpby(const double alpha, const CSparseMatrix *A,
84 const double *restrict x,
85 const double beta, double *restrict y)
86 {
87
88 CS_INT n, m, *Ap, *Ai ;
89 double *Ax ;
90 if(!CS_CSC(A) || !x || !y) return (0); /* check inputs */
91 {
92 n = A->n;
93 m = A->m;
94 Ap = A->p;
95 Ai = A->i;
96 Ax = A->x;
97
98
99 for(int j=0; j<m; j++)
100 {
101 y[j] *= beta;
102 }
103
104 for(int j=0 ; j<n ; j++)
105 {
106 for(CS_INT p = Ap [j] ; p < Ap [j+1] ; p++)
107 {
108 y [Ai [p]] += alpha * Ax [p] * x [j];
109 }
110 }
111
112 }
113 return 1;
114
115 }
116 /* A <-- alpha*A */
CSparseMatrix_scal(const double alpha,const CSparseMatrix * A)117 int CSparseMatrix_scal(const double alpha, const CSparseMatrix *A)
118 {
119
120 CS_INT n, *Ap;
121 double *Ax ;
122 if(!CS_CSC(A)) return (0); /* check inputs */
123 {
124 n = A->n;
125 Ap = A->p;
126 Ax = A->x;
127 for(int j=0 ; j<n ; j++)
128 {
129 for(CS_INT p = Ap [j] ; p < Ap [j+1] ; p++)
130 {
131 Ax [p] = alpha * Ax [p];
132 }
133 }
134
135 }
136 return 1;
137 }
138
CSparseMatrix_check_triplet(CSparseMatrix * T)139 int CSparseMatrix_check_triplet(CSparseMatrix *T)
140 {
141 if(T->nz < 0)
142 {
143 fprintf(stderr, "CSparseMatrix_check_triplet :: given CSparseMatrix is not in a triplet form: nz = " CS_ID, T->nz);
144 return 1;
145 }
146 CS_INT nb_row = T->m;
147 CS_INT nb_col = T->n;
148 CS_INT* Ti = T->i;
149 CS_INT* Tp = T->p;
150 int info = 0;
151 CS_INT cc = 0;
152 CS_INT max_row = -1;
153 CS_INT max_col = -1;
154
155 for(CS_INT indx = 0; indx < T->nz; ++indx)
156 {
157 if(Ti[indx] >= nb_row)
158 {
159 printf("CSparseMatrix_check_triplet :: matrix element " CS_ID " has a row number " CS_ID " > " CS_ID " the number of rows\n", indx, Ti[indx], nb_row);
160 info = 1;
161 }
162 if(Tp[indx] >= nb_col)
163 {
164 printf("CSparseMatrix_check_triplet :: matrix element " CS_ID " has a row number " CS_ID " > " CS_ID " the number of rows\n", indx, Tp[indx], nb_col);
165 info = 1;
166 }
167 if(Tp[indx] < max_col)
168 {
169 printf("CSparseMatrix_check_csc :: " CS_ID " at index " CS_ID " > " CS_ID "\n", Tp[indx], indx, max_col);
170 }
171 if(Tp[indx] == cc)
172 {
173 if(Ti[indx] <= max_row)
174 {
175 printf("CSparseMatrix_check_triplet :: matrix element at column " CS_ID " has a row number " CS_ID " > " CS_ID " the max on that column\n", cc, Ti[indx], max_row);
176 }
177 else
178 {
179 max_row = Ti[indx];
180 }
181 }
182 else
183 {
184 cc = Tp[indx];
185 max_row = -1;
186 }
187 }
188 return info;
189 }
190
CSparseMatrix_check_csc(CSparseMatrix * T)191 int CSparseMatrix_check_csc(CSparseMatrix *T)
192 {
193 if(T->nz != -1)
194 {
195 fprintf(stderr, "CSparseMatrix_check_csc :: given CSparseMatrix is not in a csc form: nz = " SN_PTRDIFF_T_F "\n", T->nz);
196 return 1;
197 }
198
199 CS_INT nb_row = T->m;
200 CS_INT nb_col = T->n;
201 CS_INT* Ti = T->i;
202 CS_INT* Tp = T->p;
203 int info = 0;
204
205 for(CS_INT j = 0; j < nb_col; ++j)
206 {
207 CS_INT max_indx = -1;
208 if(Tp[j] > Tp[j+1])
209 {
210 printf("CSparseMatrix_check_csc :: " SN_PTRDIFF_T_F " at index " SN_PTRDIFF_T_F " smaller than " SN_PTRDIFF_T_F "\n", Tp[j+1], j, Tp[j]);
211 }
212 for(CS_INT p = Tp[j]; p < Tp[j+1]; ++p)
213 {
214 if(Ti[p] <= max_indx)
215 {
216 printf("CSparseMatrix_check_csc :: matrix element (" SN_PTRDIFF_T_F"," SN_PTRDIFF_T_F") at index " SN_PTRDIFF_T_F " has a row number < " SN_PTRDIFF_T_F " the previous max\n", Ti[p], j, p, max_indx);
217 info = 1;
218 }
219 else if(Ti[p] >= nb_row)
220 {
221 printf("CSparseMatrix_check_csc :: matrix element (" SN_PTRDIFF_T_F"," SN_PTRDIFF_T_F") at index " SN_PTRDIFF_T_F " has a row number > " SN_PTRDIFF_T_F " the max\n", Ti[p], j, p, nb_row);
222 info = 1;
223 }
224 else
225 {
226 max_indx = Ti[p];
227 }
228 }
229 }
230 return info;
231 }
232
CSparseMatrix_spfree_on_stack(CSparseMatrix * A)233 CSparseMatrix* CSparseMatrix_spfree_on_stack(CSparseMatrix* A)
234 {
235 if(!A) return NULL; /* do nothing if A already NULL */
236 cs_free(A->p);
237 A->p = NULL;
238 cs_free(A->i);
239 A->i = NULL;
240 cs_free(A->x);
241 A->x = NULL;
242 return NULL;
243 }
244
CSparseMatrix_lu_factorization(CS_INT order,const cs * A,double tol,CSparseMatrix_factors * cs_lu_A)245 int CSparseMatrix_lu_factorization(CS_INT order, const cs *A, double tol, CSparseMatrix_factors * cs_lu_A)
246 {
247 assert(A);
248 cs_lu_A->n = A->n;
249 css* S = cs_sqr(order, A, 0);
250 cs_lu_A->S = S;
251 cs_lu_A->N = cs_lu(A, S, tol);
252
253 return (S && cs_lu_A->N);
254 }
CSparseMatrix_chol_factorization(CS_INT order,const cs * A,CSparseMatrix_factors * cs_chol_A)255 int CSparseMatrix_chol_factorization(CS_INT order, const cs *A, CSparseMatrix_factors * cs_chol_A)
256 {
257 assert(A);
258 FILE * foutput = fopen("cs.py", "w");
259 CSparseMatrix_print_in_file(A, 0, foutput);
260 fclose(foutput);
261 cs_chol_A->n = A->n;
262 css* S = cs_schol(order, A);
263 cs_chol_A->S = S;
264 cs_chol_A->N = cs_chol(A, S);
265
266 return (S && cs_chol_A->N);
267 }
CSparseMatrix_ldlt_factorization(CS_INT order,const cs * A,CSparseMatrix_factors * cs_ldlt_A)268 int CSparseMatrix_ldlt_factorization(CS_INT order, const cs *A, CSparseMatrix_factors * cs_ldlt_A)
269 {
270 assert(A);
271
272 CS_INT *Ap, *Ai, *Lp, *Li;
273 CS_INT *Parent;
274 CS_ENTRY *Ax, *Lx;
275 css *S;
276 csn *N;
277 CS_INT n, lnz;
278 DEBUG_EXPR(cs_print(A,1););
279 Ap=A->p;
280 Ai=A->i;
281 Ax=A->x;
282
283 cs_ldlt_A->n = n = A->n;
284
285 /* could be good to good cs_spalloc */
286
287 cs_ldlt_A->N = N = cs_calloc (1, sizeof (csn)) ; /* allocate result N */
288 cs_ldlt_A->N->L = cs_calloc (1, sizeof (cs)) ; /* allocate the cs struct */
289 cs_ldlt_A->N->L->m =n;
290 cs_ldlt_A->N->L->n =n;
291 cs_ldlt_A->N->L->nz =-1;
292 cs_ldlt_A->N->L->p = Lp = cs_malloc (n+1, sizeof (CS_INT)) ;
293
294
295 cs_ldlt_A->S = S = cs_calloc (1, sizeof (css)) ; /* allocate result S */
296 cs_ldlt_A->S->parent = Parent = cs_malloc (n+1, sizeof (CS_INT)) ;
297
298 CS_INT* Lnz = cs_malloc (n, sizeof (CS_INT)) ;
299 CS_INT* Flag = cs_malloc (n, sizeof (CS_INT)) ;
300
301 /* ordering with amd */
302 CS_INT * Perm, *PermInv;
303 cs_ldlt_A->N->pinv = Perm = cs_amd (order, A) ; /* We used pinv to store Perm !! */
304 PermInv = cs_malloc (n, sizeof (CS_INT)) ;
305
306
307 DEBUG_EXPR(for (int k =0; k< n+1; k++){printf("%li\t", Perm[k]);}printf("\n"););
308
309 /* symbolic factorization to get Lp, Parent, Lnz, and Pinv */
310 LDL_symbolic (n, Ap, Ai, Lp, Parent, Lnz, Flag, Perm, PermInv) ;
311 DEBUG_EXPR(for (int k =0; k< n+1; k++){printf("%li\t", Lp[k]);}printf("\n"););
312 DEBUG_EXPR(for (int k =0; k< n; k++){printf("%li\t", Flag[k]);}printf("\n"););
313
314 /* factorization */
315 lnz = Lp [n] ;
316 DEBUG_PRINTF("Lp[n] = %ld\n", Lp[n]);
317 cs_ldlt_A->N->L->i = Li = cs_malloc (lnz, sizeof (CS_INT)) ;
318 cs_ldlt_A->N->L->x = Lx = cs_malloc (lnz, sizeof (CS_ENTRY)) ;
319
320 CS_INT *Pattern = cs_malloc (n, sizeof (CS_INT)) ;
321 CS_ENTRY* D;
322 cs_ldlt_A->N->B = D= cs_malloc (n, sizeof (CS_ENTRY)) ; /* We use cs_ldlt_A->N->B for storing D !! */
323 CS_ENTRY* Y = cs_malloc (n, sizeof (CS_ENTRY)) ;
324 LDL_numeric (n, Ap, Ai, Ax, Lp, Parent, Lnz, Li, Lx, D,
325 Y, Flag, Pattern, Perm, PermInv) ;
326
327 DEBUG_EXPR(cs_print(cs_ldlt_A->N->L,1););
328 DEBUG_EXPR(NV_display(D,n));
329
330
331 cs_free(Lnz);
332 cs_free(Flag);
333 cs_free(PermInv);
334 cs_free(Pattern);
335 cs_free(Y);
336
337 return (S && cs_ldlt_A->N);
338 }
339
CSparseMatrix_free_lu_factors(CSparseMatrix_factors * cs_lu_A)340 void CSparseMatrix_free_lu_factors(CSparseMatrix_factors* cs_lu_A)
341 {
342 assert(cs_lu_A);
343 if(cs_lu_A)
344 {
345 cs_lu_A->n = -1;
346
347 cs_sfree(cs_lu_A->S);
348 cs_lu_A->S = NULL;
349
350 cs_nfree(cs_lu_A->N);
351 cs_lu_A->N = NULL;
352
353 free(cs_lu_A);
354 }
355 }
356
357 /* Solve Ax = b with the factorization of A stored in the cs_lu_A
358 * This is extracted from cs_lusol, you need to synchronize any changes! */
CSparseMatrix_solve(CSparseMatrix_factors * cs_lu_A,double * x,double * b)359 CS_INT CSparseMatrix_solve(CSparseMatrix_factors* cs_lu_A, double* x, double *b)
360 {
361 assert(cs_lu_A);
362
363 CS_INT ok;
364 CS_INT n = cs_lu_A->n;
365 css* S = cs_lu_A->S;
366 csn* N = cs_lu_A->N;
367 ok = (S && N && x) ;
368 if(ok)
369 {
370 cs_ipvec(N->pinv, b, x, n) ; /* x = b(p) */
371 cs_lsolve(N->L, x) ; /* x = L\x */
372 cs_usolve(N->U, x) ; /* x = U\x */
373 cs_ipvec(S->q, x, b, n) ; /* b(q) = x */
374 }
375 return (ok);
376 }
377
378 /* Solve Ax = B with the factorization of A stored in the cs_lu_A
379 * B is a sparse matrix (CSparseMatrix_factors)
380 * This is extracted from cs_lusol, you need to synchronize any changes! */
CSparseMatrix_spsolve(CSparseMatrix_factors * cs_lu_A,CSparseMatrix * X,CSparseMatrix * B)381 CS_INT CSparseMatrix_spsolve(CSparseMatrix_factors* cs_lu_A, CSparseMatrix* X, CSparseMatrix* B)
382 {
383 assert(cs_lu_A);
384 DEBUG_BEGIN("CSparseMatrix_spsolve(...)\n");
385
386 if(!CS_CSC(X)) return 1 ; /* check inputs */
387 if(!CS_CSC(B)) return 1 ; /* check inputs */
388
389 CS_INT ok;
390 CS_INT n = cs_lu_A->n;
391 csn* N = cs_lu_A->N;
392 if(!N) return 1;
393 css* S = cs_lu_A->S;
394 if(!S) return 1;
395
396 CS_ENTRY *x, *b, *Xx, *Bx ;
397 CS_INT *xi, *pinv, *q, top, k, i, p, *Bp, *Bi, *Xp, *Xi;
398
399 x = cs_malloc(n, sizeof(CS_ENTRY)) ; /* get CS_ENTRY workspace */
400 b = cs_malloc(n, sizeof(CS_ENTRY)) ; /* get CS_ENTRY workspace */
401 xi = cs_malloc(2*n, sizeof(CS_INT)) ; /* get CS_INT workspace */
402
403 CS_INT xnz=0;
404 Xp= X->p;
405 Xi= X->i;
406 Xx= X->x;
407 Xp[0]=0;
408 pinv = N->pinv;
409 /* --- 1. First step X = L\B ---------------------------------------------- */
410 for(k = 0 ; k < B->n ; k++)
411 {
412 /* permutation of the rows of B(:,k) */
413 for(p = B->p [k] ; p < B->p [k+1] ; p++) x [B->i[p]] = B->x [p] ; /* scatter B */
414 for(p = B->p [k] ; p < B->p [k+1] ; p++)
415 {
416 CS_INT i_old= B->i[p];
417 B->i[p] = pinv[i_old]; /* permute row indices with N->pinv */
418 B->x[p] = x[i_old];
419 }
420 /* call spsolve */
421 top = cs_spsolve(N->L, B, k, xi, x, NULL, 1) ; /* x = L\B(:,col) */
422 /* store the result in X */
423 if(Xp[k]+ n-top > X->nzmax && !cs_sprealloc(X, 2*(X->nzmax)+ n-top)) /* realloc X if need */
424 {
425 return 1; /* (cs_done(X, w, x, 0)) ; */ /* out of memory */
426 }
427 Xp= X->p;
428 Xi= X->i;
429 Xx= X->x;
430 for(p = top ; p < n ; p++)
431 {
432 i = xi [p] ;/* x(i) is nonzero */
433 Xi[xnz]=i; /* store the result in X */
434 Xx[xnz++] = x[i];
435 }
436 Xp[k+1] =Xp[k] + n-top;
437 }
438 DEBUG_EXPR(cs_print(X,0););
439
440 /* --- 2. First step B = U\B ---------------------------------------------- */
441 DEBUG_PRINT("2- Second step B = U\\X\n");
442 CS_INT bnz=0;
443 Bp= B->p;
444 Bi= B->i;
445 Bx= B->x;
446 Bp[0]=0;
447 q = S->q;
448 for(k = 0 ; k < X->n ; k++)
449 {
450 top = cs_spsolve(N->U, X, k, xi, x, NULL, 0) ; /* x = U\X(:,col) */
451
452 /* store the result in B */
453 if(Bp[k]+ n-top > B->nzmax && !cs_sprealloc(B, 2*(B->nzmax)+ n-top))
454 {
455 return 1; /* (cs_done(X, w, x, 0)) ; */ /* out of memory */
456 }
457 Bp= B->p;
458 Bi= B->i;
459 Bx= B->x;
460 for(p = top ; p < n ; p++)
461 {
462 i = xi[p] ; /* x(i) is nonzero */
463 Bi[bnz] = q[i]; /* permute the indices with S->q */
464 Bx[bnz++] = x[i];
465 }
466 Bp[k+1] = Bp[k] + n-top;
467 }
468 DEBUG_EXPR(cs_print(B,0));
469 ok =1;
470 free(x);
471 free(b);
472 free(xi);
473 DEBUG_END("CSparseMatrix_spsolve(...)\n");
474 return (ok);
475 }
476
477
478
479
480
CSparseMatrix_chol_solve(CSparseMatrix_factors * cs_chol_A,double * x,double * b)481 CS_INT CSparseMatrix_chol_solve(CSparseMatrix_factors* cs_chol_A, double* x, double *b)
482 {
483 assert(cs_chol_A);
484
485 CS_INT ok;
486 CS_INT n = cs_chol_A->n;
487 css* S = cs_chol_A->S;
488 csn* N = cs_chol_A->N;
489 ok = (S && N && x) ;
490 if(ok)
491 {
492 cs_ipvec(S->pinv, b, x, n) ; /* x = P*b */
493 cs_lsolve(N->L, x) ; /* x = L\x */
494 cs_ltsolve(N->L, x) ; /* x = L'\x */
495 cs_pvec(S->pinv, x, b, n) ; /* b = P'*x */
496 }
497 return (ok);
498 }
499
500
501
502 /* Solve Ax = B with the factorization of A stored in the cs_chol_A
503 * B is a sparse matrix (CSparseMatrix_factors)
504 * This is extracted from cs_lusol, you need to synchronize any changes! */
CSparseMatrix_chol_spsolve(CSparseMatrix_factors * cs_chol_A,CSparseMatrix * X,CSparseMatrix * B)505 CS_INT CSparseMatrix_chol_spsolve(CSparseMatrix_factors* cs_chol_A, CSparseMatrix* X, CSparseMatrix* B)
506 {
507 DEBUG_BEGIN("CSparseMatrix_chol_spsolve(...)\n");
508
509 if(!CS_CSC(X)) return 1 ; /* check inputs */
510 if(!CS_CSC(B)) return 1 ; /* check inputs */
511
512 CS_INT ok;
513 CS_INT n = cs_chol_A->n;
514 csn* N = cs_chol_A->N;
515 if(!N) return 1;
516 css* S = cs_chol_A->S;
517 if(!S) return 1;
518
519 CS_ENTRY *x, *b, *Xx, *Bx ;
520 CS_INT *xi, *pinv, top, k, i, p, *Bp, *Bi, *Xp, *Xi;
521
522 x = cs_malloc(n, sizeof(CS_ENTRY)) ; /* get CS_ENTRY workspace */
523 b = cs_malloc(n, sizeof(CS_ENTRY)) ; /* get CS_ENTRY workspace */
524 xi = cs_malloc(2*n, sizeof(CS_INT)) ; /* get CS_INT workspace */
525
526 CS_INT xnz=0;
527 Xp= X->p;
528 Xi= X->i;
529 Xx= X->x;
530 Xp[0]=0;
531 pinv = S->pinv;
532
533 /* for (i =0; i <n; i++) printf("pinv[%li] = %li\t", i, pinv[i]); */
534 /* printf("\n"); */
535
536 /* --- 1. First step X = L\B ---------------------------------------------- */
537 for(k = 0 ; k < B->n ; k++)
538 {
539 /* permutation of the rows of B(:,k) */
540 for(p = B->p [k] ; p < B->p [k+1] ; p++) x [B->i[p]] = B->x [p] ; /* scatter B */
541 for(p = B->p [k] ; p < B->p [k+1] ; p++)
542 {
543 CS_INT i_old= B->i[p];
544 B->i[p] = pinv[i_old]; /* permute row indices with N->pinv */
545 B->x[p] = x[i_old];
546 }
547 /* call spsolve */
548 top = cs_spsolve(N->L, B, k, xi, x, NULL, 1) ; /* x = L\B(:,col) */
549 /* store the result in X */
550 if(Xp[k]+ n-top > X->nzmax && !cs_sprealloc(X, 2*(X->nzmax)+ n-top)) /* realloc X if need */
551 {
552 return 1; /* (cs_done(X, w, x, 0)) ; */ /* out of memory */
553 }
554 Xp= X->p;
555 Xi= X->i;
556 Xx= X->x;
557 for(p = top ; p < n ; p++)
558 {
559 i = xi [p] ;/* x(i) is nonzero */
560 Xi[xnz]=i; /* store the result in X */
561 Xx[xnz++] = x[i];
562 }
563 Xp[k+1] =Xp[k] + n-top;
564 }
565 DEBUG_EXPR(cs_print(X,0););
566
567 /* --- 2. First step B = L'\B ---------------------------------------------- */
568 DEBUG_PRINT("2- Second step B = L'\\X\n");
569
570 CSparseMatrix* LT = cs_transpose(N->L,1);
571 CS_INT bnz=0;
572 Bp= B->p;
573 Bi= B->i;
574 Bx= B->x;
575 Bp[0]=0;
576 for(k = 0 ; k < X->n ; k++)
577 {
578 top = cs_spsolve(LT, X, k, xi, x, NULL, 0) ; /* x = U\X(:,col) */
579
580 /* store the result in B */
581 if(Bp[k]+ n-top > B->nzmax && !cs_sprealloc(B, 2*(B->nzmax)+ n-top))
582 {
583 return 1; /* (cs_done(X, w, x, 0)) ; */ /* out of memory */
584 }
585 Bp= B->p;
586 Bi= B->i;
587 Bx= B->x;
588 /* permutation with S->pinv */
589 cs_pvec(pinv, x, b, n) ; /* b = P'*x */
590 //for(p = top ; p < n ; p++) b [q ? q [p] : p] = x [p] ; /* b(q) = x */
591 for(p = top ; p < n ; p++)
592 {
593 i = xi[p] ; /* x(i) is nonzero */
594 i = pinv[i]; /* permute the indices with S->pinv */ // to be checked carefully
595 Bi[bnz] = i;
596 Bx[bnz++] = b[i];
597 }
598 Bp[k+1] = Bp[k] + n-top;
599 }
600 DEBUG_EXPR(cs_print(B,0));
601 ok =1;
602 free(x);
603 free(b);
604 free(xi);
605 DEBUG_END("CSparseMatrix_chol_spsolve(...)\n");
606 return (ok);
607 }
608
609
CSparseMatrix_ldlt_solve(CSparseMatrix_factors * cs_ldlt_A,double * x,double * b)610 CS_INT CSparseMatrix_ldlt_solve(CSparseMatrix_factors* cs_ldlt_A, double* x, double *b)
611 {
612 assert(cs_ldlt_A);
613
614 CS_INT ok;
615
616 CS_INT n = cs_ldlt_A->n;
617 css* S = cs_ldlt_A->S;
618 csn* N = cs_ldlt_A->N;
619 ok = (S && N && x) ;
620
621
622 cs* L;
623 CS_INT *Lp, *Li;
624 CS_ENTRY *Lx;
625
626 if(ok)
627 {
628 CS_INT * P = cs_ldlt_A->N->pinv; /* We used pinv to store Perm !! */
629 CS_ENTRY* D = cs_ldlt_A->N->B; /* We use cs_ldlt_A->N->B for storing D !! */
630
631 L = cs_ldlt_A->N->L;
632 Lp=L->p;
633 Li=L->i;
634 Lx=L->x;
635
636 /* solve Ax=b */
637 if (P)
638 {
639 /* the factorization is LDL' = PAP' */
640 LDL_perm (n, x, b, P) ; /* y = Pb */
641 LDL_lsolve (n, x, Lp, Li, Lx) ; /* y = L\y */
642 LDL_dsolve (n, x, D) ; /* y = D\y */
643 LDL_ltsolve (n, x, Lp, Li, Lx) ; /* y = L'\y */
644 LDL_permt (n, b, x, P) ; /* x = P'y */
645 }
646 else
647 {
648 /* the factorization is LDL' = A */
649 DEBUG_EXPR(NV_display(b,n));
650 DEBUG_EXPR(NV_display(D,n));
651 LDL_lsolve (n, b, Lp, Li, Lx) ; /* x = L\x */
652 LDL_dsolve (n, b, D) ; /* x = D\x */
653 LDL_ltsolve (n, b, Lp, Li, Lx) ; /* x = L'\x */
654 DEBUG_EXPR(NV_display(b,n));
655 }
656
657
658 }
659 return (ok);
660 }
661
CSparseMatrix_new_from_file(FILE * file)662 CSparseMatrix * CSparseMatrix_new_from_file(FILE* file)
663 {
664 CS_INT m=0, n=0, nzmax=0, nz, p, j, *Ap, *Ai ;
665 long long foo;
666 double *Ax ;
667 int info = 0;
668 char line[2048];
669
670 /* CHECK_IO(fscanf(file, "%20[^\n]", line ), &info); */
671 /* fscanf(file, "%2047[^\n]", line ); */
672
673 if(fgets(line, 2047, file)!=NULL)
674 {
675 DEBUG_PRINTF("line = %s\n",line);
676 }
677 if(fgets(line, 2047, file)!=NULL)
678 {
679 DEBUG_PRINTF("line = %s\n",line);
680 }
681 if(fgets(line, 2047, file)!=NULL)
682 {
683 DEBUG_PRINTF("line = %s\n",line);
684 }
685
686 char *str1, *str2, *token, *subtoken;
687 char *saveptr1, *saveptr2;
688 const char s_1[2] = " ", s_2[2] = "-";
689 int k;
690 int is_triplet = 0;
691 for(k = 1, str1 = line; ; k++, str1 = NULL)
692 {
693 token = strtok_r(str1, s_1, &saveptr1);
694 if(token == NULL)
695 break;
696 DEBUG_PRINTF("%d: %s\n", k, token);
697 if(strncmp(token, "triplet:",8) == 0)
698 {
699 DEBUG_PRINT(" triplet matrix\n");
700 is_triplet =1;
701 }
702 if(k==1+is_triplet)
703 {
704 int kk =0;
705 for(str2 = token; ; str2 = NULL)
706 {
707 subtoken = strtok_r(str2, s_2, &saveptr2);
708 if(kk==0)
709 {
710 if(1 == sscanf(subtoken, "%lld", &foo))
711 m = (CS_INT)foo;
712 }
713 if(kk==2)
714 {
715 if(1 == sscanf(subtoken, "%lld", &foo) && kk)
716 n = (CS_INT)foo;
717 }
718 kk=kk+1;
719 if(subtoken == NULL)
720 break;
721 DEBUG_PRINTF(" --> %s\n", subtoken);
722 }
723 DEBUG_PRINTF("m = %li, n = %li \n", m, n);
724 }
725
726 if(k==3+is_triplet)
727 {
728 if(1 == sscanf(token, "%lld", &foo))
729 nzmax = (CS_INT)foo;
730 }
731 if(k==6 && is_triplet)
732 {
733 if(1 == sscanf(token, "%lld", &foo))
734 nz = (CS_INT)foo;
735 }
736 }
737
738 CSparseMatrix * out = cs_spalloc(m, n, nzmax, 1, is_triplet);
739
740 if(is_triplet)
741 {
742 out->nz=nz;
743 }
744 else
745 {
746 out->nz=-1;
747 }
748
749 Ai = out->i;
750 Ap = out->p;
751 Ax = out->x;
752
753 long long int val1, val2;
754 double val3;
755 if(out->nz < 0)
756 {
757 for(j = 0 ; j < n ; j++)
758 {
759 /* fprintf(file," col %lld : locations %lld to %lld\n", (long long int)j, (long long int)Ap [j], (long long int)Ap [j+1]-1); */
760
761 if(fgets(line, 2047, file)!=NULL)
762 {
763 DEBUG_PRINTF("line 1 = %s\n",line);
764 }
765 for(k = 1, str1 = line; ; k++, str1 = NULL)
766 {
767 token = strtok_r(str1, s_1, &saveptr1);
768 if(token == NULL)
769 break;
770 DEBUG_PRINTF("%d: %s\n", k, token);
771 if(k==2)
772 {
773 if(1 == sscanf(token, "%lld", &val1))
774 {
775 DEBUG_PRINTF(" j- col = %lli\n", j-val1);
776 }
777 assert(j-val1 == 0);
778 }
779 if(k==5)
780 {
781 if(1 == sscanf(token, "%lld", &val1))
782 Ap [j] = (CS_INT)val1;
783 }
784 if(k==7)
785 {
786 if(1 == sscanf(token, "%lld", &val2))
787 Ap [j+1] = (CS_INT)val2+1;
788 }
789 }
790 DEBUG_PRINTF(" col %lld : locations %lld to %lld\n", (long long int)j, (long long int)Ap [j], (long long int)Ap [j+1]-1);
791
792 for(p = Ap [j] ; p < Ap [j+1] ; p++)
793 {
794 if(fgets(line, 2047, file)!=NULL)
795 {
796 DEBUG_PRINTF("line 2 = %s\n",line);
797 }
798
799 for(k = 1, str1 = line; ; k++, str1 = NULL)
800 {
801 token = strtok_r(str1, s_1, &saveptr1);
802 if(token == NULL)
803 break;
804 DEBUG_PRINTF("%d: %s\n", k, token);
805 if(k==1)
806 {
807 if(1 == sscanf(token, "%lld", &val1))
808 Ai[p] = (CS_INT)val1;
809 }
810 if(k==3)
811 {
812 if(1 == sscanf(token, "%32le", &val3))
813 Ax[p] = val3;
814 }
815 }
816 }
817 }
818 }
819 else
820 {
821 /* fprintf(file,"triplet: %lld-by-%lld, nzmax: %lld nnz: %lld\n", (long long int)m, (long long int)n, */
822 /* (long long int)nzmax, (long long int)nz) ; */
823
824
825 for(p = 0 ; p < nz ; p++)
826 {
827 CHECK_IO(fscanf(file," %lld %lld : %32le\n", &val1, &val2, &val3), &info);
828 DEBUG_PRINTF(" %lld %lld : %32le\n", val1, val2, val3);
829 Ai [p] = (CS_INT) val1;
830 Ap [p] = (CS_INT) val2;
831 Ax [p] = val3;
832 DEBUG_PRINTF(" %lld %lld : %g\n", (long long int)Ai [p], (long long int)Ap [p], (double)Ax[p]);
833 }
834 }
835 return out ;
836 }
837
838
839 /* add an entry to triplet matrix only if value is not (nearly) null */
CSparseMatrix_zentry(CSparseMatrix * T,CS_INT i,CS_INT j,double x,double threshold)840 CS_INT CSparseMatrix_zentry(CSparseMatrix *T, CS_INT i, CS_INT j, double x, double threshold)
841 {
842 if(fabs(x) >= threshold)
843 {
844 return cs_entry(T, i, j, x);
845 }
846 else
847 {
848 return 1;
849 }
850 }
851
852 /* add an entry to a symmetric triplet matrix only if value is not (nearly) null */
CSparseMatrix_symmetric_zentry(CSparseMatrix * T,CS_INT i,CS_INT j,double x,double threshold)853 CS_INT CSparseMatrix_symmetric_zentry(CSparseMatrix *T, CS_INT i, CS_INT j, double x, double threshold)
854 {
855 if(fabs(x) >= threshold)
856 {
857 if(j<=i)
858 {
859 return cs_entry(T, i, j, x);
860 }
861 }
862 return 1;
863 }
864
865
CSparseMatrix_print(const CSparseMatrix * A,int brief)866 int CSparseMatrix_print(const CSparseMatrix *A, int brief)
867 {
868 cs_print(A, brief);
869 return 0;
870 }
871
872 /* add an entry to triplet matrix */
CSparseMatrix_entry(CSparseMatrix * T,CS_INT i,CS_INT j,double x)873 CS_INT CSparseMatrix_entry(CSparseMatrix *T, CS_INT i, CS_INT j, double x)
874 {
875 return cs_entry(T, i, j, x);
876 }
877
878 /* add an entry to a symmetric triplet matrix */
CSparseMatrix_symmetric_entry(CSparseMatrix * T,CS_INT i,CS_INT j,double x)879 CS_INT CSparseMatrix_symmetric_entry(CSparseMatrix *T, CS_INT i, CS_INT j, double x)
880 {
881 if(j<=i)
882 {
883 return cs_entry(T, i, j, x);
884 }
885 return 1;
886 }
887
888
CSparseMatrix_print_in_file(const CSparseMatrix * A,int brief,FILE * file)889 int CSparseMatrix_print_in_file(const CSparseMatrix *A, int brief, FILE* file)
890 {
891 CS_INT m, n, nzmax, nz, p, j, *Ap, *Ai ;
892 double *Ax ;
893 if(!A)
894 {
895 fprintf(file,"(null)\n") ;
896 return (0) ;
897 }
898 m = A->m ;
899 n = A->n ;
900 Ap = A->p ;
901 Ai = A->i ;
902 Ax = A->x ;
903 nzmax = A->nzmax ;
904 nz = A->nz ;
905 fprintf(file,"CSparse Version %d.%d.%d, %s. %s\n", CS_VER, CS_SUBVER,
906 CS_SUBSUB, CS_DATE, CS_COPYRIGHT) ;
907 if(nz < 0)
908 {
909 fprintf(file,"%lld-by-%lld, nzmax: %lld nnz: %lld, 1-norm: %g\n",
910 (long long int)m, (long long int)n, (long long int)nzmax,
911 (long long int)Ap [n], cs_norm(A)) ;
912 for(j = 0 ; j < n ; j++)
913 {
914 fprintf(file," col %lld : locations %lld to %lld\n", (long long int)j, (long long int)Ap [j], (long long int)Ap [j+1]-1);
915 for(p = Ap [j] ; p < Ap [j+1] ; p++)
916 {
917 fprintf(file," %lld : %g\n", (long long int)Ai [p], Ax ? Ax [p] : 1) ;
918 if(brief && p > 20)
919 {
920 fprintf(file," ...\n") ;
921 return (1) ;
922 }
923 }
924 }
925 }
926 else
927 {
928 fprintf(file,"triplet: %lld-by-%lld, nzmax: %lld nnz: %lld\n", (long long int)m, (long long int)n,
929 (long long int)nzmax, (long long int)nz) ;
930 for(p = 0 ; p < nz ; p++)
931 {
932 fprintf(file," %lld %lld : %g\n", (long long int)Ai [p], (long long int)Ap [p], Ax ? Ax [p] : 1) ;
933 if(brief && p > 20)
934 {
935 fprintf(file," ...\n") ;
936 return (1) ;
937 }
938 }
939 }
940 return (1) ;
941 }
942
CSparseMatrix_to_dense(const CSparseMatrix * const A,double * B)943 CS_INT CSparseMatrix_to_dense(const CSparseMatrix* const A, double * B)
944 {
945
946 CS_INT p, j, m, n, nz, *Ap, *Ai ;
947 CS_ENTRY *Ax ;
948
949 if(!A)
950 {
951 printf("CSparseMatrix_to_dense :: A = null\n") ;
952 return (1) ;
953 }
954
955 m = A->m ;
956 n = A->n ;
957 nz = A->nz ;
958 Ap = A->p ;
959 Ai = A->i ;
960 Ax = A->x ;
961 Ax = A->x;
962
963 if(nz >= 0)
964 {
965 for(p = 0 ; p < nz ; p++)
966 {
967 DEBUG_PRINTF(" %g %g : ", (double)(Ai [p]), (double)(Ap [p])) ;
968 DEBUG_PRINTF("%g\n", Ax ? Ax [p] : 1) ;
969 DEBUG_PRINTF("B %g\n", B[Ai[p] + Ap[p]*m]) ;
970 B[Ai[p] + Ap[p]*m] = Ax [p];
971 }
972 }
973 else
974 {
975 for(j = 0 ; j < n ; j++)
976 {
977 DEBUG_PRINTF(" col %g : locations %g to %g\n", (double) j,
978 (double)(Ap [j]), (double)(Ap [j+1]-1)) ;
979 for(p = Ap [j] ; p < Ap [j+1] ; p++)
980 {
981 DEBUG_PRINTF(" %g %g : ", (double)(Ai [p]), Ax ? Ax [p] : 1) ;
982 B[Ai[p]+ j * m] = Ax [p];
983 }
984 }
985 }
986 return (0) ;
987 }
988
CSparseMatrix_alloc_for_copy(const CSparseMatrix * const m)989 CSparseMatrix* CSparseMatrix_alloc_for_copy(const CSparseMatrix* const m)
990 {
991 assert(m);
992 CSparseMatrix* out = NULL;
993 if(m->nz >= 0) /* triplet */
994 {
995 out = cs_spalloc(m->m, m->n, m->nzmax, 1, 1);
996 }
997 else if(m->nz == -1) /* csc */
998 {
999 out = cs_spalloc(m->m, m->n, m->nzmax, 1, 0);
1000 }
1001 else if(m->nz == -2) /* csr */
1002 {
1003 out = cs_spalloc(m->n, m->m, m->nzmax, 1, 0);
1004 out->nz = -2;
1005 out->m = m->m;
1006 out->n = m->n;
1007 }
1008 else
1009 {
1010 fprintf(stderr, "NM_copy :: error unknown type " CS_ID
1011 " for CSparse matrix\n", m->nz);
1012 exit(EXIT_FAILURE);
1013 }
1014
1015 return out;
1016 }
1017
CSparseMatrix_copy(const CSparseMatrix * const A,CSparseMatrix * B)1018 void CSparseMatrix_copy(const CSparseMatrix* const A, CSparseMatrix* B)
1019 {
1020 assert(A);
1021 assert(B);
1022
1023 if(B->nzmax < A->nzmax)
1024 {
1025 B->x = (double *) realloc(B->x, (size_t)A->nzmax * sizeof(double));
1026 B->i = (CS_INT *) realloc(B->i, (size_t)A->nzmax * sizeof(CS_INT));
1027 }
1028 else if(!(B->x))
1029 {
1030 B->x = (double *) malloc((size_t)A->nzmax * sizeof(double));
1031 }
1032
1033 if(A->nz >= 0)
1034 {
1035 /* triplet */
1036 B->p = (CS_INT *) realloc(B->p, (size_t)A->nzmax * sizeof(CS_INT));
1037 }
1038 else if((A->nz == -1) && (B->n < A->n))
1039 {
1040 /* csc */
1041 B->p = (CS_INT *) realloc(B->p, ((size_t)A->n + 1) * sizeof(CS_INT));
1042 }
1043 else if((A->nz == -2) && (B->m < A->m))
1044 {
1045 /* csr */
1046 B->p = (CS_INT *) realloc(B->p, ((size_t)A->m + 1) * sizeof(CS_INT));
1047 }
1048
1049
1050 B->nzmax = A->nzmax;
1051 B->nz = A->nz;
1052 B->m = A->m;
1053 B->n = A->n;
1054
1055 memcpy(B->x, A->x, (size_t)A->nzmax * sizeof(double));
1056 memcpy(B->i, A->i, (size_t)A->nzmax * sizeof(CS_INT));
1057
1058 size_t size_cpy=0;
1059 if(A->nz >= 0)
1060 {
1061 size_cpy = (size_t)A->nzmax;
1062 }
1063 else if(A->nz == -1)
1064 {
1065 size_cpy = (size_t)A->n + 1;
1066 }
1067 else if(A->nz == -2)
1068 {
1069 size_cpy = (size_t)A->m + 1;
1070 }
1071 else
1072 numerics_error("CSparseMatrix_copy", "Wrong nz value in CSparseMatrix.");
1073
1074 memcpy(B->p, A->p, size_cpy * sizeof(CS_INT));
1075 }
1076
CSparseMatrix_max_by_columns(const CSparseMatrix * A,double * max)1077 int CSparseMatrix_max_by_columns(const CSparseMatrix *A, double * max)
1078 {
1079 CS_INT p, j, n, *Ap ;
1080 CS_ENTRY *Ax ;
1081 double s ;
1082
1083 if(!CS_CSC(A) || !A->x) return (-1) ; /* check inputs */
1084 n = A->n ;
1085 Ap = A->p ;
1086 Ax = A->x ;
1087 /* loop over the column */
1088 for(j = 0 ; j < n ; j++)
1089 {
1090 /* loop over the element of the row */
1091 p = Ap [j] ;
1092 s = Ax [p] ;
1093 for(p = Ap [j] ; p < Ap [j+1] ; p++)
1094 s = CS_MAX(Ax[p], s) ;
1095 max[j] = s;
1096 }
1097 return 0 ;
1098 }
CSparseMatrix_max_abs_by_columns(const CSparseMatrix * A,double * max)1099 int CSparseMatrix_max_abs_by_columns(const CSparseMatrix *A, double * max)
1100 {
1101 CS_INT p, j, n, *Ap ;
1102 CS_ENTRY *Ax ;
1103 double s ;
1104
1105 if(!CS_CSC(A) || !A->x) return (-1) ; /* check inputs */
1106 n = A->n ;
1107 Ap = A->p ;
1108 Ax = A->x ;
1109 /* loop over the column */
1110 for(j = 0 ; j < n ; j++)
1111 {
1112 /* loop over the element of the row */
1113 p = Ap [j] ;
1114 s = fabs(Ax [p]) ;
1115 for(p = Ap [j] ; p < Ap [j+1] ; p++)
1116 s = CS_MAX(fabs(Ax[p]), s) ;
1117 max[j] = s;
1118 }
1119 return 0 ;
1120 }
1121