scs 0.5.6 → 0.6.0

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

@@ -0,0 +1,250 @@
+/*
+ * Dense direct linear system solver using the Gram matrix reduction:
+ *
+ *   [R_x + P   A'] [x]   [rx]
+ *   [  A     -R_y] [y] = [ry]
+ *
+ * is reduced to:
+ *
+ *   G x = rx + A' R_y^{-1} ry,    where G = R_x + P + A' R_y^{-1} A
+ *   y = R_y^{-1} (A x - ry)
+ *
+ * G is n x n symmetric positive definite, factorized via Cholesky (dpotrf).
+ * A is stored as a dense m x n matrix for fast dgemv operations.
+ */
+
+#include "private.h"
+
+#include <string.h>
+
+/* LAPACK / BLAS prototypes */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Cholesky factorization */
+void BLAS(potrf)(const char *uplo, blas_int *n, scs_float *a, blas_int *lda,
+                 blas_int *info);
+/* Cholesky solve */
+void BLAS(potrs)(const char *uplo, blas_int *n, blas_int *nrhs,
+                 const scs_float *a, blas_int *lda, scs_float *b,
+                 blas_int *ldb, blas_int *info);
+/* Matrix-vector multiply: y = alpha * op(A) * x + beta * y */
+void BLAS(gemv)(const char *trans, blas_int *m, blas_int *n, scs_float *alpha,
+                const scs_float *a, blas_int *lda, const scs_float *x,
+                blas_int *incx, scs_float *beta, scs_float *y, blas_int *incy);
+/* Symmetric rank-k update: C = alpha * A' * A + beta * C */
+void BLAS(syrk)(const char *uplo, const char *trans, blas_int *n, blas_int *k,
+                scs_float *alpha, const scs_float *a, blas_int *lda,
+                scs_float *beta, scs_float *c, blas_int *ldc);
+
+#ifdef __cplusplus
+}
+#endif
+
+const char *scs_get_lin_sys_method(void) {
+  return "dense-direct-cholesky";
+}
+
+/* Convert sparse CSC matrix A (m x n) to dense column-major array */
+static void sparse_to_dense(const ScsMatrix *A, scs_float *A_dense) {
+  scs_int j, k;
+  scs_int m = A->m;
+  scs_int n = A->n;
+  memset(A_dense, 0, (size_t)m * n * sizeof(scs_float));
+  for (j = 0; j < n; ++j) {
+    for (k = A->p[j]; k < A->p[j + 1]; ++k) {
+      A_dense[j * m + A->i[k]] = A->x[k];
+    }
+  }
+}
+
+/* Form Gram matrix G = R_x + P + A' diag(r_y_inv) A (upper triangle).
+ * Uses BLAS dsyrk for the A' diag(r_y_inv) A product. */
+static void form_gram(ScsLinSysWork *p, const scs_float *diag_r) {
+  scs_int i, j, k;
+  scs_int n = p->n;
+  scs_int m = p->m;
+  scs_float *G = p->G;
+  const ScsMatrix *P = p->P;
+
+  /* Compute r_y_inv = 1 / R_y and sqrt_r_y_inv in tmp_m */
+  for (i = 0; i < m; ++i) {
+    p->r_y_inv[i] = 1.0 / diag_r[n + i];
+    p->tmp_m[i] = SQRTF(p->r_y_inv[i]);
+  }
+
+  /* Form S = diag(sqrt(r_y_inv)) * A in pre-allocated workspace.
+   * S is m x n, stored column-major. */
+  scs_float *S = p->S;
+  for (j = 0; j < n; ++j) {
+    for (i = 0; i < m; ++i) {
+      S[j * m + i] = p->tmp_m[i] * p->A_dense[j * m + i];
+    }
+  }
+
+  /* G = S' * S = A' diag(r_y_inv) A  (upper triangle) */
+  {
+    blas_int bn = (blas_int)n;
+    blas_int bm = (blas_int)m;
+    scs_float one = 1.0;
+    scs_float zero = 0.0;
+    char uplo = 'U';
+    char trans = 'T';
+    BLAS(syrk)(&uplo, &trans, &bn, &bm, &one, S, &bm, &zero, G, &bn);
+  }
+
+  /* Add R_x to diagonal */
+  for (i = 0; i < n; ++i) {
+    G[i * n + i] += diag_r[i];
+    p->diag_p[i] = 0.;
+  }
+
+  /* Add P (symmetric, stored upper triangular CSC) */
+  if (P) {
+    for (j = 0; j < n; ++j) {
+      for (k = P->p[j]; k < P->p[j + 1]; ++k) {
+        i = P->i[k];
+        G[j * n + i] += P->x[k]; /* upper triangle: i <= j */
+        if (i == j) {
+          p->diag_p[j] = P->x[k];
+        }
+      }
+    }
+  }
+}
+
+ScsLinSysWork *scs_init_lin_sys_work(const ScsMatrix *A, const ScsMatrix *P,
+                                     const scs_float *diag_r) {
+  blas_int info, bn;
+  ScsLinSysWork *p = (ScsLinSysWork *)scs_calloc(1, sizeof(ScsLinSysWork));
+  if (!p) {
+    return SCS_NULL;
+  }
+
+  p->n = A->n;
+  p->m = A->m;
+  p->P = P;
+
+  p->A_dense = (scs_float *)scs_calloc((size_t)A->m * A->n, sizeof(scs_float));
+  p->G = (scs_float *)scs_calloc((size_t)A->n * A->n, sizeof(scs_float));
+  p->r_y_inv = (scs_float *)scs_calloc(A->m, sizeof(scs_float));
+  p->tmp_m = (scs_float *)scs_calloc(A->m, sizeof(scs_float));
+  p->S = (scs_float *)scs_calloc((size_t)A->m * A->n, sizeof(scs_float));
+  p->diag_p = (scs_float *)scs_calloc(A->n, sizeof(scs_float));
+
+  if (!p->A_dense || !p->G || !p->r_y_inv || !p->tmp_m || !p->S || !p->diag_p) {
+    scs_free_lin_sys_work(p);
+    return SCS_NULL;
+  }
+
+  /* Convert sparse A to dense once */
+  sparse_to_dense(A, p->A_dense);
+
+  /* Form and factorize Gram matrix */
+  form_gram(p, diag_r);
+  bn = (blas_int)p->n;
+  {
+    char uplo = 'U';
+    BLAS(potrf)(&uplo, &bn, p->G, &bn, &info);
+  }
+  if (info != 0) {
+    scs_printf("Error in dense Cholesky factorization (dpotrf), info = %d\n",
+               (int)info);
+    scs_free_lin_sys_work(p);
+    return SCS_NULL;
+  }
+
+  return p;
+}
+
+/*
+ * Solve:
+ *   x = G^{-1} (rx + A' R_y^{-1} ry)
+ *   y = R_y^{-1} (A x - ry)
+ *
+ * Input b = [rx; ry], solution stored in b = [x; y].
+ */
+scs_int scs_solve_lin_sys(ScsLinSysWork *p, scs_float *b, const scs_float *s,
+                          scs_float tol) {
+  blas_int bn = (blas_int)p->n;
+  blas_int bm = (blas_int)p->m;
+  blas_int one = 1;
+  blas_int info;
+  scs_float alpha, beta;
+  scs_int i;
+
+  scs_float *rx = b;
+  scs_float *ry = b + p->n;
+
+  /* tmp_m = R_y^{-1} ry */
+  for (i = 0; i < p->m; ++i) {
+    p->tmp_m[i] = p->r_y_inv[i] * ry[i];
+  }
+
+  /* rx += A' * tmp_m   (rx = rx + A' R_y^{-1} ry) */
+  {
+    char trans = 'T';
+    alpha = 1.0;
+    beta = 1.0;
+    BLAS(gemv)(&trans, &bm, &bn, &alpha, p->A_dense, &bm, p->tmp_m, &one,
+               &beta, rx, &one);
+  }
+
+  /* Solve G x = rx, result in rx (= b[:n]) */
+  {
+    char uplo = 'U';
+    BLAS(potrs)(&uplo, &bn, &one, p->G, &bn, rx, &bn, &info);
+  }
+  if (info != 0) {
+    scs_printf("Error in dense Cholesky solve (dpotrs), info = %d\n",
+               (int)info);
+    return (scs_int)info;
+  }
+
+  /* ry = A x - ry */
+  {
+    char trans = 'N';
+    alpha = 1.0;
+    beta = -1.0;
+    BLAS(gemv)(&trans, &bm, &bn, &alpha, p->A_dense, &bm, rx, &one, &beta, ry,
+               &one);
+  }
+
+  /* ry = R_y^{-1} (A x - ry) = y */
+  for (i = 0; i < p->m; ++i) {
+    ry[i] *= p->r_y_inv[i];
+  }
+
+  return 0;
+}
+
+/* Update diagonal R entries: re-form Gram matrix and re-factorize */
+scs_int scs_update_lin_sys_diag_r(ScsLinSysWork *p, const scs_float *diag_r) {
+  blas_int info, bn;
+  form_gram(p, diag_r);
+  bn = (blas_int)p->n;
+  {
+    char uplo = 'U';
+    BLAS(potrf)(&uplo, &bn, p->G, &bn, &info);
+  }
+  if (info != 0) {
+    scs_printf(
+        "Error in dense Cholesky re-factorization (dpotrf), info = %d\n",
+        (int)info);
+    return (scs_int)info;
+  }
+  return 0;
+}
+
+void scs_free_lin_sys_work(ScsLinSysWork *p) {
+  if (p) {
+    scs_free(p->A_dense);
+    scs_free(p->G);
+    scs_free(p->r_y_inv);
+    scs_free(p->tmp_m);
+    scs_free(p->S);
+    scs_free(p->diag_p);
+    scs_free(p);
+  }
+}

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

@@ -0,0 +1,31 @@
+#ifndef PRIV_H_GUARD
+#define PRIV_H_GUARD
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "glbopts.h"
+#include "linsys.h"
+#include "scs_blas.h"
+
+struct SCS_LIN_SYS_WORK {
+  scs_int n; /* number of QP variables */
+  scs_int m; /* number of QP constraints */
+
+  scs_float *A_dense; /* dense A matrix, m x n column-major */
+  scs_float *G;       /* Gram matrix R_x + P + A' R_y^{-1} A, n x n col-major */
+  scs_float *r_y_inv; /* 1 / R_y diagonal, length m */
+  scs_float *tmp_m;   /* workspace of length m */
+  scs_float *S;       /* scratch for scaled A, m x n column-major */
+
+  /* These are required for matrix updates */
+  const ScsMatrix *P; /* does *not* own this memory */
+  scs_float *diag_p;  /* Diagonal values of P */
+};
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

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

@@ -1,30 +1,14 @@
-#include "private.h"
+/* ======================== Includes / Types ======================== */
 
-const char *scs_get_lin_sys_method(void) {
-  return "sparse-direct-amd-qdldl";
-}
+#include "private.h"
 
-void scs_free_lin_sys_work(ScsLinSysWork *p) {
-  if (p) {
-    SCS(cs_spfree)(p->L);
-    SCS(cs_spfree)(p->kkt);
-    scs_free(p->diag_p);
-    scs_free(p->perm);
-    scs_free(p->Dinv);
-    scs_free(p->bp);
-    scs_free(p->diag_r_idxs);
-    scs_free(p->Lnz);
-    scs_free(p->iwork);
-    scs_free(p->etree);
-    scs_free(p->D);
-    scs_free(p->bwork);
-    scs_free(p->fwork);
-    scs_free(p);
-  }
-}
+/* ======================== LDL Factorization Internals ======================== */
 
 static scs_int _ldl_init(ScsMatrix *A, scs_int *P, scs_float **info) {
   *info = (scs_float *)scs_calloc(AMD_INFO, sizeof(scs_float));
+  if (!*info) {
+    return -1;
+  }
   return amd_order(A->n, A->p, A->i, P, (scs_float *)SCS_NULL, *info);
 }
 
@@ -36,6 +20,9 @@ static scs_int ldl_prepare(ScsLinSysWork *p) {
   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));
+  if (!p->etree || !p->Lnz || !p->iwork || !L->p) {
+    return -1;
+  }
   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");
@@ -53,6 +40,9 @@ static scs_int ldl_prepare(ScsLinSysWork *p) {
   p->D = (scs_float *)scs_calloc(n, sizeof(scs_float));
   p->bwork = (QDLDL_bool *)scs_calloc(n, sizeof(QDLDL_bool));
   p->fwork = (scs_float *)scs_calloc(n, sizeof(scs_float));
+  if (!L->x || !L->i || !p->Dinv || !p->D || !p->bwork || !p->fwork) {
+    return -1;
+  }
   return nzmax;
 }
 
@@ -104,6 +94,8 @@ static void _ldl_solve(scs_float *b, ScsMatrix *L, scs_float *Dinv, scs_int *P,
   _ldl_permt(n, b, bp, P);
 }
 
+/* ======================== KKT Matrix Permutation ======================== */
+
 static scs_int *cs_pinv(scs_int const *p, scs_int n) {
   scs_int k, *pinv;
   if (!p) {
@@ -181,6 +173,8 @@ static ScsMatrix *permute_kkt(const ScsMatrix *A, const ScsMatrix *P,
   amd_status = _ldl_init(kkt, p->perm, &info);
   if (amd_status < 0) {
     scs_printf("AMD permutatation error.\n");
+    SCS(cs_spfree)(kkt);
+    scs_free(info);
     return SCS_NULL;
   }
 #if VERBOSITY > 0
@@ -189,7 +183,20 @@ static ScsMatrix *permute_kkt(const ScsMatrix *A, const ScsMatrix *P,
 #endif
   Pinv = cs_pinv(p->perm, A->n + A->m);
   idx_mapping = (scs_int *)scs_calloc(kkt_nnz, sizeof(scs_int));
+  if (!Pinv || !idx_mapping) {
+    SCS(cs_spfree)(kkt);
+    scs_free(Pinv);
+    scs_free(info);
+    return SCS_NULL;
+  }
   kkt_perm = cs_symperm(kkt, Pinv, idx_mapping, 1);
+  if (!kkt_perm) {
+    SCS(cs_spfree)(kkt);
+    scs_free(Pinv);
+    scs_free(info);
+    scs_free(idx_mapping);
+    return SCS_NULL;
+  }
   for (i = 0; i < A->n + A->m; i++) {
     p->diag_r_idxs[i] = idx_mapping[p->diag_r_idxs[i]];
   }
@@ -200,46 +207,44 @@ static ScsMatrix *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) {
-  scs_int i, ldl_status;
-  for (i = 0; i < p->n; ++i) {
-    /* top left is R_x + P, bottom right is -R_y */
-    p->kkt->x[p->diag_r_idxs[i]] = p->diag_p[i] + diag_r[i];
-  }
-  for (i = p->n; i < p->n + p->m; ++i) {
-    /* top left is R_x + P, bottom right is -R_y */
-    p->kkt->x[p->diag_r_idxs[i]] = -diag_r[i];
-  }
-  ldl_status = ldl_factor(p, p->n);
-  if (ldl_status < 0) {
-    scs_printf("Error in LDL factorization when updating.\n");
-    /* TODO: this is broken somehow */
-    /* SCS(free_lin_sys_work)(p); */
-    return;
-  }
+/* ======================== Public API ======================== */
+
+const char *scs_get_lin_sys_method(void) {
+  return "sparse-direct-amd-qdldl";
 }
 
 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;
+  scs_int n_plus_m, ldl_status, ldl_prepare_status;
+  if (!p)
+    return SCS_NULL;
+  n_plus_m = A->n + A->m;
   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->perm = (scs_int *)scs_calloc(n_plus_m, sizeof(scs_int));
   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;
+  if (!p->diag_p || !p->perm || !p->L || !p->bp || !p->diag_r_idxs) {
+    scs_free_lin_sys_work(p);
+    return SCS_NULL;
+  }
   p->L->m = n_plus_m;
   p->L->n = n_plus_m;
   p->kkt = permute_kkt(A, P, p, diag_r);
+  if (!p->kkt) {
+    scs_free_lin_sys_work(p);
+    return SCS_NULL;
+  }
   ldl_prepare_status = ldl_prepare(p);
-  ldl_status = ldl_factor(p, A->n);
+  ldl_status =
+      ldl_prepare_status < 0 ? ldl_prepare_status : ldl_factor(p, A->n);
   if (ldl_prepare_status < 0 || ldl_status < 0) {
     scs_printf("Error in LDL initial factorization.\n");
-    /* TODO: this is broken somehow */
-    /* SCS(free_lin_sys_work)(p); */
+    scs_free_lin_sys_work(p);
     return SCS_NULL;
   }
   return p;
@@ -252,3 +257,40 @@ scs_int scs_solve_lin_sys(ScsLinSysWork *p, scs_float *b, const scs_float *s,
   _ldl_solve(b, p->L, p->Dinv, p->perm, p->bp);
   return 0;
 }
+
+scs_int 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 */
+    p->kkt->x[p->diag_r_idxs[i]] = p->diag_p[i] + diag_r[i];
+  }
+  for (i = p->n; i < p->n + p->m; ++i) {
+    /* top left is R_x + P, bottom right is -R_y */
+    p->kkt->x[p->diag_r_idxs[i]] = -diag_r[i];
+  }
+  ldl_status = ldl_factor(p, p->n);
+  if (ldl_status < 0) {
+    scs_printf("Error in LDL factorization when updating.\n");
+    return ldl_status;
+  }
+  return 0;
+}
+
+void scs_free_lin_sys_work(ScsLinSysWork *p) {
+  if (p) {
+    SCS(cs_spfree)(p->L);
+    SCS(cs_spfree)(p->kkt);
+    scs_free(p->diag_p);
+    scs_free(p->perm);
+    scs_free(p->Dinv);
+    scs_free(p->bp);
+    scs_free(p->diag_r_idxs);
+    scs_free(p->Lnz);
+    scs_free(p->iwork);
+    scs_free(p->etree);
+    scs_free(p->D);
+    scs_free(p->bwork);
+    scs_free(p->fwork);
+    scs_free(p);
+  }
+}