scs 0.3.2 → 0.4.1

data/vendor/scs/linsys/cpu/direct/private.c

@@ -1,20 +1,10 @@
 #include "private.h"
-#include "linsys.h"
 
-const char *SCS(get_lin_sys_method)() {
-  return "sparse-direct";
+const char *scs_get_lin_sys_method() {
+  return "sparse-direct-amd-qdldl";
 }
 
-/*
-char *SCS(get_lin_sys_summary)(ScsLinSysWork *p, const ScsInfo *info) {
-  char *str = (char *)scs_malloc(sizeof(char) * 128);
-  scs_int n = p->L->n;
-  sprintf(str, "lin-sys: nnz(L): %li\n", (long)(p->L->p[n] + n));
-  return str;
-}
-*/
-
-void SCS(free_lin_sys_work)(ScsLinSysWork *p) {
+void scs_free_lin_sys_work(ScsLinSysWork *p) {
   if (p) {
     SCS(cs_spfree)(p->L);
     SCS(cs_spfree)(p->kkt);
@@ -33,155 +23,43 @@ void SCS(free_lin_sys_work)(ScsLinSysWork *p) {
   }
 }
 
-static csc *form_kkt(const ScsMatrix *A, const ScsMatrix *P, scs_float *diag_p,
-                     const scs_float *diag_r, scs_int *diag_r_idxs) {
-  /* ONLY UPPER TRIANGULAR PART IS STUFFED
-   * forms column compressed kkt matrix
-   * assumes column compressed form A matrix
-   *
-   * forms upper triangular part of [(I + P)  A'; A -I]
-   * P : n x n, A: m x n.
-   */
-  scs_int h, i, j, count;
-  csc *Kcsc, *K;
-  scs_int n = A->n;
-  scs_int m = A->m;
-  scs_int Anz = A->p[n];
-  scs_int Knzmax;
-  scs_int *idx_mapping;
-  if (P) {
-    /* Upper bound P + I upper triangular component as Pnz + n */
-    Knzmax = n + m + Anz + P->p[n];
-  } else {
-    Knzmax = n + m + Anz;
-  }
-  K = SCS(cs_spalloc)(m + n, m + n, Knzmax, 1, 1);
-
-#if VERBOSITY > 0
-  scs_printf("forming kkt\n");
-#endif
-  /* Here we generate a triplet matrix and then compress to CSC */
-  if (!K) {
-    return SCS_NULL;
-  }
-
-  count = 0; /* element counter */
-  if (P) {
-    /* R_x + P in top left */
-    for (j = 0; j < n; j++) { /* cols */
-      diag_p[j] = 0.;
-      /* empty column, add diagonal  */
-      if (P->p[j] == P->p[j + 1]) {
-        K->i[count] = j;
-        K->p[count] = j;
-        K->x[count] = diag_r[j];
-        diag_r_idxs[j] = count; /* store the indices where diag_r occurs */
-        count++;
-      }
-      for (h = P->p[j]; h < P->p[j + 1]; h++) {
-        i = P->i[h]; /* row */
-        if (i > j) { /* only upper triangular needed */
-          break;
-        }
-        K->i[count] = i;
-        K->p[count] = j;
-        K->x[count] = P->x[h];
-        if (i == j) {
-          /* P has diagonal element */
-          diag_p[j] = P->x[h];
-          K->x[count] += diag_r[j];
-          diag_r_idxs[j] = count; /* store the indices where diag_r occurs */
-        }
-        count++;
-        /* reached the end without adding diagonal, do it now */
-        if ((i < j) && (h + 1 == P->p[j + 1] || P->i[h + 1] > j)) {
-          K->i[count] = j;
-          K->p[count] = j;
-          K->x[count] = diag_r[j];
-          diag_r_idxs[j] = count; /* store the indices where diag_r occurs */
-          count++;
-        }
-      }
-    }
-  } else {
-    /* R_x in top left */
-    for (j = 0; j < n; j++) {
-      diag_p[j] = 0.;
-      K->i[count] = j;
-      K->p[count] = j;
-      K->x[count] = diag_r[j];
-      diag_r_idxs[j] = count; /* store the indices where diag_r occurs */
-      count++;
-    }
-  }
-
-  /* A^T at top right */
-  for (j = 0; j < n; j++) {
-    for (h = A->p[j]; h < A->p[j + 1]; h++) {
-      K->p[count] = A->i[h] + n;
-      K->i[count] = j;
-      K->x[count] = A->x[h];
-      count++;
-    }
-  }
-
-  /* -R_y at bottom right */
-  for (j = 0; j < m; j++) {
-    K->i[count] = j + n;
-    K->p[count] = j + n;
-    K->x[count] = -diag_r[j + n];
-    diag_r_idxs[j + n] = count; /* store the indices where diag_r occurs */
-    count++;
-  }
-
-  K->nz = count;
-  idx_mapping = (scs_int *)scs_calloc(K->nz, sizeof(scs_int));
-  Kcsc = SCS(cs_compress)(K, idx_mapping);
-  for (i = 0; i < m + n; i++) {
-    diag_r_idxs[i] = idx_mapping[diag_r_idxs[i]];
-  }
-  SCS(cs_spfree)(K);
-  scs_free(idx_mapping);
-  return Kcsc;
-}
-
-static scs_int _ldl_init(csc *A, scs_int *P, scs_float **info) {
+static scs_int _ldl_init(ScsMatrix *A, scs_int *P, scs_float **info) {
   *info = (scs_float *)scs_calloc(AMD_INFO, sizeof(scs_float));
   return amd_order(A->n, A->p, A->i, P, (scs_float *)SCS_NULL, *info);
 }
 
 /* call only once */
 static scs_int ldl_prepare(ScsLinSysWork *p) {
-  csc *kkt = p->kkt, *L = p->L;
-  scs_int n = kkt->n;
+  ScsMatrix *kkt = p->kkt, *L = p->L;
+  scs_int nzmax, n = kkt->n;
   p->etree = (scs_int *)scs_calloc(n, sizeof(scs_int));
   p->Lnz = (scs_int *)scs_calloc(n, sizeof(scs_int));
   p->iwork = (scs_int *)scs_calloc(3 * n, sizeof(scs_int));
   L->p = (scs_int *)scs_calloc((1 + n), sizeof(scs_int));
-  L->nzmax = QDLDL_etree(n, kkt->p, kkt->i, p->iwork, p->Lnz, p->etree);
-  if (L->nzmax < 0) {
+  nzmax = QDLDL_etree(n, kkt->p, kkt->i, p->iwork, p->Lnz, p->etree);
+  if (nzmax < 0) {
     scs_printf("Error in elimination tree calculation.\n");
-    if (L->nzmax == -1) {
+    if (nzmax == -1) {
       scs_printf("Matrix is not perfectly upper triangular.\n");
-    } else if (L->nzmax == -2) {
+    } else if (nzmax == -2) {
       scs_printf("Integer overflow in L nonzero count.\n");
     }
-    return L->nzmax;
+    return nzmax;
   }
 
-  L->x = (scs_float *)scs_calloc(L->nzmax, sizeof(scs_float));
-  L->i = (scs_int *)scs_calloc(L->nzmax, sizeof(scs_int));
+  L->x = (scs_float *)scs_calloc(nzmax, sizeof(scs_float));
+  L->i = (scs_int *)scs_calloc(nzmax, sizeof(scs_int));
   p->Dinv = (scs_float *)scs_calloc(n, sizeof(scs_float));
   p->D = (scs_float *)scs_calloc(n, sizeof(scs_float));
   p->bwork = (scs_int *)scs_calloc(n, sizeof(scs_int));
   p->fwork = (scs_float *)scs_calloc(n, sizeof(scs_float));
-  return L->nzmax;
+  return nzmax;
 }
 
 /* can call many times */
 static scs_int ldl_factor(ScsLinSysWork *p, scs_int num_vars) {
   scs_int factor_status;
-  csc *kkt = p->kkt, *L = p->L;
+  ScsMatrix *kkt = p->kkt, *L = p->L;
 #if VERBOSITY > 0
   scs_printf("numeric factorization\n");
 #endif
@@ -217,7 +95,7 @@ static void _ldl_permt(scs_int n, scs_float *x, scs_float *b, scs_int *P) {
     x[P[j]] = b[j];
 }
 
-static void _ldl_solve(scs_float *b, csc *L, scs_float *Dinv, scs_int *P,
+static void _ldl_solve(scs_float *b, ScsMatrix *L, scs_float *Dinv, scs_int *P,
                        scs_float *bp) {
   /* solves PLDL'P' x = b for x */
   scs_int n = L->n;
@@ -240,11 +118,11 @@ static scs_int *cs_pinv(scs_int const *p, scs_int n) {
   return pinv;      /* return result */
 }
 
-static csc *cs_symperm(const csc *A, const scs_int *pinv, scs_int *idx_mapping,
-                       scs_int values) {
+static ScsMatrix *cs_symperm(const ScsMatrix *A, const scs_int *pinv,
+                             scs_int *idx_mapping, scs_int values) {
   scs_int i, j, p, q, i2, j2, n, *Ap, *Ai, *Cp, *Ci, *w;
   scs_float *Cx, *Ax;
-  csc *C;
+  ScsMatrix *C;
   n = A->n;
   Ap = A->p;
   Ai = A->i;
@@ -290,14 +168,16 @@ static csc *cs_symperm(const csc *A, const scs_int *pinv, scs_int *idx_mapping,
                       1); /* success; free workspace, return C */
 }
 
-static csc *permute_kkt(const ScsMatrix *A, const ScsMatrix *P,
-                        ScsLinSysWork *p, const scs_float *diag_r) {
+static ScsMatrix *permute_kkt(const ScsMatrix *A, const ScsMatrix *P,
+                              ScsLinSysWork *p, const scs_float *diag_r) {
   scs_float *info;
-  scs_int *Pinv, amd_status, *idx_mapping, i;
-  csc *kkt_perm, *kkt = form_kkt(A, P, p->diag_p, diag_r, p->diag_r_idxs);
+  scs_int *Pinv, amd_status, *idx_mapping, i, kkt_nnz;
+  ScsMatrix *kkt_perm;
+  ScsMatrix *kkt = SCS(form_kkt)(A, P, p->diag_p, diag_r, p->diag_r_idxs, 1);
   if (!kkt) {
     return SCS_NULL;
   }
+  kkt_nnz = kkt->p[kkt->n];
   amd_status = _ldl_init(kkt, p->perm, &info);
   if (amd_status < 0) {
     scs_printf("AMD permutatation error.\n");
@@ -308,7 +188,7 @@ static csc *permute_kkt(const ScsMatrix *A, const ScsMatrix *P,
   amd_info(info);
 #endif
   Pinv = cs_pinv(p->perm, A->n + A->m);
-  idx_mapping = (scs_int *)scs_calloc(kkt->nzmax, sizeof(scs_int));
+  idx_mapping = (scs_int *)scs_calloc(kkt_nnz, sizeof(scs_int));
   kkt_perm = cs_symperm(kkt, Pinv, idx_mapping, 1);
   for (i = 0; i < A->n + A->m; i++) {
     p->diag_r_idxs[i] = idx_mapping[p->diag_r_idxs[i]];
@@ -320,7 +200,7 @@ static csc *permute_kkt(const ScsMatrix *A, const ScsMatrix *P,
   return kkt_perm;
 }
 
-void SCS(update_lin_sys_diag_r)(ScsLinSysWork *p, const scs_float *diag_r) {
+void scs_update_lin_sys_diag_r(ScsLinSysWork *p, const scs_float *diag_r) {
   scs_int i, ldl_status;
   for (i = 0; i < p->n; ++i) {
     /* top left is R_x + P, bottom right is -R_y */
@@ -339,21 +219,20 @@ void SCS(update_lin_sys_diag_r)(ScsLinSysWork *p, const scs_float *diag_r) {
   }
 }
 
-ScsLinSysWork *SCS(init_lin_sys_work)(const ScsMatrix *A, const ScsMatrix *P,
-                                      const scs_float *diag_r) {
+ScsLinSysWork *scs_init_lin_sys_work(const ScsMatrix *A, const ScsMatrix *P,
+                                     const scs_float *diag_r) {
   ScsLinSysWork *p = (ScsLinSysWork *)scs_calloc(1, sizeof(ScsLinSysWork));
   scs_int n_plus_m = A->n + A->m, ldl_status, ldl_prepare_status;
   p->m = A->m;
   p->n = A->n;
   p->diag_p = (scs_float *)scs_calloc(A->n, sizeof(scs_float));
   p->perm = (scs_int *)scs_calloc(sizeof(scs_int), n_plus_m);
-  p->L = (csc *)scs_calloc(1, sizeof(csc));
+  p->L = (ScsMatrix *)scs_calloc(1, sizeof(ScsMatrix));
   p->bp = (scs_float *)scs_calloc(n_plus_m, sizeof(scs_float));
   p->diag_r_idxs = (scs_int *)scs_calloc(n_plus_m, sizeof(scs_int));
   p->factorizations = 0;
   p->L->m = n_plus_m;
   p->L->n = n_plus_m;
-  p->L->nz = -1;
   p->kkt = permute_kkt(A, P, p, diag_r);
   ldl_prepare_status = ldl_prepare(p);
   ldl_status = ldl_factor(p, A->n);
@@ -366,8 +245,8 @@ ScsLinSysWork *SCS(init_lin_sys_work)(const ScsMatrix *A, const ScsMatrix *P,
   return p;
 }
 
-scs_int SCS(solve_lin_sys)(ScsLinSysWork *p, scs_float *b, const scs_float *s,
-                           scs_float tol) {
+scs_int scs_solve_lin_sys(ScsLinSysWork *p, scs_float *b, const scs_float *s,
+                          scs_float tol) {
   /* returns solution to linear system */
   /* Ax = b with solution stored in b */
   _ldl_solve(b, p->L, p->Dinv, p->perm, p->bp);

data/vendor/scs/linsys/cpu/direct/private.h

@@ -9,15 +9,15 @@ extern "C" {
 #include "external/amd/amd.h"
 #include "external/qdldl/qdldl.h"
 #include "glbopts.h"
-#include "scs.h"
+#include "linsys.h"
 #include "scs_matrix.h"
 
 struct SCS_LIN_SYS_WORK {
-  scs_int m, n;    /* linear system dimensions */
-  csc *kkt, *L;    /* KKT, and factorization matrix L resp. */
-  scs_float *Dinv; /* inverse diagonal matrix of factorization */
-  scs_int *perm;   /* permutation of KKT matrix for factorization */
-  scs_float *bp;   /* workspace memory for solves */
+  scs_int m, n;       /* linear system dimensions */
+  ScsMatrix *kkt, *L; /* KKT, and factorization matrix L resp. */
+  scs_float *Dinv;    /* inverse diagonal matrix of factorization */
+  scs_int *perm;      /* permutation of KKT matrix for factorization */
+  scs_float *bp;      /* workspace memory for solves */
   scs_int *diag_r_idxs;
   scs_int factorizations;
   /* ldl factorization workspace */

data/vendor/scs/linsys/cpu/indirect/private.c

@@ -1,22 +1,9 @@
 #include "private.h"
-#include "linsys.h"
-#include "util.h"
-#include <limits.h>
 
-const char *SCS(get_lin_sys_method)() {
-  return "sparse-indirect";
+const char *scs_get_lin_sys_method() {
+  return "sparse-indirect-scs";
 }
 
-/*
-char *SCS(get_lin_sys_summary)(ScsLinSysWork *p, const ScsInfo *info) {
-  char *str = (char *)scs_malloc(sizeof(char) * 128);
-  sprintf(str, "lin-sys: avg cg its: %2.2f\n",
-          (scs_float)p->tot_cg_its / (info->iter + 1));
-  p->tot_cg_its = 0;
-  return str;
-}
-*/
-
 /* Not possible to do this on the fly due to M_ii += a_i' (R_y)^-1 a_i */
 /* set M = inv ( diag ( R_x + P + A' R_y^{-1} A ) ) */
 static void set_preconditioner(ScsLinSysWork *p) {
@@ -97,7 +84,7 @@ static void transpose(const ScsMatrix *A, ScsLinSysWork *p) {
 #endif
 }
 
-void SCS(free_lin_sys_work)(ScsLinSysWork *p) {
+void scs_free_lin_sys_work(ScsLinSysWork *p) {
   if (p) {
     scs_free(p->p);
     scs_free(p->r);
@@ -162,13 +149,13 @@ static void apply_pre_conditioner(scs_float *z, scs_float *r, scs_int n,
 }
 
 /* no need to update anything in this case */
-void SCS(update_lin_sys_diag_r)(ScsLinSysWork *p, const scs_float *diag_r) {
+void scs_update_lin_sys_diag_r(ScsLinSysWork *p, const scs_float *diag_r) {
   p->diag_r = diag_r; /* this isn't needed but do it to be safe */
   set_preconditioner(p);
 }
 
-ScsLinSysWork *SCS(init_lin_sys_work)(const ScsMatrix *A, const ScsMatrix *P,
-                                      const scs_float *diag_r) {
+ScsLinSysWork *scs_init_lin_sys_work(const ScsMatrix *A, const ScsMatrix *P,
+                                     const scs_float *diag_r) {
   ScsLinSysWork *p = (ScsLinSysWork *)scs_calloc(1, sizeof(ScsLinSysWork));
   p->A = A;
   p->P = P;
@@ -198,7 +185,7 @@ ScsLinSysWork *SCS(init_lin_sys_work)(const ScsMatrix *A, const ScsMatrix *P,
   p->tot_cg_its = 0;
   if (!p->p || !p->r || !p->Gp || !p->tmp || !p->At || !p->At->i || !p->At->p ||
       !p->At->x) {
-    SCS(free_lin_sys_work)(p);
+    scs_free_lin_sys_work(p);
     return SCS_NULL;
   }
   return p;
@@ -288,8 +275,8 @@ static scs_int pcg(ScsLinSysWork *pr, const scs_float *s, scs_float *b,
  * y = R_y^{-1} (Ax - ry)
  *
  */
-scs_int SCS(solve_lin_sys)(ScsLinSysWork *p, scs_float *b, const scs_float *s,
-                           scs_float tol) {
+scs_int scs_solve_lin_sys(ScsLinSysWork *p, scs_float *b, const scs_float *s,
+                          scs_float tol) {
   scs_int cg_its, max_iters;
 
   if (tol <= 0.) {

data/vendor/scs/linsys/cpu/indirect/private.h

@@ -5,11 +5,13 @@
 extern "C" {
 #endif
 
+#include "csparse.h"
 #include "glbopts.h"
 #include "linalg.h"
-#include "scs.h"
+#include "linsys.h"
 #include "scs_matrix.h"
-#include <math.h>
+#include "util.h" /* timer */
+#include <string.h>
 
 struct SCS_LIN_SYS_WORK {
   scs_int n, m; /* linear system dimensions */

data/vendor/scs/linsys/csparse.c

@@ -2,39 +2,37 @@
 
 #include "csparse.h"
 
-csc *SCS(cs_spalloc)(scs_int m, scs_int n, scs_int nzmax, scs_int values,
-                     scs_int triplet) {
-  csc *A = (csc *)scs_calloc(1, sizeof(csc)); /* allocate the csc struct */
+ScsMatrix *SCS(cs_spalloc)(scs_int m, scs_int n, scs_int nzmax, scs_int values,
+                           scs_int triplet) {
+  ScsMatrix *A = (ScsMatrix *)scs_calloc(1, sizeof(ScsMatrix));
   if (!A) {
     return SCS_NULL;
   }         /* out of memory */
   A->m = m; /* define dimensions and nzmax */
   A->n = n;
-  A->nzmax = nzmax = MAX(nzmax, 1);
-  A->nz = triplet ? 0 : -1; /* allocate triplet or comp.col */
   A->p = (scs_int *)scs_calloc((triplet ? nzmax : n + 1), sizeof(scs_int));
   A->i = (scs_int *)scs_calloc(nzmax, sizeof(scs_int));
   A->x = values ? (scs_float *)scs_calloc(nzmax, sizeof(scs_float)) : SCS_NULL;
   return (!A->p || !A->i || (values && !A->x)) ? SCS(cs_spfree)(A) : A;
 }
 
-csc *SCS(cs_done)(csc *C, void *w, void *x, scs_int ok) {
+ScsMatrix *SCS(cs_done)(ScsMatrix *C, void *w, void *x, scs_int ok) {
   scs_free(w); /* free workspace */
   scs_free(x);
   return ok ? C : SCS(cs_spfree)(C); /* return result if OK, else free it */
 }
 
 /* C = compressed-column form of a triplet matrix T */
-csc *SCS(cs_compress)(const csc *T, scs_int *idx_mapping) {
-  scs_int m, n, nz, p, k, *Cp, *Ci, *w, *Ti, *Tj;
+ScsMatrix *SCS(cs_compress)(const ScsMatrix *T, scs_int nz,
+                            scs_int *idx_mapping) {
+  scs_int m, n, p, k, *Cp, *Ci, *w, *Ti, *Tj;
   scs_float *Cx, *Tx;
-  csc *C;
+  ScsMatrix *C;
   m = T->m;
   n = T->n;
   Ti = T->i;
   Tj = T->p;
   Tx = T->x;
-  nz = T->nz;
   C = SCS(cs_spalloc)(m, n, nz, Tx != SCS_NULL, 0); /* allocate result */
   w = (scs_int *)scs_calloc(n, sizeof(scs_int));    /* get workspace */
   if (!C || !w) {
@@ -75,7 +73,7 @@ scs_float SCS(cumsum)(scs_int *p, scs_int *c, scs_int n) {
   return nz2; /* return sum (c [0..n-1]) */
 }
 
-csc *SCS(cs_spfree)(csc *A) {
+ScsMatrix *SCS(cs_spfree)(ScsMatrix *A) {
   if (!A) {
     return SCS_NULL;
   } /* do nothing if A already SCS_NULL */
@@ -83,5 +81,135 @@ csc *SCS(cs_spfree)(csc *A) {
   scs_free(A->i);
   scs_free(A->x);
   scs_free(A);
-  return (csc *)SCS_NULL; /* free the csc struct and return SCS_NULL */
+  /* free the ScsMatrix struct and return SCS_NULL */
+  return (ScsMatrix *)SCS_NULL;
+}
+
+/* Build the KKT matrix */
+ScsMatrix *SCS(form_kkt)(const ScsMatrix *A, const ScsMatrix *P,
+                         scs_float *diag_p, const scs_float *diag_r,
+                         scs_int *diag_r_idxs, scs_int upper) {
+  /*
+   * Forms column compressed KKT matrix assumes column compressed A,P matrices.
+   * Only upper OR lower triangular part is stuffed, depending on `upper` flag.
+   *
+   * Forms upper/lower triangular part of [(R_x + P)  A'; A -R_y]
+   * Shapes: P : n x n, A: m x n.
+   *
+   * Output: `diag_p` will contain values of P diagonal upon completion,
+   * and `diag_r_idxs` will contain the indices corresponding to the entries
+   * in the returned matrix corresponding to the entries of R.
+   *
+   */
+  scs_int h, i, j, count;
+  ScsMatrix *Kcsc, *K;
+  scs_int n = A->n;
+  scs_int m = A->m;
+  scs_int Anz = A->p[n];
+  scs_int Knzmax;
+  scs_int *idx_mapping;
+  if (P) {
+    /* Upper bound P + I triangular component NNZs as Pnz + n */
+    Knzmax = n + m + Anz + P->p[n];
+  } else {
+    Knzmax = n + m + Anz;
+  }
+  K = SCS(cs_spalloc)(m + n, m + n, Knzmax, 1, 1);
+
+#if VERBOSITY > 0
+  scs_printf("forming kkt\n");
+#endif
+  /* Here we generate a triplet matrix and then compress to CSC */
+  if (!K) {
+    return SCS_NULL;
+  }
+
+  count = 0; /* element counter */
+  if (P) {
+    /* R_x + P in top left */
+    for (j = 0; j < n; j++) { /* cols */
+      diag_p[j] = 0.;
+      /* empty column, add diagonal  */
+      if (P->p[j] == P->p[j + 1]) {
+        K->i[count] = j;
+        K->p[count] = j;
+        K->x[count] = diag_r[j];
+        diag_r_idxs[j] = count; /* store the indices where diag_r occurs */
+        count++;
+      }
+      for (h = P->p[j]; h < P->p[j + 1]; h++) {
+        i = P->i[h]; /* row */
+        if (i > j) { /* only upper triangular needed */
+          break;
+        }
+        if (upper) {
+          K->i[count] = i;
+          K->p[count] = j;
+        } else { /* lower triangular */
+          /* P is passed in upper triangular, need to flip that here */
+          K->i[count] = j; /* col -> row */
+          K->p[count] = i; /* row -> col */
+        }
+        K->x[count] = P->x[h];
+        if (i == j) {
+          /* P has diagonal element */
+          diag_p[j] = P->x[h];
+          K->x[count] += diag_r[j];
+          diag_r_idxs[j] = count; /* store the indices where diag_r occurs */
+        }
+        count++;
+        /* reached the end without adding diagonal, do it now */
+        if ((i < j) && (h + 1 == P->p[j + 1] || P->i[h + 1] > j)) {
+          K->i[count] = j;
+          K->p[count] = j;
+          K->x[count] = diag_r[j];
+          diag_r_idxs[j] = count; /* store the indices where diag_r occurs */
+          count++;
+        }
+      }
+    }
+  } else {
+    /* R_x in top left */
+    for (j = 0; j < n; j++) {
+      diag_p[j] = 0.;
+      K->i[count] = j;
+      K->p[count] = j;
+      K->x[count] = diag_r[j];
+      diag_r_idxs[j] = count; /* store the indices where diag_r occurs */
+      count++;
+    }
+  }
+
+  /* A in bottom left or A^T top right */
+  for (j = 0; j < n; j++) { /* column */
+    for (h = A->p[j]; h < A->p[j + 1]; h++) {
+      if (upper) {
+        K->p[count] = A->i[h] + n; /* column */
+        K->i[count] = j;           /*row */
+      } else {                     /* lower triangular */
+        K->p[count] = j;           /* column */
+        K->i[count] = A->i[h] + n; /* row */
+      }
+      K->x[count] = A->x[h];
+      count++;
+    }
+  }
+
+  /* -R_y at bottom right */
+  for (j = 0; j < m; j++) {
+    K->i[count] = j + n;
+    K->p[count] = j + n;
+    K->x[count] = -diag_r[j + n];
+    diag_r_idxs[j + n] = count; /* store the indices where diag_r occurs */
+    count++;
+  }
+
+  idx_mapping = (scs_int *)scs_calloc(count, sizeof(scs_int));
+  Kcsc = SCS(cs_compress)(K, count, idx_mapping);
+  for (i = 0; i < m + n; i++) {
+    diag_r_idxs[i] = idx_mapping[diag_r_idxs[i]];
+  }
+  SCS(cs_spfree)(K);
+  scs_free(idx_mapping);
+  return Kcsc;
 }

data/vendor/scs/linsys/csparse.h

@@ -10,24 +10,17 @@ extern "C" {
 #include "glbopts.h"
 #include "scs.h"
 
-/* matrix in compressed-column or triplet form */
-typedef struct SPARSE_MATRIX {
-  scs_int nzmax; /* maximum number of entries */
-  scs_int m;     /* number of rows */
-  scs_int n;     /* number of columns */
-  scs_int *p;    /* column pointers (size n+1) or col indices (size nzmax) */
-  scs_int *i;    /* row indices, size nzmax */
-  scs_float *x;  /* numerical values, size nzmax */
-  scs_int nz;    /* # of entries in triplet matrix, -1 for compressed-col */
-} csc;
-
-csc *SCS(cs_spalloc)(scs_int m, scs_int n, scs_int nzmax, scs_int values,
-                     scs_int triplet);
-csc *SCS(cs_done)(csc *C, void *w, void *x, scs_int ok);
-csc *SCS(cs_compress)(const csc *T, scs_int *idx_mapping);
+ScsMatrix *SCS(cs_spalloc)(scs_int m, scs_int n, scs_int nzmax, scs_int values,
+                           scs_int triplet);
+ScsMatrix *SCS(cs_done)(ScsMatrix *C, void *w, void *x, scs_int ok);
+ScsMatrix *SCS(cs_compress)(const ScsMatrix *T, scs_int nz,
+                            scs_int *idx_mapping);
+ScsMatrix *SCS(cs_spfree)(ScsMatrix *A);
 scs_float SCS(cumsum)(scs_int *p, scs_int *c, scs_int n);
-csc *SCS(cs_spfree)(csc *A);
-
+/* Forms KKT matrix */
+ScsMatrix *SCS(form_kkt)(const ScsMatrix *A, const ScsMatrix *P,
+                         scs_float *diag_p, const scs_float *diag_r,
+                         scs_int *diag_r_idxs, scs_int upper);
 #ifdef __cplusplus
 }
 #endif