QProblem.cpp
Go to the documentation of this file.
00001 /*
00002  *      This file is part of qpOASES.
00003  *
00004  *      qpOASES -- An Implementation of the Online Active Set Strategy.
00005  *      Copyright (C) 2007-2011 by Hans Joachim Ferreau, Andreas Potschka,
00006  *      Christian Kirches et al. All rights reserved.
00007  *
00008  *      qpOASES is free software; you can redistribute it and/or
00009  *      modify it under the terms of the GNU Lesser General Public
00010  *      License as published by the Free Software Foundation; either
00011  *      version 2.1 of the License, or (at your option) any later version.
00012  *
00013  *      qpOASES is distributed in the hope that it will be useful,
00014  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016  *      See the GNU Lesser General Public License for more details.
00017  *
00018  *      You should have received a copy of the GNU Lesser General Public
00019  *      License along with qpOASES; if not, write to the Free Software
00020  *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00021  *
00022  */
00023 
00024 
00036 #include <qpOASES/QProblem.hpp>
00037 
00038 
00039 BEGIN_NAMESPACE_QPOASES
00040 
00041 
00042 /*****************************************************************************
00043  *  P U B L I C                                                              *
00044  *****************************************************************************/
00045 
00046 
00047 /*
00048  *      Q P r o b l e m
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  *      Q P r o b l e m
00079  */
00080 QProblem::QProblem( int _nV, int _nC, HessianType _hessianType ) : QProblemB( _nV,_hessianType )
00081 {
00082         /* consistency checks */
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                 /* prevent segmentation faults in case nC == 0 
00103                  * (avoiding checks for A!=0 around all calls to A->... */
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; /* y of no constraints version too short! */
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];                        /* nFR */
00136         ZFR_delta_xFRz = new real_t[_nV];       /* nFR */
00137         delta_xFRz = new real_t[_nV];           /* nZ */
00138 
00139         if ( _nC > 0 )
00140         {
00141                 tempB = new real_t[_nC];                        /* nAC */
00142                 delta_xFRy = new real_t[_nC];           /* nAC */
00143                 delta_yAC_TMP = new real_t[_nC];    /* nAC */
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  *      Q P r o b l e m
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  *      ~ Q P r o b l e m
00170  */
00171 QProblem::~QProblem( )
00172 {
00173         clear( );
00174 }
00175 
00176 
00177 /*
00178  *      o p e r a t o r =
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  *      r e s e t
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         /* 1) Reset bounds, Cholesky decomposition and status flags. */
00207         if ( QProblemB::reset( ) != SUCCESSFUL_RETURN )
00208                 return THROWERROR( RET_RESET_FAILED );
00209 
00210         /* 2) Reset constraints. */
00211         constraints.init( nC );
00212 
00213         /* 3) Reset TQ factorisation. */
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         /* 4) Reset constraint product pointer. */
00221         constraintProduct = 0;
00222 
00223         return SUCCESSFUL_RETURN;
00224 }
00225 
00226 
00227 /*
00228  *      i n i t
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         /* 1) Consistency check. */
00240         if ( isInitialised( ) == BT_TRUE )
00241         {
00242                 THROWWARNING( RET_QP_ALREADY_INITIALISED );
00243                 reset( );
00244         }
00245 
00246         /* 2) Setup QP data. */
00247         if ( setupQPdata( _H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
00248                 return THROWERROR( RET_INVALID_ARGUMENTS );
00249 
00250         /* 3) Call to main initialisation routine (without any additional information). */
00251         return solveInitialQP( 0,0,0,0, nWSR,cputime );
00252 }
00253 
00254 
00255 /*
00256  *      i n i t
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         /* 1) Consistency check. */
00268         if ( isInitialised( ) == BT_TRUE )
00269         {
00270                 THROWWARNING( RET_QP_ALREADY_INITIALISED );
00271                 reset( );
00272         }
00273 
00274         /* 2) Setup QP data. */
00275         if ( setupQPdata( _H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
00276                 return THROWERROR( RET_INVALID_ARGUMENTS );
00277 
00278         /* 3) Call to main initialisation routine (without any additional information). */
00279         return solveInitialQP( 0,0,0,0, nWSR,cputime );
00280 }
00281 
00282 
00283 /*
00284  *      i n i t
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         /* 1) Consistency check. */
00296         if ( isInitialised( ) == BT_TRUE )
00297         {
00298                 THROWWARNING( RET_QP_ALREADY_INITIALISED );
00299                 reset( );
00300         }
00301 
00302         /* 2) Setup QP data from files. */
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         /* 3) Call to main initialisation routine (without any additional information). */
00307         return solveInitialQP( 0,0,0,0, nWSR,cputime );
00308 }
00309 
00310 
00311 /*
00312  *      i n i t
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         /* 1) Consistency checks. */
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         /* exclude these possibilities in order to avoid inconsistencies */
00353         if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) ) )
00354                 return THROWERROR( RET_INVALID_ARGUMENTS );
00355 
00356         /* 2) Setup QP data. */
00357         if ( setupQPdata( _H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
00358                 return THROWERROR( RET_INVALID_ARGUMENTS );
00359 
00360         /* 3) Call to main initialisation routine. */
00361         return solveInitialQP( xOpt,yOpt,guessedBounds,guessedConstraints, nWSR,cputime );
00362 }
00363 
00364 
00365 /*
00366  *      i n i t
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         /* 1) Consistency checks. */
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         /* exclude these possibilities in order to avoid inconsistencies */
00407         if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) ) )
00408                 return THROWERROR( RET_INVALID_ARGUMENTS );
00409 
00410         /* 2) Setup QP data. */
00411         if ( setupQPdata( _H,_g,_A,_lb,_ub,_lbA,_ubA ) != SUCCESSFUL_RETURN )
00412                 return THROWERROR( RET_INVALID_ARGUMENTS );
00413 
00414         /* 3) Call to main initialisation routine. */
00415         return solveInitialQP( xOpt,yOpt,guessedBounds,guessedConstraints, nWSR,cputime );
00416 }
00417 
00418 
00419 /*
00420  *      i n i t
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         /* 1) Consistency checks. */
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         /* exclude these possibilities in order to avoid inconsistencies */
00455         if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( ( guessedBounds != 0 ) || ( guessedConstraints != 0 ) ) )
00456                 return THROWERROR( RET_INVALID_ARGUMENTS );
00457 
00458         /* 2) Setup QP data from files. */
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         /* 3) Call to main initialisation routine. */
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                 /* Factorise full Hessian if no bounds/constraints are active. */
00473                 returnvalueCholesky = setupCholeskyDecomposition( );
00474 
00475                 /* If Hessian is not positive definite, regularise and try again. */
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                 /* Factorise projected Hessian if there active bounds/constraints. */
00491                 returnvalueCholesky = setupCholeskyDecompositionProjected( );
00492 
00493                 /* If Hessian is not positive definite, regularise and try again. */
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  *      h o t s t a r t
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                 /* possibly extend initial far bounds to largest bound/constraint data */
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                         /* TODO: encapsule this part to avoid code duplication! */
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                         /* Check for active far-bounds and move them away */
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                                         /* TODO: encapsule this part to avoid code duplication! */
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                                         /* TODO: encapsule this part to avoid code duplication! */
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                                 /* some other error */
00788                                 break;
00789                         }
00790 
00791                         /* swap ramp0 and ramp1 */
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  *      h o t s t a r t
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         /* consistency check */
00822         if ( g_file == 0 )
00823                 return THROWERROR( RET_INVALID_ARGUMENTS );
00824 
00825 
00826         /* 1) Allocate memory (if bounds exist). */
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         /* 2) Load new QP vectors from file. */
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         /* 3) Actually perform hotstart. */
00863         returnvalue = hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime );
00864 
00865         /* 4) Free memory. */
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  *      h o t s t a r t
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         /* start runtime measurement */
00898         real_t starttime = 0.0;
00899         if ( cputime != 0 )
00900                 starttime = getCPUtime( );
00901 
00902 
00903         /* 1) Update working sets according to guesses for working sets of bounds and constraints. */
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                 /* create empty bounds for setting up auxiliary QP */
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                 /* create empty constraints for setting up auxiliary QP */
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                 /* create empty bounds and constraints for setting up auxiliary QP */
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         /* 2) Perform usual homotopy. */
00950 
00951         /* Allow only remaining CPU time for usual hotstart. */
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         /* stop runtime measurement */
00958         if ( cputime != 0 )
00959                 *cputime = getCPUtime( ) - starttime;
00960 
00961         return returnvalue;
00962 }
00963 
00964 
00965 /*
00966  *      h o t s t a r t
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         /* consistency check */
00982         if ( g_file == 0 )
00983                 return THROWERROR( RET_INVALID_ARGUMENTS );
00984 
00985 
00986         /* 1) Allocate memory (if bounds exist). */
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         /* 2) Load new QP vectors from file. */
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         /* 3) Actually perform hotstart using initialised homotopy. */
01023         returnvalue = hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime,
01024                                                         guessedBounds,guessedConstraints
01025                                                         );
01026 
01027         /* 4) Free memory. */
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         /* 1) Determine index arrays. */
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  *      g e t N Z
01116  */
01117 int QProblem::getNZ( ) const
01118 {
01119         /* nZ = nFR - nAC */
01120         return getNFR( ) - getNAC( );
01121 }
01122 
01123 
01124 /*
01125  *      g e t D u a l S o l u t i o n
01126  */
01127 returnValue QProblem::getDualSolution( real_t* const yOpt ) const
01128 {
01129         int i;
01130 
01131         /* return optimal dual solution vector
01132          * only if current QP has been solved */
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  *      s e t C o n s t r a i n t P r o d u c t
01152  */
01153 returnValue QProblem::setConstraintProduct( ConstraintProduct* const _constraintProduct )
01154 {
01155         constraintProduct = _constraintProduct;
01156 
01157         return SUCCESSFUL_RETURN;
01158 }
01159 
01160 
01161 /*
01162  *      p r i n t P r o p e r t i e s
01163  */
01164 returnValue QProblem::printProperties( )
01165 {
01166         #ifndef __XPCTARGET__
01167         /* Do not print properties if print level is set to none! */
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         /* 1) Variables properties. */
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         /* 2) Constraints properties. */
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         /* 3) Further properties. */
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         /* 4) QP object properties. */
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  *  P R O T E C T E D                                                        *
01316  *****************************************************************************/
01317 
01318 /*
01319  *      c l e a r
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  *      c o p y
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; /* y of no constraints version too short! */
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];                        /* nFR */
01508         ZFR_delta_xFRz = new real_t[_nV];       /* nFR */
01509         delta_xFRz = new real_t[_nV];           /* nZ */
01510 
01511         if ( _nC > 0 )
01512         {
01513                 delta_xFRy = new real_t[_nC];           /* nAC */
01514                 tempB = new real_t[_nC];                        /* nAC */
01515                 delta_yAC_TMP = new real_t[_nC];    /* nAC */
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  *      s o l v e I n i t i a l Q P
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         /* some definitions */
01540         int nV = getNV( );
01541         int nC = getNC( );
01542 
01543 
01544         /* start runtime measurement */
01545         real_t starttime = 0.0;
01546         if ( cputime != 0 )
01547                 starttime = getCPUtime( );
01548 
01549         status = QPS_NOTINITIALISED;
01550 
01551 
01552         /* I) ANALYSE QP DATA: */
01553         #ifdef __MANY_CONSTRAINTS__
01554         /* 0) Checks if l1-norm of each constraint is not greater than 1. */
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) ); /* Andreas: do many constraints case later */
01562 
01563                 if ( l1 > 1.0 + 10.0*EPS )
01564                         return THROWERROR( RET_CONSTRAINTS_ARE_NOT_SCALED );
01565         }
01566         #endif
01567 
01568         /* 1) Check if Hessian happens to be the identity matrix. */
01569         if ( determineHessianType( ) != SUCCESSFUL_RETURN )
01570                 return THROWERROR( RET_INIT_FAILED );
01571 
01572         /* 2) Setup type of bounds and constraints (i.e. unbounded, implicitly fixed etc.). */
01573         if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
01574                 return THROWERROR( RET_INIT_FAILED );
01575 
01576         status = QPS_PREPARINGAUXILIARYQP;
01577 
01578 
01579         /* II) SETUP AUXILIARY QP WITH GIVEN OPTIMAL SOLUTION: */
01580         /* 1) Setup bounds and constraints data structure. */
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         /* 2) Setup optimal primal/dual solution for auxiliary QP. */
01588         if ( setupAuxiliaryQPsolution( xOpt,yOpt ) != SUCCESSFUL_RETURN )
01589                 return THROWERROR( RET_INIT_FAILED );
01590 
01591         /* 3) Obtain linear independent working set for auxiliary QP. */
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         /* 4) Setup working set of auxiliary QP and setup matrix factorisations. */
01600         /* a) Regularise Hessian if necessary. */
01601         if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_SEMIDEF ) )
01602         {
01603                 if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
01604                         return THROWERROR( RET_INIT_FAILED_REGULARISATION );
01605         }
01606 
01607         /* b) TQ factorisation. */
01608         if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
01609                 return THROWERROR( RET_INIT_FAILED_TQ );
01610 
01611         /* c) Working set of auxiliary QP. */
01612         if ( setupAuxiliaryWorkingSet( &auxiliaryBounds,&auxiliaryConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
01613                 return THROWERROR( RET_INIT_FAILED );
01614 
01615         haveCholesky = BT_FALSE;
01616 
01617         /* 5) Store original QP formulation... */
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         /* ... and setup QP data of an auxiliary QP having an optimal solution
01638          * as specified by the user (or xOpt = yOpt = 0, by default). */
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         /* III) SOLVE ACTUAL INITIAL QP: */
01659         /* Allow only remaining CPU time for usual hotstart. */
01660         if ( cputime != 0 )
01661                 *cputime -= getCPUtime( ) - starttime;
01662 
01663         /* Use hotstart method to find the solution of the original initial QP,... */
01664         returnValue returnvalue = hotstart( g_original,lb_original,ub_original,lbA_original,ubA_original, nWSR,cputime );
01665 
01666         /* ... deallocate memory,... */
01667         delete[] ubA_original; delete[] lbA_original; delete[] ub_original; delete[] lb_original; delete[] g_original;
01668 
01669         /* ... check for infeasibility and unboundedness... */
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         /* ... and internal errors. */
01677         if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
01678                 return THROWERROR( RET_INIT_FAILED_HOTSTART );
01679 
01680 
01681         /* stop runtime measurement */
01682         if ( cputime != 0 )
01683                 *cputime = getCPUtime( ) - starttime;
01684 
01685         THROWINFO( RET_INIT_SUCCESSFUL );
01686 
01687         return returnvalue;
01688 }
01689 
01690 
01691 /*
01692  *      s o l v e Q P
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         /* consistency check */
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         /* start runtime measurement */
01713         real_t starttime = 0.0;
01714         if ( cputime != 0 )
01715                 starttime = getCPUtime( );
01716 
01717 
01718         /* I) PREPARATIONS */
01719         /* 1) Allocate delta vectors of gradient and (constraints') bounds,
01720          *    index arrays and step direction arrays. */
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         /* 2) Update type of bounds and constraints, e.g.
01744          *    a former equality constraint might have become a normal one etc. */
01745         if ( setupSubjectToType( lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
01746                 return THROWERROR( RET_HOTSTART_FAILED );
01747 
01748         /* 3) Reset status flags. */
01749         infeasible = BT_FALSE;
01750         unbounded  = BT_FALSE;
01751 
01752 
01753         /* II) MAIN HOMOTOPY LOOP */
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                         /* If CPU time limit is exceeded, stop homotopy loop immediately!
01761                         * Assign number of working set recalculations (runtime measurement is stopped later). */
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                 /* 2) Detemination of shift direction of the gradient and the (constraints') bounds. */
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                         /* Assign number of working set recalculations and stop runtime measurement. */
01784                         nWSR = iter;
01785                         if ( cputime != 0 )
01786                                 *cputime = getCPUtime( ) - starttime;
01787 
01788                         THROWERROR( RET_SHIFT_DETERMINATION_FAILED );
01789                         return returnvalue;
01790                 }
01791 
01792                 /* 3) Determination of step direction of X and Y. */
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                         /* Assign number of working set recalculations and stop runtime measurement. */
01803                         nWSR = iter;
01804                         if ( cputime != 0 )
01805                                 *cputime = getCPUtime( ) - starttime;
01806 
01807                         THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
01808                         return returnvalue;
01809                 }
01810 
01811                 /* 4) Determination of step length TAU.
01812                  *    This step along the homotopy path is also taken (without changing working set). */
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                         /* Assign number of working set recalculations and stop runtime measurement. */
01823                         nWSR = iter;
01824                         if ( cputime != 0 )
01825                                 *cputime = getCPUtime( ) - starttime;
01826 
01827                         THROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED );
01828                         return returnvalue;
01829                 }
01830 
01831                 /* 5) Termination criterion. */
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 ); /* do not pass this as return value! */
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                 /* 6) Change active set. */
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                         /* Assign number of working set recalculations and stop runtime measurement. */
01861                         nWSR = iter;
01862                         if ( cputime != 0 )
01863                                 *cputime = getCPUtime( ) - starttime;
01864 
01865                         /* Checks for infeasibility... */
01866                         if ( isInfeasible( ) == BT_TRUE )
01867                         {
01868                                 status = QPS_HOMOTOPYQPSOLVED;
01869                                 return setInfeasibilityFlag( RET_HOTSTART_STOPPED_INFEASIBILITY );
01870                         }
01871 
01872                         /* ...unboundedness... */
01873                         if ( unbounded == BT_TRUE ) /* not necessary since objective function convex! */
01874                                 return THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS );
01875 
01876                         /* ... and throw unspecific error otherwise */
01877                         THROWERROR( RET_HOMOTOPY_STEP_FAILED );
01878                         return returnvalue;
01879                 }
01880 
01881                 /* 6.5) Possibly refactorise projected Hessian from scratch. */
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                 /* stationarity */
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                 /* feasibility */
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                 /* complementarity */
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 /* __DEBUG_ITER__ */
01968 
01969 
01970                 /* 7) Output information of successful QP iteration. */
01971                 status = QPS_HOMOTOPYQPSOLVED;
01972 
01973                 if ( printIteration( iter,BC_idx,BC_status,BC_isBound ) != SUCCESSFUL_RETURN )
01974                         THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
01975 
01976                 /* 8) Perform Ramping Strategy on zero homotopy step or drift correction (if desired). */
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( );  /* always returns SUCCESSFUL_RETURN */
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         /* stop runtime measurement */
01989         if ( cputime != 0 )
01990                 *cputime = getCPUtime( ) - starttime;
01991 
01992 
01993         /* if programm gets to here, output information that QP could not be solved
01994          * within the given maximum numbers of working set changes */
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  *      s o l v e R e g u l a r i s e d Q P
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         /* Stop here if QP has not been regularised (i.e. normal QP solution). */
02025         if ( usingRegularisation( ) == BT_FALSE )
02026                 return solveQP( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,cputime,nWSRperformed );
02027 
02028 
02029         /* I) SOLVE USUAL REGULARISED QP */
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         /* Only continue if QP solution has been successful. */
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         /* II) PERFORM SUCCESSIVE REGULARISATION STEPS */
02064         real_t* gMod = new real_t[nV];
02065 
02066         for( step=0; step<options.numRegularisationSteps; ++step )
02067         {
02068                 /* 1) Modify gradient: gMod = g - eps*xOpt
02069                  *    (assuming regularisation matrix to be eps*Id). */
02070                 for( i=0; i<nV; ++i )
02071                         gMod[i] = g_new[i] - options.epsRegularisation*x[i];
02072 
02073                 /* 2) Solve regularised QP with modified gradient allowing
02074                  *    only as many working set recalculations and CPU time
02075                  *    as have been left from previous QP solutions. */
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                 /* Only continue if QP solution has been successful. */
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  *      s e t u p S u b j e c t T o T y p e
02117  */
02118 returnValue QProblem::setupSubjectToType( )
02119 {
02120         return setupSubjectToType( lb,ub,lbA,ubA );
02121 }
02122 
02123 
02124 /*
02125  *      s e t u p S u b j e c t T o T y p e
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         /* I) SETUP SUBJECTTOTYPE FOR BOUNDS */
02136         if ( QProblemB::setupSubjectToType( lb_new,ub_new ) != SUCCESSFUL_RETURN )
02137                 return THROWERROR( RET_SETUPSUBJECTTOTYPE_FAILED );
02138 
02139 
02140         /* II) SETUP SUBJECTTOTYPE FOR CONSTRAINTS */
02141         /* 1) Check if lower constraints' bounds are present. */
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         /* 2) Check if upper constraints' bounds are present. */
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         /* 3) Determine implicit equality constraints and unbounded constraints. */
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  *      c h o l e s k y D e c o m p o s i t i o n P r o j e c t e d
02208  */
02209 returnValue QProblem::setupCholeskyDecompositionProjected( )
02210 {
02211         int i, j;
02212         int nV  = getNV( );
02213         int nZ  = getNZ( );
02214 
02215         /* 1) Initialises R with all zeros. */
02216         for( i=0; i<nV*nV; ++i )
02217                 R[i] = 0.0;
02218 
02219         /* 2) Calculate Cholesky decomposition of projected Hessian Z'*H*Z. */
02220         if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_IDENTITY ) )
02221         {
02222                 if ( hessianType == HST_ZERO )
02223                 {
02224                         /* if Hessian is zero matrix, it is assumed that it has been
02225                          * regularised and thus its Cholesky factor is the identity
02226                          * matrix scaled by sqrt(eps). */
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                         /* if Hessian is identity, so is its Cholesky factor. */
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                         /* calculate Z'*H*Z */
02253                         if (constraints.getActive ()->getLength () == 0) {
02254                                 /* make Z trivial */
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                                 /* now Z is trivial, and so is Z'HZ */
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                                 /* this is expensive if Z is large! */
02266                                 H->bilinear(bounds.getFree(), nZ, Q, nV, R, nV);
02267                         }
02268 
02269                         /* R'*R = Z'*H*Z */
02270                         long info = 0;
02271                         unsigned long _nZ = nZ, _nV = nV;
02272 
02273                         POTRF ("U", &_nZ, R, &_nV, &info);
02274 
02275                         /* <0 = invalid call, =0 ok, >0 not spd */
02276                         if (info > 0) {
02277                                 hessianType = HST_SEMIDEF;
02278                                 return RET_HESSIAN_NOT_SPD;
02279                         }
02280 
02281                         /* zero first subdiagonal to make givens updates work */
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  *      s e t u p T Q f a c t o r i s a t i o n
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         /* 1) Set Q to unity matrix. */
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         /* 2) Set T to zero matrix. */
02314         for( i=0; i<sizeT*sizeT; ++i )
02315                 T[i] = 0.0;
02316 
02317         return SUCCESSFUL_RETURN;
02318 }
02319 
02320 
02321 /*
02322  *      o b t a i n A u x i l i a r y W o r k i n g S e t
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         /* 1) Ensure that desiredBounds is allocated (and different from guessedBounds). */
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         /* 2) Setup working set of bounds for auxiliary initial QP. */
02345         if ( QProblemB::obtainAuxiliaryWorkingSet( xOpt,yOpt,guessedBounds, auxiliaryBounds ) != SUCCESSFUL_RETURN )
02346                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
02347 
02348         /* 3) Setup working set of constraints for auxiliary initial QP. */
02349         if ( guessedConstraints != 0 )
02350         {
02351                 /* If an initial working set is specific, use it!
02352                  * Moreover, add all equality constraints if specified. */
02353                 for( i=0; i<nC; ++i )
02354                 {
02355                         /* Add constraint only if it is not (goint to be) disabled! */
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    /* No initial working set specified. */
02373         {
02374                 /* Obtain initial working set by "clipping". */
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                                 /* Moreover, add all equality constraints if specified. */
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                 /* Obtain initial working set in accordance to sign of dual solution vector. */
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                                 /* Moreover, add all equality constraints if specified. */
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                 /* If xOpt and yOpt are null pointer and no initial working is specified,
02445                  * start with empty working set (or implicitly fixed bounds and equality constraints only)
02446                  * for auxiliary QP. */
02447                 if ( ( xOpt == 0 ) && ( yOpt == 0 ) )
02448                 {
02449                         for( i=0; i<nC; ++i )
02450                         {
02451                                 /* Only add all equality constraints if specified. */
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  *      s e t u p A u x i l i a r y W o r k i n g S e t
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         /* consistency checks */
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         /* Check for trivial working set (all and only bounds active) */
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         /* I) SETUP CHOLESKY FLAG:
02533          *    Cholesky decomposition shall only be updated if working set
02534          *    shall be updated (i.e. NOT setup afresh!) */
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         /* options.epsLITests = 1e-1; */
02547 
02548         /* II) REMOVE FORMERLY ACTIVE (CONSTRAINTS') BOUNDS (IF NECESSARY): */
02549         if ( setupAfresh == BT_FALSE )
02550         {
02551                 /* 1) Remove all active constraints that shall be inactive or disabled AND
02552                 *    all active constraints that are active at the wrong bound. */
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                 /* 2) Remove all active bounds that shall be inactive AND
02565                 *    all active bounds that are active at the wrong bound. */
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         /* III) ADD NEWLY ACTIVE (CONSTRAINTS') BOUNDS: */
02580 
02581         /* 1) Add all equality bounds. */
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                         /* No check for linear independence necessary. */
02587                         if ( addBound( i,auxiliaryBounds->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
02588                                 return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
02589                 }
02590         }
02591 
02592         /* 2) Add all equality constraints. */
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                         /* Add constraint only if it is linearly independent from the current working set. */
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                                 /* Equalities are not linearly independent! */
02606                                 constraints.setType(i, ST_BOUNDED);
02607                         }
02608                 }
02609         }
02610 
02611 
02612         /* 3) Add all inactive bounds that shall be active AND
02613          *    all formerly active bounds that have been active at the wrong bound. */
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                         /* Add bound only if it is linearly independent from the current working set. */
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         /* 4) Add all inactive constraints that shall be active AND
02628          *    all formerly active constraints that have been active at the wrong bound. */
02629         for( i=0; i<nC; ++i )
02630         {
02631                 if ( ( constraints.getType( i ) != ST_EQUALITY ) && ( auxiliaryConstraints->getStatus( i ) != ST_INACTIVE ) )
02632                 {
02633                         /* formerly inactive */
02634                         if ( constraints.getStatus( i ) == ST_INACTIVE )
02635                         {
02636                                 /* Add constraint only if it is linearly independent from the current working set. */
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  *      s e t u p A u x i l i a r y Q P s o l u t i o n
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         /* Setup primal/dual solution vector for auxiliary initial QP:
02665          * if a null pointer is passed, a zero vector is assigned;
02666          *  old solution vector is kept if pointer to internal solution vevtor is passed. */
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  *      s e t u p A u x i l i a r y Q P g r a d i e n t
02712  */
02713 returnValue QProblem::setupAuxiliaryQPgradient( )
02714 {
02715         int i;
02716         int nV = getNV( );
02717         int nC = getNC( );
02718 
02719 
02720         /* Setup gradient vector: g = -H*x + [Id A]'*[yB yC]
02721          *                          = yB - H*x + A'*yC. */
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                         /* y'*Id */
02740                         for ( i=0; i<nV; ++i )
02741                                 g[i] = y[i];
02742 
02743                         /* - H*x */
02744                         H->times(1, -1.0, x, nV, 1.0, g, nV);
02745                         break;
02746         }
02747 
02748         /* + A'*yC */
02749         A->transTimes(1, 1.0, y + nV, nC, 1.0, g, nV);
02750 
02751         return SUCCESSFUL_RETURN;
02752 }
02753 
02754 
02755 /*
02756  *      s e t u p A u x i l i a r y Q P b o u n d s
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         /* 1) Setup bound vectors. */
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                                                 /* If a bound is inactive although it was supposed to be
02784                                                 * active by the auxiliaryBounds, it could not be added
02785                                                 * due to linear dependence. Thus set it "strongly inactive". */
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         /* 2) Setup constraints vectors. */
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                                                 /* If a constraint is inactive although it was supposed to be
02846                                                 * active by the auxiliaryConstraints, it could not be added
02847                                                 * due to linear dependence. Thus set it "strongly inactive". */
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  *      a d d C o n s t r a i n t
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         /* consistency checks */
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         /* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET,
02925          *    i.e. remove a constraint or bound if linear dependence occurs. */
02926         /* check for LI only if Cholesky decomposition shall be updated! */
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         /* some definitions */
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         /* II) ADD NEW ACTIVE CONSTRAINT TO MATRIX T: */
02969         /* 1) Add row [wZ wY] = aFR'*[Z Y] to the end of T: assign aFR. */
02970         A->getRow(number, bounds.getFree(), 1.0, aFR);
02971 
02972         /* calculate wZ */
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         /* 2) Calculate wY and store it directly into T. */
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                 /* II) RESTORE TRIANGULAR FORM OF T: */
03001                 /*     Use column-wise Givens rotations to restore reverse triangular form
03002                 *      of T, simultanenous change of Q (i.e. Z) and R. */
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                         /* III) RESTORE TRIANGULAR FORM OF R:
03029                          *      Use row-wise Givens rotations to restore upper triangular form of R. */
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 ) /* last column of R is thrown away */
03036                                         applyGivens( c,s,nu,RR(i,j),RR(1+i,j), RR(i,j),RR(1+i,j) );
03037                         }
03038                         /* last column of R is thrown away */
03039                         for( i=0; i<nZ; ++i )
03040                                 RR(i,nZ-1) = 0.0;
03041                 }
03042         }
03043 
03044         delete[] wZ;
03045 
03046 
03047         /* IV) UPDATE INDICES */
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  *      a d d C o n s t r a i n t _ c h e c k L I
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                  * expensive LI test. Backsolve with refinement using special right
03081                  * hand side. This gives an estimate for what should be considered
03082                  * "zero". We then check linear independence relative to this estimate.
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                 /* compute the weight in inf-norm */
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                 /* look at the "zero" in a relative inf-norm */
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                 /* relative test against zero in inf-norm */
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                  * cheap LI test for constraint. Check if constraint <number> is
03151                  * linearly independent from the the active ones (<=> is element of null
03152                  * space of Afr).
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                                 /*fprintf(stderr, "LI test: |sum| = %9.2e, l2 = %9.2e, var = %d\n", fabs(sum), l2, jj+1); */
03176                                 returnvalue = RET_LINEARLY_INDEPENDENT;
03177                                 break;
03178                         }
03179                 }
03180 
03181                 delete[] Arow;
03182         }
03183 
03184         return THROWINFO( returnvalue );
03185 }
03186 
03187 
03188 /*
03189  *      a d d C o n s t r a i n t _ e n s u r e L I
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         /* I) Check if new constraint is linearly independent from the active ones. */
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         /* II) NEW CONSTRAINT IS LINEARLY DEPENDENT: */
03212         /* 1) Determine coefficients of linear combination,
03213          *    cf. M.J. Best. Applied Mathematics and Parallel Computing, chapter:
03214          *    An Algorithm for the Solution of the Parametric Quadratic Programming
03215          *    Problem, pages 57-76. Physica-Verlag, Heidelberg, 1996. */
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         /* 2) Calculate xiC */
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         /* 3) Calculate xiB. */
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         /* III) DETERMINE CONSTRAINT/BOUND TO BE REMOVED. */
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         /* 1) Constraints. */
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         /* 2) Bounds. */
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         /* setup output preferences */
03293         char messageString[80];
03294 
03295         /* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */
03296         if ( y_min_number >= 0 )
03297         {
03298                 /* Update Lagrange multiplier... */
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                 /* ... also for newly active constraint... */
03311                 if ( C_status == ST_LOWER )
03312                         y[nV+number] = y_min;
03313                 else
03314                         y[nV+number] = -y_min;
03315 
03316                 /* ... and for constraint to be removed. */
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                 /* no constraint/bound can be removed => QP is infeasible! */
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  *      a d d B o u n d
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         /* consistency checks */
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         /* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET,
03395          *    i.e. remove a constraint or bound if linear dependence occurs. */
03396         /* check for LI only if Cholesky decomposition shall be updated! */
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         /* some definitions */
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         /* II) SWAP INDEXLIST OF FREE VARIABLES:
03431          *     move the variable to be fixed to the end of the list of free variables. */
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         /* III) ADD NEW ACTIVE BOUND TO TOP OF MATRIX T: */
03445         /* 1) add row [wZ wY] = [Z Y](number) at the top of T: assign w */
03446         for( i=0; i<nFR; ++i )
03447                 w[i] = QQ(FR_idx[nFR-1],i);
03448 
03449 
03450         /* 2) Use column-wise Givens rotations to restore reverse triangular form
03451          *    of the first row of T, simultanenous change of Q (i.e. Z) and R. */
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 )    /* ( nAC == 0 ) <=> ( nZ == nFR ) <=> Y and T are empty => nothing to do */
03475         {
03476                 /* store new column a in a temporary vector instead of shifting T one column to the left */
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                 /* IV) RESTORE TRIANGULAR FORM OF R:
03521                  *     use row-wise Givens rotations to restore upper triangular form of R */
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 ) /* last column of R is thrown away */
03528                                 applyGivens( c,s,nu,RR(i,j),RR(1+i,j), RR(i,j),RR(1+i,j) );
03529                 }
03530                 /* last column of R is thrown away */
03531                 for( i=0; i<nZ; ++i )
03532                         RR(i,nZ-1) = 0.0;
03533         }
03534 
03535 
03536         /* V) UPDATE INDICES */
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  *      a d d B o u n d _ c h e c k L I
03547  */
03548 returnValue QProblem::addBound_checkLI( int number )
03549 {
03550         int i, ii;
03551         int nV  = getNV( );  /* for QQ() macro */
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                  * expensive LI test. Backsolve with refinement using special right
03562                  * hand side. This gives an estimate for what should be considered
03563                  * "zero". We then check linear independence relative to this estimate.
03564                  */
03565 
03566                 /*
03567                  * expensive LI test. Backsolve with refinement using special right
03568                  * hand side. This gives an estimate for what should be considered
03569                  * "zero". We then check linear independence relative to this estimate.
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;  /* sign doesn't matter here */
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                 /* compute the weight in inf-norm */
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                 /* look at the "zero" in a relative inf-norm */
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                 /* relative test against zero in inf-norm */
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                  * cheap LI test for simple bound. Check if constraint <number> is
03635                  * linearly independent from the the active ones (<=> is element of null
03636                  * space of Afr).
03637                  */
03638 
03639                 /* some definitions */
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  *      a d d B o u n d _ e n s u r e L I
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         /* I) Check if new constraint is linearly independent from the active ones. */
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         /* II) NEW BOUND IS LINEARLY DEPENDENT: */
03677         /* 1) Determine coefficients of linear combination,
03678          *    cf. M.J. Best. Applied Mathematics and Parallel Computing, chapter:
03679          *    An Algorithm for the Solution of the Parametric Quadratic Programming
03680          *    Problem, pages 57-76. Physica-Verlag, Heidelberg, 1996. */
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         /* 2) Calculate xiC. */
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         /* 3) Calculate xiB. */
03716         A->transTimes(constraints.getActive(), bounds.getFixed(), 1, -1.0, xiC, nAC, 0.0, xiB, nFX);
03717 
03718 
03719         /* III) DETERMINE CONSTRAINT/BOUND TO BE REMOVED. */
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         /* 1) Constraints. */
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         /* 2) Bounds. */
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         /* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */
03756         char messageString[80];
03757 
03758         if ( y_min_number >= 0 )
03759         {
03760                 /* Update Lagrange multiplier... */
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                 /* ... also for newly active bound ... */
03773                 if ( B_status == ST_LOWER )
03774                         y[number] = y_min;
03775                 else
03776                         y[number] = -y_min;
03777 
03778                 /* ... and for bound to be removed. */
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                 /* no constraint/bound can be removed => QP is infeasible! */
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  *      r e m o v e C o n s t r a i n t
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         /* consistency check */
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         /* some definitions */
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         /* consistency checks */
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         /* N) PERFORM ZERO CURVATURE TEST. */
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         /* save index sets and decompositions for flipping bounds strategy */
03884         if ( ( exchangeHappened == BT_FALSE ) && ( options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) )
03885                 flipper.set( &bounds,R,&constraints,Q,T );
03886 
03887         /* I) REMOVE <number>th ROW FROM T,
03888          *    i.e. shift rows number+1 through nAC  upwards (instead of the actual
03889          *    constraint number its corresponding index within matrix A is used). */
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                 /* gimmick: write zeros into the last row of T */
03896                 for( j=0; j<nAC; ++j )
03897                         TT(nAC-1,tcol+j) = 0.0;
03898 
03899 
03900                 /* II) RESTORE TRIANGULAR FORM OF T,
03901                  *     use column-wise Givens rotations to restore reverse triangular form
03902                  *     of T simultanenous change of Q (i.e. Y). */
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                 /* gimmick: write zeros into the last row of T */
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                 /* III) UPDATE CHOLESKY DECOMPOSITION,
03932                  *      calculate new additional column (i.e. [r sqrt(rho2)]')
03933                  *      of the Cholesky factor R. */
03934                 real_t* Hz = new real_t[nFR];
03935                 real_t* z = new real_t[nFR];
03936                 real_t rho2 = 0.0;
03937 
03938                 /* 1) Calculate Hz = H*z, where z is the new rightmost column of Z
03939                  *    (i.e. the old leftmost column of Y).  */
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                         /* 2) Calculate ZHz = Z'*Hz (old Z). */
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                         /* 3) Calculate r = R^-T * ZHz. */
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                         /* 4) Calculate rho2 = rho^2 = z'*Hz - r'*r
03969                          *    and store r into R. */
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                 /* 5) Store rho into R. */
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         /* IV) UPDATE INDICES */
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                 /* add bound or constraint */
04041 
04042                 /* hessianType = HST_SEMIDEF; */
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  *      r e m o v e B o u n d
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         /* consistency checks */
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         /* some definitions */
04085         int nV  = getNV( );
04086         int nFR = getNFR( );
04087         int nAC = getNAC( );
04088         int nZ  = getNZ( );
04089 
04090         int tcol = sizeT - nAC;
04091 
04092         /* N) PERFORM ZERO CURVATURE TEST. */
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         /* save index sets and decompositions for flipping bounds strategy */
04102         if ( ( options.enableFlippingBounds == BT_TRUE ) && ( allowFlipping == BT_TRUE ) && ( exchangeHappened == BT_FALSE ) )
04103                 flipper.set( &bounds,R,&constraints,Q,T );
04104 
04105         /* I) UPDATE INDICES */
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         /* I) APPEND <nFR+1>th UNITY VECTOR TO Q. */
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                 /* store new column a in a temporary vector instead of shifting T one column to the left and appending a */
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                 /* II) RESTORE TRIANGULAR FORM OF T,
04134                  *     use column-wise Givens rotations to restore reverse triangular form
04135                  *     of T = [T A(:,number)], simultanenous change of Q (i.e. Y and Z). */
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                                 /* nZ+1+nAC = nFR+1  /  nZ+(1) = nZ+1 */
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                 /* III) UPDATE CHOLESKY DECOMPOSITION,
04162                  *      calculate new additional column (i.e. [r sqrt(rho2)]')
04163                  *      of the Cholesky factor R: */
04164                 real_t z2 = QQ(nnFRp1,nZ);
04165                 real_t rho2 = H->diag(nnFRp1)*z2*z2; /* rho2 = h2*z2*z2 */
04166 
04167                 if ( nFR > 0 )
04168                 {
04169                         /* Attention: Index list of free variables has already grown by one! */
04170                         real_t* Hz = new real_t[nFR+1];
04171                         real_t* z = new real_t[nFR+1];
04172                         /* 1) Calculate R'*r = Zfr'*Hfr*z1 + z2*Zfr'*h1 =: Zfr'*Hz + z2*Zfr'*h1 =: rhs and
04173                          *    rho2 = z1'*Hfr*z1 + 2*z2*h1'*z1 + h2*z2^2 - r'*r =: z1'*Hz + 2*z2*h1'*z1 + h2*z2^2 - r'r */
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                                 /* 2) Calculate rhs. */
04189                                 for( j=0; j<nFR; ++j )
04190                                 {
04191                                         jj = FR_idx[j];
04192                                         for( i=0; i<nZ; ++i )
04193                                                                                 /* Zfr' * ( Hz + z2*h1 ) */
04194                                                 rhs[i] += QQ(jj,i) * ( Hz[j] + z2 * z[j] );
04195                                 }
04196 
04197                                 /* 3) Calculate r = R^-T * rhs. */
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                                 /* 4) Calculate rho2 = rho^2 = z'*Hz - r'*r
04207                                  *    and store r into R. */
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                                                         /* z1' * ( Hz + 2*z2*h1 ) */
04221                                 rho2 += QQ(jj,nZ) * ( Hz[j] + 2.0*z2*z[j] );
04222                         }
04223 
04224                         delete[] z;
04225                         delete[] Hz;
04226                 }
04227 
04228                 /* 5) Store rho into R. */
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                         /* add bound or constraint */
04268 
04269                         /* hessianType = HST_SEMIDEF; */
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; /* exchange indices */
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]; /* minus e_k (bound k is removed) */
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]; /* minus e_k (constraint k is removed) */
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         /* compute the weight in inf-norm */
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         /* look at the "zero" in a relative inf-norm */
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         /* relative test against zero in inf-norm */
04406         if (normXi < options.epsNZCTests * normS)
04407         {
04408                 /* determine jump in x via ratio tests */
04409                 real_t sigmaLBnd, sigmaLCnstr, sigmaUBnd, sigmaUCnstr, sigma;
04410 
04411                 /* bounds */
04412 
04413                 /* compress x-u */
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                 /* performRatioTest( nFR,FR_idx,&bounds, x_W,delta_xFR, options.epsDen,options.epsDen, sigmaUBnd,addUBndIdx ); */
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                         /* also consider bound which is to be removed */
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                 /* compress x-l */
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                 /* performRatioTest( nFR,FR_idx,&bounds, x_W,delta_xFR, options.epsDen,options.epsDen, sigmaLBnd,addLBndIdx ); */
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                         /* also consider bound which is to be removed */
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                 /* constraints */
04457 
04458                 /* compute As (compressed to inactive constraints) */
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                 /* compress Ax_u */
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                 /* performRatioTest( nIAC,IAC_idx,&constraints, Ax_W,As, options.epsDen,options.epsDen, sigmaUCnstr,addUCnstrIdx ); */
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                         /* also consider constraint which is to be removed */
04477                         real_t eins = 1.0;
04478                         performPlainRatioTest(1, &remIdx, &Ax_u[remIdx], &eins, options.epsNum, options.epsDen, sigmaUCnstr, addUCnstrIdx);
04479                 }
04480 
04481                 /* compress Ax_l */
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                 /* performRatioTest( nIAC,IAC_idx,&constraints, Ax_W,As, options.epsDen,options.epsDen, sigmaLCnstr,addLCnstrIdx ); */
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                         /* also consider constraint which is to be removed */
04496                         real_t eins = 1.0;
04497                         performPlainRatioTest(1, &remIdx, &Ax_l[remIdx], &eins, options.epsNum, options.epsDen, sigmaLCnstr, addLCnstrIdx);
04498                 }
04499 
04500                 /* perform primal jump */
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                         /* update Ax, Ax_u, and Ax_l */
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                         /* change working set later */
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  *      b a c k s o l v e T
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         /* nothing to do */
04556         if ( nT <= 0 )
04557                 return SUCCESSFUL_RETURN;
04558 
04559 
04560         /* Solve Ta = b, where T might be transposed. */
04561         if ( transposed == BT_FALSE )
04562         {
04563                 /* solve Ta = b */
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                 /* solve T^T*a = b */
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  *      d e t e r m i n e D a t a S h i f t
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         /* I) DETERMINE DATA SHIFT FOR BOUNDS */
04620         QProblemB::determineDataShift(  g_new,lb_new,ub_new,
04621                                                                         delta_g,delta_lb,delta_ub,
04622                                                                         Delta_bB_isZero );
04623 
04624 
04625         /* II) DETERMINE DATA SHIFT FOR CONSTRAINTS */
04626         /* 1) Calculate shift directions. */
04627         for( i=0; i<nC; ++i )
04628         {
04629                 /* if lower constraints' bounds are to be disabled or do not exist, shift them to -infinity */
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                 /* if upper constraints' bounds are to be disabled or do not exist, shift them to infinity */
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         /* 2) Determine if active constraints' bounds are to be shifted. */
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  *      d e t e r m i n e S t e p D i r e c t i o n
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         /* I) DETERMINE delta_xFX (this is exact, does not need refinement) */
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         /* tempA and tempB hold the residuals in gFR and bA (= lbA or ubA)
04710          * delta_xFR, delta_yAC hold the steps that get refined */
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         /* Iterative refinement loop for delta_xFRz, delta_xFRy, delta_yAC */
04737         for ( r=0; r<=options.numRefinementSteps; ++r )
04738         {
04739                 /* II) DETERMINE delta_xFR */
04740                 if ( nFR > 0 )
04741                 {
04742                         for( i=0; i<nFR; ++i )
04743                                 delta_xFR_TMP[i] = 0.0;
04744 
04745                         /* 1) Determine delta_xFRy. */
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                                         /* compute bA - A * delta_xFX. tempB already holds bA->
04756                                          * in refinements r>=1, delta_xFX is exactly zero */
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                         /* 2) Determine delta_xFRz. */
04774                         for( i=0; i<nZ; ++i )
04775                                 delta_xFRz[i] = 0.0;
04776 
04777                         if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_IDENTITY ) )
04778                         {
04779                                 /* compute Z*delta_gFR [/eps] (delta_gFR is stored in tempA) */
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                                 /* compute HMX*delta_xFX. DESTROY delta_gFR that was in tempA */
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                                 /* compute HFR*delta_xFRy */
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                                 /* compute ZFR_delta_xFRz = (Z'*HFR*Z) \ Z * (HFR*delta_xFR + HMX*delta_xFX + delta_gFR) */
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                         /* compute Z * ZFR_delta_xFRz */
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                 /* III) DETERMINE delta_yAC */
04838                 if ( nAC > 0 ) /* => ( nFR = nZ + nAC > 0 ) */
04839                 {
04840                         if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_IDENTITY ) )
04841                         {
04842                                 /* if zero:     delta_yAC = (T')^-1 * ( Yfr*delta_gFR + eps*delta_xFRy ),
04843                                  * if identity: delta_yAC = (T')^-1 * ( Yfr*delta_gFR +     delta_xFRy )
04844                                  *
04845                                  * DESTROY residual_bA that was stored in tempB
04846                                  * If we come here, residual_gFR in tempA is STILL VALID
04847                                  */
04848                                 if ( hessianType == HST_IDENTITY )
04849                                 {
04850                                         for( j=0; j<nAC; ++j )
04851                                                 tempB[j] = delta_xFRy[j];
04852                                 }
04853                                 else /* hessianType == HST_ZERO */
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                                 /* Compute HFR * delta_xFR + HMX*delta_xFX
04879                                  * Here, tempA holds (HFR*delta_xFRy + HMX*delta_xFX) */
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                 /* refine the solution found so far */
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                         /* compute residuals in tempA and tempB, and max-norm */
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                         /* early termination of residual norm small enough */
04958                         if ( rnrm < options.epsIterRef )
04959                                 break;
04960                 }
04961         } /* end of refinement loop for delta_xFRz, delta_xFRy, delta_yAC */
04962 
04963 
04964         /* IV) DETERMINE delta_yFX */
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  *      p e r f o r m S t e p
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         /* initialise maximum steplength array */
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         /* I) DETERMINE MAXIMUM DUAL STEPLENGTH: */
05052         /* 1) Ensure that active dual constraints' bounds remain valid
05053          *    (ignoring inequality constraints).  */
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         /* 2) Ensure that active dual bounds remain valid
05073          *    (ignoring implicitly fixed variables). */
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         /* II) DeETERMINE MAXIMUM PRIMAL STEPLENGTH */
05092         /* 1) Ensure that inactive constraints' bounds remain valid
05093          *    (ignoring unbounded constraints). */
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                                 /* calculate products A*x and A*deltaX */
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]; /* Andreas: Do not touch many constraints case yet */
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                                 /* inactive lower constraints' bounds */
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                                 /* inactive upper constraints' bounds */
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 /* NOT __MANY_CONSTRAINTS__ */
05184         /* calculate product A*x */
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 /* __MANY_CONSTRAINTS__ */
05257 
05258 
05259         /* 2) Ensure that inactive bounds remain valid
05260          *    (ignoring unbounded variables). */
05261         /* inactive lower bounds */
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         /* inactive upper bounds */
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         /* III) PERFORM STEP ALONG HOMOTOPY PATH */
05319         if ( tau > ZERO )
05320         {
05321                 /* 1) Perform step in primal und dual space... */
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                 /* 2) Shift QP data. */
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                 /* recompute Ax. */
05356                 /*A->times(1, 1.0, x, nV, 0.0, Ax, nC);
05357                 for( i=0; i<nC; ++i )
05358                 {
05359                         Ax_u[i] = ubA[i] - Ax[i];
05360                         Ax_l[i] = Ax[i] - lbA[i];
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                 /* print a stepsize warning if stepsize is zero */
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  *      c h a n g e A c t i v e S e t
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                 /* Optimal solution found as no working set change detected. */
05408                 case ST_UNDEFINED:
05409                         return RET_OPTIMAL_SOLUTION_FOUND;
05410 
05411                 /* Remove one variable from active set. */
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                 /* Add one variable to active set. */
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  * r e l a t i v e H o m o t o p y L e n g t h
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         /* lower constraint bounds */
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         /* upper constraint bounds */
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  * p e r f o r m R a m p i n g
05516  */
05517 returnValue QProblem::performRamping( )
05518 {
05519         int nV = getNV( ), nC = getNC( ), bstat, cstat, i;
05520         real_t t, rampVal;
05521 
05522         /* ramp inactive variable bounds and active dual bound variables */
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; /* reestablish exact feasibility */
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; /* reestablish exact complementarity */
05540         }
05541 
05542         /* ramp inactive constraints and active dual constraint variables */
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; /* reestablish exact feasibility */
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; /* reestablish exact complementarity */
05560         }
05561 
05562         /* reestablish exact stationarity */
05563         setupAuxiliaryQPgradient( );
05564 
05565         /* swap ramp direction */
05566         t = ramp0; ramp0 = ramp1; ramp1 = t;
05567 
05568         return SUCCESSFUL_RETURN;
05569 }
05570 
05571 
05572 /*
05573  * p e r f o r m D r i f t C o r r e c t i o n
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  *      s e t u p A u x i l i a r y Q P
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         /* consistency check */
05676         if ( ( guessedBounds == 0 ) || ( guessedConstraints == 0 ) )
05677                 return THROWERROR( RET_INVALID_ARGUMENTS );
05678 
05679         /* nothing to do */
05680         if ( ( guessedBounds == &bounds ) && ( guessedConstraints == &constraints ) )
05681                 return SUCCESSFUL_RETURN;
05682 
05683         status = QPS_PREPARINGAUXILIARYQP;
05684 
05685 
05686         /* I) SETUP WORKING SET ... */
05687         if ( shallRefactorise( guessedBounds,guessedConstraints ) == BT_TRUE )
05688         {
05689                 /* ... WITH REFACTORISATION: */
05690                 /* 1) Reset bounds/constraints ... */
05691                 bounds.init( nV );
05692                 constraints.init( nC );
05693 
05694                 /*    ... and set them up afresh. */
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                 /* 2) Setup TQ factorisation. */
05705                 if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
05706                         return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
05707 
05708                 /* 3) Setup guessed working sets afresh. */
05709                 if ( setupAuxiliaryWorkingSet( guessedBounds,guessedConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
05710                         THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
05711 
05712                 /* 4) Calculate Cholesky decomposition. */
05713                 if ( ( getNAC( ) + getNFX( ) ) == 0 )
05714                 {
05715                         /* Factorise full Hessian if no bounds/constraints are active. */
05716                         if ( setupCholeskyDecomposition( ) != SUCCESSFUL_RETURN )
05717                                 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
05718                 }
05719                 else
05720                 {
05721                         /* Factorise projected Hessian if there active bounds/constraints. */
05722                         if ( setupCholeskyDecompositionProjected( ) != SUCCESSFUL_RETURN )
05723                                 return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
05724                 }
05725         }
05726         else
05727         {
05728                 /* ... WITHOUT REFACTORISATION: */
05729                 if ( setupAuxiliaryWorkingSet( guessedBounds,guessedConstraints,BT_FALSE ) != SUCCESSFUL_RETURN )
05730                         THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
05731         }
05732 
05733 
05734         /* II) SETUP AUXILIARY QP DATA: */
05735         /* 1) Ensure that dual variable is zero for fixed bounds and active constraints. */
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         /* 2) Setup gradient and (constraints') bound vectors. */
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         /* (also sets Ax_l and Ax_u) */
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  *      s h a l l R e f a c t o r i s e
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         /* always refactorise if Hessian is not known to be positive definite */
05776         if ( getHessianType( ) == HST_SEMIDEF )
05777                 return BT_TRUE;
05778 
05779         /* 1) Determine number of bounds that have same status
05780          *    in guessed AND current bounds.*/
05781         int differenceNumberBounds = 0;
05782 
05783         for( i=0; i<nV; ++i )
05784                 if ( guessedBounds->getStatus( i ) != bounds.getStatus( i ) )
05785                         ++differenceNumberBounds;
05786 
05787         /* 2) Determine number of constraints that have same status
05788          *    in guessed AND current constraints.*/
05789         int differenceNumberConstraints = 0;
05790 
05791         for( i=0; i<nC; ++i )
05792                 if ( guessedConstraints->getStatus( i ) != constraints.getStatus( i ) )
05793                         ++differenceNumberConstraints;
05794 
05795         /* 3) Decide wheter to refactorise or not. */
05796         if ( 2*(differenceNumberBounds+differenceNumberConstraints) > guessedConstraints->getNAC( )+guessedBounds->getNFX( ) )
05797                 return BT_TRUE;
05798         else
05799                 return BT_FALSE;
05800 }
05801 
05802 
05803 /*
05804  *      s e t u p Q P d a t a
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         /* 1) Load Hessian matrix as well as lower and upper bounds vectors. */
05816         if ( QProblemB::setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
05817                 return THROWERROR( RET_INVALID_ARGUMENTS );
05818 
05819         /* 2) Load constraint matrix. */
05820         if ( ( nC > 0 ) && ( _A == 0 ) )
05821                 return THROWERROR( RET_INVALID_ARGUMENTS );
05822 
05823         if ( nC > 0 )
05824         {
05825                 setA( _A );
05826 
05827                 /* 3) Setup lower constraints' bounds vector. */
05828                 if ( _lbA != 0 )
05829                 {
05830                         setLBA( _lbA );
05831                 }
05832                 else
05833                 {
05834                         /* if no lower constraints' bounds are specified, set them to -infinity */
05835                         for( i=0; i<nC; ++i )
05836                                 lbA[i] = -INFTY;
05837                 }
05838 
05839                 /* 4) Setup upper constraints' bounds vector. */
05840                 if ( _ubA != 0 )
05841                 {
05842                         setUBA( _ubA );
05843                 }
05844                 else
05845                 {
05846                         /* if no upper constraints' bounds are specified, set them to infinity */
05847                         for( i=0; i<nC; ++i )
05848                                 ubA[i] = INFTY;
05849                 }
05850         }
05851 
05852         return SUCCESSFUL_RETURN;
05853 }
05854 
05855 
05856 /*
05857  *      s e t u p Q P d a t a
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         /* 1) Load Hessian matrix as well as lower and upper bounds vectors. */
05869         if ( QProblemB::setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
05870                 return THROWERROR( RET_INVALID_ARGUMENTS );
05871 
05872         /* 2) Load constraint matrix. */
05873         if ( ( nC > 0 ) && ( _A == 0 ) )
05874                 return THROWERROR( RET_INVALID_ARGUMENTS );
05875 
05876         if ( nC > 0 )
05877         {
05878                 setA( _A );
05879 
05880                 /* 3) Setup lower constraints' bounds vector. */
05881                 if ( _lbA != 0 )
05882                 {
05883                         setLBA( _lbA );
05884                 }
05885                 else
05886                 {
05887                         /* if no lower constraints' bounds are specified, set them to -infinity */
05888                         for( i=0; i<nC; ++i )
05889                                 lbA[i] = -INFTY;
05890                 }
05891 
05892                 /* 4) Setup upper constraints' bounds vector. */
05893                 if ( _ubA != 0 )
05894                 {
05895                         setUBA( _ubA );
05896                 }
05897                 else
05898                 {
05899                         /* if no upper constraints' bounds are specified, set them to infinity */
05900                         for( i=0; i<nC; ++i )
05901                                 ubA[i] = INFTY;
05902                 }
05903         }
05904 
05905         return SUCCESSFUL_RETURN;
05906 }
05907 
05908 
05909 /*
05910  *      s e t u p Q P d a t a F r o m F i l e
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         /* 1) Load Hessian matrix as well as lower and upper bounds vectors from files. */
05925         returnvalue = QProblemB::setupQPdataFromFile( H_file,g_file,lb_file,ub_file );
05926         if ( returnvalue != SUCCESSFUL_RETURN )
05927                 return THROWERROR( returnvalue );
05928 
05929         /* 2) Load constraint matrix from file. */
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                 /* 3) Load lower constraints' bounds vector from file. */
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                         /* if no lower constraints' bounds are specified, set them to -infinity */
05955                         for( i=0; i<nC; ++i )
05956                                 lbA[i] = -INFTY;
05957                 }
05958 
05959                 /* 4) Load upper constraints' bounds vector from file. */
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                         /* if no upper constraints' bounds are specified, set them to infinity */
05969                         for( i=0; i<nC; ++i )
05970                                 ubA[i] = INFTY;
05971                 }
05972         }
05973 
05974         return SUCCESSFUL_RETURN;
05975 }
05976 
05977 
05978 /*
05979  *      l o a d Q P v e c t o r s F r o m F i l e
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         /* 1) Load gradient vector as well as lower and upper bounds vectors from files. */
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                 /* 2) Load lower constraints' bounds vector from file. */
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                                 /* If filename is given, storage must be provided! */
06011                                 return THROWERROR( RET_INVALID_ARGUMENTS );
06012                         }
06013                 }
06014 
06015                 /* 3) Load upper constraints' bounds vector from file. */
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                                 /* If filename is given, storage must be provided! */
06027                                 return THROWERROR( RET_INVALID_ARGUMENTS );
06028                         }
06029                 }
06030         }
06031 
06032         return SUCCESSFUL_RETURN;
06033 }
06034 
06035 
06036 /*
06037  *      p r i n t I t e r a t i o n
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         /* consistency check */
06048         if ( iteration < 0 )
06049                 return THROWERROR( RET_INVALID_ARGUMENTS );
06050 
06051         /* nothing to do */
06052         if ( options.printLevel != PL_MEDIUM )
06053                 return SUCCESSFUL_RETURN;
06054 
06055 
06056         /* 1) Print header at first iteration. */
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         /* 2) Print iteration line. */
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  *      end of file
06103  */


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:38:43