-
Notifications
You must be signed in to change notification settings - Fork 346
/
pardiso_interface.c
338 lines (281 loc) · 11.1 KB
/
pardiso_interface.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
#include "pardiso_interface.h"
#if EMBEDDED != 1
#include "kkt.h"
#endif
#define MKL_INT c_int
// Single Dynamic library interface
#define MKL_INTERFACE_LP64 0x0
#define MKL_INTERFACE_ILP64 0x1
// Solver Phases
#define PARDISO_SYMBOLIC (11)
#define PARDISO_NUMERIC (22)
#define PARDISO_SOLVE (33)
#define PARDISO_CLEANUP (-1)
// Prototypes for Pardiso functions
void pardiso(void**, // pt
const c_int*, // maxfct
const c_int*, // mnum
const c_int*, // mtype
const c_int*, // phase
const c_int*, // n
const c_float*, // a
const c_int*, // ia
const c_int*, // ja
c_int*, // perm
const c_int*, //nrhs
c_int*, // iparam
const c_int*, //msglvl
c_float*, // b
c_float*, // x
c_int* // error
);
c_int mkl_set_interface_layer(c_int);
c_int mkl_get_max_threads();
// Free LDL Factorization structure
void free_linsys_solver_pardiso(pardiso_solver *s) {
if (s) {
// Free pardiso solver using internal function
s->phase = PARDISO_CLEANUP;
pardiso (s->pt, &(s->maxfct), &(s->mnum), &(s->mtype), &(s->phase),
&(s->nKKT), &(s->fdum), s->KKT_p, s->KKT_i, &(s->idum), &(s->nrhs),
s->iparm, &(s->msglvl), &(s->fdum), &(s->fdum), &(s->error));
if ( s->error != 0 ){
#ifdef PRINTING
c_eprint("Error during MKL Pardiso cleanup: %d", (int)s->error);
#endif
}
// Check each attribute of the structure and free it if it exists
if (s->KKT) csc_spfree(s->KKT);
if (s->KKT_i) c_free(s->KKT_i);
if (s->KKT_p) c_free(s->KKT_p);
if (s->bp) c_free(s->bp);
if (s->sol) c_free(s->sol);
if (s->rho_inv_vec) c_free(s->rho_inv_vec);
// These are required for matrix updates
if (s->Pdiag_idx) c_free(s->Pdiag_idx);
if (s->PtoKKT) c_free(s->PtoKKT);
if (s->AtoKKT) c_free(s->AtoKKT);
if (s->rhotoKKT) c_free(s->rhotoKKT);
c_free(s);
}
}
// Initialize factorization structure
c_int init_linsys_solver_pardiso(pardiso_solver ** sp, const OSQPMatrix * P, const OSQPMatrix * A, c_float sigma, const OSQPVectorf * rho_vec, c_int polish){
c_int i; // loop counter
c_int nnzKKT; // Number of nonzeros in KKT
// Define Variables
c_int n_plus_m; // n_plus_m dimension
c_float* rhov; //used for direct access to rho_vec data when polish=false
// Allocate private structure to store KKT factorization
pardiso_solver *s;
s = c_calloc(1, sizeof(pardiso_solver));
*sp = s;
// Size of KKT
s->n = OSQPMatrix_get_n(P);
s->m = OSQPMatrix_get_m(A);
n_plus_m = s->n + s->m;
s->nKKT = n_plus_m;
// Sigma parameter
s->sigma = sigma;
// Polishing flag
s->polish = polish;
// Link Functions
s->solve = &solve_linsys_pardiso;
s->free = &free_linsys_solver_pardiso;
s->update_matrices = &update_linsys_solver_matrices_pardiso;
s->update_rho_vec = &update_linsys_solver_rho_vec_pardiso;
// Assign type
s->type = MKL_PARDISO_SOLVER;
// Working vector
s->bp = (c_float *)c_malloc(sizeof(c_float) * n_plus_m);
// Solution vector
s->sol = (c_float *)c_malloc(sizeof(c_float) * n_plus_m);
// Parameter vector
s->rho_inv_vec = (c_float *)c_malloc(sizeof(c_float) * n_plus_m);
// Form KKT matrix
if (polish){ // Called from polish()
// Use s->rho_inv_vec for storing param2 = vec(delta)
for (i = 0; i < s->m; i++){
s->rho_inv_vec[i] = sigma;
}
s->KKT = form_KKT(OSQPMatrix_get_x(P),
OSQPMatrix_get_i(P),
OSQPMatrix_get_p(P),
OSQPMatrix_get_x(A),
OSQPMatrix_get_i(A),
OSQPMatrix_get_p(A),
OSQPMatrix_get_m(A),
OSQPMatrix_get_n(P),
1, sigma, s->rho_inv_vec, OSQP_NULL, OSQP_NULL, OSQP_NULL, OSQP_NULL, OSQP_NULL);
}
else { // Called from ADMM algorithm
// Allocate vectors of indices
s->PtoKKT = c_malloc(OSQPMatrix_get_nz(P) * sizeof(c_int));
s->AtoKKT = c_malloc(OSQPMatrix_get_nz(A) * sizeof(c_int));
s->rhotoKKT = c_malloc(OSQPMatrix_get_m(A) * sizeof(c_int));
// Use s->rho_inv_vec for storing param2 = rho_inv_vec
rhov = OSQPVectorf_data(rho_vec);
for (i = 0; i < s->m; i++){
s->rho_inv_vec[i] = 1. / rhov[i];
}
s->KKT = form_KKT(OSQPMatrix_get_x(P),
OSQPMatrix_get_i(P),
OSQPMatrix_get_p(P),
OSQPMatrix_get_x(A),
OSQPMatrix_get_i(A),
OSQPMatrix_get_p(A),
OSQPMatrix_get_m(A),
OSQPMatrix_get_n(P),
1, sigma, s->rho_inv_vec,
s->PtoKKT, s->AtoKKT,
&(s->Pdiag_idx), &(s->Pdiag_n), s->rhotoKKT);
}
// Check if matrix has been created
if (!(s->KKT)) {
#ifdef PRINTING
c_eprint("Error in forming KKT matrix");
#endif
free_linsys_solver_pardiso(s);
return OSQP_LINSYS_SOLVER_INIT_ERROR;
} else {
// Adjust indexing for Pardiso
nnzKKT = s->KKT->p[s->KKT->m];
s->KKT_i = c_malloc((nnzKKT) * sizeof(c_int));
s->KKT_p = c_malloc((s->KKT->m + 1) * sizeof(c_int));
for(i = 0; i < nnzKKT; i++){
s->KKT_i[i] = s->KKT->i[i] + 1;
}
for(i = 0; i < n_plus_m+1; i++){
s->KKT_p[i] = s->KKT->p[i] + 1;
}
}
// Set MKL interface layer (Long integers if activated)
#ifdef DLONG
mkl_set_interface_layer(MKL_INTERFACE_ILP64);
#else
mkl_set_interface_layer(MKL_INTERFACE_LP64);
#endif
// Set Pardiso variables
s->mtype = -2; // Real symmetric indefinite matrix
s->nrhs = 1; // Number of right hand sides
s->maxfct = 1; // Maximum number of numerical factorizations
s->mnum = 1; // Which factorization to use
s->msglvl = 0; // Do not print statistical information
s->error = 0; // Initialize error flag
for ( i = 0; i < 64; i++ ) {
s->iparm[i] = 0; // Setup Pardiso control parameters
s->pt[i] = 0; // Initialize the internal solver memory pointer
}
s->iparm[0] = 1; // No solver default
s->iparm[1] = 3; // Fill-in reordering from OpenMP
if (polish) {
s->iparm[5] = 1; // Write solution into b
} else {
s->iparm[5] = 0; // Do NOT write solution into b
}
/* s->iparm[7] = 2; // Max number of iterative refinement steps */
s->iparm[7] = 0; // Number of iterative refinement steps (auto, performs them only if perturbed pivots are obtained)
s->iparm[9] = 13; // Perturb the pivot elements with 1E-13
s->iparm[34] = 0; // Use Fortran-style indexing for indices
/* s->iparm[34] = 1; // Use C-style indexing for indices */
// Print number of threads
s->nthreads = mkl_get_max_threads();
// Reordering and symbolic factorization
s->phase = PARDISO_SYMBOLIC;
pardiso (s->pt, &(s->maxfct), &(s->mnum), &(s->mtype), &(s->phase),
&(s->nKKT), s->KKT->x, s->KKT_p, s->KKT_i, &(s->idum), &(s->nrhs),
s->iparm, &(s->msglvl), &(s->fdum), &(s->fdum), &(s->error));
if ( s->error != 0 ){
#ifdef PRINTING
c_eprint("Error during symbolic factorization: %d", (int)s->error);
#endif
free_linsys_solver_pardiso(s);
*sp = OSQP_NULL;
return OSQP_LINSYS_SOLVER_INIT_ERROR;
}
// Numerical factorization
s->phase = PARDISO_NUMERIC;
pardiso (s->pt, &(s->maxfct), &(s->mnum), &(s->mtype), &(s->phase),
&(s->nKKT), s->KKT->x, s->KKT_p, s->KKT_i, &(s->idum), &(s->nrhs),
s->iparm, &(s->msglvl), &(s->fdum), &(s->fdum), &(s->error));
if ( s->error ){
#ifdef PRINTING
c_eprint("Error during numerical factorization: %d", (int)s->error);
#endif
free_linsys_solver_pardiso(s);
*sp = OSQP_NULL;
return OSQP_LINSYS_SOLVER_INIT_ERROR;
}
// No error
return 0;
}
// Returns solution to linear system Ax = b with solution stored in b
c_int solve_linsys_pardiso(pardiso_solver * s, OSQPVectorf * b) {
c_int j;
c_float* bv = OSQPVectorf_data(b);
// Back substitution and iterative refinement
s->phase = PARDISO_SOLVE;
pardiso (s->pt, &(s->maxfct), &(s->mnum), &(s->mtype), &(s->phase),
&(s->nKKT), s->KKT->x, s->KKT_p, s->KKT_i, &(s->idum), &(s->nrhs),
s->iparm, &(s->msglvl), bv, s->sol, &(s->error));
if ( s->error != 0 ){
#ifdef PRINTING
c_eprint("Error during linear system solution: %d", (int)s->error);
#endif
return 1;
}
if (!(s->polish)) {
/* copy x_tilde from s->sol */
for (j = 0 ; j < s->n ; j++) {
bv[j] = s->sol[j];
}
/* compute z_tilde from b and s->sol */
for (j = 0 ; j < s->m ; j++) {
bv[j + s->n] += s->rho_inv_vec[j] * s->sol[j + s->n];
}
}
return 0;
}
// Update solver structure with new P and A
c_int update_linsys_solver_matrices_pardiso(pardiso_solver * s, const OSQPMatrix *P, const OSQPMatrix *A) {
// Update KKT matrix with new P
update_KKT_P(s->KKT,
OSQPMatrix_get_x(P),
OSQPMatrix_get_i(P),
OSQPMatrix_get_p(P),
OSQPMatrix_get_m(A),
OSQPMatrix_get_n(P),
s->PtoKKT, s->sigma, s->Pdiag_idx, s->Pdiag_n);
// Update KKT matrix with new A
update_KKT_A(s->KKT,
OSQPMatrix_get_x(A),
OSQPMatrix_get_i(A),
OSQPMatrix_get_p(A),
OSQPMatrix_get_m(A),
OSQPMatrix_get_n(P),
s->AtoKKT);
// Perform numerical factorization
s->phase = PARDISO_NUMERIC;
pardiso (s->pt, &(s->maxfct), &(s->mnum), &(s->mtype), &(s->phase),
&(s->nKKT), s->KKT->x, s->KKT_p, s->KKT_i, &(s->idum), &(s->nrhs),
s->iparm, &(s->msglvl), &(s->fdum), &(s->fdum), &(s->error));
// Return exit flag
return s->error;
}
c_int update_linsys_solver_rho_vec_pardiso(pardiso_solver * s,const OSQPVectorf * rho){
c_int i;
c_float* rho_vec = OSQPVectorf_data(rho);
// Update internal rho_inv_vec
for (i = 0; i < s->m; i++){
s->rho_inv_vec[i] = 1. / rho_vec[i];
}
// Update KKT matrix with new rho_vec
update_KKT_param2(s->KKT, s->rho_inv_vec, s->rhotoKKT, s->m);
// Perform numerical factorization
s->phase = PARDISO_NUMERIC;
pardiso (s->pt, &(s->maxfct), &(s->mnum), &(s->mtype), &(s->phase),
&(s->nKKT), s->KKT->x, s->KKT_p, s->KKT_i, &(s->idum), &(s->nrhs),
s->iparm, &(s->msglvl), &(s->fdum), &(s->fdum), &(s->error));
// Return exit flag
return s->error;
}