Squashed 'third_party/osqp/' content from commit 33454b3e23
Change-Id: I056df0582ca06664e86554c341a94c47ab932001
git-subtree-dir: third_party/osqp
git-subtree-split: 33454b3e236f1f44193bfbbb6b8c8e71f8f04e9a
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/src/cs.c b/src/cs.c
new file mode 100644
index 0000000..a678ceb
--- /dev/null
+++ b/src/cs.c
@@ -0,0 +1,318 @@
+#include "cs.h"
+
+
+static void* csc_malloc(c_int n, c_int size) {
+ return c_malloc(n * size);
+}
+
+static void* csc_calloc(c_int n, c_int size) {
+ return c_calloc(n, size);
+}
+
+csc* csc_matrix(c_int m, c_int n, c_int nzmax, c_float *x, c_int *i, c_int *p)
+{
+ csc *M = (csc *)c_malloc(sizeof(csc));
+
+ if (!M) return OSQP_NULL;
+
+ M->m = m;
+ M->n = n;
+ M->nz = -1;
+ M->nzmax = nzmax;
+ M->x = x;
+ M->i = i;
+ M->p = p;
+ return M;
+}
+
+csc* csc_spalloc(c_int m, c_int n, c_int nzmax, c_int values, c_int triplet) {
+ csc *A = csc_calloc(1, sizeof(csc)); /* allocate the csc struct */
+
+ if (!A) return OSQP_NULL; /* out of memory */
+
+ A->m = m; /* define dimensions and nzmax */
+ A->n = n;
+ A->nzmax = nzmax = c_max(nzmax, 1);
+ A->nz = triplet ? 0 : -1; /* allocate triplet or comp.col */
+ A->p = csc_malloc(triplet ? nzmax : n + 1, sizeof(c_int));
+ A->i = csc_malloc(nzmax, sizeof(c_int));
+ A->x = values ? csc_malloc(nzmax, sizeof(c_float)) : OSQP_NULL;
+ if (!A->p || !A->i || (values && !A->x)){
+ csc_spfree(A);
+ return OSQP_NULL;
+ } else return A;
+}
+
+void csc_spfree(csc *A) {
+ if (A){
+ if (A->p) c_free(A->p);
+ if (A->i) c_free(A->i);
+ if (A->x) c_free(A->x);
+ c_free(A);
+ }
+}
+
+csc* triplet_to_csc(const csc *T, c_int *TtoC) {
+ c_int m, n, nz, p, k, *Cp, *Ci, *w, *Ti, *Tj;
+ c_float *Cx, *Tx;
+ csc *C;
+
+ m = T->m;
+ n = T->n;
+ Ti = T->i;
+ Tj = T->p;
+ Tx = T->x;
+ nz = T->nz;
+ C = csc_spalloc(m, n, nz, Tx != OSQP_NULL, 0); /* allocate result */
+ w = csc_calloc(n, sizeof(c_int)); /* get workspace */
+
+ if (!C || !w) return csc_done(C, w, OSQP_NULL, 0); /* out of memory */
+
+ Cp = C->p;
+ Ci = C->i;
+ Cx = C->x;
+
+ for (k = 0; k < nz; k++) w[Tj[k]]++; /* column counts */
+ csc_cumsum(Cp, w, n); /* column pointers */
+
+ for (k = 0; k < nz; k++) {
+ Ci[p = w[Tj[k]]++] = Ti[k]; /* A(i,j) is the pth entry in C */
+
+ if (Cx) {
+ Cx[p] = Tx[k];
+
+ if (TtoC != OSQP_NULL) TtoC[k] = p; // Assign vector of indices
+ }
+ }
+ return csc_done(C, w, OSQP_NULL, 1); /* success; free w and return C */
+}
+
+csc* triplet_to_csr(const csc *T, c_int *TtoC) {
+ c_int m, n, nz, p, k, *Cp, *Cj, *w, *Ti, *Tj;
+ c_float *Cx, *Tx;
+ csc *C;
+
+ m = T->m;
+ n = T->n;
+ Ti = T->i;
+ Tj = T->p;
+ Tx = T->x;
+ nz = T->nz;
+ C = csc_spalloc(m, n, nz, Tx != OSQP_NULL, 0); /* allocate result */
+ w = csc_calloc(m, sizeof(c_int)); /* get workspace */
+
+ if (!C || !w) return csc_done(C, w, OSQP_NULL, 0); /* out of memory */
+
+ Cp = C->p;
+ Cj = C->i;
+ Cx = C->x;
+
+ for (k = 0; k < nz; k++) w[Ti[k]]++; /* row counts */
+ csc_cumsum(Cp, w, m); /* row pointers */
+
+ for (k = 0; k < nz; k++) {
+ Cj[p = w[Ti[k]]++] = Tj[k]; /* A(i,j) is the pth entry in C */
+
+ if (Cx) {
+ Cx[p] = Tx[k];
+
+ if (TtoC != OSQP_NULL) TtoC[k] = p; // Assign vector of indices
+ }
+ }
+ return csc_done(C, w, OSQP_NULL, 1); /* success; free w and return C */
+}
+
+c_int csc_cumsum(c_int *p, c_int *c, c_int n) {
+ c_int i, nz = 0;
+
+ if (!p || !c) return -1; /* check inputs */
+
+ for (i = 0; i < n; i++)
+ {
+ p[i] = nz;
+ nz += c[i];
+ c[i] = p[i];
+ }
+ p[n] = nz;
+ return nz; /* return sum (c [0..n-1]) */
+}
+
+c_int* csc_pinv(c_int const *p, c_int n) {
+ c_int k, *pinv;
+
+ if (!p) return OSQP_NULL; /* p = OSQP_NULL denotes identity */
+
+ pinv = csc_malloc(n, sizeof(c_int)); /* allocate result */
+
+ if (!pinv) return OSQP_NULL; /* out of memory */
+
+ for (k = 0; k < n; k++) pinv[p[k]] = k; /* invert the permutation */
+ return pinv; /* return result */
+}
+
+csc* csc_symperm(const csc *A, const c_int *pinv, c_int *AtoC, c_int values) {
+ c_int i, j, p, q, i2, j2, n, *Ap, *Ai, *Cp, *Ci, *w;
+ c_float *Cx, *Ax;
+ csc *C;
+
+ n = A->n;
+ Ap = A->p;
+ Ai = A->i;
+ Ax = A->x;
+ C = csc_spalloc(n, n, Ap[n], values && (Ax != OSQP_NULL),
+ 0); /* alloc result*/
+ w = csc_calloc(n, sizeof(c_int)); /* get workspace */
+
+ if (!C || !w) return csc_done(C, w, OSQP_NULL, 0); /* out of memory */
+
+ Cp = C->p;
+ Ci = C->i;
+ Cx = C->x;
+
+ for (j = 0; j < n; j++) /* count entries in each column of C */
+ {
+ j2 = pinv ? pinv[j] : j; /* column j of A is column j2 of C */
+
+ for (p = Ap[j]; p < Ap[j + 1]; p++) {
+ i = Ai[p];
+
+ if (i > j) continue; /* skip lower triangular part of A */
+ i2 = pinv ? pinv[i] : i; /* row i of A is row i2 of C */
+ w[c_max(i2, j2)]++; /* column count of C */
+ }
+ }
+ csc_cumsum(Cp, w, n); /* compute column pointers of C */
+
+ for (j = 0; j < n; j++) {
+ j2 = pinv ? pinv[j] : j; /* column j of A is column j2 of C */
+
+ for (p = Ap[j]; p < Ap[j + 1]; p++) {
+ i = Ai[p];
+
+ if (i > j) continue; /* skip lower triangular
+ part of A*/
+ i2 = pinv ? pinv[i] : i; /* row i of A is row i2
+ of C */
+ Ci[q = w[c_max(i2, j2)]++] = c_min(i2, j2);
+
+ if (Cx) Cx[q] = Ax[p];
+
+ if (AtoC) { // If vector AtoC passed, store values of the mappings
+ AtoC[p] = q;
+ }
+ }
+ }
+ return csc_done(C, w, OSQP_NULL, 1); /* success; free workspace, return C */
+}
+
+csc* copy_csc_mat(const csc *A) {
+ csc *B = csc_spalloc(A->m, A->n, A->p[A->n], 1, 0);
+
+ if (!B) return OSQP_NULL;
+
+ prea_int_vec_copy(A->p, B->p, A->n + 1);
+ prea_int_vec_copy(A->i, B->i, A->p[A->n]);
+ prea_vec_copy(A->x, B->x, A->p[A->n]);
+
+ return B;
+}
+
+void prea_copy_csc_mat(const csc *A, csc *B) {
+ prea_int_vec_copy(A->p, B->p, A->n + 1);
+ prea_int_vec_copy(A->i, B->i, A->p[A->n]);
+ prea_vec_copy(A->x, B->x, A->p[A->n]);
+
+ B->nzmax = A->nzmax;
+}
+
+csc* csc_done(csc *C, void *w, void *x, c_int ok) {
+ c_free(w); /* free workspace */
+ c_free(x);
+ if (ok) return C;
+ else {
+ csc_spfree(C);
+ return OSQP_NULL;
+ }
+}
+
+csc* csc_to_triu(csc *M) {
+ csc *M_trip; // Matrix in triplet format
+ csc *M_triu; // Resulting upper triangular matrix
+ c_int nnzorigM; // Number of nonzeros from original matrix M
+ c_int nnzmaxM; // Estimated maximum number of elements of upper triangular M
+ c_int n; // Dimension of M
+ c_int ptr, i, j; // Counters for (i,j) and index in M
+ c_int z_M = 0; // Counter for elements in M_trip
+
+
+ // Check if matrix is square
+ if (M->m != M->n) {
+#ifdef PRINTING
+ c_eprint("Matrix M not square");
+#endif /* ifdef PRINTING */
+ return OSQP_NULL;
+ }
+ n = M->n;
+
+ // Get number of nonzeros full M
+ nnzorigM = M->p[n];
+
+ // Estimate nnzmaxM
+ // Number of nonzero elements in original M + diagonal part.
+ // -> Full matrix M as input: estimate is half the number of total elements +
+ // diagonal = .5 * (nnzorigM + n)
+ // -> Upper triangular matrix M as input: estimate is the number of total
+ // elements + diagonal = nnzorigM + n
+ // The maximum between the two is nnzorigM + n
+ nnzmaxM = nnzorigM + n;
+
+ // OLD
+ // nnzmaxM = n*(n+1)/2; // Full upper triangular matrix (This version
+ // allocates too much memory!)
+ // nnzmaxM = .5 * (nnzorigM + n); // half of the total elements + diagonal
+
+ // Allocate M_trip
+ M_trip = csc_spalloc(n, n, nnzmaxM, 1, 1); // Triplet format
+
+ if (!M_trip) {
+#ifdef PRINTING
+ c_eprint("Upper triangular matrix extraction failed (out of memory)");
+#endif /* ifdef PRINTING */
+ return OSQP_NULL;
+ }
+
+ // Fill M_trip with only elements in M which are in the upper triangular
+ for (j = 0; j < n; j++) { // Cycle over columns
+ for (ptr = M->p[j]; ptr < M->p[j + 1]; ptr++) {
+ // Get row index
+ i = M->i[ptr];
+
+ // Assign element only if in the upper triangular
+ if (i <= j) {
+ // c_print("\nM(%i, %i) = %.4f", M->i[ptr], j, M->x[ptr]);
+
+ M_trip->i[z_M] = i;
+ M_trip->p[z_M] = j;
+ M_trip->x[z_M] = M->x[ptr];
+
+ // Increase counter for the number of elements
+ z_M++;
+ }
+ }
+ }
+
+ // Set number of nonzeros
+ M_trip->nz = z_M;
+
+ // Convert triplet matrix to csc format
+ M_triu = triplet_to_csc(M_trip, OSQP_NULL);
+
+ // Assign number of nonzeros of full matrix to triu M
+ M_triu->nzmax = nnzmaxM;
+
+ // Cleanup and return result
+ csc_spfree(M_trip);
+
+ // Return matrix in triplet form
+ return M_triu;
+}