00001
00002
00003
00019
00020
00021
00022
00023
00024
00025
00026 #ifdef MOSEK
00027 #include <stdio.h>
00028 #include <assert.h>
00029 #include "defs.h"
00030 #include "mosek_quad_solve.h"
00031 #include "quad_prog_vpsc.h"
00032
00033
00034
00035
00036 static FILE *logfile;
00037 static void MSKAPI printstr(void *handle, char str[])
00038 {
00039 fprintf(logfile, "%s", str);
00040 }
00041
00042 #define INIT_sub_val(a,b) \
00043 MSKidxt subi[2]; \
00044 double vali[2]; \
00045 subi[0] = a; \
00046 subi[1] = b; \
00047 vali[0] = 1.0; \
00048 vali[1] = -1.0;
00049
00050 #define INIT_sub_val3(a,b,c) \
00051 MSKidxt subi[3]; \
00052 double vali[3]; \
00053 subi[0] = a; \
00054 subi[1] = b; \
00055 subi[2] = c; \
00056 vali[0] = 1.0; \
00057 vali[1] = -2.0; \
00058 vali[2] = 1.0;
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074 MosekEnv *mosek_init_hier(float *lap, int n, int *ordering,
00075 int *level_indexes, int num_divisions,
00076 float separation)
00077 {
00078 int count = 0;
00079 int i, j, num_levels = num_divisions + 1;
00080 int num_constraints;
00081 MosekEnv *mskEnv = GNEW(MosekEnv);
00082 DigColaLevel *levels;
00083 int nonzero_lapsize = (n * (n - 1)) / 2;
00084
00085
00086
00087
00088 mskEnv->num_variables = n + num_divisions + 1;
00089
00090 logfile = fopen("quad_solve_log", "w");
00091 levels = assign_digcola_levels(ordering, n, level_indexes, num_divisions);
00092 #ifdef DUMP_CONSTRAINTS
00093 print_digcola_levels(logfile, levels, num_levels);
00094 #endif
00095
00096
00097
00098 mskEnv->qval = N_GNEW(nonzero_lapsize, double);
00099 mskEnv->qsubi = N_GNEW(nonzero_lapsize, int);
00100 mskEnv->qsubj = N_GNEW(nonzero_lapsize, int);
00101
00102
00103 mskEnv->xx = N_GNEW(mskEnv->num_variables, double);
00104
00105
00106 separation /= 2.0;
00107 num_constraints = get_num_digcola_constraints(levels,
00108 num_levels) + num_divisions + 1;
00109
00110
00111
00112 #ifdef EQUAL_WIDTH_LEVELS
00113 num_constraints += num_divisions;
00114 #endif
00115
00116
00117 for (i = 0; i < n - 1; i++) {
00118 for (j = i; j < n - 1; j++) {
00119 mskEnv->qval[count] = -2 * lap[count + n];
00120 assert(mskEnv->qval[count] != 0);
00121 mskEnv->qsubi[count] = j;
00122 mskEnv->qsubj[count] = i;
00123 count++;
00124 }
00125 }
00126 #ifdef DUMP_CONSTRAINTS
00127 fprintf(logfile, "Q=[");
00128 int lapcntr = n;
00129 for (i = 0; i < mskEnv->num_variables; i++) {
00130 if (i != 0)
00131 fprintf(logfile, ";");
00132 for (j = 0; j < mskEnv->num_variables; j++) {
00133 if (j < i || i >= n - 1 || j >= n - 1) {
00134 fprintf(logfile, "0 ");
00135 } else {
00136 fprintf(logfile, "%f ", -2 * lap[lapcntr++]);
00137 }
00138 }
00139 }
00140 fprintf(logfile, "]\nQ=Q-diag(diag(Q))+Q'\n");
00141 #endif
00142 fprintf(logfile, "\n");
00143
00144 mskEnv->r = MSK_makeenv(&mskEnv->env, NULL, NULL, NULL, NULL);
00145
00146
00147 if (mskEnv->r == MSK_RES_OK) {
00148
00149
00150
00151 MSK_linkfunctoenvstream(mskEnv->env, MSK_STREAM_LOG, NULL,
00152 printstr);
00153 }
00154
00155
00156 mskEnv->r = MSK_initenv(mskEnv->env);
00157 if (mskEnv->r == MSK_RES_OK) {
00158
00159 mskEnv->r =
00160 MSK_maketask(mskEnv->env, num_constraints,
00161 mskEnv->num_variables, &mskEnv->task);
00162
00163 if (mskEnv->r == MSK_RES_OK) {
00164 int c_ind = 0;
00165 int c_var = n - 1;
00166 mskEnv->r =
00167 MSK_linkfunctotaskstream(mskEnv->task, MSK_STREAM_LOG,
00168 NULL, printstr);
00169
00170 if (mskEnv->r == MSK_RES_OK)
00171 mskEnv->r = MSK_resizetask(mskEnv->task, num_constraints, mskEnv->num_variables, 0,
00172
00173 2 * num_constraints +
00174 num_divisions, nonzero_lapsize);
00175
00176
00177 if (mskEnv->r == MSK_RES_OK)
00178 mskEnv->r = MSK_append(mskEnv->task, 1, num_constraints);
00179
00180
00181 if (mskEnv->r == MSK_RES_OK)
00182 mskEnv->r =
00183 MSK_append(mskEnv->task, 0, mskEnv->num_variables);
00184
00185 for (j = 0;
00186 j < mskEnv->num_variables && mskEnv->r == MSK_RES_OK; ++j)
00187 mskEnv->r =
00188 MSK_putbound(mskEnv->task, 0, j, MSK_BK_RA,
00189 -MSK_INFINITY, MSK_INFINITY);
00190 for (j = 0; j < levels[0].num_nodes && mskEnv->r == MSK_RES_OK;
00191 j++) {
00192 int node = levels[0].nodes[j] - 1;
00193 if (node >= 0) {
00194 INIT_sub_val(c_var,node);
00195 mskEnv->r =
00196 MSK_putavec(mskEnv->task, 1, c_ind, 2, subi, vali);
00197 } else {
00198
00199 mskEnv->r =
00200 MSK_putaij(mskEnv->task, c_ind, c_var, 1.0);
00201 }
00202 mskEnv->r =
00203 MSK_putbound(mskEnv->task, 1, c_ind, MSK_BK_LO,
00204 separation, MSK_INFINITY);
00205 c_ind++;
00206 }
00207 for (i = 0; i < num_divisions && mskEnv->r == MSK_RES_OK; i++) {
00208 c_var = n + i;
00209 for (j = 0;
00210 j < levels[i].num_nodes && mskEnv->r == MSK_RES_OK;
00211 j++) {
00212
00213 int node = levels[i].nodes[j] - 1;
00214 if (node >= 0) {
00215 INIT_sub_val(node,c_var);
00216 mskEnv->r =
00217 MSK_putavec(mskEnv->task, 1, c_ind, 2, subi,
00218 vali);
00219 } else {
00220
00221 mskEnv->r =
00222 MSK_putaij(mskEnv->task, c_ind, c_var, -1.0);
00223 }
00224 mskEnv->r =
00225 MSK_putbound(mskEnv->task, 1, c_ind, MSK_BK_LO,
00226 separation, MSK_INFINITY);
00227 c_ind++;
00228 }
00229 for (j = 0;
00230 j < levels[i + 1].num_nodes
00231 && mskEnv->r == MSK_RES_OK; j++) {
00232 int node = levels[i + 1].nodes[j] - 1;
00233 if (node >= 0) {
00234 INIT_sub_val(c_var,node);
00235 mskEnv->r =
00236 MSK_putavec(mskEnv->task, 1, c_ind, 2, subi,
00237 vali);
00238 } else {
00239
00240 mskEnv->r =
00241 MSK_putaij(mskEnv->task, c_ind, c_var, 1.0);
00242 }
00243 mskEnv->r =
00244 MSK_putbound(mskEnv->task, 1, c_ind, MSK_BK_LO,
00245 separation, MSK_INFINITY);
00246 c_ind++;
00247 }
00248 }
00249 c_var = n + i;
00250 for (j = 0; j < levels[i].num_nodes && mskEnv->r == MSK_RES_OK;
00251 j++) {
00252
00253 int node = levels[i].nodes[j] - 1;
00254 if (node >= 0) {
00255 INIT_sub_val(node,c_var);
00256 mskEnv->r =
00257 MSK_putavec(mskEnv->task, 1, c_ind, 2, subi, vali);
00258 } else {
00259
00260 mskEnv->r =
00261 MSK_putaij(mskEnv->task, c_ind, c_var, -1.0);
00262 }
00263 mskEnv->r =
00264 MSK_putbound(mskEnv->task, 1, c_ind, MSK_BK_LO,
00265 separation, MSK_INFINITY);
00266 c_ind++;
00267 }
00268
00269 for (i = 0; i < num_divisions + 1 && mskEnv->r == MSK_RES_OK;
00270 i++) {
00271 int c_var = n - 1 + i, c_var2 = c_var + 1;
00272 INIT_sub_val(c_var,c_var2);
00273 mskEnv->r =
00274 MSK_putavec(mskEnv->task, 1, c_ind, 2, subi, vali);
00275 mskEnv->r =
00276 MSK_putbound(mskEnv->task, 1, c_ind, MSK_BK_LO, 0,
00277 MSK_INFINITY);
00278 c_ind++;
00279 }
00280 #ifdef EQUAL_WIDTH_LEVELS
00281 for (i = 1; i < num_divisions + 1 && mskEnv->r == MSK_RES_OK;
00282 i++) {
00283 int c_var = n - 1 + i, c_var_lo = c_var - 1, c_var_hi =
00284 c_var + 1;
00285 INIT_sub_val3(c_var_lo, c_var, c_var_h);
00286 mskEnv->r =
00287 MSK_putavec(mskEnv->task, 1, c_ind, 3, subi, vali);
00288 mskEnv->r =
00289 MSK_putbound(mskEnv->task, 1, c_ind, MSK_BK_FX, 0, 0);
00290 c_ind++;
00291 }
00292 #endif
00293 assert(c_ind == num_constraints);
00294 #ifdef DUMP_CONSTRAINTS
00295 fprintf(logfile, "A=[");
00296 for (i = 0; i < num_constraints; i++) {
00297 if (i != 0)
00298 fprintf(logfile, ";");
00299 for (j = 0; j < mskEnv->num_variables; j++) {
00300 double aij;
00301 MSK_getaij(mskEnv->task, i, j, &aij);
00302 fprintf(logfile, "%f ", aij);
00303 }
00304 }
00305 fprintf(logfile, "]\n");
00306 fprintf(logfile, "b=[");
00307 for (i = 0; i < num_constraints; i++) {
00308 fprintf(logfile, "%f ", separation);
00309 }
00310 fprintf(logfile, "]\n");
00311 #endif
00312 if (mskEnv->r == MSK_RES_OK) {
00313
00314
00315
00316
00317 mskEnv->r =
00318 MSK_putqobj(mskEnv->task, nonzero_lapsize,
00319 mskEnv->qsubi, mskEnv->qsubj,
00320 mskEnv->qval);
00321 }
00322 }
00323 }
00324 delete_digcola_levels(levels, num_levels);
00325 return mskEnv;
00326 }
00327
00328
00329
00330
00331
00332
00333
00334
00335 void mosek_quad_solve_hier(MosekEnv * mskEnv, float *b, int n,
00336 float *coords, float *hierarchy_boundaries)
00337 {
00338 int i, j;
00339 for (i = 1; i < n && mskEnv->r == MSK_RES_OK; i++) {
00340 mskEnv->r = MSK_putcj(mskEnv->task, i - 1, -2 * b[i]);
00341 }
00342 #ifdef DUMP_CONSTRAINTS
00343 fprintf(logfile, "x0=[");
00344 for (j = 0; j < mskEnv->num_variables; j++) {
00345 fprintf(logfile, "%f ", j < n ? b[j] : 0);
00346 }
00347 fprintf(logfile, "]\n");
00348 fprintf(logfile, "f=[");
00349 double *c = N_GNEW(mskEnv->num_variables, double);
00350 MSK_getc(mskEnv->task, c);
00351 for (j = 0; j < mskEnv->num_variables; j++) {
00352 fprintf(logfile, "%f ", c[j]);
00353 }
00354 free(c);
00355 fprintf(logfile, "]\n");
00356 #endif
00357 if (mskEnv->r == MSK_RES_OK)
00358 mskEnv->r = MSK_optimize(mskEnv->task);
00359
00360 if (mskEnv->r == MSK_RES_OK) {
00361 MSK_getsolutionslice(mskEnv->task,
00362 MSK_SOL_ITR,
00363 MSK_SOL_ITEM_XX,
00364 0, mskEnv->num_variables, mskEnv->xx);
00365
00366 #ifdef DUMP_CONSTRAINTS
00367 fprintf(logfile, "Primal solution\n");
00368 #endif
00369 coords[0] = 0;
00370 for (j = 0; j < mskEnv->num_variables; ++j) {
00371 #ifdef DUMP_CONSTRAINTS
00372 fprintf(logfile, "x[%d]: %.2f\n", j, mskEnv->xx[j]);
00373 #endif
00374 if (j < n - 1) {
00375 coords[j + 1] = -mskEnv->xx[j];
00376 } else if (j >= n && j < mskEnv->num_variables - 1) {
00377 hierarchy_boundaries[j - n] = -mskEnv->xx[j];
00378 }
00379 }
00380 }
00381 fprintf(logfile, "Return code: %d\n", mskEnv->r);
00382 }
00383
00384
00385
00386
00387
00388
00389
00390 MosekEnv *mosek_init_sep(float *lap, int num_variables, int num_dummy_vars,
00391 Constraint ** cs, int num_constraints)
00392 {
00393 int i, j;
00394 MosekEnv *mskEnv = GNEW(MosekEnv);
00395 int count = 0;
00396 int nonzero_lapsize = num_variables * (num_variables - 1) / 2;
00397
00398 mskEnv->num_variables = num_variables + num_dummy_vars - 1;
00399
00400 fprintf(stderr, "MOSEK!\n");
00401 logfile = fopen("quad_solve_log", "w");
00402
00403
00404 mskEnv->qval = N_GNEW(nonzero_lapsize, double);
00405 mskEnv->qsubi = N_GNEW(nonzero_lapsize, int);
00406 mskEnv->qsubj = N_GNEW(nonzero_lapsize, int);
00407
00408
00409 mskEnv->xx = N_GNEW(mskEnv->num_variables, double);
00410
00411
00412
00413 for (i = 0; i < num_variables - 1; i++) {
00414 for (j = i; j < num_variables - 1; j++) {
00415 mskEnv->qval[count] = -2 * lap[count + num_variables];
00416
00417 mskEnv->qsubi[count] = j;
00418 mskEnv->qsubj[count] = i;
00419 count++;
00420 }
00421 }
00422 #ifdef DUMP_CONSTRAINTS
00423 fprintf(logfile, "Q=[");
00424 count = 0;
00425 for (i = 0; i < num_variables - 1; i++) {
00426 if (i != 0)
00427 fprintf(logfile, ";");
00428 for (j = 0; j < num_variables - 1; j++) {
00429 if (j < i) {
00430 fprintf(logfile, "0 ");
00431 } else {
00432 fprintf(logfile, "%f ", -2 * lap[num_variables + count++]);
00433 }
00434 }
00435 }
00436 fprintf(logfile, "]\nQ=Q-diag(diag(Q))+Q'\n");
00437 #endif
00438
00439 mskEnv->r = MSK_makeenv(&mskEnv->env, NULL, NULL, NULL, NULL);
00440
00441
00442 if (mskEnv->r == MSK_RES_OK) {
00443
00444
00445 MSK_linkfunctoenvstream(mskEnv->env, MSK_STREAM_LOG, NULL,
00446 printstr);
00447 }
00448
00449
00450 mskEnv->r = MSK_initenv(mskEnv->env);
00451 if (mskEnv->r == MSK_RES_OK) {
00452
00453 mskEnv->r =
00454 MSK_maketask(mskEnv->env, num_constraints,
00455 mskEnv->num_variables, &mskEnv->task);
00456
00457 if (mskEnv->r == MSK_RES_OK) {
00458 mskEnv->r =
00459 MSK_linkfunctotaskstream(mskEnv->task, MSK_STREAM_LOG,
00460 NULL, printstr);
00461
00462 if (mskEnv->r == MSK_RES_OK)
00463 mskEnv->r = MSK_resizetask(mskEnv->task, num_constraints, mskEnv->num_variables, 0,
00464
00465
00466
00467 2 * num_constraints,
00468 nonzero_lapsize);
00469
00470
00471 if (mskEnv->r == MSK_RES_OK)
00472 mskEnv->r = MSK_append(mskEnv->task, 1, num_constraints);
00473
00474
00475 if (mskEnv->r == MSK_RES_OK)
00476 mskEnv->r =
00477 MSK_append(mskEnv->task, 0, mskEnv->num_variables);
00478
00479 for (j = 0;
00480 j < mskEnv->num_variables && mskEnv->r == MSK_RES_OK; j++)
00481 mskEnv->r =
00482 MSK_putbound(mskEnv->task, 0, j, MSK_BK_RA,
00483 -MSK_INFINITY, MSK_INFINITY);
00484 for (i = 0; i < num_constraints; i++) {
00485 int u = getLeftVarID(cs[i]) - 1;
00486 int v = getRightVarID(cs[i]) - 1;
00487 double separation = getSeparation(cs[i]);
00488 if (u < 0) {
00489 mskEnv->r =
00490 MSK_putbound(mskEnv->task, 0, v, MSK_BK_RA,
00491 -MSK_INFINITY, -separation);
00492 assert(mskEnv->r == MSK_RES_OK);
00493 } else if (v < 0) {
00494 mskEnv->r =
00495 MSK_putbound(mskEnv->task, 0, u, MSK_BK_RA,
00496 separation, MSK_INFINITY);
00497 assert(mskEnv->r == MSK_RES_OK);
00498 } else {
00499
00500 INIT_sub_val(u,v);
00501 mskEnv->r =
00502 MSK_putavec(mskEnv->task, 1, i, 2, subi, vali);
00503 assert(mskEnv->r == MSK_RES_OK);
00504 mskEnv->r =
00505 MSK_putbound(mskEnv->task, 1, i, MSK_BK_LO,
00506 separation, MSK_INFINITY);
00507 assert(mskEnv->r == MSK_RES_OK);
00508 }
00509 }
00510 if (mskEnv->r == MSK_RES_OK) {
00511
00512
00513
00514
00515 mskEnv->r =
00516 MSK_putqobj(mskEnv->task, nonzero_lapsize,
00517 mskEnv->qsubi, mskEnv->qsubj,
00518 mskEnv->qval);
00519 assert(mskEnv->r == MSK_RES_OK);
00520 }
00521 }
00522 }
00523 return mskEnv;
00524 }
00525
00526
00527
00528
00529
00530
00531
00532 void mosek_quad_solve_sep(MosekEnv * mskEnv, int n, float *b,
00533 float *coords)
00534 {
00535 int i, j;
00536 assert(n <= mskEnv->num_variables + 1);
00537 for (i = 0; i < n - 1 && mskEnv->r == MSK_RES_OK; i++) {
00538 mskEnv->r = MSK_putcj(mskEnv->task, i, -2 * b[i + 1]);
00539 }
00540 if (mskEnv->r == MSK_RES_OK)
00541 mskEnv->r = MSK_optimize(mskEnv->task);
00542
00543 if (mskEnv->r == MSK_RES_OK) {
00544 MSK_getsolutionslice(mskEnv->task,
00545 MSK_SOL_ITR,
00546 MSK_SOL_ITEM_XX,
00547 0, mskEnv->num_variables, mskEnv->xx);
00548
00549 #ifdef DUMP_CONSTRAINTS
00550 fprintf(logfile, "Primal solution\n");
00551 #endif
00552 coords[0] = 0;
00553 for (j = 1; j <= n; j++) {
00554 #ifdef DUMP_CONSTRAINTS
00555 fprintf(logfile, "x[%d]: %.2f\n", j, mskEnv->xx[j - 1]);
00556 #endif
00557 coords[j] = -mskEnv->xx[j - 1];
00558 }
00559 }
00560 fprintf(logfile, "Return code: %d\n", mskEnv->r);
00561 }
00562
00563
00564
00565
00566 void mosek_delete(MosekEnv * mskEnv)
00567 {
00568 MSK_deletetask(&mskEnv->task);
00569 MSK_deleteenv(&mskEnv->env);
00570
00571 if (logfile) {
00572 fclose(logfile);
00573 logfile = NULL;
00574 }
00575 free(mskEnv->qval);
00576 free(mskEnv->qsubi);
00577 free(mskEnv->qsubj);
00578 free(mskEnv->xx);
00579 free(mskEnv);
00580 }
00581 #endif