diff --git a/algebra/builtin/qdldl_backend.c b/algebra/builtin/qdldl_backend.c index f04b2163..d6a4ecec 100644 --- a/algebra/builtin/qdldl_backend.c +++ b/algebra/builtin/qdldl_backend.c @@ -26,7 +26,7 @@ typedef char qoco_qdldl_float_size_must_match // Contains data for linear system. struct LinSysData { - /** KKT matrix in CSC form. */ + /** KKT matrix in CSC form (size N_exp x N_exp). */ QOCOCscMatrix* K; /** Permutation vector. */ @@ -54,33 +54,60 @@ struct LinSysData { unsigned char* bwork; + /** Positive diagonal mask in the unpermuted expanded KKT ordering. */ + unsigned char* positive_diag; + QOCOFloat* fwork; - /** Buffer of size n + m + p. */ + /** Buffer of size N_exp. */ QOCOFloat* xyzbuff1; - /** Buffer of size n + m + p. */ + /** Buffer of size N_exp. */ QOCOFloat* xyzbuff2; - /** Mapping from elements in the Nesterov-Todd scaling matrix to elements in - * the KKT matrix. */ + /** Mapping from WtW entries (LP + dense SOC + sparse SOC diagonal) to KKT. */ QOCOInt* nt2kkt; - /** Mapping from elements on the main diagonal of the Nesterov-Todd scaling - * matrices to elements in the KKT matrix. Used for regularization.*/ + /** Mapping from all m diagonal entries to KKT (for regularization). */ QOCOInt* ntdiag2kkt; - /** Mapping from elements in regularized P to elements in the KKT matrix. */ + /** Mapping from regularized P to KKT. */ QOCOInt* PregtoKKT; - /** Mapping from elements in At to elements in the KKT matrix. */ + /** Mapping from At to KKT. */ QOCOInt* AttoKKT; - /** Mapping from elements in Gt to elements in the KKT matrix. */ + /** Mapping from Gt to KKT. */ QOCOInt* GttoKKT; + /** Number of entries in nt2kkt: l + dense SOC upper-tri + sparse SOC + * diagonal. */ QOCOInt Wnnz; + /** Number of sparse SOC cones. */ + QOCOInt nsoc_sparse; + + /** Per-SOC sparse flag (length nsoc). */ + QOCOInt* soc_is_sparse; + + /** Expanded system size: n + p + m + 2*nsoc_sparse. */ + QOCOInt N_exp; + + /** Total elements in u/v vectors: sum q[i] for sparse SOCs. */ + QOCOInt nt_sparse_nnz; + + /** Index into u/v arrays for each sparse SOC (length nsoc_sparse). */ + QOCOInt* sparse_soc_nt_idx; + + /** Mapping from u vector entries to KKT (length nt_sparse_nnz). */ + QOCOInt* nt_u2kkt; + + /** Mapping from v vector entries to KKT (length nt_sparse_nnz). */ + QOCOInt* nt_v2kkt; + + /** Mapping for extra 2x2 diagonal per sparse SOC (length 2*nsoc_sparse). */ + QOCOInt* nt_uvdiag2kkt; + /** Static regularization for the (1,1) P block. */ QOCOFloat kkt_static_reg_P; @@ -92,88 +119,132 @@ struct LinSysData { }; static LinSysData* qdldl_setup(QOCOProblemData* data, QOCOSettings* settings, - QOCOInt Wnnz, QOCOFloat* analysis_time_sec) + QOCOInt Wnnz, QOCOInt nsoc_sparse, + QOCOInt* soc_is_sparse, QOCOInt nt_sparse_nnz, + QOCOInt* sparse_soc_nt_idx, + QOCOFloat* analysis_time_sec) { - // Number of columns of KKT matrix. - QOCOInt Kn = data->n + data->m + data->p; + QOCOInt N = data->n + data->m + data->p; + QOCOInt N_exp = N + 2 * nsoc_sparse; + + // Compute Wnnz for QDLDL: LP + dense SOC upper-tri + sparse SOC diagonal. + QOCOInt* q = get_data_vectori(data->q); + QOCOInt Wnnz_qdldl = Wnnz; + for (QOCOInt i = 0; i < data->nsoc; ++i) { + if (soc_is_sparse && soc_is_sparse[i]) { + QOCOInt qi = q[i]; + Wnnz_qdldl -= (qi * qi - qi) / 2; + } + } LinSysData* linsys_data = malloc(sizeof(LinSysData)); - // Allocate vector buffers. - linsys_data->xyzbuff1 = qoco_malloc(sizeof(QOCOFloat) * Kn); - linsys_data->xyzbuff2 = qoco_malloc(sizeof(QOCOFloat) * Kn); - linsys_data->Wnnz = Wnnz; + linsys_data->N_exp = N_exp; + linsys_data->Wnnz = Wnnz_qdldl; + linsys_data->nsoc_sparse = nsoc_sparse; + linsys_data->nt_sparse_nnz = nt_sparse_nnz; linsys_data->kkt_static_reg_P = settings->kkt_static_reg_P; linsys_data->kkt_static_reg_A = settings->kkt_static_reg_A; linsys_data->kkt_static_reg_G = settings->kkt_static_reg_G; - // Allocate memory for QDLDL. - linsys_data->etree = qoco_malloc(sizeof(QOCOInt) * Kn); - linsys_data->Lnz = qoco_malloc(sizeof(QOCOInt) * Kn); - linsys_data->Lp = qoco_malloc(sizeof(QOCOInt) * (Kn + 1)); - linsys_data->D = qoco_malloc(sizeof(QOCOFloat) * Kn); - linsys_data->Dinv = qoco_malloc(sizeof(QOCOFloat) * Kn); - linsys_data->iwork = qoco_malloc(sizeof(QOCOInt) * 3 * Kn); - linsys_data->bwork = qoco_malloc(sizeof(unsigned char) * Kn); - linsys_data->fwork = qoco_malloc(sizeof(QOCOFloat) * Kn); - - // Allocate memory for mappings to KKT matrix. - linsys_data->nt2kkt = qoco_calloc(Wnnz, sizeof(QOCOInt)); + linsys_data->soc_is_sparse = + qoco_calloc(data->nsoc > 0 ? data->nsoc : 1, sizeof(QOCOInt)); + if (soc_is_sparse) { + for (QOCOInt i = 0; i < data->nsoc; ++i) + linsys_data->soc_is_sparse[i] = soc_is_sparse[i]; + } + + linsys_data->sparse_soc_nt_idx = + qoco_calloc(nsoc_sparse > 0 ? nsoc_sparse : 1, sizeof(QOCOInt)); + if (sparse_soc_nt_idx) { + for (QOCOInt i = 0; i < nsoc_sparse; ++i) + linsys_data->sparse_soc_nt_idx[i] = sparse_soc_nt_idx[i]; + } + + linsys_data->xyzbuff1 = qoco_malloc(sizeof(QOCOFloat) * N_exp); + linsys_data->xyzbuff2 = qoco_malloc(sizeof(QOCOFloat) * N_exp); + + linsys_data->etree = qoco_malloc(sizeof(QOCOInt) * N_exp); + linsys_data->Lnz = qoco_malloc(sizeof(QOCOInt) * N_exp); + linsys_data->Lp = qoco_malloc(sizeof(QOCOInt) * (N_exp + 1)); + linsys_data->D = qoco_malloc(sizeof(QOCOFloat) * N_exp); + linsys_data->Dinv = qoco_malloc(sizeof(QOCOFloat) * N_exp); + linsys_data->iwork = qoco_malloc(sizeof(QOCOInt) * 3 * N_exp); + linsys_data->bwork = qoco_malloc(sizeof(unsigned char) * N_exp); + linsys_data->positive_diag = qoco_calloc(N_exp, sizeof(unsigned char)); + linsys_data->fwork = qoco_malloc(sizeof(QOCOFloat) * N_exp); + for (QOCOInt i = 0; i < data->n; ++i) { + linsys_data->positive_diag[i] = 1; + } + for (QOCOInt i = 0; i < nsoc_sparse; ++i) { + QOCOInt aux_idx = N + 2 * i; + linsys_data->positive_diag[aux_idx] = 1; + } + + linsys_data->nt2kkt = qoco_calloc(Wnnz_qdldl, sizeof(QOCOInt)); linsys_data->ntdiag2kkt = qoco_calloc(data->m, sizeof(QOCOInt)); linsys_data->PregtoKKT = qoco_calloc(get_nnz(data->P), sizeof(QOCOInt)); linsys_data->AttoKKT = qoco_calloc(get_nnz(data->A), sizeof(QOCOInt)); linsys_data->GttoKKT = qoco_calloc(get_nnz(data->G), sizeof(QOCOInt)); - - QOCOInt* nt2kkt_temp = qoco_calloc(Wnnz, sizeof(QOCOInt)); + linsys_data->nt_u2kkt = + qoco_calloc(nt_sparse_nnz > 0 ? nt_sparse_nnz : 1, sizeof(QOCOInt)); + linsys_data->nt_v2kkt = + qoco_calloc(nt_sparse_nnz > 0 ? nt_sparse_nnz : 1, sizeof(QOCOInt)); + linsys_data->nt_uvdiag2kkt = + qoco_calloc(nsoc_sparse > 0 ? 2 * nsoc_sparse : 1, sizeof(QOCOInt)); + + QOCOInt* nt2kkt_temp = qoco_calloc(Wnnz_qdldl, sizeof(QOCOInt)); QOCOInt* ntdiag2kkt_temp = qoco_calloc(data->m, sizeof(QOCOInt)); QOCOInt* PregtoKKT_temp = data->P ? qoco_calloc(get_nnz(data->P), sizeof(QOCOInt)) : NULL; QOCOInt* AttoKKT_temp = qoco_calloc(get_nnz(data->A), sizeof(QOCOInt)); QOCOInt* GttoKKT_temp = qoco_calloc(get_nnz(data->G), sizeof(QOCOInt)); + QOCOInt* nt_u2kkt_temp = + qoco_calloc(nt_sparse_nnz > 0 ? nt_sparse_nnz : 1, sizeof(QOCOInt)); + QOCOInt* nt_v2kkt_temp = + qoco_calloc(nt_sparse_nnz > 0 ? nt_sparse_nnz : 1, sizeof(QOCOInt)); + QOCOInt* nt_uvdiag2kkt_temp = + qoco_calloc(nsoc_sparse > 0 ? 2 * nsoc_sparse : 1, sizeof(QOCOInt)); linsys_data->K = construct_kkt( data->P ? get_csc_matrix(data->P) : NULL, get_csc_matrix(data->A), get_csc_matrix(data->G), get_csc_matrix(data->At), get_csc_matrix(data->Gt), settings->kkt_static_reg_A, data->n, data->m, - data->p, data->l, data->nsoc, get_data_vectori(data->q), PregtoKKT_temp, - AttoKKT_temp, GttoKKT_temp, nt2kkt_temp, ntdiag2kkt_temp, Wnnz); - - // Compute AMD ordering. - linsys_data->p = qoco_malloc(linsys_data->K->n * sizeof(QOCOInt)); - linsys_data->pinv = qoco_malloc(linsys_data->K->n * sizeof(QOCOInt)); - QOCOInt amd_status = - amd_order(linsys_data->K->n, linsys_data->K->p, linsys_data->K->i, - linsys_data->p, (double*)NULL, (double*)NULL); - if (amd_status < 0) { + data->p, data->l, data->nsoc, q, PregtoKKT_temp, AttoKKT_temp, + GttoKKT_temp, nt2kkt_temp, ntdiag2kkt_temp, Wnnz_qdldl, soc_is_sparse, + nsoc_sparse, nt_sparse_nnz, sparse_soc_nt_idx, nt_u2kkt_temp, + nt_v2kkt_temp, nt_uvdiag2kkt_temp); + + linsys_data->p = qoco_malloc(N_exp * sizeof(QOCOInt)); + linsys_data->pinv = qoco_malloc(N_exp * sizeof(QOCOInt)); + QOCOInt amd_status = amd_order(N_exp, linsys_data->K->p, linsys_data->K->i, + linsys_data->p, (double*)NULL, (double*)NULL); + if (amd_status < 0) return NULL; - } - invert_permutation(linsys_data->p, linsys_data->pinv, linsys_data->K->n); + invert_permutation(linsys_data->p, linsys_data->pinv, N_exp); - // Permute KKT matrix. QOCOInt* KtoPKPt = qoco_malloc(linsys_data->K->nnz * sizeof(QOCOInt)); QOCOCscMatrix* PKPt = csc_symperm(linsys_data->K, linsys_data->pinv, KtoPKPt); - // Update mappings to permuted matrix. - for (QOCOInt i = 0; i < Wnnz; ++i) { + for (QOCOInt i = 0; i < Wnnz_qdldl; ++i) linsys_data->nt2kkt[i] = KtoPKPt[nt2kkt_temp[i]]; - } - for (QOCOInt i = 0; i < data->m; ++i) { + for (QOCOInt i = 0; i < data->m; ++i) linsys_data->ntdiag2kkt[i] = KtoPKPt[ntdiag2kkt_temp[i]]; - } - if (data->P && PregtoKKT_temp) { - for (QOCOInt i = 0; i < get_nnz(data->P); ++i) { + for (QOCOInt i = 0; i < get_nnz(data->P); ++i) linsys_data->PregtoKKT[i] = KtoPKPt[PregtoKKT_temp[i]]; - } } - - for (QOCOInt i = 0; i < get_nnz(data->A); ++i) { + for (QOCOInt i = 0; i < get_nnz(data->A); ++i) linsys_data->AttoKKT[i] = KtoPKPt[AttoKKT_temp[i]]; - } - - for (QOCOInt i = 0; i < get_nnz(data->G); ++i) { + for (QOCOInt i = 0; i < get_nnz(data->G); ++i) linsys_data->GttoKKT[i] = KtoPKPt[GttoKKT_temp[i]]; + for (QOCOInt i = 0; i < nt_sparse_nnz; ++i) { + linsys_data->nt_u2kkt[i] = KtoPKPt[nt_u2kkt_temp[i]]; + linsys_data->nt_v2kkt[i] = KtoPKPt[nt_v2kkt_temp[i]]; } + QOCOInt n_uvdiag = 2 * nsoc_sparse; + for (QOCOInt i = 0; i < n_uvdiag; ++i) + linsys_data->nt_uvdiag2kkt[i] = KtoPKPt[nt_uvdiag2kkt_temp[i]]; free_qoco_csc_matrix(linsys_data->K); qoco_free(KtoPKPt); @@ -182,21 +253,23 @@ static LinSysData* qdldl_setup(QOCOProblemData* data, QOCOSettings* settings, qoco_free(PregtoKKT_temp); qoco_free(AttoKKT_temp); qoco_free(GttoKKT_temp); + qoco_free(nt_u2kkt_temp); + qoco_free(nt_v2kkt_temp); + qoco_free(nt_uvdiag2kkt_temp); linsys_data->K = PKPt; // Compute elimination tree. QOCOTimer analysis_timer; start_timer(&analysis_timer); QOCOInt sumLnz = - QDLDL_etree(Kn, linsys_data->K->p, linsys_data->K->i, linsys_data->iwork, - linsys_data->Lnz, linsys_data->etree); + QDLDL_etree(N_exp, linsys_data->K->p, linsys_data->K->i, + linsys_data->iwork, linsys_data->Lnz, linsys_data->etree); stop_timer(&analysis_timer); if (analysis_time_sec) { *analysis_time_sec = get_elapsed_time_sec(&analysis_timer); } - if (sumLnz < 0) { + if (sumLnz < 0) return NULL; - } linsys_data->Li = qoco_malloc(sizeof(QOCOInt) * sumLnz); linsys_data->Lx = qoco_malloc(sizeof(QOCOFloat) * sumLnz); @@ -211,7 +284,7 @@ static void qdldl_factor(LinSysData* linsys_data, QOCOInt n, linsys_data->Lx, linsys_data->D, linsys_data->Dinv, linsys_data->Lnz, linsys_data->etree, linsys_data->bwork, linsys_data->iwork, linsys_data->fwork, linsys_data->p, n, - kkt_dynamic_reg); + kkt_dynamic_reg, linsys_data->positive_diag); } /** @@ -224,18 +297,18 @@ static QOCOFloat compute_linsys_residual(LinSysData* linsys_data, QOCOWorkspace* work, QOCOFloat* b, QOCOFloat* x_scratch) { + QOCOInt N = linsys_data->K->n; + QOCOInt n = work->data->n; + QOCOInt N_base = work->data->n + work->data->p + work->data->m; QOCOFloat* nt_scaling = get_data_vectorf(work->nt_scaling); QOCOInt* nt_scaling_soc_idx = get_data_vectori(work->nt_scaling_soc_idx); QOCOInt* soc_idx = get_data_vectori(work->soc_idx); QOCOFloat* xbuff = get_data_vectorf(work->xbuff); QOCOFloat* ubuff1 = get_data_vectorf(work->ubuff1); QOCOFloat* ubuff2 = get_data_vectorf(work->ubuff2); - QOCOInt n = work->data->n; - QOCOInt N = linsys_data->K->n; - // Unscramble solution from permuted space into x_scratch. - for (QOCOInt k = 0; k < N; ++k) { - x_scratch[linsys_data->p[k]] = linsys_data->xyzbuff1[k]; + for (QOCOInt i = 0; i < N; ++i) { + x_scratch[linsys_data->p[i]] = linsys_data->xyzbuff1[i]; } // Compute K_true * x_scratch -> xyzbuff2 against the unregularized matrix. @@ -243,14 +316,40 @@ static QOCOFloat compute_linsys_residual(LinSysData* linsys_data, // regularization contribution from the x block to recover the true product. kkt_multiply(x_scratch, linsys_data->xyzbuff2, work->data, nt_scaling, nt_scaling_soc_idx, soc_idx, xbuff, ubuff1, ubuff2); - for (QOCOInt k = 0; k < n; ++k) { - linsys_data->xyzbuff2[k] -= linsys_data->kkt_static_reg_P * x_scratch[k]; + for (QOCOInt i = 0; i < n; ++i) { + linsys_data->xyzbuff2[i] -= linsys_data->kkt_static_reg_P * x_scratch[i]; + } + + if (N == N_base) { + for (QOCOInt k = 0; k < N; ++k) { + x_scratch[k] = b[k] - linsys_data->xyzbuff2[linsys_data->p[k]]; + } + return inf_norm(x_scratch, N); + } + + for (QOCOInt i = 0; i < N; ++i) { + x_scratch[i] = 0.0; + } + for (QOCOInt col = 0; col < N; ++col) { + for (QOCOInt p = linsys_data->K->p[col]; p < linsys_data->K->p[col + 1]; + ++p) { + QOCOInt row = linsys_data->K->i[p]; + QOCOFloat val = linsys_data->K->x[p]; + x_scratch[row] += val * linsys_data->xyzbuff1[col]; + if (row != col) { + x_scratch[col] += val * linsys_data->xyzbuff1[row]; + } + } } - // Compute r = b_perm - P*(K_true*x). Since P is a permutation, - // norm(P*v, inf) = norm(v, inf), so this equals norm(K_true*x - b, inf). for (QOCOInt k = 0; k < N; ++k) { - x_scratch[k] = b[k] - linsys_data->xyzbuff2[linsys_data->p[k]]; + QOCOInt idx = linsys_data->p[k]; + if (idx < N_base) { + x_scratch[k] = b[k] - linsys_data->xyzbuff2[idx]; + } + else { + x_scratch[k] = b[k] - x_scratch[k]; + } } return inf_norm(x_scratch, N); @@ -272,6 +371,10 @@ static void qdldl_solve(LinSysData* linsys_data, QOCOWorkspace* work, { QOCOFloat* b = get_data_vectorf(b_vec); QOCOFloat* x = get_data_vectorf(x_vec); + QOCOInt N_base = work->data->n + work->data->p + work->data->m; + for (QOCOInt i = N_base; i < linsys_data->K->n; ++i) { + b[i] = 0.0; + } // Permute b and store in xyzbuff. for (QOCOInt i = 0; i < linsys_data->K->n; ++i) { @@ -352,8 +455,9 @@ static void qdldl_solve(LinSysData* linsys_data, QOCOWorkspace* work, } } -static void qdldl_set_nt_identity(LinSysData* linsys_data, QOCOInt m) +static void qdldl_set_nt_identity(LinSysData* linsys_data, QOCOWorkspace* work) { + QOCOInt m = work->data->m; for (QOCOInt i = 0; i < linsys_data->Wnnz; ++i) { linsys_data->K->x[linsys_data->nt2kkt[i]] = 0; } @@ -361,17 +465,70 @@ static void qdldl_set_nt_identity(LinSysData* linsys_data, QOCOInt m) for (QOCOInt i = 0; i < m; ++i) { linsys_data->K->x[linsys_data->ntdiag2kkt[i]] = -1.0; } + + for (QOCOInt i = 0; i < linsys_data->nt_sparse_nnz; ++i) { + linsys_data->K->x[linsys_data->nt_u2kkt[i]] = 0.0; + linsys_data->K->x[linsys_data->nt_v2kkt[i]] = 0.0; + } + for (QOCOInt i = 0; i < linsys_data->nsoc_sparse; ++i) { + QOCOInt uvdiag_idx = 2 * i; + linsys_data->K->x[linsys_data->nt_uvdiag2kkt[uvdiag_idx]] = 1.0; + linsys_data->K->x[linsys_data->nt_uvdiag2kkt[uvdiag_idx + 1]] = -1.0; + } } -static void qdldl_update_nt(LinSysData* linsys_data, QOCOVectorf* WtW_vec, - QOCOFloat kkt_static_reg_G, QOCOInt m) +static void qdldl_update_nt(LinSysData* linsys_data, QOCOWorkspace* work, + QOCOFloat kkt_static_reg_G) { - QOCOFloat* WtW = get_data_vectorf(WtW_vec); - for (QOCOInt i = 0; i < linsys_data->Wnnz; ++i) { - linsys_data->K->x[linsys_data->nt2kkt[i]] = -WtW[i]; + QOCOFloat* WtW = get_data_vectorf(work->WtW); + QOCOFloat* eta2 = get_data_vectorf(work->nt_eta2_sparse); + QOCOFloat* d = get_data_vectorf(work->nt_d_sparse); + QOCOFloat* u = get_data_vectorf(work->nt_u_sparse); + QOCOFloat* v = get_data_vectorf(work->nt_v_sparse); + QOCOInt* q = get_data_vectori(work->data->q); + + QOCOInt nt_src = 0; + QOCOInt nt_dst = 0; + for (QOCOInt i = 0; i < work->data->l; ++i) { + linsys_data->K->x[linsys_data->nt2kkt[nt_dst++]] = -WtW[nt_src++]; + } + + QOCOInt sp_cone = 0; + for (QOCOInt c = 0; c < work->data->nsoc; ++c) { + QOCOInt qi = q[c]; + if (work->soc_is_sparse && work->soc_is_sparse[c]) { + for (QOCOInt col = 0; col < qi; ++col) { + for (QOCOInt row = 0; row <= col; ++row) { + if (row == col) { + QOCOFloat diag = eta2[sp_cone] * (row == 0 ? d[sp_cone] : 1.0); + linsys_data->K->x[linsys_data->nt2kkt[nt_dst++]] = -diag; + } + nt_src++; + } + } + QOCOInt sidx = get_element_vectori(work->sparse_soc_nt_idx, sp_cone); + for (QOCOInt j = 0; j < qi; ++j) { + linsys_data->K->x[linsys_data->nt_u2kkt[sidx + j]] = + -eta2[sp_cone] * u[sidx + j]; + linsys_data->K->x[linsys_data->nt_v2kkt[sidx + j]] = + -eta2[sp_cone] * v[sidx + j]; + } + QOCOInt uvdiag_idx = 2 * sp_cone; + linsys_data->K->x[linsys_data->nt_uvdiag2kkt[uvdiag_idx]] = eta2[sp_cone]; + linsys_data->K->x[linsys_data->nt_uvdiag2kkt[uvdiag_idx + 1]] = + -eta2[sp_cone]; + sp_cone++; + } + else { + QOCOInt ntri = (qi * qi + qi) / 2; + for (QOCOInt j = 0; j < ntri; ++j) { + linsys_data->K->x[linsys_data->nt2kkt[nt_dst++]] = -WtW[nt_src++]; + } + } } // Regularize Nesterov-Todd block of KKT matrix. + QOCOInt m = work->data->m; for (QOCOInt i = 0; i < m; ++i) { linsys_data->K->x[linsys_data->ntdiag2kkt[i]] -= kkt_static_reg_G; } @@ -414,6 +571,7 @@ static void qdldl_cleanup(LinSysData* linsys_data) qoco_free(linsys_data->Dinv); qoco_free(linsys_data->iwork); qoco_free(linsys_data->bwork); + qoco_free(linsys_data->positive_diag); qoco_free(linsys_data->fwork); qoco_free(linsys_data->xyzbuff1); qoco_free(linsys_data->xyzbuff2); @@ -422,6 +580,11 @@ static void qdldl_cleanup(LinSysData* linsys_data) qoco_free(linsys_data->PregtoKKT); qoco_free(linsys_data->AttoKKT); qoco_free(linsys_data->GttoKKT); + qoco_free(linsys_data->soc_is_sparse); + qoco_free(linsys_data->sparse_soc_nt_idx); + qoco_free(linsys_data->nt_u2kkt); + qoco_free(linsys_data->nt_v2kkt); + qoco_free(linsys_data->nt_uvdiag2kkt); qoco_free(linsys_data); } diff --git a/algebra/cuda/cudss_backend.cu b/algebra/cuda/cudss_backend.cu index ea0df3cb..d3c3d45e 100644 --- a/algebra/cuda/cudss_backend.cu +++ b/algebra/cuda/cudss_backend.cu @@ -390,8 +390,15 @@ static void csc_to_csr_device(const QOCOCscMatrix* csc, QOCOInt** csr_row_ptr, } static LinSysData* cudss_setup(QOCOProblemData* data, QOCOSettings* settings, - QOCOInt Wnnz, QOCOFloat* analysis_time_sec) + QOCOInt Wnnz, QOCOInt nsoc_sparse, + QOCOInt* soc_is_sparse, QOCOInt nt_sparse_nnz, + QOCOInt* sparse_soc_nt_idx, + QOCOFloat* analysis_time_sec) { + (void)nsoc_sparse; + (void)soc_is_sparse; + (void)nt_sparse_nnz; + (void)sparse_soc_nt_idx; // Load CUDA libraries dynamically if (!load_cuda_libraries()) { fprintf(stderr, "Failed to load CUDA libraries\n"); @@ -447,9 +454,10 @@ static LinSysData* cudss_setup(QOCOProblemData* data, QOCOSettings* settings, QOCOCscMatrix* Kcsc = construct_kkt( get_csc_matrix(data->P), get_csc_matrix(data->A), get_csc_matrix(data->G), get_csc_matrix(data->At), get_csc_matrix(data->Gt), - settings->kkt_static_reg_A, data->n, data->m, data->p, data->l, data->nsoc, - get_data_vectori(data->q), linsys_data->PregtoKKT, linsys_data->AttoKKT, - linsys_data->GttoKKT, linsys_data->nt2kkt, linsys_data->ntdiag2kkt, Wnnz); + settings->kkt_static_reg_A, data->n, data->m, data->p, data->l, + data->nsoc, get_data_vectori(data->q), linsys_data->PregtoKKT, + linsys_data->AttoKKT, linsys_data->GttoKKT, linsys_data->nt2kkt, + linsys_data->ntdiag2kkt, Wnnz, NULL, 0, 0, NULL, NULL, NULL, NULL); set_cpu_mode(0); // Convert KKT matrix from CSC (CPU) to CSR (GPU) for cuDSS @@ -460,8 +468,8 @@ static LinSysData* cudss_setup(QOCOProblemData* data, QOCOSettings* settings, QOCOInt* h_csr_col_ind; QOCOInt* csc2csr; - csc_to_csr_device(Kcsc, &csr_row_ptr, &csr_col_ind, &csr_val, - &h_csr_row_ptr, &h_csr_col_ind, &csc2csr); + csc_to_csr_device(Kcsc, &csr_row_ptr, &csr_col_ind, &csr_val, &h_csr_row_ptr, + &h_csr_col_ind, &csc2csr); // Build nt2kktcsr and ntdiag2kktcsr mappings (CSR indices instead of CSC) QOCOInt* nt2kktcsr = NULL; @@ -610,8 +618,7 @@ static LinSysData* cudss_setup(QOCOProblemData* data, QOCOSettings* settings, __global__ void update_csr_nt_blocks_kernel(const QOCOFloat* WtW, // NT block values (on GPU) QOCOFloat* csr_val, // CSR values to update (on GPU) - const QOCOInt* nt2kktcsr, - QOCOInt Wnnz) + const QOCOInt* nt2kktcsr, QOCOInt Wnnz) { QOCOInt idx = blockIdx.x * blockDim.x + threadIdx.x; @@ -711,8 +718,7 @@ static void cudss_solve_system(LinSysData* linsys_data, const QOCOFloat* rhs, */ static QOCOFloat compute_linsys_residual(LinSysData* linsys_data, QOCOWorkspace* work, - const QOCOFloat* b, - const QOCOFloat* x, + const QOCOFloat* b, const QOCOFloat* x, QOCOFloat* residual_scratch) { QOCOFloat* nt_scaling = get_data_vectorf(work->nt_scaling); @@ -767,14 +773,13 @@ static void cudss_solve(LinSysData* linsys_data, QOCOWorkspace* work, #ifdef QOCO_LOGGING FILE* log_f = fopen("qoco_log.txt", "a"); if (log_f) { - log_linsys_error(linsys_data, work, b, x, residual, "initial solve", - log_f); + log_linsys_error(linsys_data, work, b, x, residual, "initial solve", log_f); } #endif QOCOFloat* best_sol = get_data_vectorf(work->xyzbuff1); - QOCOFloat best_res = compute_linsys_residual(linsys_data, work, b, x, - residual); + QOCOFloat best_res = + compute_linsys_residual(linsys_data, work, b, x, residual); copy_arrayf(x, best_sol, linsys_data->Kn); QOCOInt ir_count = 0; @@ -791,8 +796,8 @@ static void cudss_solve(LinSysData* linsys_data, QOCOWorkspace* work, // x_new = x_old + dx. qoco_axpy(linsys_data->d_xyz_matrix_data, x, x, 1.0, linsys_data->Kn); - QOCOFloat new_res = compute_linsys_residual(linsys_data, work, b, x, - residual); + QOCOFloat new_res = + compute_linsys_residual(linsys_data, work, b, x, residual); #ifdef QOCO_LOGGING if (log_f) { @@ -819,8 +824,9 @@ static void cudss_solve(LinSysData* linsys_data, QOCOWorkspace* work, #endif } -void cudss_set_nt_identity(LinSysData* linsys_data, QOCOInt m) +void cudss_set_nt_identity(LinSysData* linsys_data, QOCOWorkspace* work) { + QOCOInt m = work->data->m; int Wnnz = linsys_data->Wnnz; int N = max(Wnnz, m); @@ -848,9 +854,11 @@ void cudss_set_nt_identity(LinSysData* linsys_data, QOCOInt m) } } -static void cudss_update_nt(LinSysData* linsys_data, QOCOVectorf* WtW_vec, - QOCOFloat kkt_static_reg_G, QOCOInt m) +static void cudss_update_nt(LinSysData* linsys_data, QOCOWorkspace* work, + QOCOFloat kkt_static_reg_G) { + QOCOVectorf* WtW_vec = work->WtW; + QOCOInt m = work->data->m; QOCOFloat* WtW = get_data_vectorf(WtW_vec); // Update CSR matrix values on GPU directly for NT blocks if (linsys_data->Wnnz > 0 && linsys_data->d_nt2kktcsr) { diff --git a/include/cone.h b/include/cone.h index f34c6ae4..c43e639b 100644 --- a/include/cone.h +++ b/include/cone.h @@ -18,6 +18,9 @@ #ifdef __cplusplus extern "C" { #endif + +/** SOC cones with dim > this threshold use the sparse NT expansion. */ +#define SOC_SPARSE_THRESHOLD 4 #include "qoco_linalg.h" #include "qoco_utils.h" #include "structs.h" @@ -151,4 +154,4 @@ void add_e(QOCOFloat* x, QOCOFloat a, QOCOInt l, QOCOInt nsoc, QOCOVectori* q); } #endif -#endif \ No newline at end of file +#endif diff --git a/include/kkt.h b/include/kkt.h index 9c349669..08a8ab7f 100644 --- a/include/kkt.h +++ b/include/kkt.h @@ -49,14 +49,15 @@ * @param Wnnz Number of nonzeros in upper triangular portion of NT scaling. * @return QOCOCscMatrix* */ -QOCOCscMatrix* construct_kkt(QOCOCscMatrix* P, QOCOCscMatrix* A, - QOCOCscMatrix* G, QOCOCscMatrix* At, - QOCOCscMatrix* Gt, QOCOFloat kkt_static_reg_A, - QOCOInt n, QOCOInt m, QOCOInt p, QOCOInt l, - QOCOInt nsoc, QOCOInt* q, QOCOInt* PregtoKKT, - QOCOInt* AttoKKT, QOCOInt* GttoKKT, - QOCOInt* nt2kkt, QOCOInt* ntdiag2kkt, - QOCOInt Wnnz); +QOCOCscMatrix* +construct_kkt(QOCOCscMatrix* P, QOCOCscMatrix* A, QOCOCscMatrix* G, + QOCOCscMatrix* At, QOCOCscMatrix* Gt, QOCOFloat kkt_static_reg_A, + QOCOInt n, QOCOInt m, QOCOInt p, QOCOInt l, QOCOInt nsoc, + QOCOInt* q, QOCOInt* PregtoKKT, QOCOInt* AttoKKT, + QOCOInt* GttoKKT, QOCOInt* nt2kkt, QOCOInt* ntdiag2kkt, + QOCOInt Wnnz, QOCOInt* soc_is_sparse, QOCOInt nsoc_sparse, + QOCOInt nt_sparse_nnz, QOCOInt* sparse_soc_nt_idx, + QOCOInt* nt_u2kkt, QOCOInt* nt_v2kkt, QOCOInt* nt_uvdiag2kkt); /** * @brief Gets initial values for primal and dual variables such that (s,z) \in @@ -164,4 +165,4 @@ void kkt_multiply(QOCOFloat* x, QOCOFloat* y, QOCOProblemData* data, QOCOFloat* nt_scaling, QOCOInt* nt_scaling_soc_idx, QOCOInt* soc_idx, QOCOFloat* nbuff, QOCOFloat* mbuff1, QOCOFloat* mbuff2); -#endif /* #ifndef QOCO_KKT_H */ \ No newline at end of file +#endif /* #ifndef QOCO_KKT_H */ diff --git a/include/structs.h b/include/structs.h index 8e0eda49..d9ddd081 100644 --- a/include/structs.h +++ b/include/structs.h @@ -230,9 +230,13 @@ typedef struct { /** Upper triangular part of Nesterov-Todd Scaling */ QOCOVectorf* W; - /** NT scaling data used by nt_multiply(). Shared layout across backends: + /** NT scaling data used by nt_multiply(). + * + * Builtin backend layout: * LP entries: scalar scales sqrt(s_i / z_i), length l. * SOC i block: [eta, w0, w1[0], ..., w1[q_i - 2]], length q_i + 1. + * + * CUDA backend currently keeps the historical dense q_i-by-q_i SOC blocks. */ QOCOVectorf* nt_scaling; @@ -249,6 +253,31 @@ typedef struct { * block */ QOCOVectori* soc_idx; + /** Number of SOCs using sparse NT expansion (dim > SOC_SPARSE_THRESHOLD). */ + QOCOInt nsoc_sparse; + + /** Per-SOC flag: 1 if cone i uses sparse NT expansion, 0 otherwise. */ + QOCOInt* soc_is_sparse; + + /** Packed u vectors for sparse NT expansion (length nt_sparse_nnz). */ + QOCOVectorf* nt_u_sparse; + + /** Packed v vectors for sparse NT expansion (length nt_sparse_nnz). */ + QOCOVectorf* nt_v_sparse; + + /** eta^2 values for sparse SOCs (length nsoc_sparse). */ + QOCOVectorf* nt_eta2_sparse; + + /** d scalars for sparse SOCs (length nsoc_sparse). */ + QOCOVectorf* nt_d_sparse; + + /** Index into nt_u/v_sparse for the k-th sparse SOC (length nsoc_sparse). */ + QOCOVectori* sparse_soc_nt_idx; + + /** Total elements in nt_u_sparse / nt_v_sparse: sum of q[i] for sparse SOCs. + */ + QOCOInt nt_sparse_nnz; + /** Scaled variables. */ QOCOVectorf* lambda; @@ -364,10 +393,13 @@ typedef struct LinSysData LinSysData; typedef struct { const char* (*linsys_name)(); LinSysData* (*linsys_setup)(QOCOProblemData* data, QOCOSettings* settings, - QOCOInt Wnnz, QOCOFloat* analysis_time_sec); - void (*linsys_set_nt_identity)(LinSysData* linsys_data, QOCOInt m); - void (*linsys_update_nt)(LinSysData* linsys_data, QOCOVectorf* WtW_vec, - QOCOFloat kkt_static_reg_G, QOCOInt m); + QOCOInt Wnnz, QOCOInt nsoc_sparse, + QOCOInt* soc_is_sparse, QOCOInt nt_sparse_nnz, + QOCOInt* sparse_soc_nt_idx, + QOCOFloat* analysis_time_sec); + void (*linsys_set_nt_identity)(LinSysData* linsys_data, QOCOWorkspace* work); + void (*linsys_update_nt)(LinSysData* linsys_data, QOCOWorkspace* work, + QOCOFloat kkt_static_reg_G); void (*linsys_update_data)(LinSysData* linsys_data, QOCOProblemData* data); void (*linsys_factor)(LinSysData* linsys_data, QOCOInt n, QOCOFloat kkt_dynamic_reg); @@ -400,4 +432,4 @@ typedef struct { } QOCOSolver; -#endif /* #ifndef QOCO_STRUCTS_H */ \ No newline at end of file +#endif /* #ifndef QOCO_STRUCTS_H */ diff --git a/lib/qdldl/include/qdldl.h b/lib/qdldl/include/qdldl.h index 131c660f..65632b13 100644 --- a/lib/qdldl/include/qdldl.h +++ b/lib/qdldl/include/qdldl.h @@ -140,7 +140,8 @@ QDLDL_API QDLDL_int QDLDL_factor(const QDLDL_int n, const QDLDL_int* Ap, const QDLDL_int* Lnz, const QDLDL_int* etree, QDLDL_bool* bwork, QDLDL_int* iwork, QDLDL_float* fwork, QDLDL_int* perm, - QDLDL_int pos_diags, QDLDL_float dyn_reg); + QDLDL_int pos_diags, QDLDL_float dyn_reg, + const QDLDL_bool* positive_diag); /** * Solves LDL'x = b diff --git a/lib/qdldl/src/qdldl.c b/lib/qdldl/src/qdldl.c index ee45654c..04a3d089 100644 --- a/lib/qdldl/src/qdldl.c +++ b/lib/qdldl/src/qdldl.c @@ -98,7 +98,8 @@ QDLDL_int QDLDL_factor(const QDLDL_int n, const QDLDL_int* Ap, QDLDL_float* D, QDLDL_float* Dinv, const QDLDL_int* Lnz, const QDLDL_int* etree, QDLDL_bool* bwork, QDLDL_int* iwork, QDLDL_float* fwork, QDLDL_int* perm, - QDLDL_int pos_diags, QDLDL_float dyn_reg) + QDLDL_int pos_diags, QDLDL_float dyn_reg, + const QDLDL_bool* positive_diag) { QDLDL_int i, j, k, nnzY, bidx, cidx, nextIdx, nnzE, tmpIdx; @@ -138,7 +139,7 @@ QDLDL_int QDLDL_factor(const QDLDL_int n, const QDLDL_int* Ap, } // Dynamic regularization - if (perm[0] < pos_diags) { + if (positive_diag ? positive_diag[perm[0]] : perm[0] < pos_diags) { D[0] = D[0] < 1e-11 ? dyn_reg : D[0]; } else { @@ -248,7 +249,7 @@ QDLDL_int QDLDL_factor(const QDLDL_int n, const QDLDL_int* Ap, } // end for i // Dynamic regularization - if (perm[k] < pos_diags) { + if (positive_diag ? positive_diag[perm[k]] : perm[k] < pos_diags) { D[k] = D[k] < 1e-11 ? dyn_reg : D[k]; } else { diff --git a/src/cone.c b/src/cone.c index 0d8dfcb0..ebd37abd 100644 --- a/src/cone.c +++ b/src/cone.c @@ -222,7 +222,7 @@ static void nt_multiply_impl(QOCOFloat* W, QOCOInt* nt_scaling_soc_idx, (void)m; // Compute product for LP cone part of W. for (QOCOInt i = 0; i < l; ++i) { - z[i] = inverse ? (safe_div(1.0, W[i]) * x[i]) : (W[i] * x[i]); + z[i] = inverse ? safe_div(x[i], W[i]) : (W[i] * x[i]); } // Compute product for second-order cones using fast O(m) operations @@ -280,6 +280,7 @@ void compute_nt_scaling(QOCOWorkspace* work) QOCOFloat* sbar = get_data_vectorf(work->sbar); QOCOFloat* zbar = get_data_vectorf(work->zbar); QOCOFloat* lambda = get_data_vectorf(work->lambda); + // Compute Nesterov-Todd scaling for LP cone. QOCOInt idx; for (idx = 0; idx < work->data->l; ++idx) { @@ -292,9 +293,13 @@ void compute_nt_scaling(QOCOWorkspace* work) // Compute Nesterov-Todd scaling for second-order cones. QOCOInt nt_idx = idx; - QOCOInt nt_idx_fast = idx; + QOCOInt nt_idx_full = idx; + QOCOInt sp_count = 0; // index into sparse SOC arrays + for (QOCOInt i = 0; i < work->data->nsoc; ++i) { QOCOInt qi = get_element_vectori(work->data->q, i); + QOCOInt is_sparse = work->soc_is_sparse ? work->soc_is_sparse[i] : 0; + // Compute normalized vectors. Bottom of page 8 in coneprog. QOCOFloat s_scal = soc_residual2(get_pointer_vectorf(work->s, idx), qi); s_scal = qoco_sqrt(s_scal); @@ -315,75 +320,78 @@ void compute_nt_scaling(QOCOWorkspace* work) for (QOCOInt j = 1; j < qi; ++j) { sbar[j] = f * (sbar[j] - zbar[j]); } + QOCOFloat w1sq = qoco_dot(&sbar[1], &sbar[1], qi - 1); + sbar[0] = qoco_sqrt(1.0 + w1sq); // eta = sqrt(s_scal / z_scal). Eq (7) in ecos. f = qoco_sqrt(safe_div(s_scal, z_scal)); - // Store fast scaling parameters: [eta, w0, w1[0], ..., w1[qi-2]] - nt_scaling[nt_idx_fast] = f; - nt_scaling[nt_idx_fast + 1] = sbar[0]; + // Store fast scaling parameters for O(qi) W*x and W^{-1}*x operations. + nt_scaling[nt_idx_full] = f; + nt_scaling[nt_idx_full + 1] = sbar[0]; for (QOCOInt j = 1; j < qi; ++j) { - nt_scaling[nt_idx_fast + 1 + j] = sbar[j]; + nt_scaling[nt_idx_full + 1 + j] = sbar[j]; } - // Compute W (upper triangular) and Winv for the sparse KKT update. - // Also compute WtW = eta^2 * (2*w*w' - J) in upper triangular storage. - QOCOFloat finv = safe_div(1.0, f); - QOCOFloat eta2 = f * f; - - // W is given by Eq (7) in ecos, but in terms of wbar that has a block - // structure (scalar, first row/column, and bottom block) with differing - // coefficients, so it cannot be built with a single uniform outer product. - // We instead introduce v so that W = eta * (2 * v * v' - J), which has the - // same rank-1-minus-J form as WtW = eta^2 * (2 * wbar * wbar' - J). This - // lets W, Winv, and WtW share one outer-product loop, and makes Winv just a - // sign flip of the cross terms of W. v is defined as - // v0 = (wbar0 + 1) / sqrt(2 * (wbar0 + 1)), - // vj = wbar_j / sqrt(2 * (wbar0 + 1)) for j >= 1, - // i.e. wbar with its leading entry shifted by 1 and the whole vector scaled - // by 1 / sqrt(2 * (wbar0 + 1)).. - QOCOFloat fv = safe_div(1.0, qoco_sqrt(2 * (sbar[0] + 1))); - zbar[0] = fv * (sbar[0] + 1.0); - for (QOCOInt j = 1; j < qi; ++j) { - zbar[j] = fv * sbar[j]; + // Compute WtW = eta^2 * (2*w*w' - J). Sparse SOCs only need the diagonal + // entries because off-diagonals are represented by the ECOS rank-2 lift. + QOCOFloat eta2_soc = f * f; + if (is_sparse) { + WtW[nt_idx] = eta2_soc * (2.0 * sbar[0] * sbar[0] - 1.0); + for (QOCOInt j = 1; j < qi; ++j) { + QOCOInt diag_idx = nt_idx + (j * (j + 1)) / 2 + j; + WtW[diag_idx] = eta2_soc * (2.0 * sbar[j] * sbar[j] + 1.0); + } } - - QOCOInt shift = 0; - for (QOCOInt j = 0; j < qi; ++j) { - for (QOCOInt k = 0; k <= j; ++k) { - W[nt_idx + shift] = 2 * (zbar[k] * zbar[j]); - if (j != 0 && k == 0) { - Winv[nt_idx + shift] = -W[nt_idx + shift]; - } - else { - Winv[nt_idx + shift] = W[nt_idx + shift]; - } - if (j == k && j == 0) { - W[nt_idx + shift] -= 1; - Winv[nt_idx + shift] -= 1; - } - else if (j == k) { - W[nt_idx + shift] += 1; - Winv[nt_idx + shift] += 1; + else { + QOCOInt shift = 0; + for (QOCOInt j = 0; j < qi; ++j) { + for (QOCOInt k = 0; k <= j; ++k) { + QOCOFloat val = eta2_soc * 2.0 * sbar[j] * sbar[k]; + if (j == k && j == 0) { + val -= eta2_soc; + } + else if (j == k) { + val += eta2_soc; + } + WtW[nt_idx + shift] = val; + shift += 1; } - W[nt_idx + shift] *= f; - Winv[nt_idx + shift] *= finv; + } + } - QOCOFloat val = eta2 * 2.0 * sbar[j] * sbar[k]; - if (j == k && j == 0) { - val -= eta2; - } - else if (j == k) { - val += eta2; - } - WtW[nt_idx + shift] = val; - shift += 1; + // For sparse SOC cones, additionally compute u, v, d, eta² for the + // rank-2 KKT expansion. sbar is QOCO's normalized SOC NT scaling point w. + if (is_sparse) { + QOCOFloat* u_arr = get_data_vectorf(work->nt_u_sparse); + QOCOFloat* v_arr = get_data_vectorf(work->nt_v_sparse); + QOCOFloat* eta2_arr = get_data_vectorf(work->nt_eta2_sparse); + QOCOFloat* d_arr = get_data_vectorf(work->nt_d_sparse); + QOCOInt* sp_idx = get_data_vectori(work->sparse_soc_nt_idx); + + QOCOFloat wsq = sbar[0] * sbar[0] + w1sq; + QOCOFloat wsqinv = safe_div(1.0, wsq); + + d_arr[sp_count] = 0.5 * wsqinv; + eta2_arr[sp_count] = f * f; + + QOCOFloat u0 = qoco_sqrt(wsq - d_arr[sp_count]); + QOCOFloat u1 = safe_div(2.0 * sbar[0], u0); + QOCOFloat v1 = qoco_sqrt(2.0 * (2.0 + wsqinv) / (2.0 * wsq - wsqinv)); + + QOCOInt sidx = sp_idx[sp_count]; + u_arr[sidx] = u0; + v_arr[sidx] = 0.0; + for (QOCOInt k = 1; k < qi; ++k) { + u_arr[sidx + k] = u1 * sbar[k]; + v_arr[sidx + k] = v1 * sbar[k]; } + sp_count++; } idx += qi; nt_idx += (qi * qi + qi) / 2; - nt_idx_fast += qi + 1; + nt_idx_full += qi + 1; } // Compute scaled variable lambda. lambda = W * z. diff --git a/src/cone.cu b/src/cone.cu index 5b9a07b0..b25595d5 100644 --- a/src/cone.cu +++ b/src/cone.cu @@ -85,12 +85,11 @@ __device__ void soc_division(const QOCOFloat* lam, const QOCOFloat* v, } } -__global__ void set_nt_scaling_linear(QOCOFloat* W, QOCOInt nt_scaling_nnz, - QOCOInt l) +__global__ void set_Wfull_linear(QOCOFloat* W, QOCOInt Wnnzfull, QOCOInt l) { QOCOInt i = blockIdx.x * blockDim.x + threadIdx.x; - if (i < nt_scaling_nnz) { + if (i < Wnnzfull) { W[i] = 0.0; } if (i < l) { @@ -98,24 +97,21 @@ __global__ void set_nt_scaling_linear(QOCOFloat* W, QOCOInt nt_scaling_nnz, } } -__global__ void set_nt_scaling_soc(QOCOFloat* W, QOCOInt* q, - QOCOInt* nt_scaling_soc_idx, QOCOInt nsoc, - QOCOInt l) +__global__ void set_Wfull_soc(QOCOFloat* W, QOCOInt* q, QOCOInt* Wsoc_idx, + QOCOInt nsoc, QOCOInt l) { - (void)q; - (void)l; QOCOInt soc = blockIdx.x; if (soc >= nsoc) return; - // Only one thread per SOC writes the compact identity scaling - // [eta = 1, w0 = 1, w1 = 0, ...]. - if (threadIdx.x != 0) + QOCOInt dim = q[soc]; + QOCOInt k = threadIdx.x; + + if (k >= dim) return; - QOCOInt base = nt_scaling_soc_idx[soc]; - W[base] = 1.0; // eta - W[base + 1] = 1.0; // w0 + // diagonal element + W[Wsoc_idx[soc] + k * dim + k] = 1.0; } __global__ void cone_residual_stage1(const QOCOFloat* u, QOCOInt l, @@ -222,11 +218,11 @@ __global__ void bring2cone_kernel(QOCOFloat* u, QOCOInt* q, QOCOInt l, } __global__ void compute_nt_scaling_kernel(QOCOFloat* W, QOCOFloat* WtW, - QOCOFloat* nt_scaling, - QOCOFloat* Winv, QOCOFloat* s, - QOCOFloat* z, QOCOFloat* sbar, - QOCOFloat* zbar, QOCOInt l, - QOCOInt nsoc, const QOCOInt* q) + QOCOFloat* Wfull, QOCOFloat* Winv, + QOCOFloat* s, QOCOFloat* z, + QOCOFloat* sbar, QOCOFloat* zbar, + QOCOInt l, QOCOInt nsoc, + const QOCOInt* q) { QOCOInt tid = blockIdx.x * blockDim.x + threadIdx.x; @@ -237,7 +233,7 @@ __global__ void compute_nt_scaling_kernel(QOCOFloat* W, QOCOFloat* WtW, QOCOFloat w = qoco_sqrt(val); W[tid] = w; - nt_scaling[tid] = w; + Wfull[tid] = w; QOCOFloat winv = safe_div((QOCOFloat)1.0, w); Winv[tid] = winv; @@ -252,12 +248,12 @@ __global__ void compute_nt_scaling_kernel(QOCOFloat* W, QOCOFloat* WtW, /* ---- compute SOC offsets ---- */ QOCOInt idx = l; QOCOInt nt_idx = l; - QOCOInt nt_idx_fast = l; + QOCOInt nt_idx_full = l; for (QOCOInt k = 0; k < soc_id; ++k) { idx += q[k]; nt_idx += (q[k] * q[k] + q[k]) / 2; - nt_idx_fast += q[k] + 1; + nt_idx_full += q[k] * q[k]; } QOCOInt qi = q[soc_id]; @@ -287,19 +283,7 @@ __global__ void compute_nt_scaling_kernel(QOCOFloat* W, QOCOFloat* WtW, for (QOCOInt j = 1; j < qi; ++j) sbar[idx + j] = f * (sbar[idx + j] - zbar[idx + j]); - /* eta = sqrt(s_scal / z_scal) */ - QOCOFloat eta = qoco_sqrt(safe_div(s_scal, z_scal)); - QOCOFloat finv = safe_div((QOCOFloat)1.0, eta); - QOCOFloat eta2 = eta * eta; - - /* Store compact fast scaling parameters [eta, w0, w1[0], ..., w1[qi-2]]. - * sbar currently holds the wbar vector. */ - nt_scaling[nt_idx_fast] = eta; - nt_scaling[nt_idx_fast + 1] = sbar[idx + 0]; - for (QOCOInt j = 1; j < qi; ++j) - nt_scaling[nt_idx_fast + 1 + j] = sbar[idx + j]; - - /* overwrite zbar with v (needed for the sparse W / Winv blocks) */ + /* overwrite zbar with v */ f = safe_div((QOCOFloat)1.0, qoco_sqrt((QOCOFloat)2.0 * (sbar[idx + 0] + (QOCOFloat)1.0))); @@ -307,8 +291,10 @@ __global__ void compute_nt_scaling_kernel(QOCOFloat* W, QOCOFloat* WtW, for (QOCOInt j = 1; j < qi; ++j) zbar[idx + j] = f * sbar[idx + j]; - /* --- build W, Winv (sparse upper triangular) and WtW = eta^2 (2 w w' - J) - * --- */ + /* --- build W and Winv --- */ + QOCOFloat fwd = qoco_sqrt(safe_div(s_scal, z_scal)); + QOCOFloat finv = 1.0 / fwd; + QOCOInt shift = 0; for (QOCOInt j = 0; j < qi; ++j) { for (QOCOInt k = 0; k <= j; ++k) { @@ -328,68 +314,54 @@ __global__ void compute_nt_scaling_kernel(QOCOFloat* W, QOCOFloat* WtW, winv_val += (QOCOFloat)1.0; } - val *= eta; + val *= fwd; winv_val *= finv; + QOCOInt full1 = nt_idx_full + j * qi + k; + QOCOInt full2 = nt_idx_full + k * qi + j; + W[nt_idx + shift] = val; Winv[nt_idx + shift] = winv_val; - QOCOFloat wtw = eta2 * (QOCOFloat)2.0 * sbar[idx + j] * sbar[idx + k]; - if (j == k && j == 0) - wtw -= eta2; - else if (j == k) - wtw += eta2; - WtW[nt_idx + shift] = wtw; + Wfull[full1] = val; + Wfull[full2] = val; shift++; } } + + /* --- compute WtW --- */ + shift = 0; + for (QOCOInt j = 0; j < qi; ++j) { + for (QOCOInt k = 0; k <= j; ++k) { + WtW[nt_idx + shift] = qoco_dot_dev(&Wfull[nt_idx_full + j * qi], + &Wfull[nt_idx_full + k * qi], qi); + shift++; + } + } } -__global__ void nt_multiply_kernel(const QOCOFloat* W, - const QOCOInt* nt_scaling_soc_idx, +__global__ void nt_multiply_kernel(const QOCOFloat* W, const QOCOInt* Wsoc_idx, const QOCOInt* soc_idx, const QOCOFloat* x, QOCOFloat* z, QOCOInt l, QOCOInt m, - QOCOInt nsoc, const QOCOInt* q, - QOCOInt inverse) + QOCOInt nsoc, const QOCOInt* q) { - (void)m; QOCOInt i = blockIdx.x * blockDim.x + threadIdx.x; if (i >= l + nsoc) return; /* ================= LP cone ================= */ if (i < l) { - z[i] = inverse ? (safe_div((QOCOFloat)1.0, W[i]) * x[i]) : (W[i] * x[i]); + z[i] = W[i] * x[i]; return; } - /* ================= SOC cones ================= - * Fast O(q) product using the compact scaling [eta, w0, w1...] and - * equations (14) and (15) in the ECOS paper. */ + /* ================= SOC cones ================= */ QOCOInt soc = i - l; QOCOInt qi = q[soc]; - QOCOInt nt_idx = nt_scaling_soc_idx[soc]; - QOCOInt xi = soc_idx[soc]; - - QOCOFloat scale = inverse ? safe_div((QOCOFloat)1.0, W[nt_idx]) : W[nt_idx]; - QOCOFloat w0 = W[nt_idx + 1]; - const QOCOFloat* w1 = &W[nt_idx + 2]; - QOCOFloat x0 = x[xi]; - QOCOFloat zeta = qoco_dot_dev(w1, &x[xi + 1], qi - 1); - QOCOFloat w0p1_inv = safe_div((QOCOFloat)1.0, (QOCOFloat)1.0 + w0); - - if (inverse) { - z[xi] = scale * (w0 * x0 - zeta); - QOCOFloat coeff = -x0 + zeta * w0p1_inv; - for (QOCOInt j = 1; j < qi; ++j) - z[xi + j] = scale * (x[xi + j] + coeff * w1[j - 1]); - } - else { - z[xi] = scale * (w0 * x0 + zeta); - QOCOFloat coeff = x0 + zeta * w0p1_inv; - for (QOCOInt j = 1; j < qi; ++j) - z[xi + j] = scale * (x[xi + j] + coeff * w1[j - 1]); + for (QOCOInt k = 0; k < qi; ++k) { + z[soc_idx[soc] + k] = + qoco_dot_dev(&W[Wsoc_idx[soc] + k * qi], &x[soc_idx[soc]], qi); } } @@ -476,16 +448,16 @@ void set_nt_scaling_identity(QOCOVectorf* nt_scaling, QOCOInt nt_scaling_nnz, // kernel 1: zero + linear cone if (data->l > 0) { - set_nt_scaling_linear<<>>(W, nt_scaling_nnz, data->l); + set_Wfull_linear<<>>(W, nt_scaling_nnz, data->l); CUDA_CHECK(cudaGetLastError()); } // kernel 2: SOC blocks const int blocks2 = data->nsoc; if (data->nsoc > 0) { - set_nt_scaling_soc<<>>(W, get_data_vectori(data->q), - get_data_vectori(nt_scaling_soc_idx), - data->nsoc, data->l); + set_Wfull_soc<<>>(W, get_data_vectori(data->q), + get_data_vectori(nt_scaling_soc_idx), + data->nsoc, data->l); CUDA_CHECK(cudaGetLastError()); } } @@ -579,7 +551,7 @@ void nt_multiply(QOCOFloat* W, QOCOInt* nt_scaling_soc_idx, QOCOInt* soc_idx, int blocks = (l + nsoc + threads - 1) / threads; if (m > 0) { nt_multiply_kernel<<>>(W, nt_scaling_soc_idx, soc_idx, x, - z, l, m, nsoc, q, 0); + z, l, m, nsoc, q); } CUDA_CHECK(cudaGetLastError()); } @@ -588,22 +560,18 @@ void nt_multiply_inv(QOCOFloat* W, QOCOInt* nt_scaling_soc_idx, QOCOInt* soc_idx, QOCOFloat* x, QOCOFloat* z, QOCOInt l, QOCOInt m, QOCOInt nsoc, QOCOInt* q) { - int threads = 256; - int blocks = (l + nsoc + threads - 1) / threads; - if (m > 0) { - nt_multiply_kernel<<>>(W, nt_scaling_soc_idx, soc_idx, x, - z, l, m, nsoc, q, 1); - } - CUDA_CHECK(cudaGetLastError()); + // TODO: Port CUDA to the compact ECOS fast-scaling representation. The + // builtin backend implements this without storing inverse scaling data. + nt_multiply(W, nt_scaling_soc_idx, soc_idx, x, z, l, m, nsoc, q); } void compute_nt_scaling(QOCOWorkspace* work) { QOCOFloat* W = get_data_vectorf(work->W); QOCOFloat* WtW = get_data_vectorf(work->WtW); - QOCOFloat* nt_scaling = get_data_vectorf(work->nt_scaling); + QOCOFloat* Wfull = get_data_vectorf(work->nt_scaling); QOCOFloat* Winv = get_data_vectorf(work->Winv); - QOCOInt* nt_scaling_soc_idx = get_data_vectori(work->nt_scaling_soc_idx); + QOCOInt* Wsoc_idx = get_data_vectori(work->nt_scaling_soc_idx); QOCOInt* soc_idx = get_data_vectori(work->soc_idx); QOCOFloat* s = get_data_vectorf(work->s); QOCOFloat* z = get_data_vectorf(work->z); @@ -619,14 +587,14 @@ void compute_nt_scaling(QOCOWorkspace* work) QOCOInt grid = (total_threads + block - 1) / block; if (work->data->m > 0) { - compute_nt_scaling_kernel<<>>(W, WtW, nt_scaling, Winv, s, z, - sbar, zbar, l, nsoc, q); + compute_nt_scaling_kernel<<>>(W, WtW, Wfull, Winv, s, z, sbar, + zbar, l, nsoc, q); } CUDA_CHECK(cudaGetLastError()); /* ================= lambda = W * z ================= */ - nt_multiply(nt_scaling, nt_scaling_soc_idx, soc_idx, z, lambda, work->data->l, - work->data->m, work->data->nsoc, q); + nt_multiply(Wfull, Wsoc_idx, soc_idx, z, lambda, work->data->l, work->data->m, + work->data->nsoc, q); } void compute_centering(QOCOSolver* solver) diff --git a/src/kkt.c b/src/kkt.c index d119b4cc..8e795d8e 100644 --- a/src/kkt.c +++ b/src/kkt.c @@ -11,20 +11,26 @@ #include "kkt.h" #include "qoco_utils.h" -QOCOCscMatrix* construct_kkt(QOCOCscMatrix* P, QOCOCscMatrix* A, - QOCOCscMatrix* G, QOCOCscMatrix* At, - QOCOCscMatrix* Gt, QOCOFloat kkt_static_reg_A, - QOCOInt n, QOCOInt m, QOCOInt p, QOCOInt l, - QOCOInt nsoc, QOCOInt* q, QOCOInt* PregtoKKT, - QOCOInt* AttoKKT, QOCOInt* GttoKKT, - QOCOInt* nt2kkt, QOCOInt* ntdiag2kkt, QOCOInt Wnnz) +QOCOCscMatrix* +construct_kkt(QOCOCscMatrix* P, QOCOCscMatrix* A, QOCOCscMatrix* G, + QOCOCscMatrix* At, QOCOCscMatrix* Gt, QOCOFloat kkt_static_reg_A, + QOCOInt n, QOCOInt m, QOCOInt p, QOCOInt l, QOCOInt nsoc, + QOCOInt* q, QOCOInt* PregtoKKT, QOCOInt* AttoKKT, + QOCOInt* GttoKKT, QOCOInt* nt2kkt, QOCOInt* ntdiag2kkt, + QOCOInt Wnnz, QOCOInt* soc_is_sparse, QOCOInt nsoc_sparse, + QOCOInt nt_sparse_nnz, QOCOInt* sparse_soc_nt_idx, + QOCOInt* nt_u2kkt, QOCOInt* nt_v2kkt, QOCOInt* nt_uvdiag2kkt) { QOCOCscMatrix* KKT = qoco_malloc(sizeof(QOCOCscMatrix)); - KKT->m = n + m + p; - KKT->n = n + m + p; + QOCOInt N = n + m + p; + KKT->m = N + 2 * nsoc_sparse; + KKT->n = N + 2 * nsoc_sparse; QOCOInt Pnnz = P ? P->nnz : 0; - KKT->nnz = Pnnz + A->nnz + G->nnz + Wnnz + p; + // Each sparse SOC of dim q adds 2*(q+1) extra nonzeros (u and v columns with + // q entries + diagonal each). + KKT->nnz = + Pnnz + A->nnz + G->nnz + Wnnz + p + 2 * nt_sparse_nnz + 2 * nsoc_sparse; KKT->x = qoco_calloc(KKT->nnz, sizeof(QOCOFloat)); KKT->i = qoco_calloc(KKT->nnz, sizeof(QOCOInt)); @@ -48,7 +54,6 @@ QOCOCscMatrix* construct_kkt(QOCOCscMatrix* P, QOCOCscMatrix* A, } } else { - // No quadratic term; KKT->p[0..n] already zero from calloc. col = n + 1; } @@ -56,7 +61,6 @@ QOCOCscMatrix* construct_kkt(QOCOCscMatrix* P, QOCOCscMatrix* A, for (QOCOInt Atcol = 0; Atcol < At->n; ++Atcol) { QOCOInt nzadded = 0; for (QOCOInt k = At->p[Atcol]; k < At->p[Atcol + 1]; ++k) { - // If the nonzero is in row i of A then add if (AttoKKT) { AttoKKT[k] = nz; } @@ -65,8 +69,6 @@ QOCOCscMatrix* construct_kkt(QOCOCscMatrix* P, QOCOCscMatrix* A, nz += 1; nzadded += 1; } - - // Add -e * Id regularization. KKT->x[nz] = -kkt_static_reg_A; KKT->i[nz] = n + Atcol; nz += 1; @@ -75,12 +77,10 @@ QOCOCscMatrix* construct_kkt(QOCOCscMatrix* P, QOCOCscMatrix* A, col += 1; } - // Add non-negative orthant part of G^T. + // Add LP cone part of G^T (diagonal NT block). QOCOInt nz_nt = 0; QOCOInt diag = 0; for (QOCOInt Gtcol = 0; Gtcol < l; ++Gtcol) { - - // Counter for number of nonzeros from G added to this column of KKT matrix QOCOInt nzadded = 0; for (QOCOInt k = Gt->p[Gtcol]; k < Gt->p[Gtcol + 1]; ++k) { if (GttoKKT) { @@ -91,75 +91,135 @@ QOCOCscMatrix* construct_kkt(QOCOCscMatrix* P, QOCOCscMatrix* A, nz += 1; nzadded += 1; } - - // Add -Id to NT block. KKT->x[nz] = -1.0; KKT->i[nz] = n + p + Gtcol; KKT->p[col] = KKT->p[col - 1] + nzadded + 1; - - // Mapping from NT matrix entries to KKT matrix entries. - if (nt2kkt) { + if (nt2kkt) nt2kkt[nz_nt] = nz; - } - if (ntdiag2kkt) { + if (ntdiag2kkt) ntdiag2kkt[diag] = nz; - } diag++; nz_nt += 1; - nz += 1; col += 1; } - // Add second-order cone parts of G^T. + // Add SOC parts of G^T. QOCOInt idx = l; + QOCOInt sp_cone = 0; // sparse SOC counter for (QOCOInt c = 0; c < nsoc; ++c) { + QOCOInt is_sparse = soc_is_sparse ? soc_is_sparse[c] : 0; for (QOCOInt Gtcol = idx; Gtcol < idx + q[c]; ++Gtcol) { - // Loop over columns of G - - // Counter for number of nonzeros from G added to this column of KKT - // matrix QOCOInt nzadded = 0; for (QOCOInt k = Gt->p[Gtcol]; k < Gt->p[Gtcol + 1]; ++k) { - if (GttoKKT) { + if (GttoKKT) GttoKKT[k] = nz; - } KKT->x[nz] = Gt->x[k]; KKT->i[nz] = Gt->i[k]; nz += 1; nzadded += 1; } - // Add NT block. - for (QOCOInt i = idx; i < idx + q[c]; i++) { - // Only add upper triangular part. - if (i + n + p <= col - 1) { - // Add -1 if element is on main diagonal and 0 otherwise. - if (i + n + p == col - 1) { - KKT->x[nz] = -1.0; - if (ntdiag2kkt) { - ntdiag2kkt[diag] = nz; + if (is_sparse) { + // Sparse SOC: only add diagonal entry for this column. + KKT->x[nz] = -1.0; + KKT->i[nz] = n + p + Gtcol; + if (nt2kkt) + nt2kkt[nz_nt] = nz; + if (ntdiag2kkt) + ntdiag2kkt[diag] = nz; + diag++; + nz_nt += 1; + nz += 1; + nzadded += 1; + } + else { + // Dense SOC: add full upper triangular NT block. + for (QOCOInt i = idx; i < idx + q[c]; i++) { + if (i + n + p <= col - 1) { + if (i + n + p == col - 1) { + KKT->x[nz] = -1.0; + if (ntdiag2kkt) + ntdiag2kkt[diag] = nz; + diag++; } - diag++; - } - else { - KKT->x[nz] = 0.0; - } - KKT->i[nz] = n + p + i; - if (nt2kkt) { - nt2kkt[nz_nt] = nz; + else { + KKT->x[nz] = 0.0; + } + KKT->i[nz] = n + p + i; + if (nt2kkt) + nt2kkt[nz_nt] = nz; + nz_nt += 1; + nz += 1; + nzadded += 1; } - nz_nt += 1; - nz += 1; - nzadded += 1; } } KKT->p[col] = KKT->p[col - 1] + nzadded; - // Mapping from NT matrix entries to KKT matrix entries. col += 1; } + if (is_sparse) + sp_cone++; idx += q[c]; } + + // Add extra columns for sparse SOC u and v vectors (at positions + // N..N+2*nsoc_sparse-1). Reset to iterate over sparse cones in order. + sp_cone = 0; + QOCOInt cone_start = l; // z-block start index for current SOC + for (QOCOInt c = 0; c < nsoc; ++c) { + if (soc_is_sparse && soc_is_sparse[c]) { + QOCOInt qi = q[c]; + QOCOInt soc_row_start = n + p + cone_start; // KKT row of first z-element + + // u column: q off-diagonal entries + 1 diagonal. + QOCOInt nzadded = 0; + for (QOCOInt j = 0; j < qi; ++j) { + KKT->x[nz] = 0.0; // filled by update_nt: -eta2 * u[j] + KKT->i[nz] = soc_row_start + j; + if (nt_u2kkt) + nt_u2kkt[sparse_soc_nt_idx ? sparse_soc_nt_idx[sp_cone] + j : 0] = nz; + nz += 1; + nzadded += 1; + } + // Diagonal entry for u column at row N+2k. + KKT->x[nz] = 1.0; // filled by update_nt: +eta2 + KKT->i[nz] = col - 1; // current column index + QOCOInt uvdiag_idx = 2 * sp_cone; + if (nt_uvdiag2kkt) + nt_uvdiag2kkt[uvdiag_idx] = nz; + nz += 1; + nzadded += 1; + KKT->p[col] = KKT->p[col - 1] + nzadded; + col += 1; + + // v column: q off-diagonal entries + 1 diagonal. + nzadded = 0; + for (QOCOInt j = 0; j < qi; ++j) { + KKT->x[nz] = 0.0; // filled by update_nt: -eta2 * v[j] + KKT->i[nz] = soc_row_start + j; + if (nt_v2kkt) + nt_v2kkt[sparse_soc_nt_idx ? sparse_soc_nt_idx[sp_cone] + j : 0] = nz; + nz += 1; + nzadded += 1; + } + // Diagonal entry for v column at row N+2k+1. + KKT->x[nz] = -1.0; // filled by update_nt: -eta2 + KKT->i[nz] = col - 1; // current column index + if (nt_uvdiag2kkt) + nt_uvdiag2kkt[uvdiag_idx + 1] = nz; + nz += 1; + nzadded += 1; + KKT->p[col] = KKT->p[col - 1] + nzadded; + col += 1; + + sp_cone++; + } + cone_start += q[c]; + } + + KKT->p[KKT->n] = nz; + return KKT; } @@ -176,7 +236,7 @@ void initialize_ipm(QOCOSolver* solver) // iterative refinement). set_nt_scaling_identity(work->nt_scaling, work->nt_scaling_nnz, work->nt_scaling_soc_idx, data); - solver->linsys->linsys_set_nt_identity(solver->linsys_data, data->m); + solver->linsys->linsys_set_nt_identity(solver->linsys_data, work); // Needs to be set to 1.0 not 0.0 due to low tolerance stopping criteria // checks which only occur when a = 0.0. If a is set to 0.0 then the low @@ -516,4 +576,4 @@ void kkt_multiply(QOCOFloat* x, QOCOFloat* y, QOCOProblemData* data, qoco_axpy(mbuff2, &y[data->n + data->p], &y[data->n + data->p], -1.0, data->m); } -} \ No newline at end of file +} diff --git a/src/qoco_api.c b/src/qoco_api.c index d6e6ceb4..fe967479 100644 --- a/src/qoco_api.c +++ b/src/qoco_api.c @@ -120,14 +120,32 @@ QOCOInt qoco_setup(QOCOSolver* solver, QOCOInt n, QOCOInt m, QOCOInt p, } set_cpu_mode(0); - // Compute number of nonzeros in upper triangular NT scaling matrix. + // Compute number of nonzeros in upper triangular NT scaling matrix and mark + // SOC blocks that should use the sparse rank-2 expansion in the KKT. QOCOInt Wsoc_nnz = 0; + QOCOInt nsoc_sparse = 0; + QOCOInt nt_sparse_nnz = 0; + QOCOInt* soc_is_sparse = + nsoc > 0 ? (QOCOInt*)qoco_calloc(nsoc, sizeof(QOCOInt)) : NULL; + QOCOInt* sparse_soc_nt_idx = + nsoc > 0 ? (QOCOInt*)qoco_calloc(nsoc, sizeof(QOCOInt)) : NULL; for (QOCOInt i = 0; i < nsoc; ++i) { Wsoc_nnz += q[i] * q[i] - q[i]; + if (q[i] > SOC_SPARSE_THRESHOLD) { + soc_is_sparse[i] = 1; + sparse_soc_nt_idx[nsoc_sparse] = nt_sparse_nnz; + nt_sparse_nnz += q[i]; + nsoc_sparse += 1; + } } Wsoc_nnz /= 2; QOCOInt Wnnz = m + Wsoc_nnz; work->Wnnz = Wnnz; + work->nsoc_sparse = nsoc_sparse; + work->nt_sparse_nnz = nt_sparse_nnz; + work->soc_is_sparse = soc_is_sparse; + work->sparse_soc_nt_idx = new_qoco_vectori(sparse_soc_nt_idx, nsoc_sparse); + qoco_free(sparse_soc_nt_idx); QOCOInt* nt_scaling_soc_idx = NULL; QOCOInt* soc_idx = NULL; @@ -137,7 +155,11 @@ QOCOInt qoco_setup(QOCOSolver* solver, QOCOInt n, QOCOInt m, QOCOInt p, nt_scaling_soc_idx[0] = l; soc_idx[0] = l; for (QOCOInt i = 1; i < nsoc; ++i) { +#ifdef QOCO_ALGEBRA_BACKEND_CUDA + nt_scaling_soc_idx[i] = nt_scaling_soc_idx[i - 1] + q[i - 1] * q[i - 1]; +#else nt_scaling_soc_idx[i] = nt_scaling_soc_idx[i - 1] + q[i - 1] + 1; +#endif soc_idx[i] = soc_idx[i - 1] + q[i - 1]; } } @@ -150,8 +172,9 @@ QOCOInt qoco_setup(QOCOSolver* solver, QOCOInt n, QOCOInt m, QOCOInt p, // Set up linear system data. QOCOFloat analysis_time_sec = 0.0; - solver->linsys_data = solver->linsys->linsys_setup(data, solver->settings, - Wnnz, &analysis_time_sec); + solver->linsys_data = solver->linsys->linsys_setup( + data, solver->settings, Wnnz, nsoc_sparse, soc_is_sparse, nt_sparse_nnz, + get_data_vectori(work->sparse_soc_nt_idx), &analysis_time_sec); if (!solver->linsys_data) { return QOCO_SETUP_ERROR; } @@ -183,7 +206,12 @@ QOCOInt qoco_setup(QOCOSolver* solver, QOCOInt n, QOCOInt m, QOCOInt p, QOCOInt nt_scaling_nnz = data->l; set_cpu_mode(1); for (QOCOInt i = 0; i < data->nsoc; ++i) { +#ifdef QOCO_ALGEBRA_BACKEND_CUDA + QOCOInt qi = get_element_vectori(data->q, i); + nt_scaling_nnz += qi * qi; +#else nt_scaling_nnz += get_element_vectori(data->q, i) + 1; +#endif } set_cpu_mode(0); @@ -192,6 +220,10 @@ QOCOInt qoco_setup(QOCOSolver* solver, QOCOInt n, QOCOInt m, QOCOInt p, work->nt_scaling_nnz = nt_scaling_nnz; work->Winv = new_qoco_vectorf(NULL, work->Wnnz); work->WtW = new_qoco_vectorf(NULL, work->Wnnz); + work->nt_u_sparse = new_qoco_vectorf(NULL, nt_sparse_nnz); + work->nt_v_sparse = new_qoco_vectorf(NULL, nt_sparse_nnz); + work->nt_eta2_sparse = new_qoco_vectorf(NULL, nsoc_sparse); + work->nt_d_sparse = new_qoco_vectorf(NULL, nsoc_sparse); work->lambda = new_qoco_vectorf(NULL, m); // For serial implementations of compute_nt scaling, sbar/zbar only need to be @@ -207,10 +239,11 @@ QOCOInt qoco_setup(QOCOSolver* solver, QOCOInt n, QOCOInt m, QOCOInt p, work->ubuff2 = new_qoco_vectorf(NULL, m); work->ubuff3 = new_qoco_vectorf(NULL, m); work->Ds = new_qoco_vectorf(NULL, m); - work->rhs = new_qoco_vectorf(NULL, n + m + p); - work->kktres = new_qoco_vectorf(NULL, n + m + p); - work->xyz = new_qoco_vectorf(NULL, n + m + p); - work->xyzbuff1 = new_qoco_vectorf(NULL, n + m + p); + QOCOInt N_exp = n + m + p + 2 * nsoc_sparse; + work->rhs = new_qoco_vectorf(NULL, N_exp); + work->kktres = new_qoco_vectorf(NULL, N_exp); + work->xyz = new_qoco_vectorf(NULL, N_exp); + work->xyzbuff1 = new_qoco_vectorf(NULL, N_exp); // Allocate solution struct. solver->sol = qoco_malloc(sizeof(QOCOSolution)); @@ -514,9 +547,8 @@ QOCOInt qoco_solve(QOCOSolver* solver) compute_nt_scaling(work); // Update Nestrov-Todd block of KKT matrix. - solver->linsys->linsys_update_nt(solver->linsys_data, work->WtW, - solver->settings->kkt_static_reg_G, - data->m); + solver->linsys->linsys_update_nt(solver->linsys_data, work, + solver->settings->kkt_static_reg_G); // Reset IR iteration counter for this IPM step. work->ir_iters = 0; @@ -592,6 +624,12 @@ QOCOInt qoco_cleanup(QOCOSolver* solver) free_qoco_vectorf(solver->work->nt_scaling); free_qoco_vectorf(solver->work->Winv); free_qoco_vectorf(solver->work->WtW); + qoco_free(solver->work->soc_is_sparse); + free_qoco_vectorf(solver->work->nt_u_sparse); + free_qoco_vectorf(solver->work->nt_v_sparse); + free_qoco_vectorf(solver->work->nt_eta2_sparse); + free_qoco_vectorf(solver->work->nt_d_sparse); + free_qoco_vectori(solver->work->sparse_soc_nt_idx); free_qoco_vectori(solver->work->nt_scaling_soc_idx); free_qoco_vectori(solver->work->soc_idx); free_qoco_vectorf(solver->work->lambda); @@ -626,4 +664,4 @@ QOCOInt qoco_cleanup(QOCOSolver* solver) qoco_free(solver); return 1; -} \ No newline at end of file +} diff --git a/tests/simple_tests/missing_constraints_test.cpp b/tests/simple_tests/missing_constraints_test.cpp index 1898d46c..993c2abc 100644 --- a/tests/simple_tests/missing_constraints_test.cpp +++ b/tests/simple_tests/missing_constraints_test.cpp @@ -338,6 +338,52 @@ TEST(simple_socp_test, p1) free(G); } +TEST(simple_socp_test, sparse_nt_soc) +{ + QOCOInt p = 0; + QOCOInt m = 6; + QOCOInt n = 6; + QOCOInt l = 1; + QOCOInt nsoc = 1; + QOCOInt q[] = {5}; + + QOCOFloat Px[] = {1, 2, 3, 4, 5, 6}; + QOCOInt Pp[] = {0, 1, 2, 3, 4, 5, 6}; + QOCOInt Pi[] = {0, 1, 2, 3, 4, 5}; + + QOCOFloat Gx[] = {-1, -1, -1, -1, -1, -1}; + QOCOInt Gp[] = {0, 1, 2, 3, 4, 5, 6}; + QOCOInt Gi[] = {0, 1, 2, 3, 4, 5}; + + QOCOFloat c[] = {-1, -1, 0, 0, 0, 0}; + QOCOFloat h[] = {1, 2, 0, 0, 0, 0}; + + QOCOCscMatrix* P = (QOCOCscMatrix*)malloc(sizeof(QOCOCscMatrix)); + QOCOCscMatrix* G = (QOCOCscMatrix*)malloc(sizeof(QOCOCscMatrix)); + + qoco_set_csc(P, n, n, 6, Px, Pp, Pi); + qoco_set_csc(G, m, n, 6, Gx, Gp, Gi); + + QOCOSettings* settings = (QOCOSettings*)malloc(sizeof(QOCOSettings)); + set_default_settings(settings); + settings->verbose = 0; + + QOCOSolver* solver = (QOCOSolver*)malloc(sizeof(QOCOSolver)); + QOCOInt exit = + qoco_setup(solver, n, m, p, P, c, nullptr, nullptr, G, h, l, nsoc, q, + settings); + ASSERT_EQ(exit, QOCO_NO_ERROR); + ASSERT_EQ(solver->work->nsoc_sparse, 1); + + exit = qoco_solve(solver); + ASSERT_EQ(exit, QOCO_SOLVED); + + qoco_cleanup(solver); + free(settings); + free(P); + free(G); +} + TEST(simple_socp_test, p2) { QOCOInt p = 2; @@ -816,4 +862,4 @@ TEST(simple_socp_test, update_cost_matrix_test) free(P); free(A); free(G); -} \ No newline at end of file +}