1 /* 2 * ECOS - Embedded Conic Solver. 3 * Copyright (C) 2012-2015 A. Domahidi [domahidi@embotech.com], 4 * Automatic Control Lab, ETH Zurich & embotech GmbH, Zurich, Switzerland. 5 * 6 * This program is free software: you can redistribute it and/or modify 7 * it under the terms of the GNU General Public License as published by 8 * the Free Software Foundation, either version 3 of the License, or 9 * (at your option) any later version. 10 * 11 * This program is distributed in the hope that it will be useful, 12 * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 * GNU General Public License for more details. 15 * 16 * You should have received a copy of the GNU General Public License 17 * along with this program. If not, see <http://www.gnu.org/licenses/>. 18 */ 19 20 21 /* 22 * Sparse linear algebra library for setup phase, i.e. this module 23 * accesses MALLOC and hence should not go on an embedded platform. 24 */ 25 26 #include "splamm.h" 27 28 /* SYSTEM INCLUDES FOR MEMORY ALLOCATION ------------------------------- */ 29 #if PRINTLEVEL > 2 30 #include <stdlib.h> 31 #endif 32 33 34 /* 35 * Cumulative sum. p[i] = sum w[0..i-1] and w = p on output. 36 * Defined for vectors of length m. 37 */ 38 void spla_cumsum(idxint* p, idxint* w, idxint m) 39 { 40 idxint cumsum = 0; 41 idxint i; 42 for( i=0; i < m; i++ ){ 43 p[i] = cumsum; 44 cumsum += w[i]; 45 w[i] = p[i]; 46 } 47 } 48 49 50 /** 51 * Returns the inverse of permutation p of length n. 52 */ 53 void pinv(idxint n, idxint* p, idxint* pinv) 54 { 55 idxint i; 56 for( i=0; i<n; i++ ){ pinv[p[i]] = i; } 57 } 58 59 60 /** 61 * Transpose a matrix; returns A = M', 62 * and an index vector MtoMt that directly maps elements of M 63 * to elements of M'. 64 */ 65 spmat* transposeSparseMatrix(spmat* M, idxint* MtoMt) 66 { 67 idxint j, i, k, q; 68 idxint* w; 69 70 spmat* A = newSparseMatrix(M->n, M->m, M->nnz); 71 if (M->nnz == 0) return A; 72 73 w = (idxint *)MALLOC(M->m*sizeof(idxint)); 74 75 /* row count: how often does row k occur in M? */ 76 for( i=0; i < M->m; i++ ) { w[i] = 0; } 77 for( k=0; k < M->nnz; k++ ) { w[M->ir[k]]++; } 78 79 /* row pointers: cumulative sum of w gives A->jc */ 80 spla_cumsum(A->jc, w, M->m); 81 82 /* now walk through M and copy data to right places and set row counter */ 83 for( j=0; j < M->n; j++ ){ 84 for( k = M->jc[j]; k < M->jc[j+1]; k++ ){ 85 q = w[M->ir[k]]++; 86 A->ir[q] = j; 87 A->pr[q] = M->pr[k]; 88 MtoMt[k] = q; 89 } 90 } 91 92 FREE(w); 93 return A; 94 } 95 96 97 /** 98 * Create a new sparse matrix (uses MALLOC!) 99 */ 100 spmat* newSparseMatrix(idxint m, idxint n, idxint nnz) 101 { 102 idxint* jc = (idxint *)MALLOC((n+1)*sizeof(idxint)); 103 idxint* ir = (idxint *)MALLOC(nnz*sizeof(idxint)); 104 pfloat* pr = (pfloat *)MALLOC(nnz*sizeof(pfloat)); 105 jc[n] = nnz; 106 return ecoscreateSparseMatrix(m, n, nnz, jc, ir, pr); 107 } 108 109 110 /** 111 * Create a sparse matrix from existing arrays (no MALLOC) 112 */ 113 spmat* ecoscreateSparseMatrix(idxint m, idxint n, idxint nnz, idxint* jc, idxint* ir, pfloat* pr) 114 { 115 spmat* M = (spmat *)MALLOC(sizeof(spmat)); 116 M->m = m; 117 M->n = n; 118 M->nnz = nnz; 119 M->jc = jc; 120 M->ir = ir; 121 M->pr = pr; 122 if (M->jc) M->jc[n] = nnz; 123 return M; 124 } 125 126 127 /** 128 * Create a new sparse matrix (uses FREE!) 129 */ 130 void freeSparseMatrix(spmat* M) 131 { 132 if (M->ir) FREE(M->ir); 133 if (M->jc) FREE(M->jc); 134 if (M->pr) FREE(M->pr); 135 FREE(M); 136 } 137 138 139 /** 140 * Permutes a symmetric matrix with only the upper triangular part stored. 141 * Writes the upper triangular part of C = A(p,p) in column compressed 142 * storage format. 143 * 144 * The function additionally returns the mapping PK that maps the row indices 145 * of the sparse matrix A on the row indices of C, such that C[P[k]] = A[k]. 146 * 147 * NOTE: The matrix C and the vector PK are NOT created within this function 148 * - you need to allocate them beforehand!! 149 * 150 * If PK is NULL then the last output argument is ignored. 151 */ 152 void permuteSparseSymmetricMatrix(spmat* A, idxint* pinv, spmat* C, idxint* PK) 153 { 154 idxint i, i2, j, j2, k, q; 155 idxint* w; 156 157 /* create matrix, permutation vector and temporary work space */ 158 w = (idxint *)MALLOC(A->n*sizeof(idxint)); 159 160 /* count number of entries per column of C, store this in w */ 161 for( j=0; j < A->n; j++ ){ w[j] = 0; } 162 for( j=0; j < A->n; j++ ){ 163 j2 = pinv[j]; 164 for( k=A->jc[j]; k < A->jc[j+1]; k++ ){ 165 i = A->ir[k]; 166 if( i > j ) continue; /* skip lower triangular part of A */ 167 i2 = pinv[i]; 168 w[ i2 > j2 ? i2 : j2]++; 169 } 170 } 171 172 /* cumulative sum gives column pointers */ 173 spla_cumsum(C->jc, w, A->n); 174 175 /* copy data over */ 176 for( j=0; j < A->n; j++ ){ 177 j2 = pinv[j]; 178 for( k=A->jc[j]; k < A->jc[j+1]; k++ ){ 179 i = A->ir[k]; 180 if( i > j ) continue; /* skip lower triangular part of A */ 181 i2 = pinv[i]; 182 q = w[i2 > j2 ? i2 : j2]++; 183 C->ir[q] = i2 < j2 ? i2 : j2; 184 C->pr[q] = A->pr[k]; 185 if( PK ) PK[k] = q; 186 } 187 } 188 189 /* free work space */ 190 FREE(w); 191 } 192 193 194 /* 195 * Returns a copy of a sparse matrix A. 196 */ 197 spmat* copySparseMatrix(spmat* A) 198 { 199 idxint i; 200 spmat* B = newSparseMatrix(A->m, A->n, A->nnz); 201 202 /* copy over */ 203 for( i=0; i<=A->n; i++ ){ B->jc[i] = A->jc[i]; } 204 for( i=0; i<A->nnz; i++ ){ B->ir[i] = A->ir[i]; } 205 for( i=0; i<A->nnz; i++ ){ B->pr[i] = A->pr[i]; } 206 207 return B; 208 } 209 210 211 212 /* ================================= DEBUG FUNCTIONS ======================= */ 213 #if PRINTLEVEL > 2 214 /** 215 * Prints a dense matrix. 216 */ 217 void printDenseMatrix(pfloat *M, idxint dim1, idxint dim2, char *name) 218 { 219 idxint i,j; 220 PRINTTEXT("%s = \n\t",name); 221 for( i=0; i<dim1; i++ ){ 222 for( j=0; j<dim2; j++ ){ 223 if( j<dim2-1 ) 224 PRINTTEXT("% 14.12e, ",M[i*dim2+j]); 225 else 226 PRINTTEXT("% 14.12e; ",M[i*dim2+j]); 227 } 228 if( i<dim1-1){ 229 PRINTTEXT("\n\t"); 230 } 231 } 232 PRINTTEXT("\n"); 233 } 234 235 236 /** 237 * Prints a dense integer matrix. 238 */ 239 void printDenseMatrix_i(idxint *M, idxint dim1, idxint dim2, char *name) 240 { 241 idxint i,j; 242 PRINTTEXT("%s = \n\t",name); 243 for( i=0; i<dim1; i++ ){ 244 for( j=0; j<dim2; j++ ){ 245 if( j<dim2-1 ) 246 PRINTTEXT("%d, ",(int)M[i*dim2+j]); 247 else 248 PRINTTEXT("%d; ",(int)M[i*dim2+j]); 249 } 250 if( i<dim1-1){ 251 PRINTTEXT("\n\t"); 252 } 253 } 254 PRINTTEXT("\n"); 255 } 256 257 258 /* 259 * Prints a sparse matrix. 260 */ 261 void printSparseMatrix(spmat* M) 262 { 263 idxint j, i, row_strt, row_stop; 264 idxint k = 0; 265 for(j=0; j<M->n; j++){ 266 row_strt = M->jc[j]; 267 row_stop = M->jc[j+1]; 268 if (row_strt == row_stop) 269 continue; 270 else { 271 for(i=row_strt; i<row_stop; i++ ){ 272 PRINTTEXT("\t(%3u,%3u) = %g\n", (int)M->ir[i]+1, (int)j+1, M->pr[k++]); 273 } 274 } 275 } 276 } 277 278 279 /* dump a sparse matrix in Matlab format */ 280 /* use LOAD and SPCONVERT to read in the file in MATLAB */ 281 void dumpSparseMatrix(spmat* M, char* fn) 282 { 283 idxint j, i, row_strt, row_stop; 284 idxint k = 0; 285 FILE *f = fopen(fn,"w"); 286 if( f != NULL ){ 287 for(j=0; j<M->n; j++){ 288 row_strt = M->jc[j]; 289 row_stop = M->jc[j+1]; 290 if (row_strt == row_stop) 291 continue; 292 else { 293 for(i=row_strt; i<row_stop; i++ ){ 294 fprintf(f,"%d\t%d\t%20.18e\n", (int)M->ir[i]+1, (int)j+1, M->pr[k++]); 295 } 296 } 297 } 298 fprintf(f,"%d\t%d\t%20.18e\n", (int)M->m, (int)M->n, 0.0); 299 fclose(f); 300 PRINTTEXT("File %s successfully written.\n", fn); 301 } else { 302 PRINTTEXT("Error during writing file %s.\n", fn); 303 } 304 } 305 306 307 /** 308 * Dumps a dense matrix of doubles to a CSV file. 309 */ 310 void dumpDenseMatrix(pfloat *M, int dim1, int dim2, char *fn) 311 { 312 FILE *f = fopen(fn,"w"); 313 int i,j; 314 315 /* swap dimensions if only vector to be printed */ 316 if( dim1 == 1 && dim2 > 1){ 317 i = dim2; 318 dim2 = dim1; 319 dim1 = i; 320 } 321 322 if( f != NULL ){ 323 for( i=0; i<dim1; i++ ){ 324 if( dim2 > 1 ){ 325 for( j=0; j<dim2-1; j++ ){ 326 fprintf(f, "%+18.16e,",M[i*dim2+j]); 327 } 328 j = dim2; 329 fprintf(f, "%+18.16e\n",M[i*dim2+j]); 330 } else { 331 fprintf(f, "%+18.16e\n",M[i]); 332 } 333 } 334 fclose(f); 335 printf("Written %d x %d matrix to '%s'.\n",i,dim2,fn); 336 } else { 337 printf("ERROR: file %s could not be opened. Exiting.",fn); 338 exit(1); 339 } 340 } 341 342 343 /** 344 * Dumps a dense matrix of integers to a CSV file. 345 */ 346 void dumpDenseMatrix_i(idxint *M, int dim1, int dim2, char *fn) 347 { 348 FILE *f = fopen(fn,"w"); 349 int i,j; 350 351 /* swap dimensions if only vector to be printed */ 352 if( dim1 == 1 && dim2 > 1){ 353 i = dim2; 354 dim2 = dim1; 355 dim1 = i; 356 } 357 358 if( f != NULL ){ 359 for( i=0; i<dim1; i++ ){ 360 if( dim2 > 1 ){ 361 for( j=0; j<dim2-1; j++ ){ 362 fprintf(f, "%d,",(int)M[i*dim2+j]); 363 } 364 j = dim2; 365 fprintf(f, "%d\n",(int)M[i*dim2+j]); 366 } else { 367 fprintf(f, "%d\n",(int)M[i]); 368 } 369 } 370 fclose(f); 371 printf("Written %d x %d matrix to '%s'.\n",i,dim2,fn); 372 } else { 373 printf("ERROR: file %s could not be opened. Exiting.",fn); 374 exit(1); 375 } 376 } 377 #endif 378 379 380