1 module cs; 2 3 nothrow @nogc extern(C): 4 5 import glob_opts; 6 import osqp_configure; // for c_malloc and others 7 import types; // CSC matrix type 8 import lin_alg; // Vector copy operations 9 import constants; // for OSQP_NULL 10 11 static void* csc_malloc(c_int n, c_int size) { 12 return c_malloc(n * size); 13 } 14 15 static void* csc_calloc(c_int n, c_int size) { 16 return c_calloc(n, size); 17 } 18 19 csc* csc_matrix(c_int m, c_int n, c_int nzmax, c_float *x, c_int *i, c_int *p) 20 { 21 csc *M = cast(csc *)c_malloc(csc.sizeof); 22 23 if (!M) return OSQP_NULL; 24 25 M.m = m; 26 M.n = n; 27 M.nz = -1; 28 M.nzmax = nzmax; 29 M.x = x; 30 M.i = i; 31 M.p = p; 32 return M; 33 } 34 35 csc* csc_spalloc(c_int m, c_int n, c_int nzmax, c_int values, c_int triplet) { 36 csc *A = cast(csc*)csc_calloc(1, csc.sizeof); /* allocate the csc struct */ 37 38 if (!A) return OSQP_NULL; /* out of memory */ 39 40 A.m = m; /* define dimensions and nzmax */ 41 A.n = n; 42 A.nzmax = nzmax = c_max(nzmax, 1); 43 A.nz = triplet ? 0 : -1; /* allocate triplet or comp.col */ 44 A.p = cast(c_int*)csc_malloc(triplet ? nzmax : n + 1, c_int.sizeof); 45 A.i = cast(c_int*)csc_malloc(nzmax, c_int.sizeof); 46 A.x = values ? cast(c_float*)csc_malloc(nzmax, c_float.sizeof) : OSQP_NULL; 47 if (!A.p || !A.i || (values && !A.x)){ 48 csc_spfree(A); 49 return OSQP_NULL; 50 } else return A; 51 } 52 53 void csc_spfree(csc *A) { 54 if (A){ 55 if (A.p) c_free(A.p); 56 if (A.i) c_free(A.i); 57 if (A.x) c_free(A.x); 58 c_free(A); 59 } 60 } 61 62 csc* triplet_to_csc(const csc *T, c_int *TtoC) { 63 c_int m; 64 c_int n; 65 c_int nz; 66 c_int p; 67 c_int k; 68 c_int *Cp; 69 c_int *Ci; 70 c_int *w; 71 c_int *Ti; 72 c_int *Tj; 73 c_float *Cx; 74 c_float *Tx; 75 csc *C; 76 77 m = T.m; 78 n = T.n; 79 Ti = cast(c_int*)T.i; 80 Tj = cast(c_int*)T.p; 81 Tx = cast(c_float*)T.x; 82 nz = T.nz; 83 C = csc_spalloc(m, n, nz, Tx != OSQP_NULL, 0); /* allocate result */ 84 w = cast(c_int*)csc_calloc(n, c_int.sizeof); /* get workspace */ 85 86 if (!C || !w) return csc_done(C, w, OSQP_NULL, 0); /* out of memory */ 87 88 Cp = C.p; 89 Ci = C.i; 90 Cx = C.x; 91 92 for (k = 0; k < nz; k++) w[Tj[k]]++; /* column counts */ 93 csc_cumsum(Cp, w, n); /* column pointers */ 94 95 for (k = 0; k < nz; k++) { 96 Ci[p = w[Tj[k]]++] = Ti[k]; /* A(i,j) is the pth entry in C */ 97 98 if (Cx) { 99 Cx[p] = Tx[k]; 100 101 if (TtoC != OSQP_NULL) TtoC[k] = p; // Assign vector of indices 102 } 103 } 104 return csc_done(C, w, OSQP_NULL, 1); /* success; free w and return C */ 105 } 106 107 csc* triplet_to_csr(const csc *T, c_int *TtoC) { 108 c_int m; 109 c_int n; 110 c_int nz; 111 c_int p; 112 c_int k; 113 c_int *Cp; 114 c_int *Cj; 115 c_int *w; 116 c_int *Ti; 117 c_int *Tj; 118 c_float *Cx; 119 c_float *Tx; 120 csc *C; 121 122 m = T.m; 123 n = T.n; 124 Ti = cast(c_int*)T.i; 125 Tj = cast(c_int*)T.p; 126 Tx = cast(c_float*)T.x; 127 nz = T.nz; 128 C = csc_spalloc(m, n, nz, Tx != OSQP_NULL, 0); /* allocate result */ 129 w = cast(c_int*)csc_calloc(m, c_int.sizeof); /* get workspace */ 130 131 if (!C || !w) return csc_done(C, w, OSQP_NULL, 0); /* out of memory */ 132 133 Cp = C.p; 134 Cj = C.i; 135 Cx = C.x; 136 137 for (k = 0; k < nz; k++) w[Ti[k]]++; /* row counts */ 138 csc_cumsum(Cp, w, m); /* row pointers */ 139 140 for (k = 0; k < nz; k++) { 141 Cj[p = w[Ti[k]]++] = Tj[k]; /* A(i,j) is the pth entry in C */ 142 143 if (Cx) { 144 Cx[p] = Tx[k]; 145 146 if (TtoC != OSQP_NULL) TtoC[k] = p; // Assign vector of indices 147 } 148 } 149 return csc_done(C, w, OSQP_NULL, 1); /* success; free w and return C */ 150 } 151 152 c_int csc_cumsum(c_int *p, c_int *c, c_int n) { 153 c_int i, nz = 0; 154 155 if (!p || !c) return -1; /* check inputs */ 156 157 for (i = 0; i < n; i++) 158 { 159 p[i] = nz; 160 nz += c[i]; 161 c[i] = p[i]; 162 } 163 p[n] = nz; 164 return nz; /* return sum (c [0..n-1]) */ 165 } 166 167 //c_int* csc_pinv(c_int const *p, c_int n) { 168 c_int* csc_pinv(c_int *p, c_int n) { 169 c_int k; 170 c_int *pinv; 171 172 if (!p) return OSQP_NULL; /* p = OSQP_NULL denotes identity */ 173 174 pinv = cast(c_int*)csc_malloc(n, c_int.sizeof); /* allocate result */ 175 176 if (!pinv) return OSQP_NULL; /* out of memory */ 177 178 for (k = 0; k < n; k++) pinv[p[k]] = k; /* invert the permutation */ 179 return pinv; /* return result */ 180 } 181 182 csc* csc_symperm(const csc *A, const c_int *pinv, c_int *AtoC, c_int values) { 183 c_int i; 184 c_int j; 185 c_int p; 186 c_int q; 187 c_int i2; 188 c_int j2; 189 c_int n; 190 c_int *Ap; 191 c_int *Ai; 192 c_int *Cp; 193 c_int *Ci; 194 c_int *w; 195 c_float *Cx; 196 c_float *Ax; 197 csc *C; 198 199 n = A.n; 200 Ap = cast(c_int*)A.p; 201 Ai = cast(c_int*)A.i; 202 Ax = cast(c_float*)A.x; 203 C = csc_spalloc(n, n, Ap[n], values && (Ax != OSQP_NULL), 204 0); /* alloc result*/ 205 w = cast(c_int*)csc_calloc(n, c_int.sizeof); /* get workspace */ 206 207 if (!C || !w) return csc_done(C, w, OSQP_NULL, 0); /* out of memory */ 208 209 Cp = C.p; 210 Ci = C.i; 211 Cx = C.x; 212 213 for (j = 0; j < n; j++) /* count entries in each column of C */ 214 { 215 j2 = pinv ? pinv[j] : j; /* column j of A is column j2 of C */ 216 217 for (p = Ap[j]; p < Ap[j + 1]; p++) { 218 i = Ai[p]; 219 220 if (i > j) continue; /* skip lower triangular part of A */ 221 i2 = pinv ? pinv[i] : i; /* row i of A is row i2 of C */ 222 w[c_max(i2, j2)]++; /* column count of C */ 223 } 224 } 225 csc_cumsum(Cp, w, n); /* compute column pointers of C */ 226 227 for (j = 0; j < n; j++) { 228 j2 = pinv ? pinv[j] : j; /* column j of A is column j2 of C */ 229 230 for (p = Ap[j]; p < Ap[j + 1]; p++) { 231 i = Ai[p]; 232 233 if (i > j) continue; /* skip lower triangular 234 part of A*/ 235 i2 = pinv ? pinv[i] : i; /* row i of A is row i2 236 of C */ 237 Ci[q = w[c_max(i2, j2)]++] = c_min(i2, j2); 238 239 if (Cx) Cx[q] = Ax[p]; 240 241 if (AtoC) { // If vector AtoC passed, store values of the mappings 242 AtoC[p] = q; 243 } 244 } 245 } 246 return csc_done(C, w, OSQP_NULL, 1); /* success; free workspace, return C */ 247 } 248 249 csc* copy_csc_mat(const csc *A) { 250 csc *B = csc_spalloc(A.m, A.n, A.p[A.n], 1, 0); 251 252 if (!B) return OSQP_NULL; 253 254 prea_int_vec_copy(A.p, B.p, A.n + 1); 255 prea_int_vec_copy(A.i, B.i, A.p[A.n]); 256 prea_vec_copy(A.x, B.x, A.p[A.n]); 257 258 return B; 259 } 260 261 void prea_copy_csc_mat(const csc *A, csc *B) { 262 prea_int_vec_copy(A.p, B.p, A.n + 1); 263 prea_int_vec_copy(A.i, B.i, A.p[A.n]); 264 prea_vec_copy(A.x, B.x, A.p[A.n]); 265 266 B.nzmax = A.nzmax; 267 } 268 269 csc* csc_done(csc *C, void *w, void *x, c_int ok) { 270 c_free(w); /* free workspace */ 271 c_free(x); 272 if (ok) return C; 273 else { 274 csc_spfree(C); 275 return OSQP_NULL; 276 } 277 } 278 279 csc* csc_to_triu(csc *M) { 280 csc *M_trip; // Matrix in triplet format 281 csc *M_triu; // Resulting upper triangular matrix 282 c_int nnzorigM; // Number of nonzeros from original matrix M 283 c_int nnzmaxM; // Estimated maximum number of elements of upper triangular M 284 c_int n; // Dimension of M 285 c_int ptr, i, j; // Counters for (i,j) and index in M 286 c_int z_M = 0; // Counter for elements in M_trip 287 288 289 // Check if matrix is square 290 if (M.m != M.n) { 291 version(PRINTING){ 292 c_eprint("ERROR in %s: Matrix M not square\n", __FUNCTION__.ptr); 293 } /* ifdef PRINTING */ 294 return OSQP_NULL; 295 } 296 n = M.n; 297 298 // Get number of nonzeros full M 299 nnzorigM = M.p[n]; 300 301 // Estimate nnzmaxM 302 // Number of nonzero elements in original M + diagonal part. 303 // . Full matrix M as input: estimate is half the number of total elements + 304 // diagonal = .5 * (nnzorigM + n) 305 // . Upper triangular matrix M as input: estimate is the number of total 306 // elements + diagonal = nnzorigM + n 307 // The maximum between the two is nnzorigM + n 308 nnzmaxM = nnzorigM + n; 309 310 // OLD 311 // nnzmaxM = n*(n+1)/2; // Full upper triangular matrix (This version 312 // allocates too much memory!) 313 // nnzmaxM = .5 * (nnzorigM + n); // half of the total elements + diagonal 314 315 // Allocate M_trip 316 M_trip = csc_spalloc(n, n, nnzmaxM, 1, 1); // Triplet format 317 318 if (!M_trip) { 319 version(PRINTING){ 320 c_eprint("ERROR in %s: Upper triangular matrix extraction failed (out of memory)\n", __FUNCTION__.ptr); 321 } /* ifdef PRINTING */ 322 return OSQP_NULL; 323 } 324 325 // Fill M_trip with only elements in M which are in the upper triangular 326 for (j = 0; j < n; j++) { // Cycle over columns 327 for (ptr = M.p[j]; ptr < M.p[j + 1]; ptr++) { 328 // Get row index 329 i = M.i[ptr]; 330 331 // Assign element only if in the upper triangular 332 if (i <= j) { 333 // c_print("\nM(%i, %i) = %.4f", M.i[ptr], j, M.x[ptr]); 334 335 M_trip.i[z_M] = i; 336 M_trip.p[z_M] = j; 337 M_trip.x[z_M] = M.x[ptr]; 338 339 // Increase counter for the number of elements 340 z_M++; 341 } 342 } 343 } 344 345 // Set number of nonzeros 346 M_trip.nz = z_M; 347 348 // Convert triplet matrix to csc format 349 M_triu = triplet_to_csc(M_trip, OSQP_NULL); 350 351 // Assign number of nonzeros of full matrix to triu M 352 M_triu.nzmax = nnzmaxM; 353 354 // Cleanup and return result 355 csc_spfree(M_trip); 356 357 // Return matrix in triplet form 358 return M_triu; 359 }