00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00036 #include <qpOASES/QProblem.hpp>
00037
00038
00039 BEGIN_NAMESPACE_QPOASES
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050 QProblem::QProblem( ) : QProblemB( )
00051 {
00052 freeConstraintMatrix = BT_FALSE;
00053 A = 0;
00054
00055 lbA = 0;
00056 ubA = 0;
00057
00058 sizeT = 0;
00059 T = 0;
00060 Q = 0;
00061
00062 Ax = 0;
00063 Ax_l = 0;
00064 Ax_u = 0;
00065
00066 constraintProduct = 0;
00067
00068 tempA = 0;
00069 ZFR_delta_xFRz = 0;
00070 delta_xFRy = 0;
00071 delta_xFRz = 0;
00072 tempB = 0;
00073 delta_yAC_TMP = 0;
00074 }
00075
00076
00077
00078
00079
00080 QProblem::QProblem( int _nV, int _nC, HessianType _hessianType ) : QProblemB( _nV,_hessianType )
00081 {
00082
00083 if ( _nV <= 0 )
00084 _nV = 1;
00085
00086 if ( _nC < 0 )
00087 {
00088 _nC = 0;
00089 THROWERROR( RET_INVALID_ARGUMENTS );
00090 }
00091
00092 if ( _nC > 0 )
00093 {
00094 freeConstraintMatrix = BT_FALSE;
00095 A = 0;
00096
00097 lbA = new real_t[_nC];
00098 ubA = new real_t[_nC];
00099 }
00100 else
00101 {
00102
00103
00104 freeConstraintMatrix = BT_TRUE;
00105 A = new DenseMatrix( );
00106
00107 lbA = 0;
00108 ubA = 0;
00109 }
00110
00111 constraints.init( _nC );
00112
00113 delete[] y;
00114 y = new real_t[_nV+_nC];
00115
00116 sizeT = (int)getMin( _nC,_nV );
00117 T = new real_t[sizeT*sizeT];
00118 Q = new real_t[_nV*_nV];
00119
00120 if ( _nC > 0 )
00121 {
00122 Ax = new real_t[_nC];
00123 Ax_l = new real_t[_nC];
00124 Ax_u = new real_t[_nC];
00125 }
00126 else
00127 {
00128 Ax = 0;
00129 Ax_l = 0;
00130 Ax_u = 0;
00131 }
00132
00133 constraintProduct = 0;
00134
00135 tempA = new real_t[_nV];
00136 ZFR_delta_xFRz = new real_t[_nV];
00137 delta_xFRz = new real_t[_nV];
00138
00139 if ( _nC > 0 )
00140 {
00141 tempB = new real_t[_nC];
00142 delta_xFRy = new real_t[_nC];
00143 delta_yAC_TMP = new real_t[_nC];
00144 }
00145 else
00146 {
00147 tempB = 0;
00148 delta_xFRy = 0;
00149 delta_yAC_TMP = 0;
00150 }
00151
00152 flipper.init( _nV,_nC );
00153 }
00154
00155
00156
00157
00158
00159 QProblem::QProblem( const QProblem& rhs ) : QProblemB( rhs )
00160 {
00161 freeConstraintMatrix = BT_FALSE;
00162 A = 0;
00163
00164 copy( rhs );
00165 }
00166
00167
00168
00169
00170
00171 QProblem::~QProblem( )
00172 {
00173 clear( );
00174 }
00175
00176
00177
00178
00179
00180 QProblem& QProblem::operator=( const QProblem& rhs )
00181 {
00182 if ( this != &rhs )
00183 {
00184 clear( );
00185 QProblemB::operator=( rhs );
00186 copy( rhs );
00187 }
00188
00189 return *this;
00190 }
00191
00192
00193
00194
00195
00196 returnValue QProblem::reset( )
00197 {
00198 int i;
00199 int nV = getNV( );
00200 int nC = getNC( );
00201
00202 if ( nV == 0 )
00203 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00204
00205
00206
00207 if ( QProblemB::reset( ) != SUCCESSFUL_RETURN )
00208 return THROWERROR( RET_RESET_FAILED );
00209
00210
00211 constraints.init( nC );
00212
00213
00214 for( i=0; i<sizeT*sizeT; ++i )
00215 T[i] = 0.0;
00216
00217 for( i=0; i<nV*nV; ++i )
00218 Q[i] = 0.0;
00219
00220
00221 constraintProduct = 0;
00222
00223 return SUCCESSFUL_RETURN;
00224 }
00225
00226
00227
00228
00229
00230 returnValue QProblem::init( SymmetricMatrix *_H, const real_t* const _g, Matrix *_A,
00231 const real_t* const _lb, const real_t* const _ub,
00232 const real_t* const _lbA, const real_t* const _ubA,
00233 int& nWSR, real_t* const cputime
00234 )
00235 {
00236 if ( getNV( ) == 0 )
00237 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00238
00239
00240 if ( isInitialised( ) == BT_TRUE )
00241 {
00242 THROWWARNING( RET_QP_ALREADY_INITIALISED );
00243 reset( );
00244 }
00245
00246
00247 if ( setupQPdata( _H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
00248 return THROWERROR( RET_INVALID_ARGUMENTS );
00249
00250
00251 return solveInitialQP( 0,0,0,0, nWSR,cputime );
00252 }
00253
00254
00255
00256
00257
00258 returnValue QProblem::init( const real_t* const _H, const real_t* const _g, const real_t* const _A,
00259 const real_t* const _lb, const real_t* const _ub,
00260 const real_t* const _lbA, const real_t* const _ubA,
00261 int& nWSR, real_t* const cputime
00262 )
00263 {
00264 if ( getNV( ) == 0 )
00265 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00266
00267
00268 if ( isInitialised( ) == BT_TRUE )
00269 {
00270 THROWWARNING( RET_QP_ALREADY_INITIALISED );
00271 reset( );
00272 }
00273
00274
00275 if ( setupQPdata( _H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
00276 return THROWERROR( RET_INVALID_ARGUMENTS );
00277
00278
00279 return solveInitialQP( 0,0,0,0, nWSR,cputime );
00280 }
00281
00282
00283
00284
00285
00286 returnValue QProblem::init( const char* const H_file, const char* const g_file, const char* const A_file,
00287 const char* const lb_file, const char* const ub_file,
00288 const char* const lbA_file, const char* const ubA_file,
00289 int& nWSR, real_t* const cputime
00290 )
00291 {
00292 if ( getNV( ) == 0 )
00293 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00294
00295
00296 if ( isInitialised( ) == BT_TRUE )
00297 {
00298 THROWWARNING( RET_QP_ALREADY_INITIALISED );
00299 reset( );
00300 }
00301
00302
00303 if ( setupQPdataFromFile( H_file,g_file,A_file,lb_file,ub_file,lbA_file,ubA_file ) != SUCCESSFUL_RETURN )
00304 return THROWERROR( RET_UNABLE_TO_READ_FILE );
00305
00306
00307 return solveInitialQP( 0,0,0,0, nWSR,cputime );
00308 }
00309
00310
00311
00312
00313
00314 returnValue QProblem::init( SymmetricMatrix *_H, const real_t* const _g, Matrix *_A,
00315 const real_t* const _lb, const real_t* const _ub,
00316 const real_t* const _lbA, const real_t* const _ubA,
00317 int& nWSR, real_t* const cputime,
00318 const real_t* const xOpt, const real_t* const yOpt,
00319 const Bounds* const guessedBounds, const Constraints* const guessedConstraints
00320 )
00321 {
00322 int i;
00323 int nV = getNV( );
00324 int nC = getNC( );
00325
00326 if ( nV == 0 )
00327 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00328
00329
00330 if ( isInitialised( ) == BT_TRUE )
00331 {
00332 THROWWARNING( RET_QP_ALREADY_INITIALISED );
00333 reset( );
00334 }
00335
00336 if ( guessedBounds != 0 )
00337 {
00338 for( i=0; i<nV; ++i )
00339 {
00340 if ( guessedBounds->getStatus( i ) == ST_UNDEFINED )
00341 return THROWERROR( RET_INVALID_ARGUMENTS );
00342 }
00343 }
00344
00345 if ( guessedConstraints != 0 )
00346 {
00347 for( i=0; i<nC; ++i )
00348 if ( guessedConstraints->getStatus( i ) == ST_UNDEFINED )
00349 return THROWERROR( RET_INVALID_ARGUMENTS );
00350 }
00351
00352
00353 if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) ) )
00354 return THROWERROR( RET_INVALID_ARGUMENTS );
00355
00356
00357 if ( setupQPdata( _H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
00358 return THROWERROR( RET_INVALID_ARGUMENTS );
00359
00360
00361 return solveInitialQP( xOpt,yOpt,guessedBounds,guessedConstraints, nWSR,cputime );
00362 }
00363
00364
00365
00366
00367
00368 returnValue QProblem::init( const real_t* const _H, const real_t* const _g, const real_t* const _A,
00369 const real_t* const _lb, const real_t* const _ub,
00370 const real_t* const _lbA, const real_t* const _ubA,
00371 int& nWSR, real_t* const cputime,
00372 const real_t* const xOpt, const real_t* const yOpt,
00373 const Bounds* const guessedBounds, const Constraints* const guessedConstraints
00374 )
00375 {
00376 int i;
00377 int nV = getNV( );
00378 int nC = getNC( );
00379
00380 if ( nV == 0 )
00381 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00382
00383
00384 if ( isInitialised( ) == BT_TRUE )
00385 {
00386 THROWWARNING( RET_QP_ALREADY_INITIALISED );
00387 reset( );
00388 }
00389
00390 if ( guessedBounds != 0 )
00391 {
00392 for( i=0; i<nV; ++i )
00393 {
00394 if ( guessedBounds->getStatus( i ) == ST_UNDEFINED )
00395 return THROWERROR( RET_INVALID_ARGUMENTS );
00396 }
00397 }
00398
00399 if ( guessedConstraints != 0 )
00400 {
00401 for( i=0; i<nC; ++i )
00402 if ( guessedConstraints->getStatus( i ) == ST_UNDEFINED )
00403 return THROWERROR( RET_INVALID_ARGUMENTS );
00404 }
00405
00406
00407 if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) ) )
00408 return THROWERROR( RET_INVALID_ARGUMENTS );
00409
00410
00411 if ( setupQPdata( _H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
00412 return THROWERROR( RET_INVALID_ARGUMENTS );
00413
00414
00415 return solveInitialQP( xOpt,yOpt,guessedBounds,guessedConstraints, nWSR,cputime );
00416 }
00417
00418
00419
00420
00421
00422 returnValue QProblem::init( const char* const H_file, const char* const g_file, const char* const A_file,
00423 const char* const lb_file, const char* const ub_file,
00424 const char* const lbA_file, const char* const ubA_file,
00425 int& nWSR, real_t* const cputime,
00426 const real_t* const xOpt, const real_t* const yOpt,
00427 const Bounds* const guessedBounds, const Constraints* const guessedConstraints
00428 )
00429 {
00430 int i;
00431 int nV = getNV( );
00432 int nC = getNC( );
00433
00434 if ( nV == 0 )
00435 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00436
00437
00438 if ( isInitialised( ) == BT_TRUE )
00439 {
00440 THROWWARNING( RET_QP_ALREADY_INITIALISED );
00441 reset( );
00442 }
00443
00444 for( i=0; i<nV; ++i )
00445 {
00446 if ( guessedBounds->getStatus( i ) == ST_UNDEFINED )
00447 return THROWERROR( RET_INVALID_ARGUMENTS );
00448 }
00449
00450 for( i=0; i<nC; ++i )
00451 if ( guessedConstraints->getStatus( i ) == ST_UNDEFINED )
00452 return THROWERROR( RET_INVALID_ARGUMENTS );
00453
00454
00455 if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) ) )
00456 return THROWERROR( RET_INVALID_ARGUMENTS );
00457
00458
00459 if ( setupQPdataFromFile( H_file,g_file,A_file,lb_file,ub_file,lbA_file,ubA_file ) != SUCCESSFUL_RETURN )
00460 return THROWERROR( RET_UNABLE_TO_READ_FILE );
00461
00462
00463 return solveInitialQP( xOpt,yOpt,guessedBounds,guessedConstraints, nWSR,cputime );
00464 }
00465
00466 returnValue QProblem::computeInitialCholesky()
00467 {
00468 returnValue returnvalueCholesky;
00469
00470 if ( ( getNAC( ) + getNFX( ) ) == 0 )
00471 {
00472
00473 returnvalueCholesky = setupCholeskyDecomposition( );
00474
00475
00476 if ( returnvalueCholesky == RET_HESSIAN_NOT_SPD )
00477 {
00478 if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
00479 return THROWERROR( RET_INIT_FAILED_REGULARISATION );
00480
00481 if ( setupCholeskyDecomposition( ) != SUCCESSFUL_RETURN )
00482 return THROWERROR( RET_INIT_FAILED_CHOLESKY );
00483 }
00484
00485 if ( returnvalueCholesky == RET_INDEXLIST_CORRUPTED )
00486 return THROWERROR( RET_INIT_FAILED_CHOLESKY );
00487 }
00488 else
00489 {
00490
00491 returnvalueCholesky = setupCholeskyDecompositionProjected( );
00492
00493
00494 if ( returnvalueCholesky == RET_HESSIAN_NOT_SPD )
00495 {
00496 if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
00497 return THROWERROR( RET_INIT_FAILED_REGULARISATION );
00498
00499 if ( setupCholeskyDecompositionProjected( ) != SUCCESSFUL_RETURN )
00500 return THROWERROR( RET_INIT_FAILED_CHOLESKY );
00501 }
00502
00503 if ( returnvalueCholesky == RET_INDEXLIST_CORRUPTED )
00504 return THROWERROR( RET_INIT_FAILED_CHOLESKY );
00505 }
00506
00507 haveCholesky = BT_TRUE;
00508 return SUCCESSFUL_RETURN;
00509 }
00510
00511
00512
00513
00514
00515 returnValue QProblem::hotstart( const real_t* const g_new,
00516 const real_t* const lb_new, const real_t* const ub_new,
00517 const real_t* const lbA_new, const real_t* const ubA_new,
00518 int& nWSR, real_t* const cputime
00519 )
00520 {
00521 if ( getNV( ) == 0 )
00522 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00523
00524 ++count;
00525
00526 returnValue returnvalue = SUCCESSFUL_RETURN;
00527 int nFar, i;
00528 int nV = getNV ();
00529 int nC = getNC ();
00530
00531 int nWSR_max = nWSR;
00532 int nWSR_performed = 0;
00533
00534 real_t cputime_remaining = INFTY;
00535 real_t cputime_needed = 0.0;
00536
00537 real_t farbound = options.initialFarBounds;
00538 real_t t, rampVal;
00539
00540 if ( options.enableFarBounds == BT_FALSE )
00541 {
00542 if (haveCholesky == BT_FALSE)
00543 {
00544 returnvalue = computeInitialCholesky();
00545 if (returnvalue != SUCCESSFUL_RETURN)
00546 return THROWERROR(returnvalue);
00547 }
00548
00549 if ( usingRegularisation( ) == BT_TRUE )
00550 returnvalue = solveRegularisedQP( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime,0 );
00551 else
00552 returnvalue = solveQP( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime,0 );
00553 }
00554 else
00555 {
00556 real_t *ub_new_far = new real_t[nV];
00557 real_t *lb_new_far = new real_t[nV];
00558 real_t *ubA_new_far = new real_t[nC];
00559 real_t *lbA_new_far = new real_t[nC];
00560
00561
00562 if (ub_new)
00563 for (i = 0; i < nV; i++)
00564 if (ub_new[i] < INFTY && ub_new[i] > farbound) farbound = ub_new[i];
00565 if (lb_new)
00566 for (i = 0; i < nV; i++)
00567 if (lb_new[i] > -INFTY && lb_new[i] < -farbound) farbound = -lb_new[i];
00568 if (ubA_new)
00569 for (i = 0; i < nC; i++)
00570 if (ubA_new[i] < INFTY && ubA_new[i] > farbound) farbound = ubA_new[i];
00571 if (lbA_new)
00572 for (i = 0; i < nC; i++)
00573 if (lbA_new[i] > -INFTY && lbA_new[i] < -farbound) farbound = -lbA_new[i];
00574
00575 if ( options.enableRamping == BT_TRUE )
00576 {
00577
00578 for ( i=0; i<nV; ++i )
00579 {
00580 t = static_cast<real_t>(i) / static_cast<real_t>(nV+nC-1);
00581 rampVal = farbound + (1.0-t) * ramp0 + t * ramp1;
00582
00583 if ( ( lb_new == 0 ) || ( lb_new[i] <= -rampVal ) )
00584 lb_new_far[i] = - rampVal;
00585 else
00586 lb_new_far[i] = lb_new[i];
00587 if ( ( ub_new == 0 ) || ( ub_new[i] >= rampVal ) )
00588 ub_new_far[i] = rampVal;
00589 else
00590 ub_new_far[i] = ub_new[i];
00591 }
00592 for ( i=0; i<nC; ++i )
00593 {
00594 t = static_cast<real_t>(nV+i) / static_cast<real_t>(nV+nC-1);
00595 rampVal = farbound + (1.0-t) * ramp0 + t * ramp1;
00596
00597 if ( ( lbA_new == 0 ) || ( lbA_new[i] <= -rampVal ) )
00598 lbA_new_far[i] = - rampVal;
00599 else
00600 lbA_new_far[i] = lbA_new[i];
00601 if ( ( ubA_new == 0 ) || ( ubA_new[i] >= rampVal ) )
00602 ubA_new_far[i] = rampVal;
00603 else
00604 ubA_new_far[i] = ubA_new[i];
00605 }
00606 }
00607 else
00608 {
00609 for ( i=0; i<nV; ++i )
00610 {
00611 lb_new_far[i] = lb_new[i];
00612 ub_new_far[i] = ub_new[i];
00613 }
00614
00615 for ( i=0; i<nC; ++i )
00616 {
00617 lbA_new_far[i] = lbA_new[i];
00618 ubA_new_far[i] = ubA_new[i];
00619 }
00620 }
00621
00622 if (haveCholesky == BT_FALSE)
00623 {
00624 returnvalue = computeInitialCholesky();
00625 if (returnvalue != SUCCESSFUL_RETURN)
00626 goto farewell;
00627 }
00628
00629 for ( ;; )
00630 {
00631 nWSR = nWSR_max;
00632 if ( cputime != 0 )
00633 cputime_remaining = *cputime - cputime_needed;
00634
00635 if ( usingRegularisation( ) == BT_TRUE )
00636 returnvalue = solveRegularisedQP( g_new,lb_new_far,ub_new_far,lbA_new_far,ubA_new_far, nWSR,&cputime_remaining,nWSR_performed );
00637 else
00638 returnvalue = solveQP( g_new,lb_new_far,ub_new_far,lbA_new_far,ubA_new_far, nWSR,&cputime_remaining,nWSR_performed );
00639
00640 nWSR_performed = nWSR;
00641 cputime_needed += cputime_remaining;
00642
00643
00644 nFar = 0;
00645 farbound *= options.growFarBounds;
00646
00647 real_t maxFarbound = 1e20;
00648 if ( infeasible == BT_TRUE )
00649 {
00650 if ( farbound > maxFarbound )
00651 {
00652 returnvalue = RET_HOTSTART_STOPPED_INFEASIBILITY;
00653 goto farewell;
00654 }
00655
00656 if ( options.enableRamping == BT_TRUE )
00657 {
00658
00659 for ( i=0; i<nV; ++i )
00660 {
00661 t = static_cast<real_t>(i) / static_cast<real_t>(nV+nC-1);
00662 rampVal = farbound + (1.0-t) * ramp0 + t * ramp1;
00663
00664 if ( lb_new == 0 )
00665 lb_new_far[i] = - rampVal;
00666 else
00667 lb_new_far[i] = getMax (- rampVal, lb_new[i]);
00668
00669 if ( ub_new == 0 )
00670 ub_new_far[i] = rampVal;
00671 else
00672 ub_new_far[i] = getMin (rampVal, ub_new[i]);
00673 }
00674 for ( i=0; i<nC; ++i )
00675 {
00676 t = static_cast<real_t>(nV+i) / static_cast<real_t>(nV+nC-1);
00677 rampVal = farbound + (1.0-t) * ramp0 + t * ramp1;
00678
00679 if ( lbA_new == 0 )
00680 lbA_new_far[i] = - rampVal;
00681 else
00682 lbA_new_far[i] = getMax (- rampVal, lbA_new[i]);
00683
00684 if ( ubA_new == 0 )
00685 ubA_new_far[i] = rampVal;
00686 else
00687 ubA_new_far[i] = getMin (rampVal, ubA_new[i]);
00688 }
00689 }
00690 else
00691 {
00692 for ( i=0; i<nV; ++i )
00693 {
00694 lb_new_far[i] = lb_new[i];
00695 ub_new_far[i] = ub_new[i];
00696 }
00697
00698 for ( i=0; i<nC; ++i )
00699 {
00700 lbA_new_far[i] = lbA_new[i];
00701 ubA_new_far[i] = ubA_new[i];
00702 }
00703 }
00704 }
00705 else if ( status == QPS_SOLVED )
00706 {
00707 real_t tol = farbound * options.boundTolerance;
00708 status = QPS_HOMOTOPYQPSOLVED;
00709
00710 nFar = 0;
00711 for ( i=0; i<nV; ++i )
00712 {
00713 if ( ( ( lb_new == 0 ) || ( lb_new_far[i] > lb_new[i] ) ) && ( fabs ( lb_new_far[i] - x[i] ) < tol ) )
00714 ++nFar;
00715 if ( ( ( ub_new == 0 ) || ( ub_new_far[i] < ub_new[i] ) ) && ( fabs ( ub_new_far[i] - x[i] ) < tol ) )
00716 ++nFar;
00717 }
00718 for ( i=0; i<nC; ++i )
00719 {
00720 if ( ( ( lbA_new == 0 ) || ( lbA_new_far[i] > lbA_new[i] ) ) && ( fabs ( lbA_new_far[i] - Ax[i] ) < tol ) )
00721 ++nFar;
00722 if ( ( ( ubA_new == 0 ) || ( ubA_new_far[i] < ubA_new[i] ) ) && ( fabs ( ubA_new_far[i] - Ax[i] ) < tol ) )
00723 ++nFar;
00724 }
00725
00726 if ( nFar == 0 )
00727 break;
00728
00729 if ( farbound > maxFarbound )
00730 {
00731 unbounded = BT_TRUE;
00732 returnvalue = RET_HOTSTART_STOPPED_UNBOUNDEDNESS;
00733 goto farewell;
00734 }
00735
00736 if ( options.enableRamping == BT_TRUE )
00737 {
00738
00739 for ( i=0; i<nV; ++i )
00740 {
00741 t = static_cast<real_t>(i) / static_cast<real_t>(nV+nC-1);
00742 rampVal = farbound + (1.0-t) * ramp0 + t * ramp1;
00743
00744 if ( lb_new == 0 )
00745 lb_new_far[i] = - rampVal;
00746 else
00747 lb_new_far[i] = getMax (- rampVal, lb_new[i]);
00748
00749 if ( ub_new == 0 )
00750 ub_new_far[i] = rampVal;
00751 else
00752 ub_new_far[i] = getMin (rampVal, ub_new[i]);
00753 }
00754 for ( i=0; i<nC; ++i )
00755 {
00756 t = static_cast<real_t>(nV+i) / static_cast<real_t>(nV+nC-1);
00757 rampVal = farbound + (1.0-t) * ramp0 + t * ramp1;
00758
00759 if ( lbA_new == 0 )
00760 lbA_new_far[i] = - rampVal;
00761 else
00762 lbA_new_far[i] = getMax (- rampVal, lbA_new[i]);
00763
00764 if ( ubA_new == 0 )
00765 ubA_new_far[i] = rampVal;
00766 else
00767 ubA_new_far[i] = getMin (rampVal, ubA_new[i]);
00768 }
00769 }
00770 else
00771 {
00772 for ( i=0; i<nV; ++i )
00773 {
00774 lb_new_far[i] = lb_new[i];
00775 ub_new_far[i] = ub_new[i];
00776 }
00777
00778 for ( i=0; i<nC; ++i )
00779 {
00780 lbA_new_far[i] = lbA_new[i];
00781 ubA_new_far[i] = ubA_new[i];
00782 }
00783 }
00784 }
00785 else
00786 {
00787
00788 break;
00789 }
00790
00791
00792 t = ramp0; ramp0 = ramp1; ramp1 = t;
00793 }
00794
00795 farewell:
00796 if ( cputime != 0 )
00797 *cputime = cputime_needed;
00798 delete[] lbA_new_far; delete[] ubA_new_far;
00799 delete[] lb_new_far; delete[] ub_new_far;
00800 }
00801
00802 return ( returnvalue != SUCCESSFUL_RETURN ) ? THROWERROR( returnvalue ) : returnvalue;
00803 }
00804
00805
00806
00807
00808
00809 returnValue QProblem::hotstart( const char* const g_file,
00810 const char* const lb_file, const char* const ub_file,
00811 const char* const lbA_file, const char* const ubA_file,
00812 int& nWSR, real_t* const cputime
00813 )
00814 {
00815 int nV = getNV( );
00816 int nC = getNC( );
00817
00818 if ( nV == 0 )
00819 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00820
00821
00822 if ( g_file == 0 )
00823 return THROWERROR( RET_INVALID_ARGUMENTS );
00824
00825
00826
00827 real_t* g_new = new real_t[nV];
00828 real_t* lb_new = 0;
00829 real_t* ub_new = 0;
00830 real_t* lbA_new = 0;
00831 real_t* ubA_new = 0;
00832
00833 if ( lb_file != 0 )
00834 lb_new = new real_t[nV];
00835 if ( ub_file != 0 )
00836 ub_new = new real_t[nV];
00837 if ( lbA_file != 0 )
00838 lbA_new = new real_t[nC];
00839 if ( ubA_file != 0 )
00840 ubA_new = new real_t[nC];
00841
00842
00843 returnValue returnvalue;
00844 returnvalue = loadQPvectorsFromFile( g_file,lb_file,ub_file,lbA_file,ubA_file,
00845 g_new,lb_new,ub_new,lbA_new,ubA_new
00846 );
00847 if ( returnvalue != SUCCESSFUL_RETURN )
00848 {
00849 if ( ubA_file != 0 )
00850 delete[] ubA_new;
00851 if ( lbA_file != 0 )
00852 delete[] lbA_new;
00853 if ( ub_file != 0 )
00854 delete[] ub_new;
00855 if ( lb_file != 0 )
00856 delete[] lb_new;
00857 delete[] g_new;
00858
00859 return THROWERROR( RET_UNABLE_TO_READ_FILE );
00860 }
00861
00862
00863 returnvalue = hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
00864
00865
00866 if ( ubA_file != 0 )
00867 delete[] ubA_new;
00868 if ( lbA_file != 0 )
00869 delete[] lbA_new;
00870 if ( ub_file != 0 )
00871 delete[] ub_new;
00872 if ( lb_file != 0 )
00873 delete[] lb_new;
00874 delete[] g_new;
00875
00876 return returnvalue;
00877 }
00878
00879
00880
00881
00882
00883 returnValue QProblem::hotstart( const real_t* const g_new,
00884 const real_t* const lb_new, const real_t* const ub_new,
00885 const real_t* const lbA_new, const real_t* const ubA_new,
00886 int& nWSR, real_t* const cputime,
00887 const Bounds* const guessedBounds, const Constraints* const guessedConstraints
00888 )
00889 {
00890 int nV = getNV( );
00891 int nC = getNC( );
00892
00893 if ( nV == 0 )
00894 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00895
00896
00897
00898 real_t starttime = 0.0;
00899 if ( cputime != 0 )
00900 starttime = getCPUtime( );
00901
00902
00903
00904 if ( ( guessedBounds != 0 ) && ( guessedConstraints != 0 ) )
00905 {
00906 if ( setupAuxiliaryQP( guessedBounds,guessedConstraints ) != SUCCESSFUL_RETURN )
00907 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00908 }
00909
00910 if ( ( guessedBounds == 0 ) && ( guessedConstraints != 0 ) )
00911 {
00912
00913 Bounds emptyBounds( nV );
00914 if ( emptyBounds.setupAllFree( ) != SUCCESSFUL_RETURN )
00915 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00916
00917 if ( setupAuxiliaryQP( &emptyBounds,guessedConstraints ) != SUCCESSFUL_RETURN )
00918 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00919 }
00920
00921 if ( ( guessedBounds != 0 ) && ( guessedConstraints == 0 ) )
00922 {
00923
00924 Constraints emptyConstraints( nC );
00925 if ( emptyConstraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
00926 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00927
00928 if ( setupAuxiliaryQP( guessedBounds,&emptyConstraints ) != SUCCESSFUL_RETURN )
00929 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00930 }
00931
00932 if ( ( guessedBounds == 0 ) && ( guessedConstraints == 0 ) )
00933 {
00934
00935 Bounds emptyBounds( nV );
00936 Constraints emptyConstraints( nC );
00937 if ( emptyBounds.setupAllFree( ) != SUCCESSFUL_RETURN )
00938 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00939 if ( emptyConstraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
00940 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00941
00942 if ( setupAuxiliaryQP( &emptyBounds,&emptyConstraints ) != SUCCESSFUL_RETURN )
00943 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00944 }
00945
00946 status = QPS_AUXILIARYQPSOLVED;
00947
00948
00949
00950
00951
00952 if ( cputime != 0 )
00953 *cputime -= getCPUtime( ) - starttime;
00954
00955 returnValue returnvalue = hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
00956
00957
00958 if ( cputime != 0 )
00959 *cputime = getCPUtime( ) - starttime;
00960
00961 return returnvalue;
00962 }
00963
00964
00965
00966
00967
00968 returnValue QProblem::hotstart( const char* const g_file,
00969 const char* const lb_file, const char* const ub_file,
00970 const char* const lbA_file, const char* const ubA_file,
00971 int& nWSR, real_t* const cputime,
00972 const Bounds* const guessedBounds, const Constraints* const guessedConstraints
00973 )
00974 {
00975 int nV = getNV( );
00976 int nC = getNC( );
00977
00978 if ( nV == 0 )
00979 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00980
00981
00982 if ( g_file == 0 )
00983 return THROWERROR( RET_INVALID_ARGUMENTS );
00984
00985
00986
00987 real_t* g_new = new real_t[nV];
00988 real_t* lb_new = 0;
00989 real_t* ub_new = 0;
00990 real_t* lbA_new = 0;
00991 real_t* ubA_new = 0;
00992
00993 if ( lb_file != 0 )
00994 lb_new = new real_t[nV];
00995 if ( ub_file != 0 )
00996 ub_new = new real_t[nV];
00997 if ( lbA_file != 0 )
00998 lbA_new = new real_t[nC];
00999 if ( ubA_file != 0 )
01000 ubA_new = new real_t[nC];
01001
01002
01003 returnValue returnvalue;
01004 returnvalue = loadQPvectorsFromFile( g_file,lb_file,ub_file,lbA_file,ubA_file,
01005 g_new,lb_new,ub_new,lbA_new,ubA_new
01006 );
01007 if ( returnvalue != SUCCESSFUL_RETURN )
01008 {
01009 if ( ubA_file != 0 )
01010 delete[] ubA_new;
01011 if ( lbA_file != 0 )
01012 delete[] lbA_new;
01013 if ( ub_file != 0 )
01014 delete[] ub_new;
01015 if ( lb_file != 0 )
01016 delete[] lb_new;
01017 delete[] g_new;
01018
01019 return THROWERROR( RET_UNABLE_TO_READ_FILE );
01020 }
01021
01022
01023 returnvalue = hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime,
01024 guessedBounds,guessedConstraints
01025 );
01026
01027
01028 if ( ubA_file != 0 )
01029 delete[] ubA_new;
01030 if ( lbA_file != 0 )
01031 delete[] lbA_new;
01032 if ( ub_file != 0 )
01033 delete[] ub_new;
01034 if ( lb_file != 0 )
01035 delete[] lb_new;
01036 delete[] g_new;
01037
01038 return returnvalue;
01039 }
01040
01041
01042
01043
01044
01045 returnValue QProblem::solveCurrentEQP ( const int n_rhs,
01046 const real_t* g_in,
01047 const real_t* lb_in,
01048 const real_t* ub_in,
01049 const real_t* lbA_in,
01050 const real_t* ubA_in,
01051 real_t* x_out,
01052 real_t* y_out
01053 )
01054 {
01055 returnValue returnvalue = SUCCESSFUL_RETURN;
01056 int ii, jj;
01057 int nV = getNV( );
01058 int nC = getNC( );
01059 int nFR = getNFR( );
01060 int nFX = getNFX( );
01061 int nAC = getNAC( );
01062
01063 real_t *delta_xFX = new real_t[nFX];
01064 real_t *delta_xFR = new real_t[nFR];
01065 real_t *delta_yAC = new real_t[nAC];
01066 real_t *delta_yFX = new real_t[nFX];
01067
01068
01069 int* FR_idx;
01070 int* FX_idx;
01071 int* AC_idx;
01072
01073 bounds.getFree( )->getNumberArray( &FR_idx );
01074 bounds.getFixed( )->getNumberArray( &FX_idx );
01075 constraints.getActive( )->getNumberArray( &AC_idx );
01076
01077 for ( ii = 0 ; ii < n_rhs; ++ii )
01078 {
01079 returnvalue = determineStepDirection(
01080 g_in, lbA_in, ubA_in, lb_in, ub_in, BT_FALSE, BT_FALSE,
01081 delta_xFX, delta_xFR, delta_yAC, delta_yFX );
01082
01083 for ( jj = 0; jj < nFX; ++jj )
01084 x_out[FX_idx[jj]] = delta_xFX[jj];
01085 for ( jj = 0; jj < nFR; ++jj )
01086 x_out[FR_idx[jj]] = delta_xFR[jj];
01087 for ( jj = 0; jj < nFX; ++jj )
01088 y_out[FX_idx[jj]] = delta_yFX[jj];
01089 for ( jj = 0; jj < nAC; ++jj )
01090 y_out[nV+AC_idx[jj]] = delta_yAC[jj];
01091 for ( jj = 0; jj < nFR; ++jj )
01092 y_out[FR_idx[jj]] = 0.0;
01093
01094 g_in += nV;
01095 lb_in += nV;
01096 ub_in += nV;
01097 lbA_in += nC;
01098 ubA_in += nC;
01099 x_out += nV;
01100 y_out += nV+nC;
01101 }
01102
01103
01104 delete[] delta_yFX;
01105 delete[] delta_yAC;
01106 delete[] delta_xFR;
01107 delete[] delta_xFX;
01108
01109 return returnvalue;
01110 }
01111
01112
01113
01114
01115
01116
01117 int QProblem::getNZ( ) const
01118 {
01119
01120 return getNFR( ) - getNAC( );
01121 }
01122
01123
01124
01125
01126
01127 returnValue QProblem::getDualSolution( real_t* const yOpt ) const
01128 {
01129 int i;
01130
01131
01132
01133 if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
01134 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
01135 ( getStatus( ) == QPS_SOLVED ) )
01136 {
01137 for( i=0; i<getNV( )+getNC( ); ++i )
01138 yOpt[i] = y[i];
01139
01140 return SUCCESSFUL_RETURN;
01141 }
01142 else
01143 {
01144 return RET_QP_NOT_SOLVED;
01145 }
01146 }
01147
01148
01149
01150
01151
01152
01153 returnValue QProblem::setConstraintProduct( ConstraintProduct* const _constraintProduct )
01154 {
01155 constraintProduct = _constraintProduct;
01156
01157 return SUCCESSFUL_RETURN;
01158 }
01159
01160
01161
01162
01163
01164 returnValue QProblem::printProperties( )
01165 {
01166 #ifndef __XPCTARGET__
01167
01168 if ( options.printLevel == PL_NONE )
01169 return SUCCESSFUL_RETURN;
01170
01171 char myPrintfString[80];
01172
01173 myPrintf( "\n################# qpOASES -- QP PROPERTIES #################\n" );
01174 myPrintf( "\n" );
01175
01176
01177 snprintf( myPrintfString,80, "Number of Variables: %4.1d\n",getNV( ) );
01178 myPrintf( myPrintfString );
01179
01180 if ( bounds.hasNoLower( ) == BT_TRUE )
01181 myPrintf( "Variables are not bounded from below.\n" );
01182 else
01183 myPrintf( "Variables are bounded from below.\n" );
01184
01185 if ( bounds.hasNoUpper( ) == BT_TRUE )
01186 myPrintf( "Variables are not bounded from above.\n" );
01187 else
01188 myPrintf( "Variables are bounded from above.\n" );
01189
01190 myPrintf( "\n" );
01191
01192
01193
01194 snprintf( myPrintfString,80, "Total number of Constraints: %4.1d\n",getNC( ) );
01195 myPrintf( myPrintfString );
01196
01197 snprintf( myPrintfString,80, "Number of Equality Constraints: %4.1d\n",getNEC( ) );
01198 myPrintf( myPrintfString );
01199
01200 snprintf( myPrintfString,80, "Number of Inequality Constraints: %4.1d\n",getNC( )-getNEC( ) );
01201 myPrintf( myPrintfString );
01202
01203 if ( getNC( ) > 0 )
01204 {
01205 if ( constraints.hasNoLower( ) == BT_TRUE )
01206 myPrintf( "Constraints are not bounded from below.\n" );
01207 else
01208 myPrintf( "Constraints are bounded from below.\n" );
01209
01210 if ( constraints.hasNoUpper( ) == BT_TRUE )
01211 myPrintf( "Constraints are not bounded from above.\n" );
01212 else
01213 myPrintf( "Constraints are bounded from above.\n" );
01214 }
01215
01216 myPrintf( "\n" );
01217
01218
01219
01220 switch ( hessianType )
01221 {
01222 case HST_ZERO:
01223 myPrintf( "Hessian is zero matrix (i.e. actually an LP is solved).\n" );
01224 break;
01225
01226 case HST_IDENTITY:
01227 myPrintf( "Hessian is identity matrix.\n" );
01228 break;
01229
01230 case HST_POSDEF:
01231 myPrintf( "Hessian matrix is (strictly) positive definite.\n" );
01232 break;
01233
01234 case HST_POSDEF_NULLSPACE:
01235 myPrintf( "Hessian matrix is positive definite on null space of active constraints.\n" );
01236 break;
01237
01238 case HST_SEMIDEF:
01239 myPrintf( "Hessian matrix is positive semi-definite.\n" );
01240 break;
01241
01242 default:
01243 myPrintf( "Hessian matrix has unknown type.\n" );
01244 break;
01245 }
01246
01247 if ( infeasible == BT_TRUE )
01248 myPrintf( "QP was found to be infeasible.\n" );
01249 else
01250 myPrintf( "QP seems to be feasible.\n" );
01251
01252 if ( unbounded == BT_TRUE )
01253 myPrintf( "QP was found to be unbounded from below.\n" );
01254 else
01255 myPrintf( "QP seems to be bounded from below.\n" );
01256
01257 myPrintf( "\n" );
01258
01259
01260
01261 switch ( status )
01262 {
01263 case QPS_NOTINITIALISED:
01264 myPrintf( "Status of QP object: freshly instantiated or reset.\n" );
01265 break;
01266
01267 case QPS_PREPARINGAUXILIARYQP:
01268 myPrintf( "Status of QP object: an auxiliary QP is currently setup.\n" );
01269 break;
01270
01271 case QPS_AUXILIARYQPSOLVED:
01272 myPrintf( "Status of QP object: an auxilary QP was solved.\n" );
01273 break;
01274
01275 case QPS_PERFORMINGHOMOTOPY:
01276 myPrintf( "Status of QP object: a homotopy step is performed.\n" );
01277 break;
01278
01279 case QPS_HOMOTOPYQPSOLVED:
01280 myPrintf( "Status of QP object: an intermediate QP along the homotopy path was solved.\n" );
01281 break;
01282
01283 case QPS_SOLVED:
01284 myPrintf( "Status of QP object: solution of the actual QP was found.\n" );
01285 break;
01286 }
01287
01288 switch ( options.printLevel )
01289 {
01290 case PL_LOW:
01291 myPrintf( "Print level of QP object is low, i.e. only error are printed.\n" );
01292 break;
01293
01294 case PL_MEDIUM:
01295 myPrintf( "Print level of QP object is medium, i.e. error and warnings are printed.\n" );
01296 break;
01297
01298 case PL_HIGH:
01299 myPrintf( "Print level of QP object is high, i.e. all available output is printed.\n" );
01300 break;
01301
01302 default:
01303 break;
01304 }
01305
01306 myPrintf( "\n" );
01307 #endif
01308
01309 return SUCCESSFUL_RETURN;
01310 }
01311
01312
01313
01314
01315
01316
01317
01318
01319
01320
01321 returnValue QProblem::clear( )
01322 {
01323 if ( ( freeConstraintMatrix == BT_TRUE ) && ( A != 0 ) )
01324 {
01325 delete A;
01326 A = 0;
01327 }
01328
01329 if ( lbA != 0 )
01330 {
01331 delete[] lbA;
01332 lbA = 0;
01333 }
01334
01335 if ( ubA != 0 )
01336 {
01337 delete[] ubA;
01338 ubA = 0;
01339 }
01340
01341 if ( T != 0 )
01342 {
01343 delete[] T;
01344 T = 0;
01345 }
01346
01347 if ( Q != 0 )
01348 {
01349 delete[] Q;
01350 Q = 0;
01351 }
01352
01353 if ( Ax != 0 )
01354 {
01355 delete[] Ax;
01356 Ax = 0;
01357 }
01358
01359 if ( Ax_l != 0 )
01360 {
01361 delete[] Ax_l;
01362 Ax_l = 0;
01363 }
01364
01365 if ( Ax_u != 0 )
01366 {
01367 delete[] Ax_u;
01368 Ax_u = 0;
01369 }
01370
01371 if ( tempA != 0 )
01372 {
01373 delete[] tempA;
01374 tempA = 0;
01375 }
01376
01377 if ( ZFR_delta_xFRz != 0 )
01378 {
01379 delete[] ZFR_delta_xFRz;
01380 ZFR_delta_xFRz = 0;
01381 }
01382
01383 if ( delta_xFRy != 0 )
01384 {
01385 delete[] delta_xFRy;
01386 delta_xFRy = 0;
01387 }
01388
01389 if ( delta_xFRz != 0 )
01390 {
01391 delete[] delta_xFRz;
01392 delta_xFRz = 0;
01393 }
01394
01395 if ( tempB != 0 )
01396 {
01397 delete[] tempB;
01398 tempB = 0;
01399 }
01400
01401 if ( delta_yAC_TMP != 0 )
01402 {
01403 delete[] delta_yAC_TMP;
01404 delta_yAC_TMP = 0;
01405 }
01406
01407 return SUCCESSFUL_RETURN;
01408 }
01409
01410
01411
01412
01413
01414 returnValue QProblem::copy( const QProblem& rhs
01415 )
01416 {
01417 int _nV = rhs.getNV( );
01418 int _nC = rhs.getNC( );
01419
01420 constraints = rhs.constraints;
01421
01422 if ( ( freeConstraintMatrix == BT_TRUE ) && ( A != 0 ) )
01423 {
01424 delete A;
01425 A = 0;
01426 }
01427
01428 freeConstraintMatrix = rhs.freeConstraintMatrix;
01429
01430 if ( freeConstraintMatrix == BT_TRUE )
01431 A = rhs.A->duplicate();
01432 else
01433 A = rhs.A;
01434
01435 if ( rhs.lbA != 0 )
01436 {
01437 lbA = new real_t[_nC];
01438 setLBA( rhs.lbA );
01439 }
01440 else
01441 lbA = 0;
01442
01443 if ( rhs.ubA != 0 )
01444 {
01445 ubA = new real_t[_nC];
01446 setUBA( rhs.ubA );
01447 }
01448 else
01449 ubA = 0;
01450
01451 if ( rhs.y != 0 )
01452 {
01453 delete[] y;
01454 y = new real_t[_nV+_nC];
01455 memcpy( y,rhs.y,(_nV+_nC)*sizeof(real_t) );
01456 }
01457 else
01458 y = 0;
01459
01460 sizeT = rhs.sizeT;
01461
01462 if ( rhs.T != 0 )
01463 {
01464 T = new real_t[sizeT*sizeT];
01465 memcpy( T,rhs.T,sizeT*sizeT*sizeof(real_t) );
01466 }
01467 else
01468 T = 0;
01469
01470 if ( rhs.Q != 0 )
01471 {
01472 Q = new real_t[_nV*_nV];
01473 memcpy( Q,rhs.Q,_nV*_nV*sizeof(real_t) );
01474 }
01475 else
01476 Q = 0;
01477
01478 if ( rhs.Ax != 0 )
01479 {
01480 Ax = new real_t[_nC];
01481 memcpy( Ax,rhs.Ax,_nC*sizeof(real_t) );
01482 }
01483 else
01484 Ax = 0;
01485
01486 if ( rhs.Ax_l != 0 )
01487 {
01488 Ax_l = new real_t[_nC];
01489 memcpy( Ax_l,rhs.Ax_l,_nC*sizeof(real_t) );
01490 }
01491 else
01492 Ax_l = 0;
01493
01494 if ( rhs.Ax_u != 0 )
01495 {
01496 Ax_u = new real_t[_nC];
01497 memcpy( Ax_u,rhs.Ax_u,_nC*sizeof(real_t) );
01498 }
01499 else
01500 Ax_u = 0;
01501
01502 if ( rhs.constraintProduct != 0 )
01503 constraintProduct = rhs.constraintProduct;
01504 else
01505 constraintProduct = 0;
01506
01507 tempA = new real_t[_nV];
01508 ZFR_delta_xFRz = new real_t[_nV];
01509 delta_xFRz = new real_t[_nV];
01510
01511 if ( _nC > 0 )
01512 {
01513 delta_xFRy = new real_t[_nC];
01514 tempB = new real_t[_nC];
01515 delta_yAC_TMP = new real_t[_nC];
01516 }
01517 else
01518 {
01519 delta_xFRy = 0;
01520 tempB = 0;
01521 delta_yAC_TMP = 0;
01522 }
01523
01524 return SUCCESSFUL_RETURN;
01525 }
01526
01527
01528
01529
01530
01531
01532 returnValue QProblem::solveInitialQP( const real_t* const xOpt, const real_t* const yOpt,
01533 const Bounds* const guessedBounds, const Constraints* const guessedConstraints,
01534 int& nWSR, real_t* const cputime
01535 )
01536 {
01537 int i;
01538
01539
01540 int nV = getNV( );
01541 int nC = getNC( );
01542
01543
01544
01545 real_t starttime = 0.0;
01546 if ( cputime != 0 )
01547 starttime = getCPUtime( );
01548
01549 status = QPS_NOTINITIALISED;
01550
01551
01552
01553 #ifdef __MANY_CONSTRAINTS__
01554
01555 int j;
01556 real_t l1;
01557 for( i=0; i<nC; ++i )
01558 {
01559 l1 = 0.0;
01560 for( j=0; j<nV; ++j )
01561 l1 += fabs( AA(i,j) );
01562
01563 if ( l1 > 1.0 + 10.0*EPS )
01564 return THROWERROR( RET_CONSTRAINTS_ARE_NOT_SCALED );
01565 }
01566 #endif
01567
01568
01569 if ( determineHessianType( ) != SUCCESSFUL_RETURN )
01570 return THROWERROR( RET_INIT_FAILED );
01571
01572
01573 if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
01574 return THROWERROR( RET_INIT_FAILED );
01575
01576 status = QPS_PREPARINGAUXILIARYQP;
01577
01578
01579
01580
01581 if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
01582 return THROWERROR( RET_INIT_FAILED );
01583
01584 if ( constraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
01585 return THROWERROR( RET_INIT_FAILED );
01586
01587
01588 if ( setupAuxiliaryQPsolution( xOpt,yOpt ) != SUCCESSFUL_RETURN )
01589 return THROWERROR( RET_INIT_FAILED );
01590
01591
01592 Bounds auxiliaryBounds( nV );
01593 Constraints auxiliaryConstraints( nC );
01594
01595 if ( obtainAuxiliaryWorkingSet( xOpt,yOpt,guessedBounds,guessedConstraints,
01596 &auxiliaryBounds,&auxiliaryConstraints ) != SUCCESSFUL_RETURN )
01597 return THROWERROR( RET_INIT_FAILED );
01598
01599
01600
01601 if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_SEMIDEF ) )
01602 {
01603 if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
01604 return THROWERROR( RET_INIT_FAILED_REGULARISATION );
01605 }
01606
01607
01608 if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
01609 return THROWERROR( RET_INIT_FAILED_TQ );
01610
01611
01612 if ( setupAuxiliaryWorkingSet( &auxiliaryBounds,&auxiliaryConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
01613 return THROWERROR( RET_INIT_FAILED );
01614
01615 haveCholesky = BT_FALSE;
01616
01617
01618 real_t* g_original = new real_t[nV];
01619 real_t* lb_original = new real_t[nV];
01620 real_t* ub_original = new real_t[nV];
01621 real_t* lbA_original = new real_t[nC];
01622 real_t* ubA_original = new real_t[nC];
01623
01624 for( i=0; i<nV; ++i )
01625 {
01626 g_original[i] = g[i];
01627 lb_original[i] = lb[i];
01628 ub_original[i] = ub[i];
01629 }
01630
01631 for( i=0; i<nC; ++i )
01632 {
01633 lbA_original[i] = lbA[i];
01634 ubA_original[i] = ubA[i];
01635 }
01636
01637
01638
01639 if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
01640 {
01641 delete[] ubA_original; delete[] lbA_original; delete[] ub_original; delete[] lb_original; delete[] g_original;
01642 return THROWERROR( RET_INIT_FAILED );
01643 }
01644
01645 if ( setupAuxiliaryQPbounds( &auxiliaryBounds,&auxiliaryConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
01646 {
01647 delete[] ubA_original; delete[] lbA_original; delete[] ub_original; delete[] lb_original; delete[] g_original;
01648 return THROWERROR( RET_INIT_FAILED );
01649 }
01650
01651 status = QPS_AUXILIARYQPSOLVED;
01652
01653
01654 if ( options.enableRamping == BT_TRUE )
01655 performRamping( );
01656
01657
01658
01659
01660 if ( cputime != 0 )
01661 *cputime -= getCPUtime( ) - starttime;
01662
01663
01664 returnValue returnvalue = hotstart( g_original,lb_original,ub_original,lbA_original,ubA_original, nWSR,cputime );
01665
01666
01667 delete[] ubA_original; delete[] lbA_original; delete[] ub_original; delete[] lb_original; delete[] g_original;
01668
01669
01670 if ( isInfeasible( ) == BT_TRUE )
01671 return THROWERROR( RET_INIT_FAILED_INFEASIBILITY );
01672
01673 if ( isUnbounded( ) == BT_TRUE )
01674 return THROWERROR( RET_INIT_FAILED_UNBOUNDEDNESS );
01675
01676
01677 if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
01678 return THROWERROR( RET_INIT_FAILED_HOTSTART );
01679
01680
01681
01682 if ( cputime != 0 )
01683 *cputime = getCPUtime( ) - starttime;
01684
01685 THROWINFO( RET_INIT_SUCCESSFUL );
01686
01687 return returnvalue;
01688 }
01689
01690
01691
01692
01693
01694 returnValue QProblem::solveQP( const real_t* const g_new,
01695 const real_t* const lb_new, const real_t* const ub_new,
01696 const real_t* const lbA_new, const real_t* const ubA_new,
01697 int& nWSR, real_t* const cputime, int nWSRperformed
01698 )
01699 {
01700 int iter;
01701 int nV = getNV( );
01702 int nC = getNC( );
01703
01704
01705 if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
01706 ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
01707 ( getStatus( ) == QPS_PERFORMINGHOMOTOPY ) )
01708 {
01709 return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
01710 }
01711
01712
01713 real_t starttime = 0.0;
01714 if ( cputime != 0 )
01715 starttime = getCPUtime( );
01716
01717
01718
01719
01720
01721 real_t* delta_xFR = new real_t[nV];
01722 real_t* delta_xFX = new real_t[nV];
01723 real_t* delta_yAC = new real_t[nC];
01724 real_t* delta_yFX = new real_t[nV];
01725
01726 real_t* delta_g = new real_t[nV];
01727 real_t* delta_lb = new real_t[nV];
01728 real_t* delta_ub = new real_t[nV];
01729 real_t* delta_lbA = new real_t[nC];
01730 real_t* delta_ubA = new real_t[nC];
01731
01732 returnValue returnvalue;
01733 BooleanType Delta_bC_isZero, Delta_bB_isZero;
01734
01735 int BC_idx;
01736 SubjectToStatus BC_status;
01737 BooleanType BC_isBound;
01738
01739 real_t homotopyLength;
01740
01741 char messageString[80];
01742
01743
01744
01745 if ( setupSubjectToType( lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
01746 return THROWERROR( RET_HOTSTART_FAILED );
01747
01748
01749 infeasible = BT_FALSE;
01750 unbounded = BT_FALSE;
01751
01752
01753
01754 for( iter=nWSRperformed; iter<nWSR; ++iter )
01755 {
01756 idxAddB = idxRemB = idxAddC = idxRemC = -1;
01757
01758 if ( isCPUtimeLimitExceeded( cputime,starttime,iter-nWSRperformed ) == BT_TRUE )
01759 {
01760
01761
01762 nWSR = iter;
01763 break;
01764 }
01765
01766 status = QPS_PERFORMINGHOMOTOPY;
01767
01768 #ifndef __XPCTARGET__
01769 snprintf( messageString,80,"%d ...",iter );
01770 getGlobalMessageHandler( )->throwInfo( RET_ITERATION_STARTED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
01771 #endif
01772
01773
01774 returnvalue = determineDataShift( g_new,lbA_new,ubA_new,lb_new,ub_new,
01775 delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,
01776 Delta_bC_isZero, Delta_bB_isZero
01777 );
01778 if ( returnvalue != SUCCESSFUL_RETURN )
01779 {
01780 delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
01781 delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
01782
01783
01784 nWSR = iter;
01785 if ( cputime != 0 )
01786 *cputime = getCPUtime( ) - starttime;
01787
01788 THROWERROR( RET_SHIFT_DETERMINATION_FAILED );
01789 return returnvalue;
01790 }
01791
01792
01793 returnvalue = determineStepDirection( delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,
01794 Delta_bC_isZero, Delta_bB_isZero,
01795 delta_xFX,delta_xFR,delta_yAC,delta_yFX
01796 );
01797 if ( returnvalue != SUCCESSFUL_RETURN )
01798 {
01799 delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
01800 delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
01801
01802
01803 nWSR = iter;
01804 if ( cputime != 0 )
01805 *cputime = getCPUtime( ) - starttime;
01806
01807 THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
01808 return returnvalue;
01809 }
01810
01811
01812
01813 returnvalue = performStep( delta_g, delta_lbA,delta_ubA,delta_lb,delta_ub,
01814 delta_xFX,delta_xFR,delta_yAC,delta_yFX,
01815 BC_idx,BC_status,BC_isBound
01816 );
01817 if ( returnvalue != SUCCESSFUL_RETURN )
01818 {
01819 delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
01820 delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
01821
01822
01823 nWSR = iter;
01824 if ( cputime != 0 )
01825 *cputime = getCPUtime( ) - starttime;
01826
01827 THROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED );
01828 return returnvalue;
01829 }
01830
01831
01832 homotopyLength = relativeHomotopyLength(g_new, lb_new, ub_new, lbA_new, ubA_new);
01833 if ( homotopyLength <= options.terminationTolerance )
01834 {
01835 status = QPS_SOLVED;
01836
01837 THROWINFO( RET_OPTIMAL_SOLUTION_FOUND );
01838
01839 if ( printIteration( iter,BC_idx,BC_status,BC_isBound ) != SUCCESSFUL_RETURN )
01840 THROWERROR( RET_PRINT_ITERATION_FAILED );
01841
01842 nWSR = iter;
01843 if ( cputime != 0 )
01844 *cputime = getCPUtime( ) - starttime;
01845
01846 delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
01847 delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
01848
01849 return SUCCESSFUL_RETURN;
01850 }
01851
01852
01853
01854 returnvalue = changeActiveSet( BC_idx,BC_status,BC_isBound );
01855 if ( returnvalue != SUCCESSFUL_RETURN )
01856 {
01857 delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
01858 delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
01859
01860
01861 nWSR = iter;
01862 if ( cputime != 0 )
01863 *cputime = getCPUtime( ) - starttime;
01864
01865
01866 if ( isInfeasible( ) == BT_TRUE )
01867 {
01868 status = QPS_HOMOTOPYQPSOLVED;
01869 return setInfeasibilityFlag( RET_HOTSTART_STOPPED_INFEASIBILITY );
01870 }
01871
01872
01873 if ( unbounded == BT_TRUE )
01874 return THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS );
01875
01876
01877 THROWERROR( RET_HOMOTOPY_STEP_FAILED );
01878 return returnvalue;
01879 }
01880
01881
01882 if (options.enableCholeskyRefactorisation > 0 && iter % options.enableCholeskyRefactorisation == 0)
01883 {
01884 returnvalue = setupCholeskyDecompositionProjected();
01885 if (returnvalue != SUCCESSFUL_RETURN)
01886 {
01887 delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
01888 delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
01889 return returnvalue;
01890 }
01891 }
01892
01893
01894 #ifdef __DEBUG_ITER__
01895 nFR = getNFR();
01896 nAC = getNAC();
01897 int i;
01898 real_t stat, bfeas, cfeas, bcmpl, ccmpl, errUnitary, errTQ, Tmaxomin;
01899 real_t *grad = new real_t[nV];
01900 real_t *AX = new real_t[nC];
01901 real_t Tmin, Tmax;
01902
01903 stat = bfeas = cfeas = bcmpl = ccmpl = errUnitary = errTQ = Tmaxomin = 0.0;
01904
01905
01906 for (i = 0; i < nV; i++) grad[i] = g[i] - y[i];
01907 H->times(1, 1.0, x, nV, 1.0, grad, nV);
01908 A->transTimes(1, -1.0, y+nV, nC, 1.0, grad, nV);
01909 for (i = 0; i < nV; i++) if (fabs(grad[i]) > stat) stat = fabs(grad[i]);
01910
01911
01912 for (i = 0; i < nV; i++) if (lb[i] - x[i] > bfeas) bfeas = lb[i] - x[i];
01913 for (i = 0; i < nV; i++) if (x[i] - ub[i] > bfeas) bfeas = x[i] - ub[i];
01914 A->times(1, 1.0, x, nV, 0.0, AX, nC);
01915 for (i = 0; i < nC; i++) if (lbA[i] - AX[i] > cfeas) cfeas = lbA[i] - AX[i];
01916 for (i = 0; i < nC; i++) if (AX[i] - ubA[i] > cfeas) cfeas = AX[i] - ubA[i];
01917
01918
01919 for (i = 0; i < nV; i++) if (y[i] > +EPS && fabs((lb[i] - x[i])*y[i]) > bcmpl) bcmpl = fabs((lb[i] - x[i])*y[i]);
01920 for (i = 0; i < nV; i++) if (y[i] < -EPS && fabs((ub[i] - x[i])*y[i]) > bcmpl) bcmpl = fabs((ub[i] - x[i])*y[i]);
01921 for (i = 0; i < nC; i++) if (y[nV+i] > +EPS && fabs((lbA[i]-AX[i])*y[nV+i]) > ccmpl) ccmpl = fabs((lbA[i]-AX[i])*y[nV+i]);
01922 for (i = 0; i < nC; i++) if (y[nV+i] < -EPS && fabs((ubA[i]-AX[i])*y[nV+i]) > ccmpl) ccmpl = fabs((ubA[i]-AX[i])*y[nV+i]);
01923
01924 Tmin = 1.0e16; Tmax = 0.0;
01925 for (i = 0; i < nAC; i++)
01926 if (fabs(TT(i,sizeT-i-1)) < Tmin)
01927 Tmin = fabs(TT(i,sizeT-i-1));
01928 else if (fabs(TT(i,sizeT-i-1)) > Tmax)
01929 Tmax = fabs(TT(i,sizeT-i-1));
01930 Tmaxomin = Tmax/Tmin;
01931
01932 if (iter % 10 == 0)
01933 fprintf(stderr, "\n%5s %4s %4s %4s %4s %9s %9s %9s %9s %9s %9s %9s %9s\n",
01934 "iter", "addB", "remB", "addC", "remC", "hom len", "tau", "stat",
01935 "bfeas", "cfeas", "bcmpl", "ccmpl", "Tmin");
01936 fprintf(stderr, "%5d ", iter);
01937 if (idxAddB >= 0) fprintf(stderr, "%4d ", idxAddB);
01938 else fprintf(stderr, "%4s ", " ");
01939 if (idxRemB >= 0) fprintf(stderr, "%4d ", idxRemB);
01940 else fprintf(stderr, "%4s ", " ");
01941 if (idxAddC >= 0) fprintf(stderr, "%4d ", idxAddC);
01942 else fprintf(stderr, "%4s ", " ");
01943 if (idxRemC >= 0) fprintf(stderr, "%4d ", idxRemC);
01944 else fprintf(stderr, "%4s ", " ");
01945 fprintf(stderr, "%9.2e %9.2e %9.2e %9.2e %9.2e %9.2e %9.2e %9.2e\n",
01946 homotopyLength, tau, stat, bfeas, cfeas, bcmpl, ccmpl, Tmin);
01947
01948 delete[] AX;
01949 delete[] grad;
01950 #else
01951 if (options.printLevel == -1)
01952 {
01953 if (iter % 10 == 0)
01954 fprintf(stdout, "\n%5s %5s %5s %5s %5s %9s %9s\n",
01955 "iter", "addB", "remB", "addC", "remC", "hom len", "tau");
01956 fprintf(stdout, "%5d ", iter);
01957 if (idxAddB >= 0) fprintf(stdout, "%5d ", idxAddB);
01958 else fprintf(stdout, "%5s ", " ");
01959 if (idxRemB >= 0) fprintf(stdout, "%5d ", idxRemB);
01960 else fprintf(stdout, "%5s ", " ");
01961 if (idxAddC >= 0) fprintf(stdout, "%5d ", idxAddC);
01962 else fprintf(stdout, "%5s ", " ");
01963 if (idxRemC >= 0) fprintf(stdout, "%5d ", idxRemC);
01964 else fprintf(stdout, "%5s ", " ");
01965 fprintf(stdout, "%9.2e %9.2e\n", homotopyLength, tau);
01966 }
01967 #endif
01968
01969
01970
01971 status = QPS_HOMOTOPYQPSOLVED;
01972
01973 if ( printIteration( iter,BC_idx,BC_status,BC_isBound ) != SUCCESSFUL_RETURN )
01974 THROWERROR( RET_PRINT_ITERATION_FAILED );
01975
01976
01977 if ( ( tau < EPS ) && ( options.enableRamping == BT_TRUE ) )
01978 performRamping( );
01979 else
01980 if ( (options.enableDriftCorrection > 0)
01981 && ((iter+1) % options.enableDriftCorrection == 0) )
01982 performDriftCorrection( );
01983 }
01984
01985 delete[] delta_yAC; delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
01986 delete[] delta_ub; delete[] delta_lb; delete[] delta_ubA; delete[] delta_lbA; delete[] delta_g;
01987
01988
01989 if ( cputime != 0 )
01990 *cputime = getCPUtime( ) - starttime;
01991
01992
01993
01994
01995 if ( options.printLevel == PL_HIGH )
01996 {
01997 #ifndef __XPCTARGET__
01998 snprintf( messageString,80,"(nWSR = %d)",iter );
01999 return getGlobalMessageHandler( )->throwWarning( RET_MAX_NWSR_REACHED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
02000 #else
02001 return RET_MAX_NWSR_REACHED;
02002 #endif
02003 }
02004 else
02005 {
02006 return RET_MAX_NWSR_REACHED;
02007 }
02008 }
02009
02010
02011
02012
02013
02014 returnValue QProblem::solveRegularisedQP( const real_t* const g_new,
02015 const real_t* const lb_new, const real_t* const ub_new,
02016 const real_t* const lbA_new, const real_t* const ubA_new,
02017 int& nWSR, real_t* const cputime, int nWSRperformed
02018 )
02019 {
02020 int i, step;
02021 int nV = getNV( );
02022
02023
02024
02025 if ( usingRegularisation( ) == BT_FALSE )
02026 return solveQP( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime,nWSRperformed );
02027
02028
02029
02030 returnValue returnvalue;
02031
02032 int nWSR_max = nWSR;
02033 int nWSR_total = nWSRperformed;
02034
02035 real_t cputime_total = 0.0;
02036 real_t cputime_cur = 0.0;
02037
02038 if ( cputime == 0 )
02039 {
02040 returnvalue = solveQP( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0,nWSRperformed );
02041 }
02042 else
02043 {
02044 cputime_cur = *cputime;
02045 returnvalue = solveQP( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,&cputime_cur,nWSRperformed );
02046 }
02047 nWSR_total = nWSR;
02048 cputime_total += cputime_cur;
02049
02050
02051 if ( returnvalue != SUCCESSFUL_RETURN )
02052 {
02053 if ( cputime != 0 )
02054 *cputime = cputime_total;
02055
02056 if ( returnvalue == RET_MAX_NWSR_REACHED )
02057 THROWWARNING( RET_NO_REGSTEP_NWSR );
02058
02059 return returnvalue;
02060 }
02061
02062
02063
02064 real_t* gMod = new real_t[nV];
02065
02066 for( step=0; step<options.numRegularisationSteps; ++step )
02067 {
02068
02069
02070 for( i=0; i<nV; ++i )
02071 gMod[i] = g_new[i] - options.epsRegularisation*x[i];
02072
02073
02074
02075
02076 if ( cputime == 0 )
02077 {
02078 nWSR = nWSR_max;
02079 returnvalue = solveQP( gMod,lb_new,ub_new,lbA_new,ubA_new, nWSR,0,nWSR_total );
02080 }
02081 else
02082 {
02083 nWSR = nWSR_max;
02084 cputime_cur = *cputime - cputime_total;
02085 returnvalue = solveQP( gMod,lb_new,ub_new,lbA_new,ubA_new, nWSR,&cputime_cur,nWSR_total );
02086 }
02087
02088 nWSR_total = nWSR;
02089 cputime_total += cputime_cur;
02090
02091
02092 if ( returnvalue != SUCCESSFUL_RETURN )
02093 {
02094 delete[] gMod;
02095
02096 if ( cputime != 0 )
02097 *cputime = cputime_total;
02098
02099 if ( returnvalue == RET_MAX_NWSR_REACHED )
02100 THROWWARNING( RET_FEWER_REGSTEPS_NWSR );
02101
02102 return returnvalue;
02103 }
02104 }
02105
02106 delete[] gMod;
02107
02108 if ( cputime != 0 )
02109 *cputime = cputime_total;
02110
02111 return SUCCESSFUL_RETURN;
02112 }
02113
02114
02115
02116
02117
02118 returnValue QProblem::setupSubjectToType( )
02119 {
02120 return setupSubjectToType( lb,ub,lbA,ubA );
02121 }
02122
02123
02124
02125
02126
02127 returnValue QProblem::setupSubjectToType( const real_t* const lb_new, const real_t* const ub_new,
02128 const real_t* const lbA_new, const real_t* const ubA_new
02129 )
02130 {
02131 int i;
02132 int nC = getNC( );
02133
02134
02135
02136 if ( QProblemB::setupSubjectToType( lb_new,ub_new ) != SUCCESSFUL_RETURN )
02137 return THROWERROR( RET_SETUPSUBJECTTOTYPE_FAILED );
02138
02139
02140
02141
02142 constraints.setNoLower( BT_TRUE );
02143 if ( lbA_new != 0 )
02144 {
02145 for( i=0; i<nC; ++i )
02146 {
02147 if ( lbA_new[i] > -INFTY )
02148 {
02149 constraints.setNoLower( BT_FALSE );
02150 break;
02151 }
02152 }
02153 }
02154
02155
02156 constraints.setNoUpper( BT_TRUE );
02157 if ( ubA_new != 0 )
02158 {
02159 for( i=0; i<nC; ++i )
02160 {
02161 if ( ubA_new[i] < INFTY )
02162 {
02163 constraints.setNoUpper( BT_FALSE );
02164 break;
02165 }
02166 }
02167 }
02168
02169
02170 if ( ( lbA_new != 0 ) && ( ubA_new != 0 ) )
02171 {
02172 for( i=0; i<nC; ++i )
02173 {
02174 if ( ( lbA_new[i] < -INFTY + options.boundTolerance ) && ( ubA_new[i] > INFTY - options.boundTolerance )
02175 && (options.enableFarBounds == BT_FALSE))
02176 {
02177 constraints.setType( i,ST_UNBOUNDED );
02178 }
02179 else
02180 {
02181 if ( options.enableEqualities && lbA_new[i] > ubA_new[i] - options.boundTolerance )
02182 constraints.setType( i,ST_EQUALITY );
02183 else
02184 constraints.setType( i,ST_BOUNDED );
02185 }
02186 }
02187 }
02188 else
02189 {
02190 if ( ( lbA_new == 0 ) && ( ubA_new == 0 ) )
02191 {
02192 for( i=0; i<nC; ++i )
02193 constraints.setType( i,ST_UNBOUNDED );
02194 }
02195 else
02196 {
02197 for( i=0; i<nC; ++i )
02198 constraints.setType( i,ST_BOUNDED );
02199 }
02200 }
02201
02202 return SUCCESSFUL_RETURN;
02203 }
02204
02205
02206
02207
02208
02209 returnValue QProblem::setupCholeskyDecompositionProjected( )
02210 {
02211 int i, j;
02212 int nV = getNV( );
02213 int nZ = getNZ( );
02214
02215
02216 for( i=0; i<nV*nV; ++i )
02217 R[i] = 0.0;
02218
02219
02220 if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_IDENTITY ) )
02221 {
02222 if ( hessianType == HST_ZERO )
02223 {
02224
02225
02226
02227 if ( usingRegularisation( ) == BT_TRUE )
02228 {
02229 for( i=0; i<nV; ++i )
02230 RR(i,i) = sqrt( options.epsRegularisation );
02231 }
02232 else
02233 return THROWERROR( RET_CHOLESKY_OF_ZERO_HESSIAN );
02234 }
02235 else
02236 {
02237
02238 for( i=0; i<nV; ++i )
02239 RR(i,i) = 1.0;
02240 }
02241 }
02242 else
02243 {
02244 if ( nZ > 0 )
02245 {
02246 int* FR_idx;
02247 bounds.getFree( )->getNumberArray( &FR_idx );
02248
02249 int* AC_idx;
02250 constraints.getActive( )->getNumberArray( &AC_idx );
02251
02252
02253 if (constraints.getActive ()->getLength () == 0) {
02254
02255 for ( j=0; j < nZ; ++j ) {
02256 for ( i=0; i < nV; ++i )
02257 QQ(i,j) = 0.0;
02258 QQ(FR_idx[j],j) = 1.0;
02259 }
02260
02261 int nFR = getNFR ();
02262 for ( j=0; j < nFR; ++j )
02263 H->getCol (FR_idx[j], bounds.getFree (), 1.0, &R[j*nV]);
02264 } else {
02265
02266 H->bilinear(bounds.getFree(), nZ, Q, nV, R, nV);
02267 }
02268
02269
02270 long info = 0;
02271 unsigned long _nZ = nZ, _nV = nV;
02272
02273 POTRF ("U", &_nZ, R, &_nV, &info);
02274
02275
02276 if (info > 0) {
02277 hessianType = HST_SEMIDEF;
02278 return RET_HESSIAN_NOT_SPD;
02279 }
02280
02281
02282 for (i=0;i<nZ-1;++i)
02283 RR(i+1,i) = 0.0;
02284 }
02285 }
02286
02287 return SUCCESSFUL_RETURN;
02288 }
02289
02290
02291
02292
02293
02294 returnValue QProblem::setupTQfactorisation( )
02295 {
02296 int i, ii;
02297 int nV = getNV( );
02298 int nFR = getNFR( );
02299
02300 int* FR_idx;
02301 bounds.getFree( )->getNumberArray( &FR_idx );
02302
02303
02304 for( i=0; i<nV*nV; ++i )
02305 Q[i] = 0.0;
02306
02307 for( i=0; i<nFR; ++i )
02308 {
02309 ii = FR_idx[i];
02310 QQ(ii,i) = 1.0;
02311 }
02312
02313
02314 for( i=0; i<sizeT*sizeT; ++i )
02315 T[i] = 0.0;
02316
02317 return SUCCESSFUL_RETURN;
02318 }
02319
02320
02321
02322
02323
02324 returnValue QProblem::obtainAuxiliaryWorkingSet( const real_t* const xOpt, const real_t* const yOpt,
02325 const Bounds* const guessedBounds, const Constraints* const guessedConstraints,
02326 Bounds* auxiliaryBounds, Constraints* auxiliaryConstraints
02327 ) const
02328 {
02329 int i = 0;
02330 int nV = getNV( );
02331 int nC = getNC( );
02332
02333
02334
02335 if ( ( auxiliaryBounds == 0 ) || ( auxiliaryBounds == guessedBounds ) )
02336 return THROWERROR( RET_INVALID_ARGUMENTS );
02337
02338 if ( ( auxiliaryConstraints == 0 ) || ( auxiliaryConstraints == guessedConstraints ) )
02339 return THROWERROR( RET_INVALID_ARGUMENTS );
02340
02341
02342 SubjectToStatus guessedStatus;
02343
02344
02345 if ( QProblemB::obtainAuxiliaryWorkingSet( xOpt,yOpt,guessedBounds, auxiliaryBounds ) != SUCCESSFUL_RETURN )
02346 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
02347
02348
02349 if ( guessedConstraints != 0 )
02350 {
02351
02352
02353 for( i=0; i<nC; ++i )
02354 {
02355
02356 guessedStatus = guessedConstraints->getStatus( i );
02357
02358 #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
02359 if ( constraints.getType( i ) == ST_EQUALITY )
02360 {
02361 if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
02362 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
02363 }
02364 else
02365 #endif
02366 {
02367 if ( auxiliaryConstraints->setupConstraint( i,guessedStatus ) != SUCCESSFUL_RETURN )
02368 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
02369 }
02370 }
02371 }
02372 else
02373 {
02374
02375 if ( ( xOpt != 0 ) && ( yOpt == 0 ) )
02376 {
02377 for( i=0; i<nC; ++i )
02378 {
02379 if ( Ax[i] - lbA[i] <= options.boundTolerance )
02380 {
02381 if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
02382 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
02383 continue;
02384 }
02385
02386 if ( ubA[i] - Ax_u[i] <= options.boundTolerance )
02387 {
02388 if ( auxiliaryConstraints->setupConstraint( i,ST_UPPER ) != SUCCESSFUL_RETURN )
02389 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
02390 continue;
02391 }
02392
02393
02394 #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
02395 if ( constraints.getType( i ) == ST_EQUALITY )
02396 {
02397 if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
02398 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
02399 }
02400 else
02401 #endif
02402 {
02403 if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
02404 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
02405 }
02406 }
02407 }
02408
02409
02410 if ( ( xOpt == 0 ) && ( yOpt != 0 ) )
02411 {
02412 for( i=0; i<nC; ++i )
02413 {
02414 if ( yOpt[nV+i] > EPS )
02415 {
02416 if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
02417 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
02418 continue;
02419 }
02420
02421 if ( yOpt[nV+i] < -EPS )
02422 {
02423 if ( auxiliaryConstraints->setupConstraint( i,ST_UPPER ) != SUCCESSFUL_RETURN )
02424 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
02425 continue;
02426 }
02427
02428
02429 #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
02430 if ( constraints.getType( i ) == ST_EQUALITY )
02431 {
02432 if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
02433 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
02434 }
02435 else
02436 #endif
02437 {
02438 if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
02439 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
02440 }
02441 }
02442 }
02443
02444
02445
02446
02447 if ( ( xOpt == 0 ) && ( yOpt == 0 ) )
02448 {
02449 for( i=0; i<nC; ++i )
02450 {
02451
02452 #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
02453 if ( constraints.getType( i ) == ST_EQUALITY )
02454 {
02455 if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
02456 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
02457 }
02458 else
02459 #endif
02460 {
02461 if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
02462 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
02463 }
02464 }
02465 }
02466 }
02467
02468 return SUCCESSFUL_RETURN;
02469 }
02470
02471
02472
02473
02474
02475
02476 returnValue QProblem::setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds,
02477 const Constraints* const auxiliaryConstraints,
02478 BooleanType setupAfresh
02479 )
02480 {
02481 int i;
02482 int nV = getNV( );
02483 int nC = getNC( );
02484 BooleanType WSisTrivial = BT_TRUE;
02485
02486
02487 if ( auxiliaryBounds != 0 )
02488 {
02489 for( i=0; i<nV; ++i )
02490 if ( ( bounds.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryBounds->getStatus( i ) == ST_UNDEFINED ) )
02491 return THROWERROR( RET_UNKNOWN_BUG );
02492 }
02493 else
02494 {
02495 return THROWERROR( RET_INVALID_ARGUMENTS );
02496 }
02497
02498 if ( auxiliaryConstraints != 0 )
02499 {
02500 for( i=0; i<nC; ++i )
02501 if ( ( constraints.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryConstraints->getStatus( i ) == ST_UNDEFINED ) )
02502 return THROWERROR( RET_UNKNOWN_BUG );
02503 }
02504 else
02505 {
02506 return THROWERROR( RET_INVALID_ARGUMENTS );
02507 }
02508
02509
02510 for (i = 0; i < nV; i++)
02511 if (auxiliaryBounds->getStatus(i) == ST_INACTIVE)
02512 {
02513 WSisTrivial = BT_FALSE;
02514 break;
02515 }
02516 for (i = 0; i < nC; i++)
02517 if (auxiliaryConstraints->getStatus(i) != ST_INACTIVE)
02518 {
02519 WSisTrivial = BT_FALSE;
02520 break;
02521 }
02522
02523 if (WSisTrivial == BT_TRUE)
02524 {
02525 for (i = 0; i < nV; i++)
02526 bounds.moveFreeToFixed(i, auxiliaryBounds->getStatus(i));
02527
02528 return SUCCESSFUL_RETURN;
02529 }
02530
02531
02532
02533
02534
02535 BooleanType updateCholesky;
02536 if ( setupAfresh == BT_TRUE )
02537 updateCholesky = BT_FALSE;
02538 else
02539 updateCholesky = BT_TRUE;
02540
02541
02542 BooleanType was_fulli = options.enableFullLITests;
02543 real_t backupEpsLITests = options.epsLITests;
02544
02545 options.enableFullLITests = BT_FALSE;
02546
02547
02548
02549 if ( setupAfresh == BT_FALSE )
02550 {
02551
02552
02553 for( i=0; i<nC; ++i )
02554 {
02555 if ( ( constraints.getStatus( i ) == ST_LOWER ) && ( auxiliaryConstraints->getStatus( i ) != ST_LOWER ) )
02556 if ( removeConstraint( i,updateCholesky,BT_FALSE,options.enableNZCTests ) != SUCCESSFUL_RETURN )
02557 return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
02558
02559 if ( ( constraints.getStatus( i ) == ST_UPPER ) && ( auxiliaryConstraints->getStatus( i ) != ST_UPPER ) )
02560 if ( removeConstraint( i,updateCholesky,BT_FALSE,options.enableNZCTests ) != SUCCESSFUL_RETURN )
02561 return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
02562 }
02563
02564
02565
02566 for( i=0; i<nV; ++i )
02567 {
02568 if ( ( bounds.getStatus( i ) == ST_LOWER ) && ( auxiliaryBounds->getStatus( i ) != ST_LOWER ) )
02569 if ( removeBound( i,updateCholesky,BT_FALSE,options.enableNZCTests ) != SUCCESSFUL_RETURN )
02570 return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
02571
02572 if ( ( bounds.getStatus( i ) == ST_UPPER ) && ( auxiliaryBounds->getStatus( i ) != ST_UPPER ) )
02573 if ( removeBound( i,updateCholesky,BT_FALSE,options.enableNZCTests ) != SUCCESSFUL_RETURN )
02574 return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
02575 }
02576 }
02577
02578
02579
02580
02581
02582 for( i=0; i<nV; ++i )
02583 {
02584 if ( ( bounds.getType( i ) == ST_EQUALITY ) && ( ( bounds.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE ) ) )
02585 {
02586
02587 if ( addBound( i,auxiliaryBounds->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
02588 return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
02589 }
02590 }
02591
02592
02593 for( i=0; i<nC; ++i )
02594 {
02595 if ( ( constraints.getType( i ) == ST_EQUALITY ) && ( ( constraints.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryConstraints->getStatus( i ) != ST_INACTIVE ) ) )
02596 {
02597
02598 if ( addConstraint_checkLI( i ) == RET_LINEARLY_INDEPENDENT )
02599 {
02600 if ( addConstraint( i,auxiliaryConstraints->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
02601 return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
02602 }
02603 else
02604 {
02605
02606 constraints.setType(i, ST_BOUNDED);
02607 }
02608 }
02609 }
02610
02611
02612
02613
02614 for( i=0; i<nV; ++i )
02615 {
02616 if ( ( bounds.getType( i ) != ST_EQUALITY ) && ( ( bounds.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE ) ) )
02617 {
02618
02619 if ( addBound_checkLI( i ) == RET_LINEARLY_INDEPENDENT )
02620 {
02621 if ( addBound( i,auxiliaryBounds->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
02622 return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
02623 }
02624 }
02625 }
02626
02627
02628
02629 for( i=0; i<nC; ++i )
02630 {
02631 if ( ( constraints.getType( i ) != ST_EQUALITY ) && ( auxiliaryConstraints->getStatus( i ) != ST_INACTIVE ) )
02632 {
02633
02634 if ( constraints.getStatus( i ) == ST_INACTIVE )
02635 {
02636
02637 if ( addConstraint_checkLI( i ) == RET_LINEARLY_INDEPENDENT )
02638 {
02639 if ( addConstraint( i,auxiliaryConstraints->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
02640 return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
02641 }
02642 }
02643 }
02644 }
02645
02646 options.enableFullLITests = was_fulli;
02647 options.epsLITests = backupEpsLITests;
02648
02649 return SUCCESSFUL_RETURN;
02650 }
02651
02652
02653
02654
02655
02656 returnValue QProblem::setupAuxiliaryQPsolution( const real_t* const xOpt, const real_t* const yOpt
02657 )
02658 {
02659 int i, j;
02660 int nV = getNV( );
02661 int nC = getNC( );
02662
02663
02664
02665
02666
02667 if ( xOpt != 0 )
02668 {
02669 if ( xOpt != x )
02670 for( i=0; i<nV; ++i )
02671 x[i] = xOpt[i];
02672
02673 A->times(1, 1.0, x, nV, 0.0, Ax, nC);
02674
02675 for ( j=0; j<nC; ++j )
02676 {
02677 Ax_l[j] = Ax[j];
02678 Ax_u[j] = Ax[j];
02679 }
02680 }
02681 else
02682 {
02683 for( i=0; i<nV; ++i )
02684 x[i] = 0.0;
02685
02686 for ( j=0; j<nC; ++j )
02687 {
02688 Ax[j] = 0.0;
02689 Ax_l[j] = 0.0;
02690 Ax_u[j] = 0.0;
02691 }
02692 }
02693
02694 if ( yOpt != 0 )
02695 {
02696 if ( yOpt != y )
02697 for( i=0; i<nV+nC; ++i )
02698 y[i] = yOpt[i];
02699 }
02700 else
02701 {
02702 for( i=0; i<nV+nC; ++i )
02703 y[i] = 0.0;
02704 }
02705
02706 return SUCCESSFUL_RETURN;
02707 }
02708
02709
02710
02711
02712
02713 returnValue QProblem::setupAuxiliaryQPgradient( )
02714 {
02715 int i;
02716 int nV = getNV( );
02717 int nC = getNC( );
02718
02719
02720
02721
02722 switch ( hessianType )
02723 {
02724 case HST_ZERO:
02725 if ( usingRegularisation( ) == BT_FALSE )
02726 for ( i=0; i<nV; ++i )
02727 g[i] = y[i];
02728 else
02729 for ( i=0; i<nV; ++i )
02730 g[i] = y[i] - options.epsRegularisation*x[i];
02731 break;
02732
02733 case HST_IDENTITY:
02734 for ( i=0; i<nV; ++i )
02735 g[i] = y[i] - x[i];
02736 break;
02737
02738 default:
02739
02740 for ( i=0; i<nV; ++i )
02741 g[i] = y[i];
02742
02743
02744 H->times(1, -1.0, x, nV, 1.0, g, nV);
02745 break;
02746 }
02747
02748
02749 A->transTimes(1, 1.0, y + nV, nC, 1.0, g, nV);
02750
02751 return SUCCESSFUL_RETURN;
02752 }
02753
02754
02755
02756
02757
02758 returnValue QProblem::setupAuxiliaryQPbounds( const Bounds* const auxiliaryBounds,
02759 const Constraints* const auxiliaryConstraints,
02760 BooleanType useRelaxation
02761 )
02762 {
02763 int i;
02764 int nV = getNV( );
02765 int nC = getNC( );
02766
02767
02768
02769 for ( i=0; i<nV; ++i )
02770 {
02771 switch ( bounds.getStatus( i ) )
02772 {
02773 case ST_INACTIVE:
02774 if ( useRelaxation == BT_TRUE )
02775 {
02776 if ( bounds.getType( i ) == ST_EQUALITY )
02777 {
02778 lb[i] = x[i];
02779 ub[i] = x[i];
02780 }
02781 else
02782 {
02783
02784
02785
02786 if ( auxiliaryBounds->getStatus( i ) == ST_LOWER )
02787 lb[i] = x[i];
02788 else
02789 lb[i] = x[i] - options.boundRelaxation;
02790
02791 if ( auxiliaryBounds->getStatus( i ) == ST_UPPER )
02792 ub[i] = x[i];
02793 else
02794 ub[i] = x[i] + options.boundRelaxation;
02795 }
02796 }
02797 break;
02798
02799 case ST_LOWER:
02800 lb[i] = x[i];
02801 if ( bounds.getType( i ) == ST_EQUALITY )
02802 {
02803 ub[i] = x[i];
02804 }
02805 else
02806 {
02807 if ( useRelaxation == BT_TRUE )
02808 ub[i] = x[i] + options.boundRelaxation;
02809 }
02810 break;
02811
02812 case ST_UPPER:
02813 ub[i] = x[i];
02814 if ( bounds.getType( i ) == ST_EQUALITY )
02815 {
02816 lb[i] = x[i];
02817 }
02818 else
02819 {
02820 if ( useRelaxation == BT_TRUE )
02821 lb[i] = x[i] - options.boundRelaxation;
02822 }
02823 break;
02824
02825 default:
02826 return THROWERROR( RET_UNKNOWN_BUG );
02827 }
02828 }
02829
02830
02831 for ( i=0; i<nC; ++i )
02832 {
02833 switch ( constraints.getStatus( i ) )
02834 {
02835 case ST_INACTIVE:
02836 if ( useRelaxation == BT_TRUE )
02837 {
02838 if ( constraints.getType( i ) == ST_EQUALITY )
02839 {
02840 lbA[i] = Ax_l[i];
02841 ubA[i] = Ax_u[i];
02842 }
02843 else
02844 {
02845
02846
02847
02848 if ( auxiliaryConstraints->getStatus( i ) == ST_LOWER )
02849 lbA[i] = Ax_l[i];
02850 else
02851 lbA[i] = Ax_l[i] - options.boundRelaxation;
02852
02853 if ( auxiliaryConstraints->getStatus( i ) == ST_UPPER )
02854 ubA[i] = Ax_u[i];
02855 else
02856 ubA[i] = Ax_u[i] + options.boundRelaxation;
02857 }
02858 }
02859 break;
02860
02861 case ST_LOWER:
02862 lbA[i] = Ax_l[i];
02863 if ( constraints.getType( i ) == ST_EQUALITY )
02864 {
02865 ubA[i] = Ax_l[i];
02866 }
02867 else
02868 {
02869 if ( useRelaxation == BT_TRUE )
02870 ubA[i] = Ax_l[i] + options.boundRelaxation;
02871 }
02872 break;
02873
02874 case ST_UPPER:
02875 ubA[i] = Ax_u[i];
02876 if ( constraints.getType( i ) == ST_EQUALITY )
02877 {
02878 lbA[i] = Ax_u[i];
02879 }
02880 else
02881 {
02882 if ( useRelaxation == BT_TRUE )
02883 lbA[i] = Ax_u[i] - options.boundRelaxation;
02884 }
02885 break;
02886
02887 default:
02888 return THROWERROR( RET_UNKNOWN_BUG );
02889 }
02890 Ax_l[i] = Ax_l[i] - lbA[i];
02891 Ax_u[i] = ubA[i] - Ax_u[i];
02892 }
02893
02894 return SUCCESSFUL_RETURN;
02895 }
02896
02897
02898
02899
02900
02901 returnValue QProblem::addConstraint( int number, SubjectToStatus C_status,
02902 BooleanType updateCholesky,
02903 BooleanType ensureLI
02904 )
02905 {
02906 int i, j, ii;
02907
02908
02909 if ( constraints.getStatus( number ) != ST_INACTIVE )
02910 return THROWERROR( RET_CONSTRAINT_ALREADY_ACTIVE );
02911
02912 if ( ( constraints.getNC( ) - getNAC( ) ) == constraints.getNUC( ) )
02913 return THROWERROR( RET_ALL_CONSTRAINTS_ACTIVE );
02914
02915 if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
02916 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
02917 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
02918 ( getStatus( ) == QPS_SOLVED ) )
02919 {
02920 return THROWERROR( RET_UNKNOWN_BUG );
02921 }
02922
02923
02924
02925
02926
02927 if ( updateCholesky == BT_TRUE && ensureLI == BT_TRUE )
02928 {
02929 returnValue ensureLIreturnvalue = addConstraint_ensureLI( number,C_status );
02930
02931 switch ( ensureLIreturnvalue )
02932 {
02933 case SUCCESSFUL_RETURN:
02934 break;
02935
02936 case RET_LI_RESOLVED:
02937 break;
02938
02939 case RET_ENSURELI_FAILED_NOINDEX:
02940 return RET_ADDCONSTRAINT_FAILED_INFEASIBILITY;
02941
02942 case RET_ENSURELI_FAILED_CYCLING:
02943 return RET_ADDCONSTRAINT_FAILED_INFEASIBILITY;
02944
02945 default:
02946 return THROWERROR( RET_ENSURELI_FAILED );
02947 }
02948 }
02949
02950
02951 int nV = getNV( );
02952 int nFR = getNFR( );
02953 int nAC = getNAC( );
02954 int nZ = getNZ( );
02955
02956 int tcol = sizeT - nAC;
02957
02958
02959 int* FR_idx;
02960 bounds.getFree( )->getNumberArray( &FR_idx );
02961
02962 real_t* aFR = new real_t[nFR];
02963 real_t* wZ = new real_t[nZ];
02964 for( i=0; i<nZ; ++i )
02965 wZ[i] = 0.0;
02966
02967
02968
02969
02970 A->getRow(number, bounds.getFree(), 1.0, aFR);
02971
02972
02973 for( i=0; i<nFR; ++i )
02974 {
02975 ii = FR_idx[i];
02976 for( j=0; j<nZ; ++j )
02977 wZ[j] += aFR[i] * QQ(ii,j);
02978 }
02979
02980
02981 if ( nAC > 0 )
02982 {
02983 for( j=0; j<nAC; ++j )
02984 TT(nAC,tcol+j) = 0.0;
02985 for( i=0; i<nFR; ++i )
02986 {
02987 ii = FR_idx[i];
02988 for( j=0; j<nAC; ++j )
02989 TT(nAC,tcol+j) += aFR[i] * QQ(ii,nZ+j);
02990 }
02991 }
02992
02993 delete[] aFR;
02994
02995
02996 real_t c, s, nu;
02997
02998 if ( nZ > 0 )
02999 {
03000
03001
03002
03003 for( j=0; j<nZ-1; ++j )
03004 {
03005 computeGivens( wZ[j+1],wZ[j], wZ[j+1],wZ[j],c,s );
03006 nu = s/(1.0+c);
03007
03008 for( i=0; i<nFR; ++i )
03009 {
03010 ii = FR_idx[i];
03011 applyGivens( c,s,nu,QQ(ii,1+j),QQ(ii,j), QQ(ii,1+j),QQ(ii,j) );
03012 }
03013
03014 if ( ( updateCholesky == BT_TRUE ) &&
03015 ( hessianType != HST_ZERO ) && ( hessianType != HST_IDENTITY ) )
03016 {
03017 for( i=0; i<=j+1; ++i )
03018 applyGivens( c,s,nu,RR(i,1+j),RR(i,j), RR(i,1+j),RR(i,j) );
03019 }
03020 }
03021
03022 TT(nAC,tcol-1) = wZ[nZ-1];
03023
03024
03025 if ( ( updateCholesky == BT_TRUE ) &&
03026 ( hessianType != HST_ZERO ) && ( hessianType != HST_IDENTITY ) )
03027 {
03028
03029
03030 for( i=0; i<nZ-1; ++i )
03031 {
03032 computeGivens( RR(i,i),RR(1+i,i), RR(i,i),RR(1+i,i),c,s );
03033 nu = s/(1.0+c);
03034
03035 for( j=(1+i); j<(nZ-1); ++j )
03036 applyGivens( c,s,nu,RR(i,j),RR(1+i,j), RR(i,j),RR(1+i,j) );
03037 }
03038
03039 for( i=0; i<nZ; ++i )
03040 RR(i,nZ-1) = 0.0;
03041 }
03042 }
03043
03044 delete[] wZ;
03045
03046
03047
03048 idxAddC = number;
03049 if ( constraints.moveInactiveToActive( number,C_status ) != SUCCESSFUL_RETURN )
03050 return THROWERROR( RET_ADDCONSTRAINT_FAILED );
03051
03052
03053 return SUCCESSFUL_RETURN;
03054 }
03055
03056
03057
03058
03059
03060
03061 returnValue QProblem::addConstraint_checkLI( int number )
03062 {
03063 returnValue returnvalue = RET_LINEARLY_DEPENDENT;
03064
03065 int i, j, ii;
03066 int nV = getNV( );
03067 int nFR = getNFR( );
03068 int nZ = getNZ( );
03069 int nC = getNC( );
03070 int nAC = getNAC();
03071 int nFX = getNFX();
03072 int *FR_idx;
03073
03074 bounds.getFree( )->getNumberArray( &FR_idx );
03075
03076
03077 if (options.enableFullLITests)
03078 {
03079
03080
03081
03082
03083
03084
03085 int *FX_idx, *AC_idx, *IAC_idx;
03086
03087 real_t *delta_g = new real_t[nV];
03088 real_t *delta_xFX = new real_t[nFX];
03089 real_t *delta_xFR = new real_t[nFR];
03090 real_t *delta_yAC = new real_t[nAC];
03091 real_t *delta_yFX = new real_t[nFX];
03092
03093 bounds.getFixed( )->getNumberArray( &FX_idx );
03094 constraints.getActive( )->getNumberArray( &AC_idx );
03095 constraints.getInactive( )->getNumberArray( &IAC_idx );
03096
03097 int dim = (nC>nV)?nC:nV;
03098 real_t *nul = new real_t[dim];
03099 for (ii = 0; ii < dim; ++ii)
03100 nul[ii]=0.0;
03101
03102 A->getRow (number, 0, 1.0, delta_g);
03103
03104 returnvalue = determineStepDirection ( delta_g,
03105 nul, nul, nul, nul,
03106 BT_FALSE, BT_FALSE,
03107 delta_xFX, delta_xFR, delta_yAC, delta_yFX);
03108 delete[] nul;
03109
03110
03111 real_t weight = 0.0;
03112 for (ii = 0; ii < nAC; ++ii)
03113 {
03114 real_t a = fabs (delta_yAC[ii]);
03115 if (weight < a) weight = a;
03116 }
03117 for (ii = 0; ii < nFX; ++ii)
03118 {
03119 real_t a = fabs (delta_yFX[ii]);
03120 if (weight < a) weight = a;
03121 }
03122
03123
03124 real_t zero = 0.0;
03125 for (ii = 0; ii < nFX; ++ii)
03126 {
03127 real_t a = fabs (delta_xFX[ii]);
03128 if (zero < a) zero = a;
03129 }
03130 for (ii = 0; ii < nFR; ++ii)
03131 {
03132 real_t a = fabs (delta_xFR[ii]);
03133 if (zero < a) zero = a;
03134 }
03135
03136
03137 if (zero > options.epsLITests * weight)
03138 returnvalue = RET_LINEARLY_INDEPENDENT;
03139
03140 delete[] delta_yFX;
03141 delete[] delta_yAC;
03142 delete[] delta_xFR;
03143 delete[] delta_xFX;
03144 delete[] delta_g;
03145
03146 }
03147 else
03148 {
03149
03150
03151
03152
03153
03154
03155 real_t *Arow = new real_t[nFR];
03156 A->getRow(number, bounds.getFree(), 1.0, Arow);
03157
03158 real_t sum, l2;
03159
03160 l2 = 0.0;
03161 for (i = 0; i < nFR; i++)
03162 l2 += Arow[i]*Arow[i];
03163
03164 for( j=0; j<nZ; ++j )
03165 {
03166 sum = 0.0;
03167 for( i=0; i<nFR; ++i )
03168 {
03169 ii = FR_idx[i];
03170 sum += Arow[i] * QQ(ii,j);
03171 }
03172
03173 if ( fabs( sum ) > options.epsLITests*l2 )
03174 {
03175
03176 returnvalue = RET_LINEARLY_INDEPENDENT;
03177 break;
03178 }
03179 }
03180
03181 delete[] Arow;
03182 }
03183
03184 return THROWINFO( returnvalue );
03185 }
03186
03187
03188
03189
03190
03191 returnValue QProblem::addConstraint_ensureLI( int number, SubjectToStatus C_status )
03192 {
03193 int i, j, ii, jj;
03194 int nV = getNV( );
03195 int nFR = getNFR( );
03196 int nFX = getNFX( );
03197 int nAC = getNAC( );
03198 int nZ = getNZ( );
03199
03200
03201
03202 returnValue returnvalueCheckLI = addConstraint_checkLI( number );
03203
03204 if ( returnvalueCheckLI == RET_INDEXLIST_CORRUPTED )
03205 return THROWERROR( RET_ENSURELI_FAILED );
03206
03207 if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )
03208 return SUCCESSFUL_RETURN;
03209
03210
03211
03212
03213
03214
03215
03216 int* FR_idx;
03217 bounds.getFree( )->getNumberArray( &FR_idx );
03218
03219 int* FX_idx;
03220 bounds.getFixed( )->getNumberArray( &FX_idx );
03221
03222 real_t* xiC = new real_t[nAC];
03223 real_t* xiC_TMP = new real_t[nAC];
03224 real_t* xiB = new real_t[nFX];
03225 real_t* Arow = new real_t[nFR];
03226
03227 A->getRow(number, bounds.getFree(), C_status == ST_LOWER ? 1.0 : -1.0, Arow);
03228
03229
03230 if ( nAC > 0 )
03231 {
03232 for( i=0; i<nAC; ++i )
03233 {
03234 xiC_TMP[i] = 0.0;
03235 for( j=0; j<nFR; ++j )
03236 {
03237 jj = FR_idx[j];
03238 xiC_TMP[i] += QQ(jj,nZ+i) * Arow[j];
03239 }
03240 }
03241
03242 if ( backsolveT( xiC_TMP, BT_TRUE, xiC ) != SUCCESSFUL_RETURN )
03243 {
03244 delete[] Arow;
03245 delete[] xiB; delete[] xiC_TMP; delete[] xiC;
03246 return THROWERROR( RET_ENSURELI_FAILED_TQ );
03247 }
03248 }
03249
03250
03251 int* AC_idx;
03252 constraints.getActive( )->getNumberArray( &AC_idx );
03253
03254 A->getRow(number, bounds.getFixed(), C_status == ST_LOWER ? 1.0 : -1.0, xiB);
03255 A->transTimes(constraints.getActive(), bounds.getFixed(), 1, -1.0, xiC, nAC, 1.0, xiB, nFX);
03256
03257
03258 real_t y_min = options.maxDualJump;
03259 int y_min_number = -1;
03260 int y_min_number_bound = -1;
03261 BooleanType y_min_isBound = BT_FALSE;
03262
03263 real_t* num = new real_t[nV];
03264
03265
03266 for( i=0; i<nAC; ++i )
03267 {
03268 ii = AC_idx[i];
03269 num[i] = y[nV+ii];
03270 }
03271
03272 performRatioTest( nAC,AC_idx,&constraints, num,xiC, options.epsDen,options.epsDen, y_min,y_min_number );
03273
03274
03275
03276 for( i=0; i<nFX; ++i )
03277 {
03278 ii = FX_idx[i];
03279 num[i] = y[ii];
03280 }
03281
03282 performRatioTest( nFX,FX_idx,&bounds, num,xiB, options.epsDen,options.epsDen, y_min,y_min_number_bound );
03283
03284 if ( y_min_number_bound >= 0 )
03285 {
03286 y_min_number = y_min_number_bound;
03287 y_min_isBound = BT_TRUE;
03288 }
03289
03290 delete[] num;
03291
03292
03293 char messageString[80];
03294
03295
03296 if ( y_min_number >= 0 )
03297 {
03298
03299 for( i=0; i<nAC; ++i )
03300 {
03301 ii = AC_idx[i];
03302 y[nV+ii] -= y_min * xiC[i];
03303 }
03304 for( i=0; i<nFX; ++i )
03305 {
03306 ii = FX_idx[i];
03307 y[ii] -= y_min * xiB[i];
03308 }
03309
03310
03311 if ( C_status == ST_LOWER )
03312 y[nV+number] = y_min;
03313 else
03314 y[nV+number] = -y_min;
03315
03316
03317 if ( y_min_isBound == BT_TRUE )
03318 {
03319 #ifndef __XPCTARGET__
03320 snprintf( messageString,80,"bound no. %d.",y_min_number );
03321 getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
03322 #endif
03323
03324 if ( removeBound( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
03325 {
03326 delete[] Arow;
03327 delete[] xiB; delete[] xiC_TMP; delete[] xiC;
03328
03329 return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
03330 }
03331 y[y_min_number] = 0.0;
03332 }
03333 else
03334 {
03335 #ifndef __XPCTARGET__
03336 snprintf( messageString,80,"constraint no. %d.",y_min_number );
03337 getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
03338 #endif
03339
03340 if ( removeConstraint( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
03341 {
03342 delete[] Arow;
03343 delete[] xiB; delete[] xiC_TMP; delete[] xiC;
03344
03345 return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
03346 }
03347
03348 y[nV+y_min_number] = 0.0;
03349 }
03350 }
03351 else
03352 {
03353
03354 delete[] Arow;
03355 delete[] xiB; delete[] xiC_TMP; delete[] xiC;
03356
03357 return setInfeasibilityFlag( RET_ENSURELI_FAILED_NOINDEX );
03358 }
03359
03360 delete[] Arow;
03361 delete[] xiB; delete[] xiC_TMP; delete[] xiC;
03362
03363 return getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
03364 }
03365
03366
03367
03368
03369
03370
03371 returnValue QProblem::addBound( int number, SubjectToStatus B_status,
03372 BooleanType updateCholesky,
03373 BooleanType ensureLI
03374 )
03375 {
03376 int i, j, ii;
03377
03378
03379 if ( bounds.getStatus( number ) != ST_INACTIVE )
03380 return THROWERROR( RET_BOUND_ALREADY_ACTIVE );
03381
03382 if ( getNFR( ) == bounds.getNUV( ) )
03383 return THROWERROR( RET_ALL_BOUNDS_ACTIVE );
03384
03385 if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
03386 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
03387 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
03388 ( getStatus( ) == QPS_SOLVED ) )
03389 {
03390 return THROWERROR( RET_UNKNOWN_BUG );
03391 }
03392
03393
03394
03395
03396
03397 if ( updateCholesky == BT_TRUE && ensureLI == BT_TRUE )
03398 {
03399 returnValue ensureLIreturnvalue = addBound_ensureLI( number,B_status );
03400
03401 switch ( ensureLIreturnvalue )
03402 {
03403 case SUCCESSFUL_RETURN:
03404 break;
03405
03406 case RET_LI_RESOLVED:
03407 break;
03408
03409 case RET_ENSURELI_FAILED_NOINDEX:
03410 return RET_ADDBOUND_FAILED_INFEASIBILITY;
03411
03412 case RET_ENSURELI_FAILED_CYCLING:
03413 return RET_ADDBOUND_FAILED_INFEASIBILITY;
03414
03415 default:
03416 return THROWERROR( RET_ENSURELI_FAILED );
03417 }
03418 }
03419
03420
03421
03422 int nV = getNV( );
03423 int nFR = getNFR( );
03424 int nAC = getNAC( );
03425 int nZ = getNZ( );
03426
03427 int tcol = sizeT - nAC;
03428
03429
03430
03431
03432 int lastfreenumber = bounds.getFree( )->getLastNumber( );
03433 if ( lastfreenumber != number )
03434 if ( bounds.swapFree( number,lastfreenumber ) != SUCCESSFUL_RETURN )
03435 THROWERROR( RET_ADDBOUND_FAILED );
03436
03437
03438 int* FR_idx;
03439 bounds.getFree( )->getNumberArray( &FR_idx );
03440
03441 real_t* w = new real_t[nFR];
03442
03443
03444
03445
03446 for( i=0; i<nFR; ++i )
03447 w[i] = QQ(FR_idx[nFR-1],i);
03448
03449
03450
03451
03452 real_t c, s, nu;
03453
03454 for( j=0; j<nZ-1; ++j )
03455 {
03456 computeGivens( w[j+1],w[j], w[j+1],w[j],c,s );
03457 nu = s/(1.0+c);
03458
03459 for( i=0; i<nFR; ++i )
03460 {
03461 ii = FR_idx[i];
03462 applyGivens( c,s,nu,QQ(ii,1+j),QQ(ii,j), QQ(ii,1+j),QQ(ii,j) );
03463 }
03464
03465 if ( ( updateCholesky == BT_TRUE ) &&
03466 ( hessianType != HST_ZERO ) && ( hessianType != HST_IDENTITY ) )
03467 {
03468 for( i=0; i<=j+1; ++i )
03469 applyGivens( c,s,nu,RR(i,1+j),RR(i,j), RR(i,1+j),RR(i,j) );
03470 }
03471 }
03472
03473
03474 if ( nAC > 0 )
03475 {
03476
03477 real_t* tmp = new real_t[nAC];
03478 for( i=0; i<nAC; ++i )
03479 tmp[i] = 0.0;
03480
03481 {
03482 j = nZ-1;
03483
03484 computeGivens( w[j+1],w[j], w[j+1],w[j],c,s );
03485 nu = s/(1.0+c);
03486
03487 for( i=0; i<nFR; ++i )
03488 {
03489 ii = FR_idx[i];
03490 applyGivens( c,s,nu,QQ(ii,1+j),QQ(ii,j), QQ(ii,1+j),QQ(ii,j) );
03491 }
03492
03493 applyGivens( c,s,nu,TT(nAC-1,tcol),tmp[nAC-1], tmp[nAC-1],TT(nAC-1,tcol) );
03494 }
03495
03496 for( j=nZ; j<nFR-1; ++j )
03497 {
03498 computeGivens( w[j+1],w[j], w[j+1],w[j],c,s );
03499 nu = s/(1.0+c);
03500
03501 for( i=0; i<nFR; ++i )
03502 {
03503 ii = FR_idx[i];
03504 applyGivens( c,s,nu,QQ(ii,1+j),QQ(ii,j), QQ(ii,1+j),QQ(ii,j) );
03505 }
03506
03507 for( i=(nFR-2-j); i<nAC; ++i )
03508 applyGivens( c,s,nu,TT(i,1+tcol-nZ+j),tmp[i], tmp[i],TT(i,1+tcol-nZ+j) );
03509 }
03510
03511 delete[] tmp;
03512 }
03513
03514 delete[] w;
03515
03516
03517 if ( ( updateCholesky == BT_TRUE ) &&
03518 ( hessianType != HST_ZERO ) && ( hessianType != HST_IDENTITY ) )
03519 {
03520
03521
03522 for( i=0; i<nZ-1; ++i )
03523 {
03524 computeGivens( RR(i,i),RR(1+i,i), RR(i,i),RR(1+i,i),c,s );
03525 nu = s/(1.0+c);
03526
03527 for( j=(1+i); j<nZ-1; ++j )
03528 applyGivens( c,s,nu,RR(i,j),RR(1+i,j), RR(i,j),RR(1+i,j) );
03529 }
03530
03531 for( i=0; i<nZ; ++i )
03532 RR(i,nZ-1) = 0.0;
03533 }
03534
03535
03536
03537 idxAddB = number;
03538 if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
03539 return THROWERROR( RET_ADDBOUND_FAILED );
03540
03541 return SUCCESSFUL_RETURN;
03542 }
03543
03544
03545
03546
03547
03548 returnValue QProblem::addBound_checkLI( int number )
03549 {
03550 int i, ii;
03551 int nV = getNV( );
03552 int nFR = getNFR( );
03553 int nAC = getNAC();
03554 int nFX = getNFX();
03555 int nC = getNC( );
03556 returnValue returnvalue = RET_LINEARLY_DEPENDENT;
03557
03558 if (options.enableFullLITests)
03559 {
03560
03561
03562
03563
03564
03565
03566
03567
03568
03569
03570
03571
03572 real_t *delta_g = new real_t[nV];
03573 real_t *delta_xFX = new real_t[nFX];
03574 real_t *delta_xFR = new real_t[nFR];
03575 real_t *delta_yAC = new real_t[nAC];
03576 real_t *delta_yFX = new real_t[nFX];
03577
03578 for (ii = 0; ii < nV; ++ii)
03579 delta_g[ii] = 0.0;
03580 delta_g[number] = 1.0;
03581
03582 int dim = (nC>nV)?nC:nV;
03583 real_t *nul = new real_t[dim];
03584 for (ii = 0; ii < dim; ++ii)
03585 nul[ii]=0.0;
03586
03587 returnvalue = determineStepDirection (delta_g,
03588 nul, nul, nul, nul,
03589 BT_FALSE, BT_FALSE,
03590 delta_xFX, delta_xFR, delta_yAC, delta_yFX);
03591
03592 delete[] nul;
03593
03594
03595 real_t weight = 0.0;
03596 for (ii = 0; ii < nAC; ++ii)
03597 {
03598 real_t a = fabs (delta_yAC[ii]);
03599 if (weight < a) weight = a;
03600 }
03601 for (ii = 0; ii < nFX; ++ii)
03602 {
03603 real_t a = fabs (delta_yFX[ii]);
03604 if (weight < a) weight = a;
03605 }
03606
03607
03608 real_t zero = 0.0;
03609 for (ii = 0; ii < nFX; ++ii)
03610 {
03611 real_t a = fabs (delta_xFX[ii]);
03612 if (zero < a) zero = a;
03613 }
03614 for (ii = 0; ii < nFR; ++ii)
03615 {
03616 real_t a = fabs (delta_xFR[ii]);
03617 if (zero < a) zero = a;
03618 }
03619
03620
03621 if (zero > options.epsLITests * weight)
03622 returnvalue = RET_LINEARLY_INDEPENDENT;
03623
03624 delete[] delta_yFX;
03625 delete[] delta_yAC;
03626 delete[] delta_xFR;
03627 delete[] delta_xFX;
03628 delete[] delta_g;
03629
03630 }
03631 else
03632 {
03633
03634
03635
03636
03637
03638
03639
03640 int nZ = getNZ( );
03641
03642 for( i=0; i<nZ; ++i )
03643 if ( fabs( QQ(number,i) ) > options.epsLITests )
03644 {
03645 returnvalue = RET_LINEARLY_INDEPENDENT;
03646 break;
03647 }
03648 }
03649
03650 return THROWINFO( returnvalue );
03651 }
03652
03653
03654
03655
03656
03657 returnValue QProblem::addBound_ensureLI( int number, SubjectToStatus B_status )
03658 {
03659 int i, ii;
03660 int nV = getNV( );
03661 int nFX = getNFX( );
03662 int nAC = getNAC( );
03663 int nZ = getNZ( );
03664
03665
03666
03667 returnValue returnvalueCheckLI = addBound_checkLI( number );
03668
03669 if ( returnvalueCheckLI == RET_INDEXLIST_CORRUPTED )
03670 return THROWERROR( RET_ENSURELI_FAILED );
03671
03672 if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )
03673 return SUCCESSFUL_RETURN;
03674
03675
03676
03677
03678
03679
03680
03681 int* FR_idx;
03682 bounds.getFree( )->getNumberArray( &FR_idx );
03683
03684 int* FX_idx;
03685 bounds.getFixed( )->getNumberArray( &FX_idx );
03686
03687 int* AC_idx;
03688 constraints.getActive( )->getNumberArray( &AC_idx );
03689
03690 real_t* xiC = new real_t[nAC];
03691 real_t* xiC_TMP = new real_t[nAC];
03692 real_t* xiB = new real_t[nFX];
03693
03694
03695 if ( nAC > 0 )
03696 {
03697 if ( B_status == ST_LOWER )
03698 {
03699 for( i=0; i<nAC; ++i )
03700 xiC_TMP[i] = QQ(number,nZ+i);
03701 }
03702 else
03703 {
03704 for( i=0; i<nAC; ++i )
03705 xiC_TMP[i] = -QQ(number,nZ+i);
03706 }
03707
03708 if ( backsolveT( xiC_TMP, BT_TRUE, xiC ) != SUCCESSFUL_RETURN )
03709 {
03710 delete[] xiB; delete[] xiC_TMP; delete[] xiC;
03711 return THROWERROR( RET_ENSURELI_FAILED_TQ );
03712 }
03713 }
03714
03715
03716 A->transTimes(constraints.getActive(), bounds.getFixed(), 1, -1.0, xiC, nAC, 0.0, xiB, nFX);
03717
03718
03719
03720 real_t y_min = options.maxDualJump;
03721 int y_min_number = -1;
03722 int y_min_number_bound = -1;
03723 BooleanType y_min_isBound = BT_FALSE;
03724
03725 real_t* num = new real_t[nV];
03726
03727
03728 for( i=0; i<nAC; ++i )
03729 {
03730 ii = AC_idx[i];
03731 num[i] = y[nV+ii];
03732 }
03733
03734 performRatioTest( nAC,AC_idx,&constraints, num,xiC, options.epsDen,options.epsDen, y_min,y_min_number );
03735
03736
03737
03738 for( i=0; i<nFX; ++i )
03739 {
03740 ii = FX_idx[i];
03741 num[i] = y[ii];
03742 }
03743
03744 performRatioTest( nFX,FX_idx,&bounds, num,xiB, options.epsDen,options.epsDen, y_min,y_min_number_bound );
03745
03746 if ( y_min_number_bound >= 0 )
03747 {
03748 y_min_number = y_min_number_bound;
03749 y_min_isBound = BT_TRUE;
03750 }
03751
03752 delete[] num;
03753
03754
03755
03756 char messageString[80];
03757
03758 if ( y_min_number >= 0 )
03759 {
03760
03761 for( i=0; i<nAC; ++i )
03762 {
03763 ii = AC_idx[i];
03764 y[nV+ii] -= y_min * xiC[i];
03765 }
03766 for( i=0; i<nFX; ++i )
03767 {
03768 ii = FX_idx[i];
03769 y[ii] -= y_min * xiB[i];
03770 }
03771
03772
03773 if ( B_status == ST_LOWER )
03774 y[number] = y_min;
03775 else
03776 y[number] = -y_min;
03777
03778
03779 if ( y_min_isBound == BT_TRUE )
03780 {
03781 #ifndef __XPCTARGET__
03782 snprintf( messageString,80,"bound no. %d.",y_min_number );
03783 getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
03784 #endif
03785
03786 if ( removeBound( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
03787 {
03788 delete[] xiB; delete[] xiC_TMP; delete[] xiC;
03789
03790 return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
03791 }
03792 y[y_min_number] = 0.0;
03793 }
03794 else
03795 {
03796 #ifndef __XPCTARGET__
03797 snprintf( messageString,80,"constraint no. %d.",y_min_number );
03798 getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
03799 #endif
03800
03801 if ( removeConstraint( y_min_number,BT_TRUE,BT_FALSE,BT_FALSE ) != SUCCESSFUL_RETURN )
03802 {
03803 delete[] xiB; delete[] xiC_TMP; delete[] xiC;
03804
03805 return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
03806 }
03807
03808 y[nV+y_min_number] = 0.0;
03809 }
03810 }
03811 else
03812 {
03813
03814 delete[] xiB; delete[] xiC_TMP; delete[] xiC;
03815
03816 return setInfeasibilityFlag( RET_ENSURELI_FAILED_NOINDEX );
03817 }
03818
03819 delete[] xiB; delete[] xiC_TMP; delete[] xiC;
03820
03821 return getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
03822 }
03823
03824
03825
03826
03827
03828
03829 returnValue QProblem::removeConstraint( int number,
03830 BooleanType updateCholesky,
03831 BooleanType allowFlipping,
03832 BooleanType ensureNZC
03833 )
03834 {
03835 int i, j, ii, jj;
03836 returnValue returnvalue = SUCCESSFUL_RETURN;
03837 BooleanType hasFlipped = BT_FALSE;
03838
03839
03840 if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
03841 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
03842 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
03843 ( getStatus( ) == QPS_SOLVED ) )
03844 {
03845 return THROWERROR( RET_UNKNOWN_BUG );
03846 }
03847
03848
03849 int nV = getNV( );
03850 int nFR = getNFR( );
03851 int nAC = getNAC( );
03852 int nZ = getNZ( );
03853
03854 int tcol = sizeT - nAC;
03855 int number_idx = constraints.getActive( )->getIndex( number );
03856
03857 int addIdx;
03858 BooleanType addBoundNotConstraint;
03859 SubjectToStatus addStatus;
03860 BooleanType exchangeHappened = BT_FALSE;
03861
03862
03863
03864 if ( constraints.getStatus( number ) == ST_INACTIVE )
03865 return THROWERROR( RET_CONSTRAINT_NOT_ACTIVE );
03866
03867 if ( ( number_idx < 0 ) || ( number_idx >= nAC ) )
03868 return THROWERROR( RET_CONSTRAINT_NOT_ACTIVE );
03869
03870
03871 int* FR_idx;
03872 bounds.getFree( )->getNumberArray( &FR_idx );
03873
03874
03875 if (ensureNZC == BT_TRUE)
03876 {
03877 returnvalue = ensureNonzeroCurvature(BT_FALSE, number, exchangeHappened, addBoundNotConstraint, addIdx, addStatus);
03878
03879 if (returnvalue != SUCCESSFUL_RETURN)
03880 return returnvalue;
03881 }
03882
03883
03884 if ( ( exchangeHappened == BT_FALSE ) && ( options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) )
03885 flipper.set( &bounds,R,&constraints,Q,T );
03886
03887
03888
03889
03890 if ( number_idx < nAC-1 )
03891 {
03892 for( i=(number_idx+1); i<nAC; ++i )
03893 for( j=(nAC-i-1); j<nAC; ++j )
03894 TT(i-1,tcol+j) = TT(i,tcol+j);
03895
03896 for( j=0; j<nAC; ++j )
03897 TT(nAC-1,tcol+j) = 0.0;
03898
03899
03900
03901
03902
03903 real_t c, s, nu;
03904
03905 for( j=(nAC-2-number_idx); j>=0; --j )
03906 {
03907 computeGivens( TT(nAC-2-j,tcol+1+j),TT(nAC-2-j,tcol+j), TT(nAC-2-j,tcol+1+j),TT(nAC-2-j,tcol+j),c,s );
03908 nu = s/(1.0+c);
03909
03910 for( i=(nAC-j-1); i<(nAC-1); ++i )
03911 applyGivens( c,s,nu,TT(i,tcol+1+j),TT(i,tcol+j), TT(i,tcol+1+j),TT(i,tcol+j) );
03912
03913 for( i=0; i<nFR; ++i )
03914 {
03915 ii = FR_idx[i];
03916 applyGivens( c,s,nu,QQ(ii,nZ+1+j),QQ(ii,nZ+j), QQ(ii,nZ+1+j),QQ(ii,nZ+j) );
03917 }
03918 }
03919 }
03920 else
03921 {
03922
03923 for( j=0; j<nAC; ++j )
03924 TT(nAC-1,tcol+j) = 0.0;
03925 }
03926
03927
03928 if ( ( updateCholesky == BT_TRUE ) &&
03929 ( hessianType != HST_ZERO ) && ( hessianType != HST_IDENTITY ) )
03930 {
03931
03932
03933
03934 real_t* Hz = new real_t[nFR];
03935 real_t* z = new real_t[nFR];
03936 real_t rho2 = 0.0;
03937
03938
03939
03940 for( j=0; j<nFR; ++j )
03941 z[j] = QQ(FR_idx[j],nZ);
03942 H->times(bounds.getFree(), bounds.getFree(), 1, 1.0, z, nFR, 0.0, Hz, nFR);
03943 delete[] z;
03944
03945 if ( nZ > 0 )
03946 {
03947 real_t* ZHz = new real_t[nZ];
03948 for ( i=0; i<nZ; ++i )
03949 ZHz[i] = 0.0;
03950 real_t* r = new real_t[nZ];
03951
03952
03953 for( j=0; j<nFR; ++j )
03954 {
03955 jj = FR_idx[j];
03956
03957 for( i=0; i<nZ; ++i )
03958 ZHz[i] += QQ(jj,i) * Hz[j];
03959 }
03960
03961
03962 if ( backsolveR( ZHz,BT_TRUE,r ) != SUCCESSFUL_RETURN )
03963 {
03964 delete[] Hz; delete[] r; delete[] ZHz;
03965 return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
03966 }
03967
03968
03969
03970 for( i=0; i<nZ; ++i )
03971 {
03972 rho2 -= r[i]*r[i];
03973 RR(i,nZ) = r[i];
03974 }
03975
03976 delete[] r; delete[] ZHz;
03977 }
03978
03979
03980 for( j=0; j<nFR; ++j )
03981 rho2 += QQ(FR_idx[j],nZ) * Hz[j];
03982
03983 delete[] Hz;
03984
03985 if ( ( options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) && ( exchangeHappened == BT_FALSE ) )
03986 {
03987 if ( rho2 > options.epsFlipping )
03988 RR(nZ,nZ) = sqrt( rho2 );
03989 else
03990 {
03991 hessianType = HST_SEMIDEF;
03992
03993 flipper.get( &bounds,R,&constraints,Q,T );
03994 constraints.flipFixed(number);
03995 idxAddC = number;
03996
03997 switch (constraints.getStatus(number))
03998 {
03999 case ST_LOWER:
04000 lbA[number] = ubA[number]; Ax_l[number] = -Ax_u[number]; break;
04001 case ST_UPPER:
04002 ubA[number] = lbA[number]; Ax_u[number] = -Ax_l[number]; break;
04003 default:
04004 return THROWERROR( RET_MOVING_BOUND_FAILED );
04005 }
04006
04007 hasFlipped = BT_TRUE;
04008 }
04009 }
04010 else if ( exchangeHappened == BT_FALSE )
04011 {
04012 if ( rho2 > ZERO )
04013 RR(nZ,nZ) = sqrt( rho2 );
04014 else
04015 {
04016 if ( allowFlipping == BT_FALSE )
04017 {
04018 RR(nZ,nZ) = 100.0*EPS;
04019 }
04020 else
04021 {
04022 hessianType = HST_SEMIDEF;
04023 return THROWERROR( RET_HESSIAN_NOT_SPD );
04024 }
04025 }
04026 }
04027 }
04028
04029
04030
04031 if ( hasFlipped == BT_FALSE )
04032 {
04033 idxRemC = number;
04034 if ( constraints.moveActiveToInactive( number ) != SUCCESSFUL_RETURN )
04035 return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
04036 }
04037
04038 if (exchangeHappened == BT_TRUE)
04039 {
04040
04041
04042
04043 RR(nZ,nZ) = 0.0;
04044
04045 if ( addBoundNotConstraint )
04046 addBound(addIdx, addStatus, BT_TRUE, BT_FALSE);
04047 else
04048 addConstraint(addIdx, addStatus, BT_TRUE, BT_FALSE);
04049 }
04050
04051 return SUCCESSFUL_RETURN;
04052 }
04053
04054
04055
04056
04057
04058 returnValue QProblem::removeBound( int number,
04059 BooleanType updateCholesky,
04060 BooleanType allowFlipping,
04061 BooleanType ensureNZC
04062 )
04063 {
04064 int i, j, ii, jj;
04065 returnValue returnvalue = SUCCESSFUL_RETURN;
04066 int addIdx;
04067 BooleanType addBoundNotConstraint;
04068 SubjectToStatus addStatus;
04069 BooleanType exchangeHappened = BT_FALSE;
04070
04071
04072
04073 if ( bounds.getStatus( number ) == ST_INACTIVE )
04074 return THROWERROR( RET_BOUND_NOT_ACTIVE );
04075
04076 if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
04077 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
04078 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
04079 ( getStatus( ) == QPS_SOLVED ) )
04080 {
04081 return THROWERROR( RET_UNKNOWN_BUG );
04082 }
04083
04084
04085 int nV = getNV( );
04086 int nFR = getNFR( );
04087 int nAC = getNAC( );
04088 int nZ = getNZ( );
04089
04090 int tcol = sizeT - nAC;
04091
04092
04093 if (ensureNZC == BT_TRUE)
04094 {
04095 returnvalue = ensureNonzeroCurvature(BT_TRUE, number, exchangeHappened, addBoundNotConstraint, addIdx, addStatus);
04096
04097 if (returnvalue != SUCCESSFUL_RETURN)
04098 return returnvalue;
04099 }
04100
04101
04102 if ( ( options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) && ( exchangeHappened == BT_FALSE ) )
04103 flipper.set( &bounds,R,&constraints,Q,T );
04104
04105
04106 idxRemB = number;
04107 if ( bounds.moveFixedToFree( number ) != SUCCESSFUL_RETURN )
04108 return THROWERROR( RET_REMOVEBOUND_FAILED );
04109
04110 int* FR_idx;
04111 bounds.getFree( )->getNumberArray( &FR_idx );
04112
04113
04114 int nnFRp1 = FR_idx[nFR];
04115 for( i=0; i<nFR; ++i )
04116 {
04117 ii = FR_idx[i];
04118 QQ(ii,nFR) = 0.0;
04119 QQ(nnFRp1,i) = 0.0;
04120 }
04121 QQ(nnFRp1,nFR) = 1.0;
04122
04123 if ( nAC > 0 )
04124 {
04125
04126 int* AC_idx;
04127 constraints.getActive( )->getNumberArray( &AC_idx );
04128
04129 real_t* tmp = new real_t[nAC];
04130 A->getCol(number, constraints.getActive(), 1.0, tmp);
04131
04132
04133
04134
04135
04136 real_t c, s, nu;
04137
04138 for( j=(nAC-1); j>=0; --j )
04139 {
04140 computeGivens( tmp[nAC-1-j],TT(nAC-1-j,tcol+j),TT(nAC-1-j,tcol+j),tmp[nAC-1-j],c,s );
04141 nu = s/(1.0+c);
04142
04143 for( i=(nAC-j); i<nAC; ++i )
04144 applyGivens( c,s,nu,tmp[i],TT(i,tcol+j),TT(i,tcol+j),tmp[i] );
04145
04146 for( i=0; i<=nFR; ++i )
04147 {
04148 ii = FR_idx[i];
04149
04150 applyGivens( c,s,nu,QQ(ii,nZ+1+j),QQ(ii,nZ+j),QQ(ii,nZ+1+j),QQ(ii,nZ+j) );
04151 }
04152 }
04153
04154 delete[] tmp;
04155 }
04156
04157
04158 if ( ( updateCholesky == BT_TRUE ) &&
04159 ( hessianType != HST_ZERO ) && ( hessianType != HST_IDENTITY ) )
04160 {
04161
04162
04163
04164 real_t z2 = QQ(nnFRp1,nZ);
04165 real_t rho2 = H->diag(nnFRp1)*z2*z2;
04166
04167 if ( nFR > 0 )
04168 {
04169
04170 real_t* Hz = new real_t[nFR+1];
04171 real_t* z = new real_t[nFR+1];
04172
04173
04174 for( j=0; j<nFR; ++j )
04175 z[j] = QQ(FR_idx[j],nZ);
04176 z[nFR] = 0.0;
04177 H->times(bounds.getFree(), bounds.getFree(), 1, 1.0, z, nFR+1, 0.0, Hz, nFR+1);
04178
04179 H->getCol(nnFRp1, bounds.getFree(), 1.0, z);
04180
04181 if ( nZ > 0 )
04182 {
04183 real_t* r = new real_t[nZ];
04184 real_t* rhs = new real_t[nZ];
04185 for( i=0; i<nZ; ++i )
04186 rhs[i] = 0.0;
04187
04188
04189 for( j=0; j<nFR; ++j )
04190 {
04191 jj = FR_idx[j];
04192 for( i=0; i<nZ; ++i )
04193
04194 rhs[i] += QQ(jj,i) * ( Hz[j] + z2 * z[j] );
04195 }
04196
04197
04198 if ( backsolveR( rhs,BT_TRUE,BT_TRUE,r ) != SUCCESSFUL_RETURN )
04199 {
04200 delete[] z;
04201 delete[] Hz; delete[] r; delete[] rhs;
04202 return THROWERROR( RET_REMOVEBOUND_FAILED );
04203 }
04204
04205
04206
04207
04208 for( i=0; i<nZ; ++i )
04209 {
04210 rho2 -= r[i]*r[i];
04211 RR(i,nZ) = r[i];
04212 }
04213
04214 delete[] rhs; delete[] r;
04215 }
04216
04217 for( j=0; j<nFR; ++j )
04218 {
04219 jj = FR_idx[j];
04220
04221 rho2 += QQ(jj,nZ) * ( Hz[j] + 2.0*z2*z[j] );
04222 }
04223
04224 delete[] z;
04225 delete[] Hz;
04226 }
04227
04228
04229 if ( ( options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) && ( exchangeHappened == BT_FALSE ) )
04230 {
04231 if ( rho2 > options.epsFlipping )
04232 RR(nZ,nZ) = sqrt( rho2 );
04233 else
04234 {
04235 hessianType = HST_SEMIDEF;
04236
04237 flipper.get( &bounds,R,&constraints,Q,T );
04238 bounds.flipFixed(number);
04239 idxAddB = number;
04240
04241 switch (bounds.getStatus(number))
04242 {
04243 case ST_LOWER: lb[number] = ub[number]; break;
04244 case ST_UPPER: ub[number] = lb[number]; break;
04245 default: return THROWERROR( RET_MOVING_BOUND_FAILED );
04246 }
04247
04248 }
04249 }
04250 else if ( exchangeHappened == BT_FALSE )
04251 {
04252 if ( rho2 > ZERO )
04253 RR(nZ,nZ) = sqrt( rho2 );
04254 else
04255 {
04256 if ( allowFlipping == BT_FALSE )
04257 RR(nZ,nZ) = 100.0*EPS;
04258 else
04259 {
04260 hessianType = HST_SEMIDEF;
04261 return THROWERROR( RET_HESSIAN_NOT_SPD );
04262 }
04263 }
04264 }
04265 else
04266 {
04267
04268
04269
04270 RR(nZ,nZ) = 0.0;
04271
04272 if ( addBoundNotConstraint )
04273 addBound(addIdx, addStatus, BT_TRUE, BT_FALSE);
04274 else
04275 addConstraint(addIdx, addStatus, BT_TRUE, BT_FALSE);
04276 }
04277 }
04278
04279 return SUCCESSFUL_RETURN;
04280 }
04281
04282
04283
04284 returnValue QProblem::performPlainRatioTest( int nIdx,
04285 const int* const idxList,
04286 const real_t* const num,
04287 const real_t* const den,
04288 real_t epsNum,
04289 real_t epsDen,
04290 real_t& t,
04291 int& BC_idx
04292 ) const
04293 {
04294 int i;
04295 for (i = 0; i < nIdx; i++)
04296 if ( (num[i] > epsNum) && (den[i] > epsDen) && (t * den[i] > num[i]) )
04297 {
04298 t = num[i] / den[i];
04299 BC_idx = idxList[i];
04300 }
04301
04302 return SUCCESSFUL_RETURN;
04303 }
04304
04305
04306 returnValue QProblem::ensureNonzeroCurvature(
04307 BooleanType removeBoundNotConstraint,
04308 int remIdx,
04309 BooleanType &exchangeHappened,
04310 BooleanType &addBoundNotConstraint,
04311 int &addIdx,
04312 SubjectToStatus &addStatus
04313 )
04314 {
04315 int i, ii;
04316 int addLBndIdx = -1, addLCnstrIdx = -1, addUBndIdx = -1, addUCnstrIdx = -1;
04317 int *FX_idx, *AC_idx, *IAC_idx;
04318 returnValue returnvalue = SUCCESSFUL_RETURN;
04319
04320 int nV = getNV( );
04321 int nFR = getNFR( );
04322 int nAC = getNAC( );
04323 int nC = getNC( );
04324 int nFX = getNFX( );
04325 int nIAC = getNIAC( );
04326
04327 int* FR_idx;
04328 bounds.getFree( )->getNumberArray( &FR_idx );
04329
04330 real_t *delta_g = new real_t[nV];
04331 real_t *delta_xFX = new real_t[nFX];
04332 real_t *delta_xFR = new real_t[nFR];
04333 real_t *delta_yAC = new real_t[nAC];
04334 real_t *delta_yFX = new real_t[nFX];
04335
04336 bounds.getFixed( )->getNumberArray( &FX_idx );
04337 constraints.getActive( )->getNumberArray( &AC_idx );
04338 constraints.getInactive( )->getNumberArray( &IAC_idx );
04339
04340 addBoundNotConstraint = BT_TRUE;
04341 addStatus = ST_INACTIVE;
04342 exchangeHappened = BT_FALSE;
04343
04344 if (removeBoundNotConstraint)
04345 {
04346 int dim = nV < nC ? nC : nV;
04347 real_t *nul = new real_t[dim];
04348 real_t *ek = new real_t[nV];
04349 for (ii = 0; ii < dim; ++ii)
04350 nul[ii]=0.0;
04351 for (ii = 0; ii < nV; ++ii)
04352 ek[ii]=0.0;
04353 ek[remIdx] = bounds.getStatus(remIdx) == ST_LOWER ? 1.0 : -1.0;
04354
04355 returnvalue = determineStepDirection (nul, nul, nul, ek, ek,
04356 BT_FALSE, BT_FALSE,
04357 delta_xFX, delta_xFR, delta_yAC, delta_yFX);
04358 delete[] ek;
04359 delete[] nul;
04360 }
04361 else
04362 {
04363 real_t *nul = new real_t[nV];
04364 real_t *ek = new real_t[nC];
04365 for (ii = 0; ii < nV; ++ii)
04366 nul[ii]=0.0;
04367 for (ii = 0; ii < nC; ++ii)
04368 ek[ii]=0.0;
04369 ek[remIdx] = constraints.getStatus(remIdx) == ST_LOWER ? 1.0 : -1.0;
04370
04371 returnvalue = determineStepDirection (nul,
04372 ek, ek, nul, nul,
04373 BT_FALSE, BT_TRUE,
04374 delta_xFX, delta_xFR, delta_yAC, delta_yFX);
04375 delete[] ek;
04376 delete[] nul;
04377 }
04378
04379
04380 real_t normXi = 0.0;
04381 for (ii = 0; ii < nAC; ++ii)
04382 {
04383 real_t a = fabs (delta_yAC[ii]);
04384 if (normXi < a) normXi = a;
04385 }
04386 for (ii = 0; ii < nFX; ++ii)
04387 {
04388 real_t a = fabs (delta_yFX[ii]);
04389 if (normXi < a) normXi = a;
04390 }
04391
04392
04393 real_t normS = 0.0;
04394 for (ii = 0; ii < nFX; ++ii)
04395 {
04396 real_t a = fabs (delta_xFX[ii]);
04397 if (normS < a) normS = a;
04398 }
04399 for (ii = 0; ii < nFR; ++ii)
04400 {
04401 real_t a = fabs (delta_xFR[ii]);
04402 if (normS < a) normS = a;
04403 }
04404
04405
04406 if (normXi < options.epsNZCTests * normS)
04407 {
04408
04409 real_t sigmaLBnd, sigmaLCnstr, sigmaUBnd, sigmaUCnstr, sigma;
04410
04411
04412
04413
04414 real_t *x_W = new real_t[getMax(1,nFR)];
04415 for (i = 0; i < nFR; i++)
04416 {
04417 ii = FR_idx[i];
04418 x_W[i] = ub[ii] - x[ii];
04419 }
04420
04421 sigmaUBnd = options.maxPrimalJump;
04422 addUBndIdx = -1;
04423 performPlainRatioTest(nFR, FR_idx, x_W, delta_xFR, options.epsNum, options.epsDen, sigmaUBnd, addUBndIdx);
04424 if (removeBoundNotConstraint == BT_TRUE && bounds.getStatus(remIdx) == ST_LOWER)
04425 {
04426
04427 real_t eins = 1.0;
04428 x_W[0] = ub[remIdx] - x[remIdx];
04429 performPlainRatioTest(1, &remIdx, x_W, &eins, options.epsNum, options.epsDen, sigmaUBnd, addUBndIdx);
04430 }
04431
04432
04433 for (i = 0; i < nFR; i++)
04434 {
04435 ii = FR_idx[i];
04436 x_W[i] = x[ii] - lb[ii];
04437 }
04438 for (i = 0; i < nFR; i++)
04439 delta_xFR[i] = -delta_xFR[i];
04440
04441 sigmaLBnd = options.maxPrimalJump;
04442 addLBndIdx = -1;
04443 performPlainRatioTest(nFR, FR_idx, x_W, delta_xFR, options.epsNum, options.epsDen, sigmaLBnd, addLBndIdx);
04444 if (removeBoundNotConstraint == BT_TRUE && bounds.getStatus(remIdx) == ST_UPPER)
04445 {
04446
04447 real_t eins = 1.0;
04448 x_W[0] = x[remIdx] - lb[remIdx];
04449 performPlainRatioTest(1, &remIdx, x_W, &eins, options.epsNum, options.epsDen, sigmaLBnd, addLBndIdx);
04450 }
04451 for (i = 0; i < nFR; i++)
04452 delta_xFR[i] = -delta_xFR[i];
04453
04454 delete[] x_W;
04455
04456
04457
04458
04459 real_t *As = new real_t[nIAC];
04460 A->times(constraints.getInactive(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 0.0, As, nIAC);
04461 A->times(constraints.getInactive(), bounds.getFree(), 1, 1.0, delta_xFR, nFR, 1.0, As, nIAC);
04462
04463
04464 real_t *Ax_W = new real_t[nIAC];
04465 for (i = 0; i < nIAC; i++)
04466 {
04467 ii = IAC_idx[i];
04468 Ax_W[i] = Ax_u[ii];
04469 }
04470
04471 sigmaUCnstr = options.maxPrimalJump;
04472 addUCnstrIdx = -1;
04473 performPlainRatioTest(nIAC, IAC_idx, Ax_W, As, options.epsNum, options.epsDen, sigmaUCnstr, addUCnstrIdx);
04474 if (removeBoundNotConstraint == BT_FALSE && constraints.getStatus(remIdx) == ST_LOWER)
04475 {
04476
04477 real_t eins = 1.0;
04478 performPlainRatioTest(1, &remIdx, &Ax_u[remIdx], &eins, options.epsNum, options.epsDen, sigmaUCnstr, addUCnstrIdx);
04479 }
04480
04481
04482 for (i = 0; i < nIAC; i++)
04483 {
04484 ii = IAC_idx[i];
04485 Ax_W[i] = Ax_l[ii];
04486 }
04487 for (i = 0; i < nIAC; i++)
04488 As[i] = -As[i];
04489
04490 sigmaLCnstr = options.maxPrimalJump;
04491 addLCnstrIdx = -1;
04492 performPlainRatioTest(nIAC, IAC_idx, Ax_W, As, options.epsNum, options.epsDen, sigmaLCnstr, addLCnstrIdx);
04493 if (removeBoundNotConstraint == BT_FALSE && constraints.getStatus(remIdx) == ST_UPPER)
04494 {
04495
04496 real_t eins = 1.0;
04497 performPlainRatioTest(1, &remIdx, &Ax_l[remIdx], &eins, options.epsNum, options.epsDen, sigmaLCnstr, addLCnstrIdx);
04498 }
04499
04500
04501 sigma = options.maxPrimalJump;
04502 if (sigmaUCnstr < sigma) { sigma = sigmaUCnstr; addStatus = ST_UPPER; addBoundNotConstraint = BT_FALSE; addIdx = addUCnstrIdx; }
04503 if (sigmaLCnstr < sigma) { sigma = sigmaLCnstr; addStatus = ST_LOWER; addBoundNotConstraint = BT_FALSE; addIdx = addLCnstrIdx; }
04504 if (sigmaUBnd < sigma) { sigma = sigmaUBnd; addStatus = ST_UPPER; addBoundNotConstraint = BT_TRUE; addIdx = addUBndIdx; }
04505 if (sigmaLBnd < sigma) { sigma = sigmaLBnd; addStatus = ST_LOWER; addBoundNotConstraint = BT_TRUE; addIdx = addLBndIdx; }
04506
04507 if (sigma >= options.maxPrimalJump)
04508 {
04509 unbounded = BT_TRUE;
04510 returnvalue = RET_HOTSTART_STOPPED_UNBOUNDEDNESS;
04511 }
04512 else
04513 {
04514 for (i = 0; i < nFR; i++)
04515 x[FR_idx[i]] += sigma * delta_xFR[i];
04516
04517 for (i = 0; i < nFX; i++)
04518 x[FX_idx[i]] += sigma * delta_xFX[i];
04519
04520
04521 A->times(1, 1.0, x, nV, 0.0, Ax, nC);
04522 for (i = 0; i < nC; i++) Ax_u[i] = ubA[i] - Ax[i];
04523 for (i = 0; i < nC; i++) Ax_l[i] = Ax[i] - lbA[i];
04524
04525
04526 exchangeHappened = BT_TRUE;
04527 }
04528
04529 delete[] Ax_W;
04530 delete[] As;
04531 }
04532
04533 delete[] delta_yFX;
04534 delete[] delta_yAC;
04535 delete[] delta_xFR;
04536 delete[] delta_xFX;
04537 delete[] delta_g;
04538
04539 return returnvalue;
04540 }
04541
04542
04543
04544
04545
04546
04547 returnValue QProblem::backsolveT( const real_t* const b, BooleanType transposed, real_t* const a ) const
04548 {
04549 int i, j;
04550 int nT = getNAC( );
04551 int tcol = sizeT - nT;
04552
04553 real_t sum;
04554
04555
04556 if ( nT <= 0 )
04557 return SUCCESSFUL_RETURN;
04558
04559
04560
04561 if ( transposed == BT_FALSE )
04562 {
04563
04564 for( i=0; i<nT; ++i )
04565 {
04566 sum = b[i];
04567 for( j=0; j<i; ++j )
04568 sum -= TT(i,sizeT-1-j) * a[nT-1-j];
04569
04570 if ( fabs( TT(i,sizeT-1-i) ) > EPS )
04571 a[nT-1-i] = sum / TT(i,sizeT-1-i);
04572 else
04573 return THROWERROR( RET_DIV_BY_ZERO );
04574 }
04575 }
04576 else
04577 {
04578
04579 for( i=0; i<nT; ++i )
04580 {
04581 sum = b[i];
04582 for( j=0; j<i; ++j )
04583 sum -= TT(nT-1-j,tcol+i) * a[nT-1-j];
04584
04585 if ( fabs( TT(nT-1-i,tcol+i) ) > EPS )
04586 a[nT-1-i] = sum / TT(nT-1-i,tcol+i);
04587 else
04588 return THROWERROR( RET_DIV_BY_ZERO );
04589 }
04590 }
04591
04592
04593 return SUCCESSFUL_RETURN;
04594 }
04595
04596
04597
04598
04599
04600 returnValue QProblem::determineDataShift( const real_t* const g_new, const real_t* const lbA_new, const real_t* const ubA_new,
04601 const real_t* const lb_new, const real_t* const ub_new,
04602 real_t* const delta_g, real_t* const delta_lbA, real_t* const delta_ubA,
04603 real_t* const delta_lb, real_t* const delta_ub,
04604 BooleanType& Delta_bC_isZero, BooleanType& Delta_bB_isZero
04605 )
04606 {
04607 int i, ii;
04608 int nC = getNC( );
04609 int nAC = getNAC( );
04610
04611 int* FX_idx;
04612 int* AC_idx;
04613
04614 bounds.getFixed( )->getNumberArray( &FX_idx );
04615 constraints.getActive( )->getNumberArray( &AC_idx );
04616
04617
04618
04619
04620 QProblemB::determineDataShift( g_new,lb_new,ub_new,
04621 delta_g,delta_lb,delta_ub,
04622 Delta_bB_isZero );
04623
04624
04625
04626
04627 for( i=0; i<nC; ++i )
04628 {
04629
04630 if ( lbA_new != 0 )
04631 delta_lbA[i] = lbA_new[i] - lbA[i];
04632 else
04633 delta_lbA[i] = -INFTY - lbA[i];
04634 }
04635
04636 for( i=0; i<nC; ++i )
04637 {
04638
04639 if ( ubA_new != 0 )
04640 delta_ubA[i] = ubA_new[i] - ubA[i];
04641 else
04642 delta_ubA[i] = INFTY - ubA[i];
04643 }
04644
04645
04646 Delta_bC_isZero = BT_TRUE;
04647
04648 for ( i=0; i<nAC; ++i )
04649 {
04650 ii = AC_idx[i];
04651
04652 if ( ( fabs( delta_lbA[ii] ) > EPS ) || ( fabs( delta_ubA[ii] ) > EPS ) )
04653 {
04654 Delta_bC_isZero = BT_FALSE;
04655 break;
04656 }
04657 }
04658
04659 return SUCCESSFUL_RETURN;
04660 }
04661
04662
04663
04664
04665
04666 returnValue QProblem::determineStepDirection( const real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA,
04667 const real_t* const delta_lb, const real_t* const delta_ub,
04668 BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero,
04669 real_t* const delta_xFX, real_t* const delta_xFR,
04670 real_t* const delta_yAC, real_t* const delta_yFX
04671 )
04672 {
04673 int i, j, ii, jj, r;
04674 int nV = getNV( );
04675 int nFR = getNFR( );
04676 int nFX = getNFX( );
04677 int nAC = getNAC( );
04678 int nZ = getNZ( );
04679
04680 int* FR_idx;
04681 int* FX_idx;
04682 int* AC_idx;
04683
04684 bounds.getFree( )->getNumberArray( &FR_idx );
04685 bounds.getFixed( )->getNumberArray( &FX_idx );
04686 constraints.getActive( )->getNumberArray( &AC_idx );
04687
04688
04689
04690 if ( Delta_bB_isZero == BT_FALSE )
04691 {
04692 for( i=0; i<nFX; ++i )
04693 {
04694 ii = FX_idx[i];
04695
04696 if ( bounds.getStatus( ii ) == ST_LOWER )
04697 delta_xFX[i] = delta_lb[ii];
04698 else
04699 delta_xFX[i] = delta_ub[ii];
04700 }
04701 }
04702 else
04703 {
04704 for( i=0; i<nFX; ++i )
04705 delta_xFX[i] = 0.0;
04706 }
04707
04708
04709
04710
04711 for ( i=0; i<nFR; ++i )
04712 {
04713 ii = FR_idx[i];
04714 tempA[i] = delta_g[ii];
04715 delta_xFR[i] = 0.0;
04716 }
04717 for ( i=0; i<nAC; ++i )
04718 delta_yAC[i] = 0.0;
04719 if ( Delta_bC_isZero == BT_FALSE )
04720 {
04721 for ( i=0; i<nAC; ++i )
04722 {
04723 ii = AC_idx[i];
04724 if ( constraints.getStatus( ii ) == ST_LOWER )
04725 tempB[i] = delta_lbA[ii];
04726 else
04727 tempB[i] = delta_ubA[ii];
04728 }
04729 }
04730 else
04731 {
04732 for ( i=0; i<nAC; ++i )
04733 tempB[i] = 0.0;
04734 }
04735
04736
04737 for ( r=0; r<=options.numRefinementSteps; ++r )
04738 {
04739
04740 if ( nFR > 0 )
04741 {
04742 for( i=0; i<nFR; ++i )
04743 delta_xFR_TMP[i] = 0.0;
04744
04745
04746 if ( nAC > 0 )
04747 {
04748 if ( ( Delta_bC_isZero == BT_TRUE ) && ( Delta_bB_isZero == BT_TRUE ) )
04749 {
04750 for( i=0; i<nAC; ++i )
04751 delta_xFRy[i] = 0.0;
04752 }
04753 else
04754 {
04755
04756
04757 if ( ( Delta_bB_isZero == BT_FALSE ) && ( r == 0 ) )
04758 A->times(constraints.getActive(), bounds.getFixed(), 1, -1.0, delta_xFX, nFX, 1.0, tempB, nAC);
04759
04760 if ( backsolveT( tempB, BT_FALSE, delta_xFRy ) != SUCCESSFUL_RETURN )
04761 return THROWERROR( RET_STEPDIRECTION_FAILED_TQ );
04762
04763 for( i=0; i<nFR; ++i )
04764 {
04765 ii = FR_idx[i];
04766 for( j=0; j<nAC; ++j )
04767 delta_xFR_TMP[i] += QQ(ii,nZ+j) * delta_xFRy[j];
04768 }
04769 }
04770 }
04771
04772
04773
04774 for( i=0; i<nZ; ++i )
04775 delta_xFRz[i] = 0.0;
04776
04777 if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_IDENTITY ) )
04778 {
04779
04780 for( j=0; j<nFR; ++j )
04781 {
04782 jj = FR_idx[j];
04783 for( i=0; i<nZ; ++i )
04784 delta_xFRz[i] -= QQ(jj,i) * tempA[j];
04785 }
04786
04787 if ( hessianType == HST_ZERO )
04788 {
04789 for( i=0; i<nZ; ++i )
04790 delta_xFRz[i] /= options.epsRegularisation;
04791 }
04792 }
04793 else
04794 {
04795
04796 if ( ( Delta_bB_isZero == BT_FALSE ) && ( r == 0 ) )
04797 H->times(bounds.getFree(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 1.0, tempA, nFR);
04798
04799
04800 if ( ( nAC > 0 ) && ( ( Delta_bC_isZero == BT_FALSE ) || ( Delta_bB_isZero == BT_FALSE ) ) )
04801 H->times(bounds.getFree(), bounds.getFree(), 1, 1.0, delta_xFR_TMP, nFR, 1.0, tempA, nFR);
04802
04803
04804 if ( nZ > 0 )
04805 {
04806 for( j=0; j<nFR; ++j )
04807 {
04808 jj = FR_idx[j];
04809 for( i=0; i<nZ; ++i )
04810 delta_xFRz[i] -= QQ(jj,i) * tempA[j];
04811 }
04812
04813 if ( backsolveR( delta_xFRz,BT_TRUE,delta_xFRz ) != SUCCESSFUL_RETURN )
04814 return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
04815
04816 if ( backsolveR( delta_xFRz,BT_FALSE,delta_xFRz ) != SUCCESSFUL_RETURN )
04817 return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
04818 }
04819 }
04820
04821
04822 if ( nZ > 0 )
04823 {
04824 for( i=0; i<nFR; ++i )
04825 {
04826 ZFR_delta_xFRz[i] = 0.0;
04827
04828 ii = FR_idx[i];
04829 for( j=0; j<nZ; ++j )
04830 ZFR_delta_xFRz[i] += QQ(ii,j) * delta_xFRz[j];
04831
04832 delta_xFR_TMP[i] += ZFR_delta_xFRz[i];
04833 }
04834 }
04835 }
04836
04837
04838 if ( nAC > 0 )
04839 {
04840 if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_IDENTITY ) )
04841 {
04842
04843
04844
04845
04846
04847
04848 if ( hessianType == HST_IDENTITY )
04849 {
04850 for( j=0; j<nAC; ++j )
04851 tempB[j] = delta_xFRy[j];
04852 }
04853 else
04854 {
04855 if ( usingRegularisation( ) == BT_TRUE )
04856 {
04857 for( j=0; j<nAC; ++j )
04858 tempB[j] = options.epsRegularisation*delta_xFRy[j];
04859 }
04860 else
04861 {
04862 for( j=0; j<nAC; ++j )
04863 tempB[j] = 0.0;
04864 }
04865 }
04866
04867 for( j=0; j<nAC; ++j )
04868 {
04869 for( i=0; i<nFR; ++i )
04870 {
04871 ii = FR_idx[i];
04872 tempB[j] += QQ(ii,nZ+j) * tempA[i];
04873 }
04874 }
04875 }
04876 else
04877 {
04878
04879
04880 if ( nZ > 0 )
04881 H->times(bounds.getFree(), bounds.getFree(), 1, 1.0, ZFR_delta_xFRz, nFR, 1.0, tempA, nFR);
04882
04883 for( i=0; i<nAC; ++i)
04884 {
04885 tempB[i] = 0.0;
04886 for( j=0; j<nFR; ++j )
04887 {
04888 jj = FR_idx[j];
04889 tempB[i] += QQ(jj,nZ+i) * tempA[j];
04890 }
04891 }
04892 }
04893
04894 if ( backsolveT( tempB,BT_TRUE,delta_yAC_TMP ) != SUCCESSFUL_RETURN )
04895 return THROWERROR( RET_STEPDIRECTION_FAILED_TQ );
04896 }
04897
04898
04899 for ( i=0; i<nFR; ++i )
04900 delta_xFR[i] += delta_xFR_TMP[i];
04901 for ( i=0; i<nAC; ++i )
04902 delta_yAC[i] += delta_yAC_TMP[i];
04903
04904 if ( options.numRefinementSteps > 0 )
04905 {
04906
04907 for ( i=0; i<nFR; ++i )
04908 {
04909 ii = FR_idx[i];
04910 tempA[i] = delta_g[ii];
04911 }
04912
04913 switch ( hessianType )
04914 {
04915 case HST_ZERO:
04916 break;
04917
04918 case HST_IDENTITY:
04919 for ( i=0; i<nFR; ++i )
04920 tempA[i] += delta_xFR[i];
04921 break;
04922
04923 default:
04924 H->times(bounds.getFree(), bounds.getFree(), 1, 1.0, delta_xFR, nFR, 1.0, tempA, nFR);
04925 H->times(bounds.getFree(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 1.0, tempA, nFR);
04926 break;
04927 }
04928
04929 A->transTimes(constraints.getActive(), bounds.getFree(), 1, -1.0, delta_yAC, nAC, 1.0, tempA, nFR);
04930 real_t rnrm = 0.0;
04931 for ( i=0; i<nFR; ++i )
04932 if (rnrm < fabs (tempA[i]))
04933 rnrm = fabs (tempA[i]);
04934
04935 if (!Delta_bC_isZero)
04936 {
04937 for ( i=0; i<nAC; ++i )
04938 {
04939 ii = AC_idx[i];
04940 if ( constraints.getStatus( ii ) == ST_LOWER )
04941 tempB[i] = delta_lbA[ii];
04942 else
04943 tempB[i] = delta_ubA[ii];
04944 }
04945 }
04946 else
04947 {
04948 for ( i=0; i<nAC; ++i )
04949 tempB[i] = 0.0;
04950 }
04951 A->times(constraints.getActive(), bounds.getFree(), 1, -1.0, delta_xFR, nFR, 1.0, tempB, nAC);
04952 A->times(constraints.getActive(), bounds.getFixed(), 1, -1.0, delta_xFX, nFX, 1.0, tempB, nAC);
04953 for ( i=0; i<nAC; ++i )
04954 if (rnrm < fabs (tempB[i]))
04955 rnrm = fabs (tempB[i]);
04956
04957
04958 if ( rnrm < options.epsIterRef )
04959 break;
04960 }
04961 }
04962
04963
04964
04965 if ( nFX > 0 )
04966 {
04967 for( i=0; i<nFX; ++i )
04968 delta_yFX[i] = delta_g[FX_idx[i]];
04969
04970 A->transTimes(constraints.getActive(), bounds.getFixed(), 1, -1.0, delta_yAC, nAC, 1.0, delta_yFX, nFX);
04971
04972 if ( hessianType == HST_ZERO )
04973 {
04974 if ( usingRegularisation( ) == BT_TRUE )
04975 for( i=0; i<nFX; ++i )
04976 delta_yFX[i] += options.epsRegularisation*delta_xFX[i];
04977 }
04978 else if ( hessianType == HST_IDENTITY )
04979 {
04980 for( i=0; i<nFX; ++i )
04981 delta_yFX[i] += delta_xFX[i];
04982 }
04983 else
04984 {
04985 H->times(bounds.getFixed(), bounds.getFree(), 1, 1.0, delta_xFR, nFR, 1.0, delta_yFX, nFX);
04986 H->times(bounds.getFixed(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 1.0, delta_yFX, nFX);
04987 }
04988 }
04989
04990 return SUCCESSFUL_RETURN;
04991 }
04992
04993
04994
04995
04996
04997 returnValue QProblem::performStep( const real_t* const delta_g,
04998 const real_t* const delta_lbA, const real_t* const delta_ubA,
04999 const real_t* const delta_lb, const real_t* const delta_ub,
05000 const real_t* const delta_xFX, const real_t* const delta_xFR,
05001 const real_t* const delta_yAC, const real_t* const delta_yFX,
05002 int& BC_idx, SubjectToStatus& BC_status, BooleanType& BC_isBound
05003 )
05004 {
05005 int i, j, ii, jj;
05006 int nV = getNV( );
05007 int nC = getNC( );
05008 int nFR = getNFR( );
05009 int nFX = getNFX( );
05010 int nAC = getNAC( );
05011 int nIAC = getNIAC( );
05012
05013 int* FR_idx;
05014 int* FX_idx;
05015 int* AC_idx;
05016 int* IAC_idx;
05017
05018 bounds.getFree( )->getNumberArray( &FR_idx );
05019 bounds.getFixed( )->getNumberArray( &FX_idx );
05020 constraints.getActive( )->getNumberArray( &AC_idx );
05021 constraints.getInactive( )->getNumberArray( &IAC_idx );
05022
05023
05024
05025 tau = 1.0;
05026 BC_idx = -1;
05027 BC_status = ST_UNDEFINED;
05028
05029 int BC_idx_tmp = -1;
05030
05031 real_t* num = new real_t[ getMax( nV,nC ) ];
05032 real_t* den = new real_t[ getMax( nV,nC ) ];
05033
05034 real_t* delta_Ax_l = new real_t[nC];
05035 real_t* delta_Ax_u = new real_t[nC];
05036 real_t* delta_Ax = new real_t[nC];
05037
05038 real_t* delta_x = new real_t[nV];
05039 for( j=0; j<nFR; ++j )
05040 {
05041 jj = FR_idx[j];
05042 delta_x[jj] = delta_xFR[j];
05043 }
05044 for( j=0; j<nFX; ++j )
05045 {
05046 jj = FX_idx[j];
05047 delta_x[jj] = delta_xFX[j];
05048 }
05049
05050
05051
05052
05053
05054 for( i=0; i<nAC; ++i )
05055 {
05056 ii = AC_idx[i];
05057
05058 num[i] = y[nV+ii];
05059 den[i] = -delta_yAC[i];
05060 }
05061
05062 performRatioTest( nAC,AC_idx,&constraints, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
05063
05064 if ( BC_idx_tmp >= 0 )
05065 {
05066 BC_idx = BC_idx_tmp;
05067 BC_status = ST_INACTIVE;
05068 BC_isBound = BT_FALSE;
05069 }
05070
05071
05072
05073
05074 for( i=0; i<nFX; ++i )
05075 {
05076 ii = FX_idx[i];
05077 num[i] = y[ii];
05078 den[i] = -delta_yFX[i];
05079 }
05080
05081 performRatioTest( nFX,FX_idx,&bounds, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
05082
05083 if ( BC_idx_tmp >= 0 )
05084 {
05085 BC_idx = BC_idx_tmp;
05086 BC_status = ST_INACTIVE;
05087 BC_isBound = BT_TRUE;
05088 }
05089
05090
05091
05092
05093
05094
05095 #ifdef __MANY_CONSTRAINTS__
05096 real_t delta_x_max = 0.0;
05097 for( i=0; i<nV; ++i )
05098 {
05099 if ( fabs( delta_x[i] ) > delta_x_max )
05100 delta_x_max = fabs( delta_x[i] );
05101 }
05102
05103 for( i=0; i<nIAC; ++i )
05104 {
05105 ii = IAC_idx[i];
05106
05107 if ( constraints.getType( ii ) != ST_UNBOUNDED )
05108 {
05109 delta_Ax_l[ii] = -delta_x_max;
05110 if ( delta_lbA[ii] > 0.0 )
05111 delta_Ax_l[ii] -= delta_lbA[ii];
05112
05113 delta_Ax_u[ii] = -delta_x_max;
05114 if ( delta_ubA[ii] > 0.0 )
05115 delta_Ax_u[ii] -= delta_ubA[ii];
05116
05117 if ( ( -delta_Ax_l[ii] >= Ax_l[ii] ) || ( -delta_Ax_u[ii] >= Ax_u[ii] ) )
05118 {
05119
05120 if ( constraintProduct == 0 )
05121 {
05122 Ax[ii] = 0.0;
05123 delta_Ax[ii] = 0.0;
05124 for( j=0; j<nV; ++j )
05125 {
05126 Ax[ii] += AA(ii,j) * x[j];
05127 delta_Ax[ii] += AA(ii,j) * delta_x[j];
05128 }
05129 }
05130 else
05131 {
05132 if ( (*constraintProduct)( ii,x, &(Ax[ii]) ) != 0 )
05133 {
05134 delete[] den; delete[] num;
05135 delete[] delta_Ax; delete[] delta_Ax_u; delete[] delta_Ax; delete[] delta_x;
05136 return THROWERROR( RET_ERROR_IN_CONSTRAINTPRODUCT );
05137 }
05138
05139 if ( (*constraintProduct)( ii,delta_x, &(delta_Ax[ii]) ) != 0 )
05140 {
05141 delete[] den; delete[] num;
05142 delete[] delta_Ax; delete[] delta_Ax_u; delete[] delta_Ax_l; delete[] delta_x;
05143 return THROWERROR( RET_ERROR_IN_CONSTRAINTPRODUCT );
05144 }
05145 }
05146
05147 Ax_u[ii] = ubA[ii] - Ax[ii];
05148 Ax_l[ii] = Ax[ii] - lbA[ii];
05149
05150
05151 if ( constraints.hasNoLower( ) == BT_FALSE )
05152 {
05153 num[i] = getMax( Ax_l[ii],0.0 );
05154 den[i] = delta_lbA[ii] - delta_Ax[ii];
05155
05156 if ( isBlocking( ii,num[i],den[i], options.epsNum,options.epsDen, tau ) == BT_TRUE )
05157 {
05158 tau = num[i]/den[i];
05159 BC_idx = ii;
05160 BC_status = ST_LOWER;
05161 BC_isBound = BT_FALSE;
05162 }
05163 }
05164 delta_Ax_l[ii] = delta_Ax[ii] - delta_lbA[ii];
05165
05166
05167 if ( constraints.hasNoUpper( ) == BT_FALSE )
05168 {
05169 num[i] = getMax( Ax_u[ii],0.0 );
05170 den[i] = delta_Ax[ii] - delta_ubA[ii];
05171
05172 if ( isBlocking( ii,num[i],den[i], options.epsNum,options.epsDen, tau ) == BT_TRUE )
05173 {
05174 tau = num[i]/den[i];
05175 BC_status = ST_UPPER;
05176 BC_isBound = BT_FALSE;
05177 }
05178 }
05179 delta_Ax_u[ii] = delta_ubA[ii] - delta_Ax[ii];
05180 }
05181 }
05182 }
05183 #else
05184
05185 if ( constraintProduct == 0 )
05186 {
05187 A->times(constraints.getInactive(), 0, 1, 1.0, delta_x, nV, 0.0, delta_Ax, nC, BT_FALSE);
05188 }
05189 else
05190 {
05191 for( i=0; i<nIAC; ++i )
05192 {
05193 ii = IAC_idx[i];
05194
05195 if ( constraints.getType( ii ) != ST_UNBOUNDED )
05196 {
05197 if ( (*constraintProduct)( ii,delta_x, &(delta_Ax[ii]) ) != 0 )
05198 {
05199 delete[] den; delete[] num;
05200 delete[] delta_Ax; delete[] delta_Ax_u; delete[] delta_Ax_l; delete[] delta_x;
05201 return THROWERROR( RET_ERROR_IN_CONSTRAINTPRODUCT );
05202 }
05203 }
05204 }
05205 }
05206
05207 if ( constraints.hasNoLower( ) == BT_FALSE )
05208 {
05209 for( i=0; i<nIAC; ++i )
05210 {
05211 ii = IAC_idx[i];
05212 num[i] = getMax( Ax_l[ii],0.0 );
05213 den[i] = delta_lbA[ii] - delta_Ax[ii];
05214 }
05215
05216 performRatioTest( nIAC,IAC_idx,&constraints, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
05217
05218 if ( BC_idx_tmp >= 0 )
05219 {
05220 BC_idx = BC_idx_tmp;
05221 BC_status = ST_LOWER;
05222 BC_isBound = BT_FALSE;
05223 }
05224 }
05225
05226 if ( constraints.hasNoUpper( ) == BT_FALSE )
05227 {
05228 for( i=0; i<nIAC; ++i )
05229 {
05230 ii = IAC_idx[i];
05231 num[i] = getMax( Ax_u[ii],0.0 );
05232 den[i] = delta_Ax[ii] - delta_ubA[ii];
05233 }
05234
05235 performRatioTest( nIAC,IAC_idx,&constraints, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
05236
05237 if ( BC_idx_tmp >= 0 )
05238 {
05239 BC_idx = BC_idx_tmp;
05240 BC_status = ST_UPPER;
05241 BC_isBound = BT_FALSE;
05242 }
05243 }
05244
05245
05246 for( i=0; i<nIAC; ++i )
05247 {
05248 ii = IAC_idx[i];
05249
05250 if ( constraints.getType( ii ) != ST_UNBOUNDED )
05251 {
05252 delta_Ax_l[ii] = delta_Ax[ii] - delta_lbA[ii];
05253 delta_Ax_u[ii] = delta_ubA[ii] - delta_Ax[ii];
05254 }
05255 }
05256 #endif
05257
05258
05259
05260
05261
05262 if ( bounds.hasNoLower( ) == BT_FALSE )
05263 {
05264 for( i=0; i<nFR; ++i )
05265 {
05266 ii = FR_idx[i];
05267 num[i] = getMax( x[ii] - lb[ii],0.0 );
05268 den[i] = delta_lb[ii] - delta_xFR[i];
05269 }
05270
05271 performRatioTest( nFR,FR_idx,&bounds, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
05272
05273 if ( BC_idx_tmp >= 0 )
05274 {
05275 BC_idx = BC_idx_tmp;
05276 BC_status = ST_LOWER;
05277 BC_isBound = BT_TRUE;
05278 }
05279 }
05280
05281
05282 if ( bounds.hasNoUpper( ) == BT_FALSE )
05283 {
05284 for( i=0; i<nFR; ++i )
05285 {
05286 ii = FR_idx[i];
05287 num[i] = getMax( ub[ii] - x[ii],0.0 );
05288 den[i] = delta_xFR[i] - delta_ub[ii];
05289 }
05290
05291 performRatioTest( nFR,FR_idx,&bounds, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
05292
05293 if ( BC_idx_tmp >= 0 )
05294 {
05295 BC_idx = BC_idx_tmp;
05296 BC_status = ST_UPPER;
05297 BC_isBound = BT_TRUE;
05298 }
05299 }
05300
05301 delete[] den;
05302 delete[] num;
05303 delete[] delta_x;
05304
05305
05306 #ifndef __XPCTARGET__
05307 char messageString[80];
05308
05309 if ( BC_status == ST_UNDEFINED )
05310 snprintf( messageString,80,"Stepsize is %.16e!",tau );
05311 else
05312 snprintf( messageString,80,"Stepsize is %.16e! (BC_idx = %d, BC_isBound = %d, BC_status = %d)",tau,BC_idx,BC_isBound,BC_status );
05313
05314 getGlobalMessageHandler( )->throwInfo( RET_STEPSIZE_NONPOSITIVE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
05315 #endif
05316
05317
05318
05319 if ( tau > ZERO )
05320 {
05321
05322 for( i=0; i<nFR; ++i )
05323 {
05324 ii = FR_idx[i];
05325 x[ii] += tau*delta_xFR[i];
05326 }
05327
05328 for( i=0; i<nFX; ++i )
05329 {
05330 ii = FX_idx[i];
05331 x[ii] += tau*delta_xFX[i];
05332 y[ii] += tau*delta_yFX[i];
05333 }
05334
05335 for( i=0; i<nAC; ++i )
05336 {
05337 ii = AC_idx[i];
05338 y[nV+ii] += tau*delta_yAC[i];
05339 }
05340
05341
05342 for( i=0; i<nV; ++i )
05343 {
05344 g[i] += tau*delta_g[i];
05345 lb[i] += tau*delta_lb[i];
05346 ub[i] += tau*delta_ub[i];
05347 }
05348
05349 for( i=0; i<nC; ++i )
05350 {
05351 lbA[i] += tau*delta_lbA[i];
05352 ubA[i] += tau*delta_ubA[i];
05353 }
05354
05355
05356
05357
05358
05359
05360
05361
05362
05363 A->times( constraints.getActive(),0, 1, 1.0, x, nV, 0.0, Ax, nC, BT_FALSE );
05364 for( i=0; i<nAC; ++i )
05365 {
05366 ii = AC_idx[i];
05367 Ax_u[ii] = ubA[ii] - Ax[ii];
05368 Ax_l[ii] = Ax[ii] - lbA[ii];
05369 }
05370 for( i=0; i<nIAC; ++i )
05371 {
05372 ii = IAC_idx[i];
05373 if ( constraints.getType( ii ) != ST_UNBOUNDED )
05374 {
05375 Ax[ii] += tau*delta_Ax[ii];
05376 Ax_l[ii] += tau*delta_Ax_l[ii];
05377 Ax_u[ii] += tau*delta_Ax_u[ii];
05378 }
05379 }
05380 }
05381 else
05382 {
05383
05384 #ifndef __XPCTARGET__
05385 snprintf( messageString,80,"Stepsize is %.16e",tau );
05386 getGlobalMessageHandler( )->throwWarning( RET_STEPSIZE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
05387 #endif
05388 }
05389
05390 delete[] delta_Ax; delete[] delta_Ax_u; delete[] delta_Ax_l;
05391
05392 return SUCCESSFUL_RETURN;
05393 }
05394
05395
05396
05397
05398
05399 returnValue QProblem::changeActiveSet( int BC_idx, SubjectToStatus BC_status, BooleanType BC_isBound )
05400 {
05401 int nV = getNV( );
05402
05403 char messageString[80];
05404
05405 switch ( BC_status )
05406 {
05407
05408 case ST_UNDEFINED:
05409 return RET_OPTIMAL_SOLUTION_FOUND;
05410
05411
05412 case ST_INACTIVE:
05413 if ( BC_isBound == BT_TRUE )
05414 {
05415 #ifndef __XPCTARGET__
05416 snprintf( messageString,80,"bound no. %d.", BC_idx );
05417 getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
05418 #endif
05419
05420 if ( removeBound( BC_idx,BT_TRUE,BT_TRUE,options.enableNZCTests ) != SUCCESSFUL_RETURN )
05421 return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
05422
05423 y[BC_idx] = 0.0;
05424 }
05425 else
05426 {
05427 #ifndef __XPCTARGET__
05428 snprintf( messageString,80,"constraint no. %d.", BC_idx );
05429 getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
05430 #endif
05431
05432 if ( removeConstraint( BC_idx,BT_TRUE,BT_TRUE,options.enableNZCTests ) != SUCCESSFUL_RETURN )
05433 return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
05434
05435 y[nV+BC_idx] = 0.0;
05436 }
05437 break;
05438
05439
05440
05441 default:
05442 returnValue returnvalue;
05443 if ( BC_isBound == BT_TRUE )
05444 {
05445 #ifndef __XPCTARGET__
05446 if ( BC_status == ST_LOWER )
05447 snprintf( messageString,80,"lower bound no. %d.", BC_idx );
05448 else
05449 snprintf( messageString,80,"upper bound no. %d.", BC_idx );
05450 getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
05451 #endif
05452
05453 returnvalue = addBound( BC_idx,BC_status,BT_TRUE );
05454 if ( returnvalue == RET_ADDBOUND_FAILED_INFEASIBILITY )
05455 return returnvalue;
05456 if ( returnvalue != SUCCESSFUL_RETURN )
05457 return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
05458 }
05459 else
05460 {
05461 #ifndef __XPCTARGET__
05462 if ( BC_status == ST_LOWER )
05463 snprintf( messageString,80,"lower constraint's bound no. %d.", BC_idx );
05464 else
05465 snprintf( messageString,80,"upper constraint's bound no. %d.", BC_idx );
05466 getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
05467 #endif
05468
05469 returnvalue = addConstraint( BC_idx,BC_status,BT_TRUE );
05470 if ( returnvalue == RET_ADDCONSTRAINT_FAILED_INFEASIBILITY )
05471 return returnvalue;
05472 if ( returnvalue != SUCCESSFUL_RETURN )
05473 return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
05474 }
05475 }
05476
05477 return SUCCESSFUL_RETURN;
05478 }
05479
05480
05481
05482
05483
05484
05485 real_t QProblem::relativeHomotopyLength( const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,
05486 const real_t* const lbA_new, const real_t* const ubA_new)
05487 {
05488 int nC = getNC( ), i;
05489 real_t len = QProblemB::relativeHomotopyLength(g_new, lb_new, ub_new);
05490 real_t d, s;
05491
05492
05493 for (i = 0; i < nC && lbA_new; i++)
05494 {
05495 s = fabs(lbA_new[i]);
05496 if (s < 1.0) s = 1.0;
05497 d = fabs(lbA_new[i] - lbA[i]) / s;
05498 if (d > len) len = d;
05499 }
05500
05501
05502 for (i = 0; i < nC && ubA_new; i++)
05503 {
05504 s = fabs(ubA_new[i]);
05505 if (s < 1.0) s = 1.0;
05506 d = fabs(ubA_new[i] - ubA[i]) / s;
05507 if (d > len) len = d;
05508 }
05509
05510 return len;
05511 }
05512
05513
05514
05515
05516
05517 returnValue QProblem::performRamping( )
05518 {
05519 int nV = getNV( ), nC = getNC( ), bstat, cstat, i;
05520 real_t t, rampVal;
05521
05522
05523 for (i = 0; i < nV; i++)
05524 {
05525 switch (bounds.getType(i))
05526 {
05527 case ST_EQUALITY: lb[i] = x[i]; ub[i] = x[i]; continue;
05528 case ST_UNBOUNDED: continue;
05529 default: break;
05530 }
05531
05532 t = static_cast<real_t>(i) / static_cast<real_t>(nV+nC-1);
05533 rampVal = (1.0-t) * ramp0 + t * ramp1;
05534 bstat = bounds.getStatus(i);
05535 if (bstat != ST_LOWER) { lb[i] = x[i] - rampVal; }
05536 if (bstat != ST_UPPER) { ub[i] = x[i] + rampVal; }
05537 if (bstat == ST_LOWER) { lb[i] = x[i]; y[i] = +rampVal; }
05538 if (bstat == ST_UPPER) { ub[i] = x[i]; y[i] = -rampVal; }
05539 if (bstat == ST_INACTIVE) y[i] = 0.0;
05540 }
05541
05542
05543 for (i = 0; i < nC; i++)
05544 {
05545 switch (constraints.getType(i))
05546 {
05547 case ST_EQUALITY: lbA[i] = Ax[i]; ubA[i] = Ax[i]; continue;
05548 case ST_BOUNDED: continue;
05549 default: break;
05550 }
05551
05552 t = static_cast<real_t>(nV+i) / static_cast<real_t>(nV+nC-1);
05553 rampVal = (1.0-t) * ramp0 + t * ramp1;
05554 cstat = bounds.getStatus(i);
05555 if (cstat != ST_LOWER) { lbA[i] = Ax[i] - rampVal; }
05556 if (cstat != ST_UPPER) { ubA[i] = Ax[i] + rampVal; }
05557 if (cstat == ST_LOWER) { lbA[i] = Ax[i]; y[nV+i] = +rampVal; }
05558 if (cstat == ST_UPPER) { ubA[i] = Ax[i]; y[nV+i] = -rampVal; }
05559 if (cstat == ST_INACTIVE) y[nV+i] = 0.0;
05560 }
05561
05562
05563 setupAuxiliaryQPgradient( );
05564
05565
05566 t = ramp0; ramp0 = ramp1; ramp1 = t;
05567
05568 return SUCCESSFUL_RETURN;
05569 }
05570
05571
05572
05573
05574
05575 returnValue QProblem::performDriftCorrection( )
05576 {
05577 int i;
05578 int nV = getNV ();
05579 int nC = getNC ();
05580
05581 for ( i=0; i<nV; ++i )
05582 {
05583 switch ( bounds.getType ( i ) )
05584 {
05585 case ST_BOUNDED:
05586 switch ( bounds.getStatus ( i ) )
05587 {
05588 case ST_LOWER:
05589 lb[i] = x[i];
05590 ub[i] = getMax (ub[i], x[i]);
05591 y[i] = getMax (y[i], 0.0);
05592 break;
05593 case ST_UPPER:
05594 lb[i] = getMin (lb[i], x[i]);
05595 ub[i] = x[i];
05596 y[i] = getMin (y[i], 0.0);
05597 break;
05598 case ST_INACTIVE:
05599 lb[i] = getMin (lb[i], x[i]);
05600 ub[i] = getMax (ub[i], x[i]);
05601 y[i] = 0.0;
05602 break;
05603 case ST_UNDEFINED:
05604 break;
05605 }
05606 break;
05607 case ST_EQUALITY:
05608 lb[i] = x[i];
05609 ub[i] = x[i];
05610 break;
05611 case ST_UNBOUNDED:
05612 case ST_UNKNOWN:
05613 break;
05614 }
05615 }
05616
05617 for ( i=0; i<nC; ++i )
05618 {
05619 switch ( constraints.getType ( i ) )
05620 {
05621 case ST_BOUNDED:
05622 switch ( constraints.getStatus ( i ) )
05623 {
05624 case ST_LOWER:
05625 lbA[i] = Ax[i];
05626 Ax_l[i] = 0.0;
05627 ubA[i] = getMax (ubA[i], Ax[i]);
05628 Ax_u[i] = ubA[i] - Ax[i];
05629 y[i+nV] = getMax (y[i+nV], 0.0);
05630 break;
05631 case ST_UPPER:
05632 lbA[i] = getMin (lbA[i], Ax[i]);
05633 Ax_l[i] = Ax[i] - lbA[i];
05634 ubA[i] = Ax[i];
05635 Ax_u[i] = 0.0;
05636 y[i+nV] = getMin (y[i+nV], 0.0);
05637 break;
05638 case ST_INACTIVE:
05639 lbA[i] = getMin (lbA[i], Ax[i]);
05640 Ax_l[i] = Ax[i] - lbA[i];
05641 ubA[i] = getMax (ubA[i], Ax[i]);
05642 Ax_u[i] = ubA[i] - Ax[i];
05643 y[i+nV] = 0.0;
05644 break;
05645 case ST_UNDEFINED:
05646 break;
05647 }
05648 break;
05649 case ST_EQUALITY:
05650 lbA[i] = Ax[i];
05651 Ax_l[i] = 0.0;
05652 ubA[i] = Ax[i];
05653 Ax_u[i] = 0.0;
05654 break;
05655 case ST_UNBOUNDED:
05656 case ST_UNKNOWN:
05657 break;
05658 }
05659 }
05660
05661 setupAuxiliaryQPgradient( );
05662
05663 return SUCCESSFUL_RETURN;
05664 }
05665
05666
05667
05668
05669 returnValue QProblem::setupAuxiliaryQP( const Bounds* const guessedBounds, const Constraints* const guessedConstraints )
05670 {
05671 int i, j;
05672 int nV = getNV( );
05673 int nC = getNC( );
05674
05675
05676 if ( ( guessedBounds == 0 ) || ( guessedConstraints == 0 ) )
05677 return THROWERROR( RET_INVALID_ARGUMENTS );
05678
05679
05680 if ( ( guessedBounds == &bounds ) && ( guessedConstraints == &constraints ) )
05681 return SUCCESSFUL_RETURN;
05682
05683 status = QPS_PREPARINGAUXILIARYQP;
05684
05685
05686
05687 if ( shallRefactorise( guessedBounds,guessedConstraints ) == BT_TRUE )
05688 {
05689
05690
05691 bounds.init( nV );
05692 constraints.init( nC );
05693
05694
05695 if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
05696 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
05697
05698 if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
05699 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
05700
05701 if ( constraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
05702 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
05703
05704
05705 if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
05706 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
05707
05708
05709 if ( setupAuxiliaryWorkingSet( guessedBounds,guessedConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
05710 THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
05711
05712
05713 if ( ( getNAC( ) + getNFX( ) ) == 0 )
05714 {
05715
05716 if ( setupCholeskyDecomposition( ) != SUCCESSFUL_RETURN )
05717 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
05718 }
05719 else
05720 {
05721
05722 if ( setupCholeskyDecompositionProjected( ) != SUCCESSFUL_RETURN )
05723 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
05724 }
05725 }
05726 else
05727 {
05728
05729 if ( setupAuxiliaryWorkingSet( guessedBounds,guessedConstraints,BT_FALSE ) != SUCCESSFUL_RETURN )
05730 THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
05731 }
05732
05733
05734
05735
05736 for ( i=0; i<nV; ++i )
05737 if ( bounds.getStatus( i ) != ST_INACTIVE )
05738 y[i] = 0.0;
05739
05740 for ( i=0; i<nC; ++i )
05741 if ( constraints.getStatus( i ) != ST_INACTIVE )
05742 y[nV+i] = 0.0;
05743
05744
05745 if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
05746 THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
05747
05748 A->times(1, 1.0, x, nV, 0.0, Ax, nC);
05749 for ( j=0; j<nC; ++j )
05750 {
05751 Ax_l[j] = Ax[j];
05752 Ax_u[j] = Ax[j];
05753 }
05754
05755
05756 if ( setupAuxiliaryQPbounds( 0,0,BT_FALSE ) != SUCCESSFUL_RETURN )
05757 THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
05758
05759 return SUCCESSFUL_RETURN;
05760 }
05761
05762
05763
05764
05765
05766
05767 BooleanType QProblem::shallRefactorise( const Bounds* const guessedBounds,
05768 const Constraints* const guessedConstraints
05769 ) const
05770 {
05771 int i;
05772 int nV = getNV( );
05773 int nC = getNC( );
05774
05775
05776 if ( getHessianType( ) == HST_SEMIDEF )
05777 return BT_TRUE;
05778
05779
05780
05781 int differenceNumberBounds = 0;
05782
05783 for( i=0; i<nV; ++i )
05784 if ( guessedBounds->getStatus( i ) != bounds.getStatus( i ) )
05785 ++differenceNumberBounds;
05786
05787
05788
05789 int differenceNumberConstraints = 0;
05790
05791 for( i=0; i<nC; ++i )
05792 if ( guessedConstraints->getStatus( i ) != constraints.getStatus( i ) )
05793 ++differenceNumberConstraints;
05794
05795
05796 if ( 2*(differenceNumberBounds+differenceNumberConstraints) > guessedConstraints->getNAC( )+guessedBounds->getNFX( ) )
05797 return BT_TRUE;
05798 else
05799 return BT_FALSE;
05800 }
05801
05802
05803
05804
05805
05806 returnValue QProblem::setupQPdata( SymmetricMatrix *_H, const real_t* const _g, Matrix *_A,
05807 const real_t* const _lb, const real_t* const _ub,
05808 const real_t* const _lbA, const real_t* const _ubA
05809 )
05810 {
05811 int i;
05812 int nC = getNC( );
05813
05814
05815
05816 if ( QProblemB::setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
05817 return THROWERROR( RET_INVALID_ARGUMENTS );
05818
05819
05820 if ( ( nC > 0 ) && ( _A == 0 ) )
05821 return THROWERROR( RET_INVALID_ARGUMENTS );
05822
05823 if ( nC > 0 )
05824 {
05825 setA( _A );
05826
05827
05828 if ( _lbA != 0 )
05829 {
05830 setLBA( _lbA );
05831 }
05832 else
05833 {
05834
05835 for( i=0; i<nC; ++i )
05836 lbA[i] = -INFTY;
05837 }
05838
05839
05840 if ( _ubA != 0 )
05841 {
05842 setUBA( _ubA );
05843 }
05844 else
05845 {
05846
05847 for( i=0; i<nC; ++i )
05848 ubA[i] = INFTY;
05849 }
05850 }
05851
05852 return SUCCESSFUL_RETURN;
05853 }
05854
05855
05856
05857
05858
05859 returnValue QProblem::setupQPdata( const real_t* const _H, const real_t* const _g, const real_t* const _A,
05860 const real_t* const _lb, const real_t* const _ub,
05861 const real_t* const _lbA, const real_t* const _ubA
05862 )
05863 {
05864 int i;
05865 int nC = getNC( );
05866
05867
05868
05869 if ( QProblemB::setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
05870 return THROWERROR( RET_INVALID_ARGUMENTS );
05871
05872
05873 if ( ( nC > 0 ) && ( _A == 0 ) )
05874 return THROWERROR( RET_INVALID_ARGUMENTS );
05875
05876 if ( nC > 0 )
05877 {
05878 setA( _A );
05879
05880
05881 if ( _lbA != 0 )
05882 {
05883 setLBA( _lbA );
05884 }
05885 else
05886 {
05887
05888 for( i=0; i<nC; ++i )
05889 lbA[i] = -INFTY;
05890 }
05891
05892
05893 if ( _ubA != 0 )
05894 {
05895 setUBA( _ubA );
05896 }
05897 else
05898 {
05899
05900 for( i=0; i<nC; ++i )
05901 ubA[i] = INFTY;
05902 }
05903 }
05904
05905 return SUCCESSFUL_RETURN;
05906 }
05907
05908
05909
05910
05911
05912 returnValue QProblem::setupQPdataFromFile( const char* const H_file, const char* const g_file, const char* const A_file,
05913 const char* const lb_file, const char* const ub_file,
05914 const char* const lbA_file, const char* const ubA_file
05915 )
05916 {
05917 int i;
05918 int nV = getNV( );
05919 int nC = getNC( );
05920
05921 returnValue returnvalue;
05922
05923
05924
05925 returnvalue = QProblemB::setupQPdataFromFile( H_file,g_file,lb_file,ub_file );
05926 if ( returnvalue != SUCCESSFUL_RETURN )
05927 return THROWERROR( returnvalue );
05928
05929
05930 if ( ( nC > 0 ) && ( A_file == 0 ) )
05931 return THROWERROR( RET_INVALID_ARGUMENTS );
05932
05933 if ( nC > 0 )
05934 {
05935 real_t* _A = new real_t[nC * nV];
05936 returnvalue = readFromFile( _A, nC,nV, A_file );
05937 if ( returnvalue != SUCCESSFUL_RETURN )
05938 {
05939 delete[] _A;
05940 return THROWERROR( returnvalue );
05941 }
05942 setA( _A );
05943 A->doFreeMemory( );
05944
05945
05946 if ( lbA_file != 0 )
05947 {
05948 returnvalue = readFromFile( lbA, nC, lbA_file );
05949 if ( returnvalue != SUCCESSFUL_RETURN )
05950 return THROWERROR( returnvalue );
05951 }
05952 else
05953 {
05954
05955 for( i=0; i<nC; ++i )
05956 lbA[i] = -INFTY;
05957 }
05958
05959
05960 if ( ubA_file != 0 )
05961 {
05962 returnvalue = readFromFile( ubA, nC, ubA_file );
05963 if ( returnvalue != SUCCESSFUL_RETURN )
05964 return THROWERROR( returnvalue );
05965 }
05966 else
05967 {
05968
05969 for( i=0; i<nC; ++i )
05970 ubA[i] = INFTY;
05971 }
05972 }
05973
05974 return SUCCESSFUL_RETURN;
05975 }
05976
05977
05978
05979
05980
05981 returnValue QProblem::loadQPvectorsFromFile( const char* const g_file, const char* const lb_file, const char* const ub_file,
05982 const char* const lbA_file, const char* const ubA_file,
05983 real_t* const g_new, real_t* const lb_new, real_t* const ub_new,
05984 real_t* const lbA_new, real_t* const ubA_new
05985 ) const
05986 {
05987 int nC = getNC( );
05988
05989 returnValue returnvalue;
05990
05991
05992
05993 returnvalue = QProblemB::loadQPvectorsFromFile( g_file,lb_file,ub_file, g_new,lb_new,ub_new );
05994 if ( returnvalue != SUCCESSFUL_RETURN )
05995 return THROWERROR( returnvalue );
05996
05997 if ( nC > 0 )
05998 {
05999
06000 if ( lbA_file != 0 )
06001 {
06002 if ( lbA_new != 0 )
06003 {
06004 returnvalue = readFromFile( lbA_new, nC, lbA_file );
06005 if ( returnvalue != SUCCESSFUL_RETURN )
06006 return THROWERROR( returnvalue );
06007 }
06008 else
06009 {
06010
06011 return THROWERROR( RET_INVALID_ARGUMENTS );
06012 }
06013 }
06014
06015
06016 if ( ubA_file != 0 )
06017 {
06018 if ( ubA_new != 0 )
06019 {
06020 returnvalue = readFromFile( ubA_new, nC, ubA_file );
06021 if ( returnvalue != SUCCESSFUL_RETURN )
06022 return THROWERROR( returnvalue );
06023 }
06024 else
06025 {
06026
06027 return THROWERROR( RET_INVALID_ARGUMENTS );
06028 }
06029 }
06030 }
06031
06032 return SUCCESSFUL_RETURN;
06033 }
06034
06035
06036
06037
06038
06039 returnValue QProblem::printIteration( int iteration,
06040 int BC_idx, SubjectToStatus BC_status, BooleanType BC_isBound
06041 )
06042 {
06043 #ifndef __XPCTARGET__
06044 char myPrintfString[80];
06045 char info[8];
06046
06047
06048 if ( iteration < 0 )
06049 return THROWERROR( RET_INVALID_ARGUMENTS );
06050
06051
06052 if ( options.printLevel != PL_MEDIUM )
06053 return SUCCESSFUL_RETURN;
06054
06055
06056
06057 if ( iteration == 0 )
06058 {
06059 snprintf( myPrintfString,80,"\n\n#################### qpOASES -- QP NO. %3.0d #####################\n\n", count );
06060 myPrintf( myPrintfString );
06061
06062 myPrintf( " Iter | StepLength | Info | nFX | nAC \n" );
06063 myPrintf( " ----------+------------------+------------------+---------+--------- \n" );
06064 }
06065
06066
06067 if ( BC_status == ST_UNDEFINED )
06068 {
06069 if ( hessianType == HST_ZERO )
06070 snprintf( info,3,"LP" );
06071 else
06072 snprintf( info,3,"QP" );
06073
06074 snprintf( myPrintfString,80," %5.1d | %1.6e | %s SOLVED | %4.1d | %4.1d \n", iteration,tau,info,getNFX( ),getNAC( ) );
06075 myPrintf( myPrintfString );
06076 }
06077 else
06078 {
06079 if ( BC_status == ST_INACTIVE )
06080 snprintf( info,5,"REM " );
06081 else
06082 snprintf( info,5,"ADD " );
06083
06084 if ( BC_isBound == BT_TRUE )
06085 snprintf( &(info[4]),4,"BND" );
06086 else
06087 snprintf( &(info[4]),4,"CON" );
06088
06089 snprintf( myPrintfString,80," %5.1d | %1.6e | %s %4.1d | %4.1d | %4.1d \n", iteration,tau,info,BC_idx,getNFX( ),getNAC( ) );
06090 myPrintf( myPrintfString );
06091 }
06092 #endif
06093
06094 return SUCCESSFUL_RETURN;
06095 }
06096
06097
06098 END_NAMESPACE_QPOASES
06099
06100
06101
06102
06103