QProblemB.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/QProblemB.hpp>
00037 
00038 BEGIN_NAMESPACE_QPOASES
00039 
00040 
00041 /*****************************************************************************
00042  *  P U B L I C                                                              *
00043  *****************************************************************************/
00044 
00045 
00046 /*
00047  *      Q P r o b l e m B
00048  */
00049 QProblemB::QProblemB( )
00050 {
00051         /* print copyright notice */
00052         if (options.printLevel > PL_NONE)
00053                 printCopyrightNotice( );
00054 
00055         /* reset global message handler */
00056         getGlobalMessageHandler( )->reset( );
00057 
00058         freeHessian = BT_FALSE;
00059         H = 0;
00060 
00061         g = 0;
00062         lb = 0;
00063         ub = 0;
00064 
00065         R = 0;
00066         haveCholesky = BT_FALSE;
00067 
00068         x = 0;
00069         y = 0;
00070 
00071         tau = 0.0;
00072 
00073         hessianType = HST_UNKNOWN;
00074         isRegularised = BT_FALSE;
00075 
00076         infeasible  = BT_FALSE;
00077         unbounded   = BT_FALSE;
00078 
00079         status = QPS_NOTINITIALISED;
00080 
00081         count = 0;
00082 
00083         ramp0 = options.initialRamping;
00084         ramp1 = options.finalRamping;
00085 
00086         delta_xFR_TMP = 0;
00087 
00088         setPrintLevel( options.printLevel );
00089 }
00090 
00091 
00092 /*
00093  *      Q P r o b l e m B
00094  */
00095 QProblemB::QProblemB( int _nV, HessianType _hessianType )
00096 {
00097         /* print copyright notice */
00098         if (options.printLevel > PL_NONE)
00099                 printCopyrightNotice( );
00100 
00101         /* consistency check */
00102         if ( _nV <= 0 )
00103         {
00104                 _nV = 1;
00105                 THROWERROR( RET_INVALID_ARGUMENTS );
00106         }
00107 
00108         /* reset global message handler */
00109         getGlobalMessageHandler( )->reset( );
00110 
00111         freeHessian = BT_FALSE;
00112         H = 0;
00113 
00114         g = new real_t[_nV];
00115         lb = new real_t[_nV];
00116         ub = new real_t[_nV];
00117 
00118         bounds.init( _nV );
00119 
00120         R = new real_t[_nV*_nV];
00121 
00122         x = new real_t[_nV];
00123         y = new real_t[_nV];
00124 
00125         tau = 0.0;
00126 
00127         hessianType = _hessianType;
00128         isRegularised = BT_FALSE;
00129 
00130         infeasible  = BT_FALSE;
00131         unbounded   = BT_FALSE;
00132 
00133         status = QPS_NOTINITIALISED;
00134 
00135         count = 0;
00136 
00137         ramp0 = options.initialRamping;
00138         ramp1 = options.finalRamping;
00139 
00140         delta_xFR_TMP = new real_t[_nV];
00141 
00142         setPrintLevel( options.printLevel );
00143 
00144         flipper.init( _nV );
00145 }
00146 
00147 
00148 /*
00149  *      Q P r o b l e m B
00150  */
00151 QProblemB::QProblemB( const QProblemB& rhs )
00152 {
00153         freeHessian = BT_FALSE;
00154         H = 0;
00155 
00156         copy( rhs );
00157 }
00158 
00159 
00160 /*
00161  *      ~ Q P r o b l e m B
00162  */
00163 QProblemB::~QProblemB( )
00164 {
00165         clear( );
00166 
00167         /* reset global message handler */
00168         getGlobalMessageHandler( )->reset( );
00169 }
00170 
00171 
00172 /*
00173  *      o p e r a t o r =
00174  */
00175 QProblemB& QProblemB::operator=( const QProblemB& rhs )
00176 {
00177         if ( this != &rhs )
00178         {
00179                 clear( );
00180                 copy( rhs );
00181         }
00182 
00183         return *this;
00184 }
00185 
00186 
00187 /*
00188  *      r e s e t
00189  */
00190 returnValue QProblemB::reset( )
00191 {
00192         int i;
00193         int nV = getNV( );
00194 
00195         if ( nV == 0 )
00196                 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00197 
00198         /* 1) Reset bounds. */
00199         bounds.init( nV );
00200 
00201         /* 2) Reset Cholesky decomposition. */
00202         for( i=0; i<nV*nV; ++i )
00203                 R[i] = 0.0;
00204         
00205         haveCholesky = BT_FALSE;
00206 
00207         /* 3) Reset steplength and status flags. */
00208         tau = 0.0;
00209 
00210         hessianType = HST_UNKNOWN;
00211         isRegularised = BT_FALSE;
00212 
00213         infeasible  = BT_FALSE;
00214         unbounded   = BT_FALSE;
00215 
00216         status = QPS_NOTINITIALISED;
00217 
00218         ramp0 = options.initialRamping;
00219         ramp1 = options.finalRamping;
00220 
00221         return SUCCESSFUL_RETURN;
00222 }
00223 
00224 
00225 /*
00226  *      i n i t
00227  */
00228 returnValue QProblemB::init(    SymmetricMatrix *_H, const real_t* const _g,
00229                                                                 const real_t* const _lb, const real_t* const _ub,
00230                                                                 int& nWSR, real_t* const cputime
00231                                                                 )
00232 {
00233         if ( getNV( ) == 0 )
00234                 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00235 
00236         /* 1) Consistency check. */
00237         if ( isInitialised( ) == BT_TRUE )
00238         {
00239                 THROWWARNING( RET_QP_ALREADY_INITIALISED );
00240                 reset( );
00241         }
00242 
00243         /* 2) Setup QP data. */
00244         if ( setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
00245                 return THROWERROR( RET_INVALID_ARGUMENTS );
00246 
00247         /* 3) Call to main initialisation routine (without any additional information). */
00248         return solveInitialQP( 0,0,0, nWSR,cputime );
00249 }
00250 
00251 
00252 /*
00253  *      i n i t
00254  */
00255 returnValue QProblemB::init(    const real_t* const _H, const real_t* const _g,
00256                                                                 const real_t* const _lb, const real_t* const _ub,
00257                                                                 int& nWSR, real_t* const cputime
00258                                                                 )
00259 {
00260         if ( getNV( ) == 0 )
00261                 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00262 
00263         /* 1) Consistency check. */
00264         if ( isInitialised( ) == BT_TRUE )
00265         {
00266                 THROWWARNING( RET_QP_ALREADY_INITIALISED );
00267                 reset( );
00268         }
00269 
00270         /* 2) Setup QP data. */
00271         if ( setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
00272                 return THROWERROR( RET_INVALID_ARGUMENTS );
00273 
00274         /* 3) Call to main initialisation routine (without any additional information). */
00275         return solveInitialQP( 0,0,0, nWSR,cputime );
00276 }
00277 
00278 
00279 /*
00280  *      i n i t
00281  */
00282 returnValue QProblemB::init(    const char* const H_file, const char* const g_file,
00283                                                                 const char* const lb_file, const char* const ub_file,
00284                                                                 int& nWSR, real_t* const cputime
00285                                                                 )
00286 {
00287         if ( getNV( ) == 0 )
00288                 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00289 
00290         /* 1) Consistency check. */
00291         if ( isInitialised( ) == BT_TRUE )
00292         {
00293                 THROWWARNING( RET_QP_ALREADY_INITIALISED );
00294                 reset( );
00295         }
00296 
00297         /* 2) Setup QP data from files. */
00298         if ( setupQPdataFromFile( H_file,g_file,lb_file,ub_file ) != SUCCESSFUL_RETURN )
00299                 return THROWERROR( RET_UNABLE_TO_READ_FILE );
00300 
00301         /* 3) Call to main initialisation routine (without any additional information). */
00302         return solveInitialQP( 0,0,0, nWSR,cputime );
00303 }
00304 
00305 
00306 /*
00307  *      i n i t
00308  */
00309 returnValue QProblemB::init(    SymmetricMatrix *_H, const real_t* const _g,
00310                                                                 const real_t* const _lb, const real_t* const _ub,
00311                                                                 int& nWSR, real_t* const cputime,
00312                                                                 const real_t* const xOpt, const real_t* const yOpt,
00313                                                                 const Bounds* const guessedBounds
00314                                                                 )
00315 {
00316         int i;
00317         int nV = getNV( );
00318 
00319         if ( nV == 0 )
00320                 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00321 
00322         /* 1) Consistency checks. */
00323         if ( isInitialised( ) == BT_TRUE )
00324         {
00325                 THROWWARNING( RET_QP_ALREADY_INITIALISED );
00326                 reset( );
00327         }
00328 
00329         if ( guessedBounds != 0 )
00330         {
00331                 for( i=0; i<nV; ++i )
00332                 {
00333                         if ( guessedBounds->getStatus( i ) == ST_UNDEFINED )
00334                                 return THROWERROR( RET_INVALID_ARGUMENTS );
00335                 }
00336         }
00337 
00338         /* exclude this possibility in order to avoid inconsistencies */
00339         if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( guessedBounds != 0 ) )
00340                 return THROWERROR( RET_INVALID_ARGUMENTS );
00341 
00342         /* 2) Setup QP data. */
00343         if ( setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
00344                 return THROWERROR( RET_INVALID_ARGUMENTS );
00345 
00346         /* 3) Call to main initialisation routine. */
00347         return solveInitialQP( xOpt,yOpt,guessedBounds, nWSR,cputime );
00348 }
00349 
00350 
00351 /*
00352  *      i n i t
00353  */
00354 returnValue QProblemB::init(    const real_t* const _H, const real_t* const _g,
00355                                                                 const real_t* const _lb, const real_t* const _ub,
00356                                                                 int& nWSR, real_t* const cputime,
00357                                                                 const real_t* const xOpt, const real_t* const yOpt,
00358                                                                 const Bounds* const guessedBounds
00359                                                                 )
00360 {
00361         int i;
00362         int nV = getNV( );
00363 
00364         if ( nV == 0 )
00365                 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00366 
00367         /* 1) Consistency checks. */
00368         if ( isInitialised( ) == BT_TRUE )
00369         {
00370                 THROWWARNING( RET_QP_ALREADY_INITIALISED );
00371                 reset( );
00372         }
00373 
00374         if ( guessedBounds != 0 )
00375         {
00376                 for( i=0; i<nV; ++i )
00377                 {
00378                         if ( guessedBounds->getStatus( i ) == ST_UNDEFINED )
00379                                 return THROWERROR( RET_INVALID_ARGUMENTS );
00380                 }
00381         }
00382 
00383         /* exclude this possibility in order to avoid inconsistencies */
00384         if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( guessedBounds != 0 ) )
00385                 return THROWERROR( RET_INVALID_ARGUMENTS );
00386 
00387         /* 2) Setup QP data. */
00388         if ( setupQPdata( _H,_g,_lb,_ub ) != SUCCESSFUL_RETURN )
00389                 return THROWERROR( RET_INVALID_ARGUMENTS );
00390 
00391         /* 3) Call to main initialisation routine. */
00392         return solveInitialQP( xOpt,yOpt,guessedBounds, nWSR,cputime );
00393 }
00394 
00395 
00396 /*
00397  *      i n i t
00398  */
00399 returnValue QProblemB::init(    const char* const H_file, const char* const g_file,
00400                                                                 const char* const lb_file, const char* const ub_file,
00401                                                                 int& nWSR, real_t* const cputime,
00402                                                                 const real_t* const xOpt, const real_t* const yOpt,
00403                                                                 const Bounds* const guessedBounds
00404                                                                 )
00405 {
00406         int i;
00407         int nV = getNV( );
00408 
00409         if ( nV == 0 )
00410                 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00411 
00412         /* 1) Consistency checks. */
00413         if ( isInitialised( ) == BT_TRUE )
00414         {
00415                 THROWWARNING( RET_QP_ALREADY_INITIALISED );
00416                 reset( );
00417         }
00418 
00419         for( i=0; i<nV; ++i )
00420         {
00421                 if ( guessedBounds->getStatus( i ) == ST_UNDEFINED )
00422                         return THROWERROR( RET_INVALID_ARGUMENTS );
00423         }
00424 
00425         /* exclude this possibility in order to avoid inconsistencies */
00426         if ( ( xOpt == 0 ) && ( yOpt != 0 ) && ( guessedBounds != 0 ) )
00427                 return THROWERROR( RET_INVALID_ARGUMENTS );
00428 
00429         /* 2) Setup QP data from files. */
00430         if ( setupQPdataFromFile( H_file,g_file,lb_file,ub_file ) != SUCCESSFUL_RETURN )
00431                 return THROWERROR( RET_UNABLE_TO_READ_FILE );
00432 
00433         /* 3) Call to main initialisation routine. */
00434         return solveInitialQP( xOpt,yOpt,guessedBounds, nWSR,cputime );
00435 }
00436 
00437 
00438 /*
00439  *      h o t s t a r t
00440  */
00441 returnValue QProblemB::hotstart(        const real_t* const g_new,
00442                                                                         const real_t* const lb_new, const real_t* const ub_new,
00443                                                                         int& nWSR, real_t* const cputime
00444                                                                         )
00445 {
00446         if ( getNV( ) == 0 )
00447                 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00448 
00449         ++count;
00450 
00451         returnValue returnvalue = SUCCESSFUL_RETURN;
00452         int nFar, i;
00453         int nV = getNV ();
00454 
00455         int nWSR_max = nWSR;
00456         int nWSR_performed = 0;
00457 
00458         real_t cputime_remaining = INFTY;
00459         real_t cputime_needed = 0.0;
00460 
00461         real_t farbound = options.initialFarBounds;
00462         real_t t, rampVal;
00463 
00464         if ( options.enableFarBounds == BT_FALSE )
00465         {
00466                 /*if (haveCholesky == BT_FALSE)
00467                 {
00468                         returnvalue = computeInitialCholesky();
00469                         if (returnvalue != SUCCESSFUL_RETURN)
00470                                 return THROWERROR(returnvalue);
00471                 }*/
00472 
00473                 if ( usingRegularisation( ) == BT_TRUE )
00474                         returnvalue = solveRegularisedQP( g_new,lb_new,ub_new, nWSR,cputime,0 );
00475                 else
00476                         returnvalue = solveQP( g_new,lb_new,ub_new, nWSR,cputime,0 );
00477         }
00478         else
00479         {
00480                 real_t *ub_new_far = new real_t[nV];
00481                 real_t *lb_new_far = new real_t[nV];
00482 
00483                 /* possibly extend initial far bounds to largest bound/constraint data */
00484                 if (ub_new)
00485                         for (i = 0; i < nV; i++)
00486                                 if (ub_new[i] < INFTY && ub_new[i] > farbound) farbound = ub_new[i];
00487                 if (lb_new)
00488                         for (i = 0; i < nV; i++)
00489                                 if (lb_new[i] > -INFTY && lb_new[i] < -farbound) farbound = -lb_new[i];
00490 
00491                 if ( options.enableRamping == BT_TRUE )
00492                 {
00493                         /* TODO: encapsule this part to avoid code duplication! */
00494                         for ( i=0; i<nV; ++i )
00495                         {
00496                                 t = static_cast<real_t>(i) / static_cast<real_t>(nV-1);
00497                                 rampVal = farbound + (1.0-t) * ramp0 + t * ramp1;
00498 
00499                                 if ( ( lb_new == 0 ) || ( lb_new[i] <= -rampVal ) )
00500                                         lb_new_far[i] = - rampVal;
00501                                 else
00502                                         lb_new_far[i] = lb_new[i];
00503                                 if ( ( ub_new == 0 ) || ( ub_new[i] >= rampVal ) )
00504                                         ub_new_far[i] = rampVal;
00505                                 else
00506                                         ub_new_far[i] = ub_new[i];
00507                         }
00508                 }
00509                 else
00510                 {
00511                         for ( i=0; i<nV; ++i )
00512                         {
00513                                 lb_new_far[i] = lb_new[i];
00514                                 ub_new_far[i] = ub_new[i];
00515                         }
00516                 }
00517 
00518                 /*if (haveCholesky == BT_FALSE)
00519                 {
00520                         returnvalue = computeInitialCholesky();
00521                         if (returnvalue != SUCCESSFUL_RETURN)
00522                                 goto farewell;
00523                 }*/
00524 
00525                 for ( ;; )
00526                 {
00527                         nWSR = nWSR_max;
00528                         if ( cputime != 0 )
00529                                 cputime_remaining = *cputime - cputime_needed;
00530 
00531                         if ( usingRegularisation( ) == BT_TRUE )
00532                                 returnvalue = solveRegularisedQP( g_new,lb_new_far,ub_new_far, nWSR,&cputime_remaining,nWSR_performed );
00533                         else
00534                                 returnvalue = solveQP( g_new,lb_new_far,ub_new_far, nWSR,&cputime_remaining,nWSR_performed );
00535 
00536                         nWSR_performed  = nWSR;
00537                         cputime_needed += cputime_remaining;
00538 
00539                         /* Check for active far-bounds and move them away */
00540                         nFar = 0;
00541                         farbound *= options.growFarBounds;
00542 
00543                         real_t maxFarbound = 1e20;
00544                         if ( infeasible == BT_TRUE )
00545                         {
00546                                 if ( farbound > maxFarbound )
00547                                 {
00548                                         returnvalue = RET_HOTSTART_STOPPED_INFEASIBILITY;
00549                                         goto farewell;
00550                                 }
00551 
00552                                 if ( options.enableRamping == BT_TRUE )
00553                                 {
00554                                         /* TODO: encapsule this part to avoid code duplication! */
00555                                         for ( i=0; i<nV; ++i )
00556                                         {
00557                                                 t = static_cast<real_t>(i) / static_cast<real_t>(nV-1);
00558                                                 rampVal = farbound + (1.0-t) * ramp0 + t * ramp1;
00559 
00560                                                 if ( lb_new == 0 )
00561                                                         lb_new_far[i] = - rampVal;
00562                                                 else
00563                                                         lb_new_far[i] = getMax (- rampVal, lb_new[i]);
00564 
00565                                                 if ( ub_new == 0 )
00566                                                         ub_new_far[i] = rampVal;
00567                                                 else
00568                                                         ub_new_far[i] = getMin (rampVal, ub_new[i]);
00569                                         }
00570                                 }
00571                                 else
00572                                 {
00573                                         for ( i=0; i<nV; ++i )
00574                                         {
00575                                                 lb_new_far[i] = lb_new[i];
00576                                                 ub_new_far[i] = ub_new[i];
00577                                         }
00578                                 }
00579                         }
00580                         else if ( status == QPS_SOLVED )
00581                         {
00582                                 real_t tol = farbound * options.boundTolerance;
00583                                 status = QPS_HOMOTOPYQPSOLVED;
00584 
00585                                 nFar = 0;
00586                                 for ( i=0; i<nV; ++i )
00587                                 {
00588                                         if ( ( ( lb_new == 0 ) || ( lb_new_far[i] > lb_new[i] ) ) && ( fabs ( lb_new_far[i] - x[i] ) < tol ) )
00589                                                 ++nFar;
00590                                         if ( ( ( ub_new == 0 ) || ( ub_new_far[i] < ub_new[i] ) ) && ( fabs ( ub_new_far[i] - x[i] ) < tol ) )
00591                                                 ++nFar;
00592                                 }
00593 
00594                                 if ( nFar == 0 )
00595                                         break;
00596 
00597                                 if ( farbound > maxFarbound )
00598                                 {
00599                                         unbounded = BT_TRUE;
00600                                         returnvalue = RET_HOTSTART_STOPPED_UNBOUNDEDNESS;
00601                                         goto farewell;
00602                                 }
00603 
00604                                 if ( options.enableRamping == BT_TRUE )
00605                                 {
00606                                         /* TODO: encapsule this part to avoid code duplication! */
00607                                         for ( i=0; i<nV; ++i )
00608                                         {
00609                                                 t = static_cast<real_t>(i) / static_cast<real_t>(nV-1);
00610                                                 rampVal = farbound + (1.0-t) * ramp0 + t * ramp1;
00611 
00612                                                 if ( lb_new == 0 )
00613                                                         lb_new_far[i] = - rampVal;
00614                                                 else
00615                                                         lb_new_far[i] = getMax (- rampVal, lb_new[i]);
00616 
00617                                                 if ( ub_new == 0 )
00618                                                         ub_new_far[i] = rampVal;
00619                                                 else
00620                                                         ub_new_far[i] = getMin (rampVal, ub_new[i]);
00621                                         }
00622                                 }
00623                                 else
00624                                 {
00625                                         for ( i=0; i<nV; ++i )
00626                                         {
00627                                                 lb_new_far[i] = lb_new[i];
00628                                                 ub_new_far[i] = ub_new[i];
00629                                         }
00630                                 }
00631                         }
00632                         else
00633                         {
00634                                 /* some other error */
00635                                 break;
00636                         }
00637 
00638                         /* swap ramp0 and ramp1 */
00639                         t = ramp0; ramp0 = ramp1; ramp1 = t;
00640                 }
00641 
00642                 farewell:
00643                         if ( cputime != 0 )
00644                                 *cputime = cputime_needed;
00645                         delete[] lb_new_far; delete[] ub_new_far;
00646         }
00647 
00648         return ( returnvalue != SUCCESSFUL_RETURN ) ? THROWERROR( returnvalue ) : returnvalue;
00649 }
00650 
00651 
00652 /*
00653  *      h o t s t a r t
00654  */
00655 returnValue QProblemB::hotstart(        const char* const g_file,
00656                                                                         const char* const lb_file, const char* const ub_file,
00657                                                                         int& nWSR, real_t* const cputime
00658                                                                         )
00659 {
00660         int nV  = getNV( );
00661 
00662         if ( nV == 0 )
00663                 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00664 
00665         /* consistency check */
00666         if ( g_file == 0 )
00667                 return THROWERROR( RET_INVALID_ARGUMENTS );
00668 
00669 
00670         /* 1) Allocate memory (if bounds exist). */
00671         real_t* g_new  = new real_t[nV];
00672         real_t* lb_new = 0;
00673         real_t* ub_new = 0;
00674 
00675         if ( lb_file != 0 )
00676                 lb_new = new real_t[nV];
00677         if ( ub_file != 0 )
00678                 ub_new = new real_t[nV];
00679 
00680         /* 2) Load new QP vectors from file. */
00681         returnValue returnvalue;
00682         returnvalue = loadQPvectorsFromFile(    g_file,lb_file,ub_file,
00683                                                                                         g_new,lb_new,ub_new
00684                                                                                         );
00685         if ( returnvalue != SUCCESSFUL_RETURN )
00686         {
00687                 if ( ub_file != 0 )
00688                         delete[] ub_new;
00689                 if ( lb_file != 0 )
00690                         delete[] lb_new;
00691                 delete[] g_new;
00692 
00693                 return THROWERROR( RET_UNABLE_TO_READ_FILE );
00694         }
00695 
00696         /* 3) Actually perform hotstart. */
00697         returnvalue = hotstart( g_new,lb_new,ub_new, nWSR,cputime );
00698 
00699         /* 4) Free memory. */
00700         if ( ub_file != 0 )
00701                 delete[] ub_new;
00702         if ( lb_file != 0 )
00703                 delete[] lb_new;
00704         delete[] g_new;
00705 
00706         return returnvalue;
00707 }
00708 
00709 
00710 /*
00711  *      h o t s t a r t
00712  */
00713 returnValue QProblemB::hotstart(        const real_t* const g_new,
00714                                                                         const real_t* const lb_new, const real_t* const ub_new,
00715                                                                         int& nWSR, real_t* const cputime,
00716                                                                         const Bounds* const guessedBounds
00717                                                                         )
00718 {
00719         int nV = getNV( );
00720 
00721         if ( nV == 0 )
00722                 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00723 
00724 
00725         /* start runtime measurement */
00726         real_t starttime = 0.0;
00727         if ( cputime != 0 )
00728                 starttime = getCPUtime( );
00729 
00730 
00731         /* 1) Update working set according to guess for working set of bounds. */
00732         if ( guessedBounds != 0 )
00733         {
00734                 if ( setupAuxiliaryQP( guessedBounds ) != SUCCESSFUL_RETURN )
00735                         return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00736         }
00737         else
00738         {
00739                 /* create empty bounds for setting up auxiliary QP */
00740                 Bounds emptyBounds( nV );
00741                 if ( emptyBounds.setupAllFree( ) != SUCCESSFUL_RETURN )
00742                         return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00743 
00744                 if ( setupAuxiliaryQP( &emptyBounds ) != SUCCESSFUL_RETURN )
00745                         return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
00746         }
00747 
00748         /* 2) Perform usual homotopy. */
00749 
00750         /* Allow only remaining CPU time for usual hotstart. */
00751         if ( cputime != 0 )
00752                 *cputime -= getCPUtime( ) - starttime;
00753 
00754         returnValue returnvalue = hotstart( g_new,lb_new,ub_new, nWSR,cputime );
00755 
00756         /* stop runtime measurement */
00757         if ( cputime != 0 )
00758                 *cputime = getCPUtime( ) - starttime;
00759 
00760         return returnvalue;
00761 }
00762 
00763 
00764 /*
00765  *      h o t s t a r t
00766  */
00767 returnValue QProblemB::hotstart(        const char* const g_file,
00768                                                                         const char* const lb_file, const char* const ub_file,
00769                                                                         int& nWSR, real_t* const cputime,
00770                                                                         const Bounds* const guessedBounds
00771                                                                         )
00772 {
00773         int nV = getNV( );
00774 
00775         if ( nV == 0 )
00776                 return THROWERROR( RET_QPOBJECT_NOT_SETUP );
00777 
00778         /* consistency check */
00779         if ( g_file == 0 )
00780                 return THROWERROR( RET_INVALID_ARGUMENTS );
00781 
00782 
00783         /* 1) Allocate memory (if bounds exist). */
00784         real_t* g_new  = new real_t[nV];
00785         real_t* lb_new = 0;
00786         real_t* ub_new = 0;
00787 
00788         if ( lb_file != 0 )
00789                 lb_new = new real_t[nV];
00790         if ( ub_file != 0 )
00791                 ub_new = new real_t[nV];
00792 
00793         /* 2) Load new QP vectors from file. */
00794         returnValue returnvalue;
00795         returnvalue = loadQPvectorsFromFile(    g_file,lb_file,ub_file,
00796                                                                                         g_new,lb_new,ub_new
00797                                                                                         );
00798         if ( returnvalue != SUCCESSFUL_RETURN )
00799         {
00800                 if ( ub_file != 0 )
00801                         delete[] ub_new;
00802                 if ( lb_file != 0 )
00803                         delete[] lb_new;
00804                 delete[] g_new;
00805 
00806                 return THROWERROR( RET_UNABLE_TO_READ_FILE );
00807         }
00808 
00809         /* 3) Actually perform hotstart using initialised homotopy. */
00810         returnvalue = hotstart( g_new,lb_new,ub_new, nWSR,cputime,
00811                                                         guessedBounds
00812                                                         );
00813 
00814         /* 4) Free memory. */
00815         if ( ub_file != 0 )
00816                 delete[] ub_new;
00817         if ( lb_file != 0 )
00818                 delete[] lb_new;
00819         delete[] g_new;
00820 
00821         return returnvalue;
00822 }
00823 
00824 
00825 /*
00826  *      g e t N Z
00827  */
00828 int QProblemB::getNZ( ) const
00829 {
00830         /* if no constraints are present: nZ=nFR */
00831         return getNFR( );
00832 }
00833 
00834 
00835 /*
00836  *      g e t O b j V a l
00837  */
00838 real_t QProblemB::getObjVal( ) const
00839 {
00840         real_t objVal;
00841 
00842         /* calculated optimal objective function value
00843          * only if current QP has been solved */
00844         if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
00845                  ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
00846                  ( getStatus( ) == QPS_SOLVED ) )
00847         {
00848                 objVal = getObjVal( x );
00849         }
00850         else
00851         {
00852                 objVal = INFTY;
00853         }
00854 
00855         return objVal;
00856 }
00857 
00858 
00859 /*
00860  *      g e t O b j V a l
00861  */
00862 real_t QProblemB::getObjVal( const real_t* const _x ) const
00863 {
00864         int i;
00865         int nV = getNV( );
00866 
00867         if ( nV == 0 )
00868                 return 0.0;
00869 
00870         real_t objVal = 0.0;
00871 
00872         for( i=0; i<nV; ++i )
00873                 objVal += _x[i]*g[i];
00874 
00875         switch ( hessianType )
00876         {
00877                 case HST_ZERO:
00878                         break;
00879 
00880                 case HST_IDENTITY:
00881                         for( i=0; i<nV; ++i )
00882                                 objVal += 0.5*_x[i]*_x[i];
00883                         break;
00884 
00885                 default:
00886                         real_t *Hx = new real_t[nV];
00887                         H->times(1, 1.0, _x, nV, 0.0, Hx, nV);
00888                         for( i=0; i<nV; ++i )
00889                                 objVal += 0.5*_x[i]*Hx[i];
00890                         delete[] Hx;
00891                         break;
00892         }
00893 
00894         /* When using regularisation, the objective function value
00895          * needs to be modified as follows:
00896          * objVal = objVal - 0.5*_x*(Hmod-H)*_x - _x'*(gMod-g)
00897          *        = objVal - 0.5*_x*eps*_x * - _x'*(-eps*_x)
00898          *        = objVal + 0.5*_x*eps*_x */
00899         if ( usingRegularisation( ) == BT_TRUE )
00900         {
00901                 for( i=0; i<nV; ++i )
00902                         objVal += 0.5*_x[i]*options.epsRegularisation*_x[i];
00903         }
00904 
00905         return objVal;
00906 }
00907 
00908 
00909 /*
00910  *      g e t P r i m a l S o l u t i o n
00911  */
00912 returnValue QProblemB::getPrimalSolution( real_t* const xOpt ) const
00913 {
00914         int i;
00915 
00916         /* return optimal primal solution vector
00917          * only if current QP has been solved */
00918         if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
00919                  ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
00920                  ( getStatus( ) == QPS_SOLVED ) )
00921         {
00922                 for( i=0; i<getNV( ); ++i )
00923                         xOpt[i] = x[i];
00924 
00925                 return SUCCESSFUL_RETURN;
00926         }
00927         else
00928         {
00929                 return RET_QP_NOT_SOLVED;
00930         }
00931 }
00932 
00933 
00934 /*
00935  *      g e t D u a l S o l u t i o n
00936  */
00937 returnValue QProblemB::getDualSolution( real_t* const yOpt ) const
00938 {
00939         int i;
00940 
00941         /* return optimal dual solution vector
00942          * only if current QP has been solved */
00943         if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
00944                  ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
00945                  ( getStatus( ) == QPS_SOLVED ) )
00946         {
00947                 for( i=0; i<getNV( ); ++i )
00948                         yOpt[i] = y[i];
00949 
00950                 return SUCCESSFUL_RETURN;
00951         }
00952         else
00953         {
00954                 return RET_QP_NOT_SOLVED;
00955         }
00956 }
00957 
00958 
00959 /*
00960  *      s e t P r i n t L e v e l
00961  */
00962 returnValue QProblemB::setPrintLevel( PrintLevel _printLevel )
00963 {
00964         #ifndef __MATLAB__
00965         if ( ( options.printLevel == PL_HIGH ) && ( options.printLevel != _printLevel ) )
00966                 THROWINFO( RET_PRINTLEVEL_CHANGED );
00967         #endif
00968 
00969         options.printLevel = _printLevel;
00970 
00971         /* update message handler preferences */
00972         switch ( options.printLevel )
00973         {
00974                 case PL_TABULAR:
00975                 case PL_NONE:
00976                         getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_HIDDEN );
00977                         getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_HIDDEN );
00978                         getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN );
00979                         break;
00980 
00981                 case PL_LOW:
00982                         #ifndef __XPCTARGET__
00983                         getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE );
00984                         getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_HIDDEN );
00985                         getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN );
00986                         #endif
00987                         break;
00988 
00989                 case PL_MEDIUM:
00990                         #ifndef __XPCTARGET__
00991                         getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE );
00992                         getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_VISIBLE );
00993                         getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN );
00994                         #endif
00995                         break;
00996 
00997                 default: /* PL_HIGH */
00998                         #ifndef __XPCTARGET__
00999                         getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE );
01000                         getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_VISIBLE );
01001                         getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_VISIBLE );
01002                         #endif
01003                         break;
01004         }
01005 
01006         return SUCCESSFUL_RETURN;
01007 }
01008 
01009 
01010 /*
01011  *      p r i n t P r o p e r t i e s
01012  */
01013 returnValue QProblemB::printProperties( )
01014 {
01015         #ifndef __XPCTARGET__
01016         #ifndef __DSPACE__
01017         /* Do not print properties if print level is set to none! */
01018         if ( options.printLevel == PL_NONE )
01019                 return SUCCESSFUL_RETURN;
01020 
01021         char myPrintfString[80];
01022 
01023         myPrintf( "\n#################   qpOASES  --  QP PROPERTIES   #################\n" );
01024         myPrintf( "\n" );
01025 
01026         /* 1) Variables properties. */
01027         snprintf( myPrintfString,80,  "Number of Variables: %4.1d\n",getNV( ) );
01028         myPrintf( myPrintfString );
01029 
01030         if ( bounds.hasNoLower( ) == BT_TRUE )
01031                         myPrintf( "Variables are not bounded from below.\n" );
01032                 else
01033                         myPrintf( "Variables are bounded from below.\n" );
01034 
01035         if ( bounds.hasNoUpper( ) == BT_TRUE )
01036                         myPrintf( "Variables are not bounded from above.\n" );
01037                 else
01038                         myPrintf( "Variables are bounded from above.\n" );
01039 
01040         myPrintf( "\n" );
01041 
01042 
01043         /* 2) Further properties. */
01044         switch ( hessianType )
01045         {
01046                 case HST_ZERO:
01047                         myPrintf( "Hessian is zero matrix (i.e. actually an LP is solved).\n" );
01048                         break;
01049 
01050                 case HST_IDENTITY:
01051                         myPrintf( "Hessian is identity matrix.\n" );
01052                         break;
01053 
01054                 case HST_POSDEF:
01055                         myPrintf( "Hessian matrix is (strictly) positive definite.\n" );
01056                         break;
01057 
01058                 case HST_POSDEF_NULLSPACE:
01059                         myPrintf( "Hessian matrix is positive definite on null space of active constraints.\n" );
01060                         break;
01061 
01062                 case HST_SEMIDEF:
01063                         myPrintf( "Hessian matrix is positive semi-definite.\n" );
01064                         break;
01065 
01066                 default:
01067                         myPrintf( "Hessian matrix has unknown type.\n" );
01068                         break;
01069         }
01070 
01071         if ( infeasible == BT_TRUE )
01072                 myPrintf( "QP was found to be infeasible.\n" );
01073         else
01074                 myPrintf( "QP seems to be feasible.\n" );
01075 
01076         if ( unbounded == BT_TRUE )
01077                 myPrintf( "QP was found to be unbounded from below.\n" );
01078         else
01079                 myPrintf( "QP seems to be bounded from below.\n" );
01080 
01081         myPrintf( "\n" );
01082 
01083 
01084         /* 3) QP object properties. */
01085         switch ( status )
01086         {
01087                 case QPS_NOTINITIALISED:
01088                         myPrintf( "Status of QP object: freshly instantiated or reset.\n" );
01089                         break;
01090 
01091                 case QPS_PREPARINGAUXILIARYQP:
01092                         myPrintf( "Status of QP object: an auxiliary QP is currently setup.\n" );
01093                         break;
01094 
01095                 case QPS_AUXILIARYQPSOLVED:
01096                         myPrintf( "Status of QP object: an auxilary QP was solved.\n" );
01097                         break;
01098 
01099                 case QPS_PERFORMINGHOMOTOPY:
01100                         myPrintf( "Status of QP object: a homotopy step is performed.\n" );
01101                         break;
01102 
01103                 case QPS_HOMOTOPYQPSOLVED:
01104                         myPrintf( "Status of QP object: an intermediate QP along the homotopy path was solved.\n" );
01105                         break;
01106 
01107                 case QPS_SOLVED:
01108                         myPrintf( "Status of QP object: solution of the actual QP was found.\n" );
01109                         break;
01110         }
01111 
01112         switch ( options.printLevel )
01113         {
01114                 case PL_LOW:
01115                                         myPrintf( "Print level of QP object is low, i.e. only error are printed.\n" );
01116                         break;
01117 
01118                 case PL_MEDIUM:
01119                         myPrintf( "Print level of QP object is medium, i.e. error and warnings are printed.\n" );
01120                         break;
01121 
01122                 case PL_HIGH:
01123                         myPrintf( "Print level of QP object is high, i.e. all available output is printed.\n" );
01124                         break;
01125 
01126                 default:
01127                         break;
01128         }
01129 
01130         myPrintf( "\n" );
01131         #endif
01132         #endif
01133 
01134         return SUCCESSFUL_RETURN;
01135 }
01136 
01137 
01138 returnValue QProblemB::printOptions( ) const
01139 {
01140         return options.print( );
01141 }
01142 
01143 
01144 
01145 /*****************************************************************************
01146  *  P R O T E C T E D                                                        *
01147  *****************************************************************************/
01148 
01149 /*
01150  *      c l e a r
01151  */
01152 returnValue QProblemB::clear( )
01153 {
01154         if ( ( freeHessian == BT_TRUE ) && ( H != 0 ) )
01155         {
01156                 delete H;
01157                 H = 0;
01158         }
01159 
01160         if ( g != 0 )
01161         {
01162                 delete[] g;
01163                 g = 0;
01164         }
01165 
01166         if ( lb != 0 )
01167         {
01168                 delete[] lb;
01169                 lb = 0;
01170         }
01171 
01172         if ( ub != 0 )
01173         {
01174                 delete[] ub;
01175                 ub = 0;
01176         }
01177 
01178         if ( R != 0 )
01179         {
01180                 delete[] R;
01181                 R = 0;
01182         }
01183 
01184         if ( x != 0 )
01185         {
01186                 delete[] x;
01187                 x = 0;
01188         }
01189 
01190         if ( y != 0 )
01191         {
01192                 delete[] y;
01193                 y = 0;
01194         }
01195 
01196         if ( delta_xFR_TMP != 0 )
01197         {
01198                 delete[] delta_xFR_TMP;
01199                 delta_xFR_TMP = 0;
01200         }
01201 
01202         return SUCCESSFUL_RETURN;
01203 }
01204 
01205 
01206 /*
01207  *      c o p y
01208  */
01209 returnValue QProblemB::copy(    const QProblemB& rhs
01210                                                                 )
01211 {
01212         int _nV = rhs.getNV( );
01213 
01214         bounds = rhs.bounds;
01215 
01216         freeHessian = rhs.freeHessian;
01217         
01218         if ( freeHessian == BT_TRUE )
01219                 H = dynamic_cast<SymmetricMatrix *>(rhs.H->duplicate());
01220         else
01221                 H = rhs.H;
01222 
01223         if ( rhs.g != 0 )
01224         {
01225                 g = new real_t[_nV];
01226                 setG( rhs.g );
01227         }
01228         else
01229                 g = 0;
01230 
01231         if ( rhs.lb != 0 )
01232         {
01233                 lb = new real_t[_nV];
01234                 setLB( rhs.lb );
01235         }
01236         else
01237                 lb = 0;
01238 
01239         if ( rhs.ub != 0 )
01240         {
01241                 ub = new real_t[_nV];
01242                 setUB( rhs.ub );
01243         }
01244         else
01245                 ub = 0;
01246 
01247         if ( rhs.R != 0 )
01248         {
01249                 R = new real_t[_nV*_nV];
01250                 memcpy( R,rhs.R,_nV*_nV*sizeof(real_t) );
01251         }
01252         else
01253                 R = 0;
01254         
01255         haveCholesky = rhs.haveCholesky;
01256 
01257         if ( rhs.x != 0 )
01258         {
01259                 x = new real_t[_nV];
01260                 memcpy( x,rhs.x,_nV*sizeof(real_t) );
01261         }
01262         else
01263                 x = 0;
01264 
01265         if ( rhs.y != 0 )
01266         {
01267                 y = new real_t[_nV];
01268                 memcpy( y,rhs.y,_nV*sizeof(real_t) );
01269         }
01270         else
01271                 y = 0;
01272 
01273         tau = rhs.tau;
01274 
01275         hessianType = rhs.hessianType;
01276         isRegularised = rhs.isRegularised;
01277 
01278         infeasible = rhs.infeasible;
01279         unbounded = rhs.unbounded;
01280 
01281         status = rhs.status;
01282 
01283         count = rhs.count;
01284 
01285         ramp0 = rhs.ramp0;
01286         ramp1 = rhs.ramp1;
01287 
01288         delta_xFR_TMP = new real_t[_nV];        /* nFR */
01289 
01290         options = rhs.options;
01291         setPrintLevel( options.printLevel );
01292 
01293         flipper = rhs.flipper;
01294 
01295         return SUCCESSFUL_RETURN;
01296 }
01297 
01298 
01299 /*
01300  *      d e t e r m i n e H e s s i a n T y p e
01301  */
01302 returnValue QProblemB::determineHessianType( )
01303 {
01304         int i;
01305         int nV = getNV( );
01306 
01307         /* if Hessian type has been set by user, do NOT change it! */
01308         if ( hessianType != HST_UNKNOWN )
01309                 return SUCCESSFUL_RETURN;
01310 
01311         /* if Hessian has not been allocated, assume it to be all zeros! */
01312         if ( H == 0 )
01313         {
01314                 hessianType = HST_ZERO;
01315                 return SUCCESSFUL_RETURN;
01316         }
01317 
01318 
01319         /* 1) If Hessian has outer-diagonal elements,
01320          *    Hessian is assumed to be positive definite. */
01321         hessianType = HST_POSDEF;
01322         if (H->isDiag() == BT_FALSE)
01323                 return SUCCESSFUL_RETURN;
01324 
01325         /* 2) Otherwise it is diagonal and test for identity or zero matrix is performed. */
01326         /* hessianType = HST_DIAGONAL; */
01327 
01328         BooleanType isIdentity = BT_TRUE;
01329         BooleanType isZero = BT_TRUE;
01330 
01331         for ( i=0; i<nV; ++i )
01332         {
01333                 if ( H->diag(i) < -ZERO ) /* TODO: Should work with flipping bounds enabled! No error message needed. */
01334                         return THROWERROR( RET_HESSIAN_INDEFINITE );
01335 
01336                 if ( fabs( H->diag(i) - 1.0 ) > EPS )
01337                         isIdentity = BT_FALSE;
01338 
01339                 if ( fabs( H->diag(i) ) > EPS )
01340                         isZero = BT_FALSE;
01341         }
01342 
01343         if ( isIdentity == BT_TRUE )
01344                 hessianType = HST_IDENTITY;
01345 
01346         if ( isZero == BT_TRUE )
01347                 hessianType = HST_ZERO;
01348 
01349         return SUCCESSFUL_RETURN;
01350 }
01351 
01352 
01353 /*
01354  *      s e t u p S u b j e c t T o T y p e
01355  */
01356 returnValue QProblemB::setupSubjectToType( )
01357 {
01358         return setupSubjectToType( lb,ub );
01359 }
01360 
01361 
01362 /*
01363  *      s e t u p S u b j e c t T o T y p e
01364  */
01365 returnValue QProblemB::setupSubjectToType( const real_t* const lb_new, const real_t* const ub_new )
01366 {
01367         int i;
01368         int nV = getNV( );
01369 
01370 
01371         /* 1) Check if lower bounds are present. */
01372         bounds.setNoLower( BT_TRUE );
01373         if ( lb_new != 0 )
01374         {
01375                 for( i=0; i<nV; ++i )
01376                 {
01377                         if ( lb_new[i] > -INFTY )
01378                         {
01379                                 bounds.setNoLower( BT_FALSE );
01380                                 break;
01381                         }
01382                 }
01383         }
01384 
01385         /* 2) Check if upper bounds are present. */
01386         bounds.setNoUpper( BT_TRUE );
01387         if ( ub_new != 0 )
01388         {
01389                 for( i=0; i<nV; ++i )
01390                 {
01391                         if ( ub_new[i] < INFTY )
01392                         {
01393                                 bounds.setNoUpper( BT_FALSE );
01394                                 break;
01395                         }
01396                 }
01397         }
01398 
01399         /* 3) Determine implicitly fixed and unbounded variables. */
01400         if ( ( lb_new != 0 ) && ( ub_new != 0 ) )
01401         {
01402                 for( i=0; i<nV; ++i )
01403                 {
01404                         if ( ( lb_new[i] < -INFTY + options.boundTolerance ) && ( ub_new[i] > INFTY - options.boundTolerance )
01405                                         && (options.enableFarBounds == BT_FALSE))
01406                         {
01407                                 bounds.setType( i,ST_UNBOUNDED );
01408                         }
01409                         else
01410                         {
01411                                 if ( options.enableEqualities && lb_new[i] > ub_new[i] - options.boundTolerance )
01412                                         bounds.setType( i,ST_EQUALITY );
01413                                 else
01414                                         bounds.setType( i,ST_BOUNDED );
01415                         }
01416                 }
01417         }
01418         else
01419         {
01420                 if ( ( lb_new == 0 ) && ( ub_new == 0 ) )
01421                 {
01422                         for( i=0; i<nV; ++i )
01423                                 bounds.setType( i,ST_UNBOUNDED );
01424                 }
01425                 else
01426                 {
01427                         for( i=0; i<nV; ++i )
01428                                 bounds.setType( i,ST_BOUNDED );
01429                 }
01430         }
01431 
01432         return SUCCESSFUL_RETURN;
01433 }
01434 
01435 
01436 /*
01437  *      s e t u p C h o l e s k y D e c o m p o s i t i o n
01438  */
01439 returnValue QProblemB::setupCholeskyDecomposition( )
01440 {
01441         int i, j;
01442         int nV  = getNV( );
01443         int nFR = getNFR( );
01444         
01445         /* 1) Initialises R with all zeros. */
01446         for( i=0; i<nV*nV; ++i )
01447                 R[i] = 0.0;
01448 
01449         /* 2) Calculate Cholesky decomposition of H (projected to free variables). */
01450         if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_IDENTITY ) )
01451         {
01452                 if ( hessianType == HST_ZERO )
01453                 {
01454                         /* if Hessian is zero matrix, it is assumed that it has been
01455                          * regularised and thus its Cholesky factor is the identity
01456                          * matrix scaled by sqrt(eps). */
01457                         if ( usingRegularisation( ) == BT_TRUE )
01458                         {
01459                                 for( i=0; i<nV; ++i )
01460                                         RR(i,i) = sqrt( options.epsRegularisation );
01461                         }
01462                         else
01463                                 return THROWERROR( RET_CHOLESKY_OF_ZERO_HESSIAN );
01464                 }
01465                 else
01466                 {
01467                         /* if Hessian is identity, so is its Cholesky factor. */
01468                         for( i=0; i<nV; ++i )
01469                                 RR(i,i) = 1.0;
01470                 }
01471         }
01472         else
01473         {
01474                 if ( nFR > 0 )
01475                 {
01476                         int* FR_idx;
01477                         bounds.getFree( )->getNumberArray( &FR_idx );
01478 
01479                         /* get H */
01480                         for ( j=0; j < nFR; ++j )
01481                                 H->getCol (FR_idx[j], bounds.getFree (), 1.0, &R[j*nV]);
01482 
01483                         /* R'*R = H */
01484                         long info = 0;
01485                         unsigned long _nFR = nFR, _nV = nV;
01486 
01487                         POTRF ("U", &_nFR, R, &_nV, &info);
01488 
01489                         /* <0 = invalid call, =0 ok, >0 not spd */
01490                         if (info > 0) {
01491                                 hessianType = HST_SEMIDEF;
01492                                 return RET_HESSIAN_NOT_SPD;
01493                         }
01494 
01495                         /* zero first subdiagonal to make givens updates work */
01496                         for (i=0;i<nFR-1;++i)
01497                                 RR(i+1,i) = 0.0;
01498 
01499                 }
01500         }
01501 
01502         return SUCCESSFUL_RETURN;
01503 }
01504 
01505 
01506 /*
01507  *      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
01508  */
01509 returnValue QProblemB::obtainAuxiliaryWorkingSet(       const real_t* const xOpt, const real_t* const yOpt,
01510                                                                                                         const Bounds* const guessedBounds, Bounds* auxiliaryBounds
01511                                                                                                         ) const
01512 {
01513         int i = 0;
01514         int nV = getNV( );
01515 
01516 
01517         /* 1) Ensure that desiredBounds is allocated (and different from guessedBounds). */
01518         if ( ( auxiliaryBounds == 0 ) || ( auxiliaryBounds == guessedBounds ) )
01519                 return THROWERROR( RET_INVALID_ARGUMENTS );
01520 
01521 
01522         /* 2) Setup working set for auxiliary initial QP. */
01523         if ( guessedBounds != 0 )
01524         {
01525                 /* If an initial working set is specific, use it!
01526                  * Moreover, add all implictly fixed variables if specified. */
01527                 for( i=0; i<nV; ++i )
01528                 {
01529                         #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
01530                         if ( bounds.getType( i ) == ST_EQUALITY )
01531                         {
01532                                 if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
01533                                         return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01534                         }
01535                         else
01536                         #endif
01537                         {
01538                                 if ( auxiliaryBounds->setupBound( i,guessedBounds->getStatus( i ) ) != SUCCESSFUL_RETURN )
01539                                         return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01540                         }
01541                 }
01542         }
01543         else    /* No initial working set specified. */
01544         {
01545                 if ( ( xOpt != 0 ) && ( yOpt == 0 ) )
01546                 {
01547                         /* Obtain initial working set by "clipping". */
01548                         for( i=0; i<nV; ++i )
01549                         {
01550                                 if ( xOpt[i] <= lb[i] + options.boundTolerance )
01551                                 {
01552                                         if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
01553                                                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01554                                         continue;
01555                                 }
01556 
01557                                 if ( xOpt[i] >= ub[i] - options.boundTolerance )
01558                                 {
01559                                         if ( auxiliaryBounds->setupBound( i,ST_UPPER ) != SUCCESSFUL_RETURN )
01560                                                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01561                                         continue;
01562                                 }
01563 
01564                                 /* Moreover, add all implictly fixed variables if specified. */
01565                                 #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
01566                                 if ( bounds.getType( i ) == ST_EQUALITY )
01567                                 {
01568                                         if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
01569                                                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01570                                 }
01571                                 else
01572                                 #endif
01573                                 {
01574                                         if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
01575                                                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01576                                 }
01577                         }
01578                 }
01579 
01580                 if ( ( xOpt == 0 ) && ( yOpt != 0 ) )
01581                 {
01582                         /* Obtain initial working set in accordance to sign of dual solution vector. */
01583                         for( i=0; i<nV; ++i )
01584                         {
01585                                 if ( yOpt[i] > EPS )
01586                                 {
01587                                         if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
01588                                                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01589                                         continue;
01590                                 }
01591 
01592                                 if ( yOpt[i] < -EPS )
01593                                 {
01594                                         if ( auxiliaryBounds->setupBound( i,ST_UPPER ) != SUCCESSFUL_RETURN )
01595                                                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01596                                         continue;
01597                                 }
01598 
01599                                 /* Moreover, add all implictly fixed variables if specified. */
01600                                 #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
01601                                 if ( bounds.getType( i ) == ST_EQUALITY )
01602                                 {
01603                                         if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
01604                                                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01605                                 }
01606                                 else
01607                                 #endif
01608                                 {
01609                                         if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
01610                                                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01611                                 }
01612                         }
01613                 }
01614 
01615                 /* If xOpt and yOpt are null pointer and no initial working is specified,
01616                  * start with empty working set (or implicitly fixed bounds only)
01617                  * for auxiliary QP. */
01618                 if ( ( xOpt == 0 ) && ( yOpt == 0 ) )
01619                 {
01620                         for( i=0; i<nV; ++i )
01621                         {
01622                                 switch( bounds.getType( i ) )
01623                                 {
01624                                         case ST_UNBOUNDED:
01625                                                 if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
01626                                                         return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01627                                                 break;
01628 
01629                                         /* Only add all implictly fixed variables if specified. */
01630                                         #ifdef __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
01631                                         case ST_EQUALITY:
01632                                                 if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
01633                                                         return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01634                                                 break;
01635                                         #endif
01636 
01637                                         default:
01638                                                 if ( auxiliaryBounds->setupBound( i,options.initialStatusBounds ) != SUCCESSFUL_RETURN )
01639                                                         return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01640                                                 break;
01641                                 }
01642                         }
01643                 }
01644         }
01645 
01646         return SUCCESSFUL_RETURN;
01647 }
01648 
01649 
01650 /*
01651  *      b a c k s o l v e R
01652  */
01653 returnValue QProblemB::backsolveR(      const real_t* const b, BooleanType transposed,
01654                                                                         real_t* const a
01655                                                                         ) const
01656 {
01657         /* Call standard backsolve procedure (i.e. removingBound == BT_FALSE). */
01658         return backsolveR( b,transposed,BT_FALSE,a );
01659 }
01660 
01661 
01662 /*
01663  *      b a c k s o l v e R
01664  */
01665 returnValue QProblemB::backsolveR(      const real_t* const b, BooleanType transposed,
01666                                                                         BooleanType removingBound,
01667                                                                         real_t* const a
01668                                                                         ) const
01669 {
01670         int i, j;
01671         int nV = getNV( );
01672         int nR = getNZ( );
01673 
01674         real_t sum;
01675 
01676         /* if backsolve is called while removing a bound, reduce nZ by one. */
01677         if ( removingBound == BT_TRUE )
01678                 --nR;
01679 
01680         /* nothing to do */
01681         if ( nR <= 0 )
01682                 return SUCCESSFUL_RETURN;
01683 
01684 
01685         /* Solve Ra = b, where R might be transposed. */
01686         if ( transposed == BT_FALSE )
01687         {
01688                 /* solve Ra = b */
01689                 for( i=(nR-1); i>=0; --i )
01690                 {
01691                         sum = b[i];
01692                         for( j=(i+1); j<nR; ++j )
01693                                 sum -= RR(i,j) * a[j];
01694 
01695                         if ( fabs( RR(i,i) ) >= ZERO*fabs( sum ) )
01696                                 a[i] = sum / RR(i,i);
01697                         else
01698                                 return THROWERROR( RET_DIV_BY_ZERO );
01699                 }
01700         }
01701         else
01702         {
01703                 /* solve R^T*a = b */
01704                 for( i=0; i<nR; ++i )
01705                 {
01706                         sum = b[i];
01707                         for( j=0; j<i; ++j )
01708                                 sum -= RR(j,i) * a[j];
01709 
01710                         if ( fabs( RR(i,i) ) >= ZERO*fabs( sum ) )
01711                                 a[i] = sum / RR(i,i);
01712                         else
01713                                 return THROWERROR( RET_DIV_BY_ZERO );
01714                 }
01715         }
01716 
01717         return SUCCESSFUL_RETURN;
01718 }
01719 
01720 
01721 /*
01722  *      d e t e r m i n e D a t a S h i f t
01723  */
01724 returnValue QProblemB::determineDataShift(      const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,
01725                                                                                         real_t* const delta_g, real_t* const delta_lb, real_t* const delta_ub,
01726                                                                                         BooleanType& Delta_bB_isZero
01727                                                                                         )
01728 {
01729         int i, ii;
01730         int nV  = getNV( );
01731         int nFX = getNFX( );
01732 
01733         int* FX_idx;
01734         bounds.getFixed( )->getNumberArray( &FX_idx );
01735 
01736 
01737         /* 1) Calculate shift directions. */
01738         for( i=0; i<nV; ++i )
01739                 delta_g[i]  = g_new[i]  - g[i];
01740 
01741         if ( lb_new != 0 )
01742         {
01743                 for( i=0; i<nV; ++i )
01744                         delta_lb[i] = lb_new[i] - lb[i];
01745         }
01746         else
01747         {
01748                 /* if no lower bounds exist, assume the new lower bounds to be -infinity */
01749                 for( i=0; i<nV; ++i )
01750                         delta_lb[i] = -INFTY - lb[i];
01751         }
01752 
01753         if ( ub_new != 0 )
01754         {
01755                 for( i=0; i<nV; ++i )
01756                         delta_ub[i] = ub_new[i] - ub[i];
01757         }
01758         else
01759         {
01760                 /* if no upper bounds exist, assume the new upper bounds to be infinity */
01761                 for( i=0; i<nV; ++i )
01762                         delta_ub[i] = INFTY - ub[i];
01763         }
01764 
01765         /* 2) Determine if active bounds are to be shifted. */
01766         Delta_bB_isZero = BT_TRUE;
01767 
01768         for ( i=0; i<nFX; ++i )
01769         {
01770                 ii = FX_idx[i];
01771 
01772                 if ( ( fabs( delta_lb[ii] ) > EPS ) || ( fabs( delta_ub[ii] ) > EPS ) )
01773                 {
01774                         Delta_bB_isZero = BT_FALSE;
01775                         break;
01776                 }
01777         }
01778 
01779         return SUCCESSFUL_RETURN;
01780 }
01781 
01782 
01783 
01784 /*
01785  *      s e t u p Q P d a t a
01786  */
01787 returnValue QProblemB::setupQPdata(     SymmetricMatrix *_H, const real_t* const _g,
01788                                                                         const real_t* const _lb, const real_t* const _ub
01789                                                                         )
01790 {
01791         int i;
01792         int nV = getNV( );
01793 
01794         /* 1) Setup Hessian matrix. */
01795         setH( _H );
01796 
01797         /* 2) Setup gradient vector. */
01798         if ( _g == 0 )
01799                 return THROWERROR( RET_INVALID_ARGUMENTS );
01800         else
01801                 setG( _g );
01802 
01803         /* 3) Setup lower bounds vector. */
01804         if ( _lb != 0 )
01805                 setLB( _lb );
01806         else
01807                 /* if no lower bounds are specified, set them to -infinity */
01808                 for( i=0; i<nV; ++i )
01809                         lb[i] = -INFTY;
01810 
01811         /* 4) Setup upper bounds vector. */
01812         if ( _ub != 0 )
01813                 setUB( _ub );
01814         else
01815                 /* if no upper bounds are specified, set them to infinity */
01816                 for( i=0; i<nV; ++i )
01817                         ub[i] = INFTY;
01818 
01819         return SUCCESSFUL_RETURN;
01820 }
01821 
01822 
01823 /*
01824  *      s e t u p Q P d a t a
01825  */
01826 returnValue QProblemB::setupQPdata(     const real_t* const _H, const real_t* const _g,
01827                                                                         const real_t* const _lb, const real_t* const _ub
01828                                                                         )
01829 {
01830         int i;
01831         int nV = getNV( );
01832 
01833         /* 1) Setup Hessian matrix. */
01834         setH( _H );
01835 
01836         /* 2) Setup gradient vector. */
01837         if ( _g == 0 )
01838                 return THROWERROR( RET_INVALID_ARGUMENTS );
01839         else
01840                 setG( _g );
01841 
01842         /* 3) Setup lower bounds vector. */
01843         if ( _lb != 0 )
01844         {
01845                 setLB( _lb );
01846         }
01847         else
01848         {
01849                 /* if no lower bounds are specified, set them to -infinity */
01850                 for( i=0; i<nV; ++i )
01851                         lb[i] = -INFTY;
01852         }
01853 
01854         /* 4) Setup upper bounds vector. */
01855         if ( _ub != 0 )
01856         {
01857                 setUB( _ub );
01858         }
01859         else
01860         {
01861                 /* if no upper bounds are specified, set them to infinity */
01862                 for( i=0; i<nV; ++i )
01863                         ub[i] = INFTY;
01864         }
01865 
01866         return SUCCESSFUL_RETURN;
01867 }
01868 
01869 
01870 /*
01871  *      s e t u p Q P d a t a F r o m F i l e
01872  */
01873 returnValue QProblemB::setupQPdataFromFile(     const char* const H_file, const char* const g_file,
01874                                                                                         const char* const lb_file, const char* const ub_file
01875                                                                                         )
01876 {
01877         int i;
01878         int nV = getNV( );
01879 
01880         returnValue returnvalue;
01881 
01882 
01883         /* 1) Load Hessian matrix from file. */
01884         if ( H_file != 0 )
01885         {
01886                 real_t* _H = new real_t[nV * nV];
01887                 returnvalue = readFromFile( _H, nV,nV, H_file );
01888                 if ( returnvalue != SUCCESSFUL_RETURN )
01889                 {
01890                         delete[] _H;
01891                         return THROWERROR( returnvalue );
01892                 }
01893                 setH( _H );
01894                 H->doFreeMemory( );
01895         }
01896         else
01897         {
01898                 real_t* _H = 0;
01899                 setH( _H );
01900         }
01901 
01902         /* 2) Load gradient vector from file. */
01903         if ( g_file == 0 )
01904                 return THROWERROR( RET_INVALID_ARGUMENTS );
01905 
01906         returnvalue = readFromFile( g, nV, g_file );
01907         if ( returnvalue != SUCCESSFUL_RETURN )
01908                 return THROWERROR( returnvalue );
01909 
01910         /* 3) Load lower bounds vector from file. */
01911         if ( lb_file != 0 )
01912         {
01913                 returnvalue = readFromFile( lb, nV, lb_file );
01914                 if ( returnvalue != SUCCESSFUL_RETURN )
01915                         return THROWERROR( returnvalue );
01916         }
01917         else
01918         {
01919                 /* if no lower bounds are specified, set them to -infinity */
01920                 for( i=0; i<nV; ++i )
01921                         lb[i] = -INFTY;
01922         }
01923 
01924         /* 4) Load upper bounds vector from file. */
01925         if ( ub_file != 0 )
01926         {
01927                 returnvalue = readFromFile( ub, nV, ub_file );
01928                 if ( returnvalue != SUCCESSFUL_RETURN )
01929                         return THROWERROR( returnvalue );
01930         }
01931         else
01932         {
01933                 /* if no upper bounds are specified, set them to infinity */
01934                 for( i=0; i<nV; ++i )
01935                         ub[i] = INFTY;
01936         }
01937 
01938         return SUCCESSFUL_RETURN;
01939 }
01940 
01941 
01942 /*
01943  *      l o a d Q P v e c t o r s F r o m F i l e
01944  */
01945 returnValue QProblemB::loadQPvectorsFromFile(   const char* const g_file, const char* const lb_file, const char* const ub_file,
01946                                                                                                 real_t* const g_new, real_t* const lb_new, real_t* const ub_new
01947                                                                                                 ) const
01948 {
01949         int nV = getNV( );
01950 
01951         returnValue returnvalue;
01952 
01953 
01954         /* 1) Load gradient vector from file. */
01955         if ( ( g_file != 0 ) && ( g_new != 0 ) )
01956         {
01957                 returnvalue = readFromFile( g_new, nV, g_file );
01958                 if ( returnvalue != SUCCESSFUL_RETURN )
01959                         return THROWERROR( returnvalue );
01960         }
01961         else
01962         {
01963                 /* At least gradient vector needs to be specified! */
01964                 return THROWERROR( RET_INVALID_ARGUMENTS );
01965         }
01966 
01967         /* 2) Load lower bounds vector from file. */
01968         if ( lb_file != 0 )
01969         {
01970                 if ( lb_new != 0 )
01971                 {
01972                         returnvalue = readFromFile( lb_new, nV, lb_file );
01973                         if ( returnvalue != SUCCESSFUL_RETURN )
01974                                 return THROWERROR( returnvalue );
01975                 }
01976                 else
01977                 {
01978                         /* If filename is given, storage must be provided! */
01979                         return THROWERROR( RET_INVALID_ARGUMENTS );
01980                 }
01981         }
01982 
01983         /* 3) Load upper bounds vector from file. */
01984         if ( ub_file != 0 )
01985         {
01986                 if ( ub_new != 0 )
01987                 {
01988                         returnvalue = readFromFile( ub_new, nV, ub_file );
01989                         if ( returnvalue != SUCCESSFUL_RETURN )
01990                                 return THROWERROR( returnvalue );
01991                 }
01992                 else
01993                 {
01994                         /* If filename is given, storage must be provided! */
01995                         return THROWERROR( RET_INVALID_ARGUMENTS );
01996                 }
01997         }
01998 
01999         return SUCCESSFUL_RETURN;
02000 }
02001 
02002 
02003 /*
02004  *      s e t I n f e a s i b i l i t y F l a g
02005  */
02006 returnValue QProblemB::setInfeasibilityFlag(    returnValue returnvalue
02007                                                                                                 )
02008 {
02009         infeasible = BT_TRUE;
02010 
02011         if ( options.enableFarBounds == BT_FALSE )
02012                 THROWERROR( returnvalue );
02013 
02014         return returnvalue;
02015 }
02016 
02017 
02018 /*
02019  *      i s C P U t i m e L i m i t E x c e e d e d
02020  */
02021 BooleanType QProblemB::isCPUtimeLimitExceeded(  const real_t* const cputime,
02022                                                                                                 real_t starttime,
02023                                                                                                 int nWSR
02024                                                                                                 ) const
02025 {
02026         /* Always perform next QP iteration if no CPU time limit is given. */
02027         if ( cputime == 0 )
02028                 return BT_FALSE;
02029 
02030         /* Always perform first QP iteration. */
02031         if ( nWSR <= 0 )
02032                 return BT_FALSE;
02033 
02034         real_t elapsedTime = getCPUtime( ) - starttime;
02035         real_t timePerIteration = elapsedTime / ((real_t) nWSR);
02036 
02037         /* Determine if next QP iteration exceed CPU time limit
02038          * considering the (current) average CPU time per iteration. */
02039         if ( ( elapsedTime + timePerIteration*1.25 ) <= ( *cputime ) )
02040                 return BT_FALSE;
02041         else
02042                 return BT_TRUE;
02043 }
02044 
02045 
02046 /*
02047  *      r e g u l a r i s e H e s s i a n
02048  */
02049 returnValue QProblemB::regulariseHessian( )
02050 {
02051         /* Do nothing if Hessian regularisation is disbaled! */
02052         if ( options.enableRegularisation == BT_FALSE )
02053                 return SUCCESSFUL_RETURN;
02054 
02055         /* Regularisation of identity Hessian not possible. */
02056         if ( hessianType == HST_IDENTITY )
02057                 return THROWERROR( RET_CANNOT_REGULARISE_IDENTITY );
02058 
02059         /* Determine regularisation parameter. */
02060         if ( usingRegularisation( ) == BT_TRUE )
02061                 return THROWERROR( RET_HESSIAN_ALREADY_REGULARISED );
02062         else
02063         {
02064                 /* Regularisation of zero Hessian is done implicitly. */
02065                 if ( hessianType != HST_ZERO )
02066                         if ( H->addToDiag( options.epsRegularisation ) == RET_NO_DIAGONAL_AVAILABLE )
02067                                 return THROWERROR( RET_CANNOT_REGULARISE_SPARSE );
02068 
02069                 isRegularised = BT_TRUE;
02070                 THROWINFO( RET_USING_REGULARISATION );
02071         }
02072 
02073         return SUCCESSFUL_RETURN;
02074 }
02075 
02076 
02077 
02078 /*
02079  *      p e r f o r m R a t i o T e s t
02080  */
02081 returnValue QProblemB::performRatioTest(        const int nIdx,
02082                                                                                         const int* const idxList,
02083                                                                                         const SubjectTo* const subjectTo,
02084                                                                                         const real_t* const num,
02085                                                                                         const real_t* const den,
02086                                                                                         real_t epsNum,
02087                                                                                         real_t epsDen,
02088                                                                                         real_t& t,
02089                                                                                         int& BC_idx
02090                                                                                         ) const
02091 {
02092         int i, ii;
02093 
02094         BC_idx = -1;
02095 
02096         for( i=0; i<nIdx; ++i )
02097         {
02098                 ii = idxList[i];
02099 
02100                 if ( subjectTo->getType( ii ) != ST_EQUALITY )
02101                 {
02102                         if ( ( subjectTo->getStatus( ii ) == ST_LOWER ) || ( subjectTo->getStatus( ii ) == ST_INACTIVE ) )
02103                         {
02104                                 if ( isBlocking( num[i],den[i],epsNum,epsDen,t ) == BT_TRUE )
02105                                 {
02106                                         t = num[i] / den[i];
02107                                         BC_idx = ii;
02108                                 }
02109                         }
02110                         else
02111                         {
02112                                 if ( isBlocking( -num[i],-den[i],epsNum,epsDen,t ) == BT_TRUE )
02113                                 {
02114                                         t = num[i] / den[i];
02115                                         BC_idx = ii;
02116                                 }
02117                         }
02118                 }
02119         }
02120 
02121         return SUCCESSFUL_RETURN;
02122 }
02123 
02124 
02125 
02126 /*
02127  * r e l a t i v e H o m o t o p y L e n g t h
02128  */
02129 real_t QProblemB::relativeHomotopyLength(const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new)
02130 {
02131         int nV = getNV( ), i;
02132         real_t d, s, len = 0.0;
02133 
02134         /* gradient */
02135         for (i = 0; i < nV; i++)
02136         {
02137                 s = fabs(g_new[i]);
02138                 if (s < 1.0) s = 1.0;
02139                 d = fabs(g_new[i] - g[i]) / s;
02140                 if (d > len) len = d;
02141         }
02142 
02143         /* lower bounds */
02144         for (i = 0; i < nV && lb_new; i++)
02145         {
02146                 s = fabs(lb_new[i]);
02147                 if (s < 1.0) s = 1.0;
02148                 d = fabs(lb_new[i] - lb[i]) / s;
02149                 if (d > len) len = d;
02150         }
02151 
02152         /* upper bounds */
02153         for (i = 0; i < nV && ub_new; i++)
02154         {
02155                 s = fabs(ub_new[i]);
02156                 if (s < 1.0) s = 1.0;
02157                 d = fabs(ub_new[i] - ub[i]) / s;
02158                 if (d > len) len = d;
02159         }
02160 
02161         return len;
02162 }
02163 
02164 
02165 /*
02166  * p e r f o r m R a m p i n g
02167  */
02168 returnValue QProblemB::performRamping( )
02169 {
02170         int nV = getNV( ), bstat, i;
02171         real_t t, rampVal;
02172 
02173         /* ramp inactive bounds and active dual variables */
02174         for (i = 0; i < nV; i++)
02175         {
02176                 switch (bounds.getType(i))
02177                 {
02178                         case ST_EQUALITY: lb[i] = x[i]; ub[i] = x[i]; continue; /* reestablish exact feasibility */
02179                         case ST_BOUNDED: continue;
02180                         default: break;
02181                 }
02182 
02183                 t = static_cast<real_t>(i) / static_cast<real_t>(nV-1);
02184                 rampVal = (1.0-t) * ramp0 + t * ramp1;
02185                 bstat = bounds.getStatus(i);
02186                 if (bstat != ST_LOWER) { lb[i] = x[i] - rampVal; }
02187                 if (bstat != ST_UPPER) { ub[i] = x[i] + rampVal; }
02188                 if (bstat == ST_LOWER) { lb[i] = x[i]; y[i] = +rampVal; }
02189                 if (bstat == ST_UPPER) { ub[i] = x[i]; y[i] = -rampVal; }
02190                 if (bstat == ST_INACTIVE) y[i] = 0.0; /* reestablish exact complementarity */
02191         }
02192 
02193         /* reestablish exact stationarity */
02194         setupAuxiliaryQPgradient( );
02195 
02196         /* swap ramp direction */
02197         t = ramp0; ramp0 = ramp1; ramp1 = t;
02198 
02199         return SUCCESSFUL_RETURN;
02200 }
02201 
02202 
02203 
02204 /*****************************************************************************
02205  *  P R I V A T E                                                            *
02206  *****************************************************************************/
02207 
02208 /*
02209  *      s o l v e I n i t i a l Q P
02210  */
02211 returnValue QProblemB::solveInitialQP(  const real_t* const xOpt, const real_t* const yOpt,
02212                                                                                 const Bounds* const guessedBounds,
02213                                                                                 int& nWSR, real_t* const cputime
02214                                                                                 )
02215 {
02216         int i;
02217         int nV = getNV( );
02218 
02219 
02220         /* start runtime measurement */
02221         real_t starttime = 0.0;
02222         if ( cputime != 0 )
02223                 starttime = getCPUtime( );
02224 
02225 
02226         status = QPS_NOTINITIALISED;
02227 
02228         /* I) ANALYSE QP DATA: */
02229         /* 1) Check if Hessian happens to be the identity matrix. */
02230         if ( determineHessianType( ) != SUCCESSFUL_RETURN )
02231                 return THROWERROR( RET_INIT_FAILED );
02232 
02233         /* 2) Setup type of bounds (i.e. unbounded, implicitly fixed etc.). */
02234         if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
02235                 return THROWERROR( RET_INIT_FAILED );
02236 
02237         status = QPS_PREPARINGAUXILIARYQP;
02238 
02239 
02240         /* II) SETUP AUXILIARY QP WITH GIVEN OPTIMAL SOLUTION: */
02241         /* 1) Setup bounds data structure. */
02242         if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
02243                 return THROWERROR( RET_INIT_FAILED );
02244 
02245         /* 2) Setup optimal primal/dual solution for auxiliary QP. */
02246         if ( setupAuxiliaryQPsolution( xOpt,yOpt ) != SUCCESSFUL_RETURN )
02247                 return THROWERROR( RET_INIT_FAILED );
02248 
02249         /* 3) Obtain linear independent working set for auxiliary QP. */
02250         Bounds auxiliaryBounds( nV );
02251         if ( obtainAuxiliaryWorkingSet( xOpt,yOpt,guessedBounds, &auxiliaryBounds ) != SUCCESSFUL_RETURN )
02252                 return THROWERROR( RET_INIT_FAILED );
02253 
02254         /* 4) Setup working set of auxiliary QP and setup cholesky decomposition. */
02255         /* a) Working set of auxiliary QP. */
02256         if ( setupAuxiliaryWorkingSet( &auxiliaryBounds,BT_TRUE ) != SUCCESSFUL_RETURN )
02257                 return THROWERROR( RET_INIT_FAILED );
02258 
02259         /* b) Regularise Hessian if necessary. */
02260         if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_SEMIDEF ) )
02261         {
02262                 if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
02263                         return THROWERROR( RET_INIT_FAILED_REGULARISATION );
02264         }
02265 
02266         haveCholesky = BT_FALSE;
02267 
02268         /* c) Cholesky decomposition. */
02269         returnValue returnvalueCholesky = setupCholeskyDecomposition( );
02270 
02271         /* If Hessian is not positive definite, regularise and try again. */
02272         if ( returnvalueCholesky == RET_HESSIAN_NOT_SPD )
02273         {
02274                 if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
02275                         return THROWERROR( RET_INIT_FAILED );
02276 
02277                 if ( setupCholeskyDecomposition( ) != SUCCESSFUL_RETURN )
02278                         return THROWERROR( RET_INIT_FAILED_CHOLESKY );
02279         }
02280 
02281         if ( returnvalueCholesky == RET_INDEXLIST_CORRUPTED )
02282                 return THROWERROR( RET_INIT_FAILED_CHOLESKY );
02283 
02284         haveCholesky = BT_TRUE;
02285 
02286         /* 5) Store original QP formulation... */
02287         real_t* g_original = new real_t[nV];
02288         real_t* lb_original = new real_t[nV];
02289         real_t* ub_original = new real_t[nV];
02290 
02291         for( i=0; i<nV; ++i )
02292         {
02293                 g_original[i]  = g[i];
02294                 lb_original[i] = lb[i];
02295                 ub_original[i] = ub[i];
02296         }
02297 
02298         /* ... and setup QP data of an auxiliary QP having an optimal solution
02299          * as specified by the user (or xOpt = yOpt = 0, by default). */
02300         if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
02301         {
02302                 delete[] ub_original; delete[] lb_original; delete[] g_original;
02303                 return THROWERROR( RET_INIT_FAILED );
02304         }
02305 
02306         if ( setupAuxiliaryQPbounds( BT_TRUE ) != SUCCESSFUL_RETURN )
02307         {
02308                 delete[] ub_original; delete[] lb_original; delete[] g_original;
02309                 return THROWERROR( RET_INIT_FAILED );
02310         }
02311 
02312         status = QPS_AUXILIARYQPSOLVED;
02313 
02314 
02315         /* III) SOLVE ACTUAL INITIAL QP: */
02316 
02317         /* Allow only remaining CPU time for usual hotstart. */
02318         if ( cputime != 0 )
02319                 *cputime -= getCPUtime( ) - starttime;
02320 
02321         /* Use hotstart method to find the solution of the original initial QP,... */
02322         returnValue returnvalue = hotstart( g_original,lb_original,ub_original, nWSR,cputime );
02323 
02324         /* ... deallocate memory,... */
02325         delete[] ub_original; delete[] lb_original; delete[] g_original;
02326 
02327         /* ... check for infeasibility and unboundedness... */
02328         if ( isInfeasible( ) == BT_TRUE )
02329                 return THROWERROR( RET_INIT_FAILED_INFEASIBILITY );
02330 
02331         if ( isUnbounded( ) == BT_TRUE )
02332                 return THROWERROR( RET_INIT_FAILED_UNBOUNDEDNESS );
02333 
02334         /* ... and internal errors. */
02335         if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
02336                 return THROWERROR( RET_INIT_FAILED_HOTSTART );
02337 
02338 
02339         /* stop runtime measurement */
02340         if ( cputime != 0 )
02341                 *cputime = getCPUtime( ) - starttime;
02342 
02343         THROWINFO( RET_INIT_SUCCESSFUL );
02344 
02345         return returnvalue;
02346 }
02347 
02348 
02349 /*
02350  *      s o l v e Q P
02351  */
02352 returnValue QProblemB::solveQP( const real_t* const g_new,
02353                                                                 const real_t* const lb_new, const real_t* const ub_new,
02354                                                                 int& nWSR, real_t* const cputime, int nWSRperformed
02355                                                                 )
02356 {
02357         int iter;
02358         int nV  = getNV( );
02359 
02360         /* consistency check */
02361         if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
02362                  ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
02363                  ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
02364         {
02365                 return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
02366         }
02367 
02368         /* start runtime measurement */
02369         real_t starttime = 0.0;
02370         if ( cputime != 0 )
02371                 starttime = getCPUtime( );
02372 
02373 
02374         /* I) PREPARATIONS */
02375         /* 1) Allocate delta vectors of gradient and bounds,
02376          *    index arrays and step direction arrays. */
02377         real_t* delta_xFR = new real_t[nV];
02378         real_t* delta_xFX = new real_t[nV];
02379         real_t* delta_yFX = new real_t[nV];
02380 
02381         real_t* delta_g  = new real_t[nV];
02382         real_t* delta_lb = new real_t[nV];
02383         real_t* delta_ub = new real_t[nV];
02384 
02385         returnValue returnvalue;
02386         BooleanType Delta_bB_isZero;
02387 
02388         int BC_idx;
02389         SubjectToStatus BC_status;
02390 
02391         real_t homotopyLength;
02392 
02393         char messageString[80];
02394 
02395         /* 2) Update type of bounds, e.g. a formerly implicitly fixed
02396          *    variable might have become a normal one etc. */
02397         if ( setupSubjectToType( lb_new,ub_new ) != SUCCESSFUL_RETURN )
02398                 return THROWERROR( RET_HOTSTART_FAILED );
02399 
02400         /* 3) Reset status flags. */
02401         infeasible = BT_FALSE;
02402         unbounded  = BT_FALSE;
02403 
02404 
02405         /* II) MAIN HOMOTOPY LOOP */
02406         for( iter=nWSRperformed; iter<nWSR; ++iter )
02407         {
02408                 if ( isCPUtimeLimitExceeded( cputime,starttime,iter-nWSRperformed ) == BT_TRUE )
02409                 {
02410                         /* Assign number of working set recalculations and stop runtime measurement. */
02411                         nWSR = iter;
02412                         if ( cputime != 0 )
02413                                 *cputime = getCPUtime( ) - starttime;
02414 
02415                         break;
02416                 }
02417 
02418                 status = QPS_PERFORMINGHOMOTOPY;
02419 
02420                 #ifndef __XPCTARGET__
02421                 snprintf( messageString,80,"%d ...",iter );
02422                 getGlobalMessageHandler( )->throwInfo( RET_ITERATION_STARTED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
02423                 #endif
02424 
02425                 /* 2) Initialise shift direction of the gradient and the bounds. */
02426                 returnvalue = determineDataShift(       g_new,lb_new,ub_new,
02427                                                                                         delta_g,delta_lb,delta_ub,
02428                                                                                         Delta_bB_isZero
02429                                                                                         );
02430                 if ( returnvalue != SUCCESSFUL_RETURN )
02431                 {
02432                         delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
02433                         delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
02434 
02435                         /* Assign number of working set recalculations and stop runtime measurement. */
02436                         nWSR = iter;
02437                         if ( cputime != 0 )
02438                                 *cputime = getCPUtime( ) - starttime;
02439 
02440                         THROWERROR( RET_SHIFT_DETERMINATION_FAILED );
02441                         return returnvalue;
02442                 }
02443 
02444                 /* 3) Determination of step direction of X and Y. */
02445                 returnvalue = determineStepDirection(   delta_g,delta_lb,delta_ub,
02446                                                                                                 Delta_bB_isZero,
02447                                                                                                 delta_xFX,delta_xFR,delta_yFX
02448                                                                                                 );
02449                 if ( returnvalue != SUCCESSFUL_RETURN )
02450                 {
02451                         delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
02452                         delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
02453 
02454                         /* Assign number of working set recalculations and stop runtime measurement. */
02455                         nWSR = iter;
02456                         if ( cputime != 0 )
02457                                 *cputime = getCPUtime( ) - starttime;
02458 
02459                         THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
02460                         return returnvalue;
02461                 }
02462 
02463 
02464                 /* 4) Determination of step length TAU.
02465                  *    This step along the homotopy path is also taken (without changing working set). */
02466                 returnvalue = performStep(      delta_g,delta_lb,delta_ub,
02467                                                                         delta_xFX,delta_xFR,delta_yFX,
02468                                                                         BC_idx,BC_status
02469                                                                         );
02470                 if ( returnvalue != SUCCESSFUL_RETURN )
02471                 {
02472                         delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
02473                         delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
02474 
02475                         /* Assign number of working set recalculations and stop runtime measurement. */
02476                         nWSR = iter;
02477                         if ( cputime != 0 )
02478                                 *cputime = getCPUtime( ) - starttime;
02479 
02480                         THROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED );
02481                         return returnvalue;
02482                 }
02483 
02484                 /* 5) Termination criterion. */
02485                 homotopyLength = relativeHomotopyLength(g_new, lb_new, ub_new);
02486                 if ( homotopyLength <= options.terminationTolerance )
02487                 {
02488                         status = QPS_SOLVED;
02489 
02490                         THROWINFO( RET_OPTIMAL_SOLUTION_FOUND );
02491 
02492                         if ( printIteration( iter,BC_idx,BC_status ) != SUCCESSFUL_RETURN )
02493                                 THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
02494 
02495                         nWSR = iter;
02496                         if ( cputime != 0 )
02497                                 *cputime = getCPUtime( ) - starttime;
02498 
02499                         delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
02500                         delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
02501 
02502                         return SUCCESSFUL_RETURN;
02503                 }
02504 
02505 
02506                 /* 6) Change active set. */
02507                 returnvalue = changeActiveSet( BC_idx,BC_status );
02508 
02509                 if ( returnvalue != SUCCESSFUL_RETURN )
02510                 {
02511                         delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
02512                         delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
02513 
02514                         /* Assign number of working set recalculations and stop runtime measurement. */
02515                         nWSR = iter;
02516                         if ( cputime != 0 )
02517                                 *cputime = getCPUtime( ) - starttime;
02518 
02519                         /* checks for infeasibility... */
02520                         if ( infeasible == BT_TRUE )
02521                         {
02522                                 status = QPS_HOMOTOPYQPSOLVED;
02523                                 return setInfeasibilityFlag( RET_HOTSTART_STOPPED_INFEASIBILITY );
02524                         }
02525 
02526                         /* ...unboundedness... */
02527                         if ( unbounded == BT_TRUE ) /* not necessary since objective function convex! */
02528                                 return THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS );
02529 
02530                         /* ... and throw unspecific error otherwise */
02531                         THROWERROR( RET_HOMOTOPY_STEP_FAILED );
02532                         return returnvalue;
02533                 }
02534 
02535                 /* 7) Perform Ramping Strategy on zero homotopy step or drift correction (if desired). */
02536                  if ( ( tau <= EPS ) && ( options.enableRamping == BT_TRUE ) )
02537                         performRamping( );
02538                 else
02539                 if ( (options.enableDriftCorrection > 0)
02540                   && ((iter+1) % options.enableDriftCorrection == 0) )
02541                         performDriftCorrection( );  /* always returns SUCCESSFUL_RETURN */
02542 
02543                 /* 8) Output information of successful QP iteration. */
02544                 status = QPS_HOMOTOPYQPSOLVED;
02545 
02546                 if ( printIteration( iter,BC_idx,BC_status ) != SUCCESSFUL_RETURN )
02547                         THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
02548         }
02549 
02550         delete[] delta_yFX; delete[] delta_xFX; delete[] delta_xFR;
02551         delete[] delta_ub; delete[] delta_lb; delete[] delta_g;
02552 
02553         /* stop runtime measurement */
02554         if ( cputime != 0 )
02555                 *cputime = getCPUtime( ) - starttime;
02556 
02557 
02558         /* if programm gets to here, output information that QP could not be solved
02559          * within the given maximum numbers of working set changes */
02560         if ( options.printLevel == PL_HIGH )
02561         {
02562                 #ifndef __XPCTARGET__
02563                 snprintf( messageString,80,"(nWSR = %d)",iter );
02564                 return getGlobalMessageHandler( )->throwWarning( RET_MAX_NWSR_REACHED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
02565                 #else
02566                 return RET_MAX_NWSR_REACHED;
02567                 #endif
02568         }
02569         else
02570         {
02571                 return RET_MAX_NWSR_REACHED;
02572         }
02573 }
02574 
02575 
02576 /*
02577  *      s o l v e R e g u l a r i s e d Q P
02578  */
02579 returnValue QProblemB::solveRegularisedQP(      const real_t* const g_new,
02580                                                                                         const real_t* const lb_new, const real_t* const ub_new,
02581                                                                                         int& nWSR, real_t* const cputime, int nWSRperformed
02582                                                                                         )
02583 {
02584         int i, step;
02585         int nV = getNV( );
02586 
02587 
02588         /* Stop here if QP has not been regularised (i.e. normal QP solution). */
02589         if ( usingRegularisation( ) == BT_FALSE )
02590                 return solveQP( g_new,lb_new,ub_new, nWSR,cputime,nWSRperformed );
02591 
02592 
02593         /* I) SOLVE USUAL REGULARISED QP */
02594         returnValue returnvalue;
02595 
02596         int nWSR_max   = nWSR;
02597         int nWSR_total = nWSRperformed;
02598 
02599         real_t cputime_total = 0.0;
02600         real_t cputime_cur   = 0.0;
02601 
02602         if ( cputime == 0 )
02603         {
02604                 returnvalue = solveQP( g_new,lb_new,ub_new, nWSR,0,nWSRperformed );
02605         }
02606         else
02607         {
02608                 cputime_cur = *cputime;
02609                 returnvalue = solveQP( g_new,lb_new,ub_new, nWSR,&cputime_cur,nWSRperformed );
02610         }
02611         nWSR_total     = nWSR;
02612         cputime_total += cputime_cur;
02613 
02614 
02615         /* Only continue if QP solution has been successful. */
02616         if ( returnvalue != SUCCESSFUL_RETURN )
02617         {
02618                 if ( cputime != 0 )
02619                         *cputime = cputime_total;
02620 
02621                 if ( returnvalue == RET_MAX_NWSR_REACHED )
02622                         THROWWARNING( RET_NO_REGSTEP_NWSR );
02623 
02624                 return returnvalue;
02625         }
02626 
02627 
02628         /* II) PERFORM SUCCESSIVE REGULARISATION STEPS */
02629         real_t* gMod = new real_t[nV];
02630 
02631         for( step=0; step<options.numRegularisationSteps; ++step )
02632         {
02633                 /* 1) Modify gradient: gMod = g - eps*xOpt
02634                  *    (assuming regularisation matrix to be eps*Id). */
02635                 for( i=0; i<nV; ++i )
02636                         gMod[i] = g_new[i] - options.epsRegularisation*x[i];
02637 
02638                 /* 2) Solve regularised QP with modified gradient allowing
02639                  *    only as many working set recalculations and CPU time
02640                  *    as have been left from previous QP solutions. */
02641                 if ( cputime == 0 )
02642                 {
02643                         nWSR = nWSR_max;
02644                         returnvalue = solveQP( gMod,lb_new,ub_new, nWSR,0,nWSR_total );
02645                 }
02646                 else
02647                 {
02648                         nWSR = nWSR_max;
02649                         cputime_cur = *cputime - cputime_total;
02650                         returnvalue = solveQP( gMod,lb_new,ub_new, nWSR,&cputime_cur,nWSR_total );
02651                 }
02652 
02653                 nWSR_total     = nWSR;
02654                 cputime_total += cputime_cur;
02655 
02656                 /* Only continue if QP solution has been successful. */
02657                 if ( returnvalue != SUCCESSFUL_RETURN )
02658                 {
02659                         delete[] gMod;
02660 
02661                         if ( cputime != 0 )
02662                                 *cputime = cputime_total;
02663 
02664                         if ( returnvalue == RET_MAX_NWSR_REACHED )
02665                                 THROWWARNING( RET_FEWER_REGSTEPS_NWSR );
02666 
02667                         return returnvalue;
02668                 }
02669         }
02670 
02671         delete[] gMod;
02672 
02673         if ( cputime != 0 )
02674                 *cputime = cputime_total;
02675 
02676         return SUCCESSFUL_RETURN;
02677 }
02678 
02679 
02680 /*
02681  *      s e t u p A u x i l i a r y W o r k i n g S e t
02682  */
02683 returnValue QProblemB::setupAuxiliaryWorkingSet(        const Bounds* const auxiliaryBounds,
02684                                                                                                         BooleanType setupAfresh
02685                                                                                                         )
02686 {
02687         int i;
02688         int nV = getNV( );
02689 
02690         /* consistency checks */
02691         if ( auxiliaryBounds != 0 )
02692         {
02693                 for( i=0; i<nV; ++i )
02694                         if ( ( bounds.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryBounds->getStatus( i ) == ST_UNDEFINED ) )
02695                                 return THROWERROR( RET_UNKNOWN_BUG );
02696         }
02697         else
02698         {
02699                 return THROWERROR( RET_INVALID_ARGUMENTS );
02700         }
02701 
02702 
02703         /* I) SETUP CHOLESKY FLAG:
02704          *    Cholesky decomposition shall only be updated if working set
02705          *    shall be updated (i.e. NOT setup afresh!) */
02706         BooleanType updateCholesky;
02707         if ( setupAfresh == BT_TRUE )
02708                 updateCholesky = BT_FALSE;
02709         else
02710                 updateCholesky = BT_TRUE;
02711 
02712 
02713         /* II) REMOVE FORMERLY ACTIVE BOUNDS (IF NECESSARY): */
02714         if ( setupAfresh == BT_FALSE )
02715         {
02716                 /* Remove all active bounds that shall be inactive AND
02717                 *  all active bounds that are active at the wrong bound. */
02718                 for( i=0; i<nV; ++i )
02719                 {
02720                         if ( ( bounds.getStatus( i ) == ST_LOWER ) && ( auxiliaryBounds->getStatus( i ) != ST_LOWER ) )
02721                                 if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )
02722                                         return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
02723 
02724                         if ( ( bounds.getStatus( i ) == ST_UPPER ) && ( auxiliaryBounds->getStatus( i ) != ST_UPPER ) )
02725                                 if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )
02726                                         return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
02727                 }
02728         }
02729 
02730 
02731         /* III) ADD NEWLY ACTIVE BOUNDS: */
02732         /*      Add all inactive bounds that shall be active AND
02733          *      all formerly active bounds that have been active at the wrong bound. */
02734         for( i=0; i<nV; ++i )
02735         {
02736                 if ( ( bounds.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE ) )
02737                 {
02738                         if ( addBound( i,auxiliaryBounds->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
02739                                 return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
02740                 }
02741         }
02742 
02743         return SUCCESSFUL_RETURN;
02744 }
02745 
02746 
02747 /*
02748  *      s e t u p A u x i l i a r y Q P s o l u t i o n
02749  */
02750 returnValue QProblemB::setupAuxiliaryQPsolution(        const real_t* const xOpt, const real_t* const yOpt
02751                                                                                                         )
02752 {
02753         int i;
02754         int nV = getNV( );
02755 
02756 
02757         /* Setup primal/dual solution vectors for auxiliary initial QP:
02758          * if a null pointer is passed, a zero vector is assigned;
02759          * old solution vector is kept if pointer to internal solution vector is passed. */
02760         if ( xOpt != 0 )
02761         {
02762                 if ( xOpt != x )
02763                         for( i=0; i<nV; ++i )
02764                                 x[i] = xOpt[i];
02765         }
02766         else
02767         {
02768                 for( i=0; i<nV; ++i )
02769                         x[i] = 0.0;
02770         }
02771 
02772         if ( yOpt != 0 )
02773         {
02774                 if ( yOpt != y )
02775                         for( i=0; i<nV; ++i )
02776                                 y[i] = yOpt[i];
02777         }
02778         else
02779         {
02780                 for( i=0; i<nV; ++i )
02781                         y[i] = 0.0;
02782         }
02783 
02784         return SUCCESSFUL_RETURN;
02785 }
02786 
02787 
02788 /*
02789  *      s e t u p A u x i l i a r y Q P g r a d i e n t
02790  */
02791 returnValue QProblemB::setupAuxiliaryQPgradient( )
02792 {
02793         int i;
02794         int nV = getNV( );
02795 
02796         /* Setup gradient vector: g = -H*x + y'*Id. */
02797         switch ( hessianType )
02798         {
02799                 case HST_ZERO:
02800                         if ( usingRegularisation( ) == BT_FALSE )
02801                                 for ( i=0; i<nV; ++i )
02802                                         g[i] = y[i];
02803                         else
02804                                 for ( i=0; i<nV; ++i )
02805                                         g[i] = y[i] - options.epsRegularisation*x[i];
02806                         break;
02807 
02808                 case HST_IDENTITY:
02809                         for ( i=0; i<nV; ++i )
02810                                 g[i] = y[i] - x[i];
02811                         break;
02812 
02813                 default:
02814                         /* y'*Id */
02815                         for ( i=0; i<nV; ++i )
02816                                 g[i] = y[i];
02817 
02818                         /* -H*x */
02819                         H->times(1, -1.0, x, nV, 1.0, g, nV);
02820 
02821                         break;
02822         }
02823 
02824         return SUCCESSFUL_RETURN;
02825 }
02826 
02827 
02828 /*
02829  *      s e t u p A u x i l i a r y Q P b o u n d s
02830  */
02831 returnValue QProblemB::setupAuxiliaryQPbounds( BooleanType useRelaxation )
02832 {
02833         int i;
02834         int nV = getNV( );
02835 
02836 
02837         /* Setup bound vectors. */
02838         for ( i=0; i<nV; ++i )
02839         {
02840                 switch ( bounds.getStatus( i ) )
02841                 {
02842                         case ST_INACTIVE:
02843                                 if ( useRelaxation == BT_TRUE )
02844                                 {
02845                                         if ( bounds.getType( i ) == ST_EQUALITY )
02846                                         {
02847                                                 lb[i] = x[i];
02848                                                 ub[i] = x[i];
02849                                         }
02850                                         else
02851                                         {
02852                                                 lb[i] = x[i] - options.boundRelaxation;
02853                                                 ub[i] = x[i] + options.boundRelaxation;
02854                                         }
02855                                 }
02856                                 break;
02857 
02858                         case ST_LOWER:
02859                                 lb[i] = x[i];
02860                                 if ( bounds.getType( i ) == ST_EQUALITY )
02861                                 {
02862                                         ub[i] = x[i];
02863                                 }
02864                                 else
02865                                 {
02866                                         if ( useRelaxation == BT_TRUE )
02867                                                 ub[i] = x[i] + options.boundRelaxation;
02868                                 }
02869                                 break;
02870 
02871                         case ST_UPPER:
02872                                 ub[i] = x[i];
02873                                 if ( bounds.getType( i ) == ST_EQUALITY )
02874                                 {
02875                                         lb[i] = x[i];
02876                                 }
02877                                 else
02878                                 {
02879                                         if ( useRelaxation == BT_TRUE )
02880                                                 lb[i] = x[i] - options.boundRelaxation;
02881                                 }
02882                                 break;
02883 
02884                         default:
02885                                 return THROWERROR( RET_UNKNOWN_BUG );
02886                 }
02887         }
02888 
02889         return SUCCESSFUL_RETURN;
02890 }
02891 
02892 
02893 /*
02894  *      s e t u p A u x i l i a r y Q P
02895  */
02896 returnValue QProblemB::setupAuxiliaryQP( const Bounds* const guessedBounds )
02897 {
02898         int i;
02899         int nV = getNV( );
02900 
02901         /* nothing to do */
02902         if ( guessedBounds == &bounds )
02903                 return SUCCESSFUL_RETURN;
02904 
02905         status = QPS_PREPARINGAUXILIARYQP;
02906 
02907 
02908         /* I) SETUP WORKING SET ... */
02909         if ( shallRefactorise( guessedBounds ) == BT_TRUE )
02910         {
02911                 /* ... WITH REFACTORISATION: */
02912                 /* 1) Reset bounds ... */
02913                 bounds.init( nV );
02914 
02915                 /*    ... and set them up afresh. */
02916                 if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
02917                         return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
02918 
02919                 if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
02920                         return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
02921 
02922                 /* 2) Setup guessed working set afresh. */
02923                 if ( setupAuxiliaryWorkingSet( guessedBounds,BT_TRUE ) != SUCCESSFUL_RETURN )
02924                         THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
02925 
02926                 /* 3) Calculate Cholesky decomposition. */
02927                 if ( setupCholeskyDecomposition( ) != SUCCESSFUL_RETURN )
02928                         return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
02929         }
02930         else
02931         {
02932                 /* ... WITHOUT REFACTORISATION: */
02933                 if ( setupAuxiliaryWorkingSet( guessedBounds,BT_FALSE ) != SUCCESSFUL_RETURN )
02934                         THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
02935         }
02936 
02937 
02938         /* II) SETUP AUXILIARY QP DATA: */
02939         /* 1) Ensure that dual variable is zero for fixed bounds. */
02940         for ( i=0; i<nV; ++i )
02941                 if ( bounds.getStatus( i ) != ST_INACTIVE )
02942                         y[i] = 0.0;
02943 
02944         /* 2) Setup gradient and bound vectors. */
02945         if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
02946                 THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
02947 
02948         if ( setupAuxiliaryQPbounds( BT_FALSE ) != SUCCESSFUL_RETURN )
02949                 THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
02950 
02951         return SUCCESSFUL_RETURN;
02952 }
02953 
02954 
02955 /*
02956  *      d e t e r m i n e S t e p D i r e c t i o n
02957  */
02958 returnValue QProblemB::determineStepDirection(  const real_t* const delta_g, const real_t* const delta_lb, const real_t* const delta_ub,
02959                                                                                                 BooleanType Delta_bB_isZero,
02960                                                                                                 real_t* const delta_xFX, real_t* const delta_xFR,
02961                                                                                                 real_t* const delta_yFX
02962                                                                                                 )
02963 {
02964         int i, ii;
02965         int r;
02966         int nFR = getNFR( );
02967         int nFX = getNFX( );
02968         
02969         int* FR_idx;
02970         int* FX_idx;
02971 
02972         bounds.getFree( )->getNumberArray( &FR_idx );
02973         bounds.getFixed( )->getNumberArray( &FX_idx );
02974 
02975 
02976         /* This routine computes
02977          * delta_xFX := delta_b
02978          * delta_xFR := R \ R' \ -( delta_g + HMX*delta_xFX )
02979          * delta_yFX := HMX'*delta_xFR + HFX*delta_xFX  { + eps*delta_xFX }
02980          */
02981 
02982         /* I) DETERMINE delta_xFX := delta_{l|u}b */
02983         if ( Delta_bB_isZero == BT_FALSE )
02984         {
02985                 for( i=0; i<nFX; ++i )
02986                 {
02987                         ii = FX_idx[i];
02988 
02989                         if ( bounds.getStatus( ii ) == ST_LOWER )
02990                                 delta_xFX[i] = delta_lb[ii];
02991                         else
02992                                 delta_xFX[i] = delta_ub[ii];
02993                 }
02994         }
02995         else
02996         {
02997                 for( i=0; i<nFX; ++i )
02998                         delta_xFX[i] = 0.0;
02999         }
03000 
03001 
03002         /* delta_xFR_TMP holds the residual, initialized with right hand side
03003          * delta_xFR holds the step that gets refined incrementally */
03004         for ( i=0; i<nFR; ++i )
03005         {
03006                 ii = FR_idx[i];
03007                 delta_xFR_TMP[i] = - delta_g[ii];
03008                 delta_xFR[i] = 0.0;
03009         }
03010 
03011 
03012         /* Iterative refinement loop for delta_xFR */
03013         for ( r=0; r<=options.numRefinementSteps; ++r )
03014         {
03015                 /* II) DETERMINE delta_xFR */
03016                 if ( nFR > 0 )
03017                 {
03018                         /* Add - HMX*delta_xFX
03019                          * This is skipped if delta_b=0 or mixed part HM=0 (H=0 or H=Id) */
03020                         if ( ( hessianType != HST_ZERO ) && ( hessianType != HST_IDENTITY ) && ( Delta_bB_isZero == BT_FALSE ) && ( r == 0 ) )
03021                                 H->times(bounds.getFree(), bounds.getFixed(), 1, -1.0, delta_xFX, nFX, 1.0, delta_xFR_TMP, nFR);
03022 
03023                         /* Determine R' \ ( - HMX*delta_xFX - delta_gFR ) where R'R = HFR */
03024                         if ( backsolveR( delta_xFR_TMP,BT_TRUE,delta_xFR_TMP ) != SUCCESSFUL_RETURN )
03025                                 return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
03026 
03027                         /* Determine HFR \ ( - HMX*delta_xFX - delta_gFR ) */
03028                         if ( backsolveR( delta_xFR_TMP,BT_FALSE,delta_xFR_TMP ) != SUCCESSFUL_RETURN )
03029                                 return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
03030                 }
03031 
03032                 /* refine solution found for delta_xFR so far */
03033                 for ( i=0; i<nFR; ++i )
03034                         delta_xFR[i] += delta_xFR_TMP[i];
03035 
03036                 if ( options.numRefinementSteps > 0 )
03037                 {
03038                         real_t rnrm = 0.0;
03039                         /* compute new residual in delta_xFR_TMP:
03040                          * residual := - HFR*delta_xFR - HMX*delta_xFX - delta_gFR
03041                          * set to -delta_gFR */
03042                         for ( i=0; i<nFR; ++i )
03043                         {
03044                                 ii = FR_idx[i];
03045                                 delta_xFR_TMP[i] = -delta_g[ii];
03046                         }
03047                         /* add - HFR*delta_xFR */
03048                         switch ( hessianType )
03049                         {
03050                                 case HST_ZERO:
03051                                         break;
03052 
03053                                 case HST_IDENTITY:
03054                                         for ( i=0; i<nFR; ++i )
03055                                         {
03056                                                 delta_xFR_TMP[i] -= delta_xFR[i];
03057 
03058                                                 /* compute max norm */
03059                                                 if (rnrm < fabs (delta_xFR_TMP[i]))
03060                                                         rnrm = fabs (delta_xFR_TMP[i]);
03061                                         }
03062                                         break;
03063 
03064                                 default:
03065                                         H->times(bounds.getFree(), bounds.getFree(),  1, -1.0, delta_xFR, nFR, 1.0, delta_xFR_TMP, nFR);
03066                                         H->times(bounds.getFree(), bounds.getFixed(), 1, -1.0, delta_xFX, nFX, 1.0, delta_xFR_TMP, nFR);
03067 
03068                                         /* compute max norm */
03069                                         for ( i=0; i<nFR; ++i )
03070                                                 if (rnrm < fabs (delta_xFR_TMP[i]))
03071                                                         rnrm = fabs (delta_xFR_TMP[i]);
03072 
03073                                         break;
03074                         }
03075                         
03076                         /* early termination of residual norm small enough */
03077                         if ( rnrm < options.epsIterRef )
03078                                 break;
03079                 }
03080 
03081         } /* end of refinement loop for delta_xFR */
03082 
03083         /* III) DETERMINE delta_yFX */
03084         if ( nFX > 0 )
03085         {
03086                 if ( ( hessianType == HST_ZERO ) || ( hessianType == HST_IDENTITY ) )
03087                 {
03088                         for( i=0; i<nFX; ++i )
03089                         {
03090                                 /* set to delta_g */
03091                                 ii = FX_idx[i];
03092                                 delta_yFX[i] = delta_g[ii];
03093 
03094                                 /* add HFX*delta_xFX = {0|I}*delta_xFX */
03095                                 if ( hessianType == HST_ZERO )
03096                                 {
03097                                         if ( usingRegularisation( ) == BT_TRUE )
03098                                                 delta_yFX[i] += options.epsRegularisation*delta_xFX[i];
03099                                 }
03100                                 else
03101                                         delta_yFX[i] += delta_xFX[i];
03102                         }
03103                 }
03104                 else
03105                 {
03106                         for( i=0; i<nFX; ++i )
03107                         {
03108                                 /* set to delta_g */
03109                                 ii = FX_idx[i];
03110                                 delta_yFX[i] = delta_g[ii];
03111                         }
03112                         H->times(bounds.getFixed(), bounds.getFree(), 1, 1.0, delta_xFR, nFR, 1.0, delta_yFX, nFX);
03113                         if (Delta_bB_isZero == BT_FALSE)
03114                                 H->times(bounds.getFixed(), bounds.getFixed(), 1, 1.0, delta_xFX, nFX, 1.0, delta_yFX, nFX);
03115                 }
03116         }
03117 
03118         return SUCCESSFUL_RETURN;
03119 }
03120 
03121 
03122 /*
03123  *      p e r f o r m S t e p
03124  */
03125 returnValue QProblemB::performStep(     const real_t* const delta_g,
03126                                                                         const real_t* const delta_lb, const real_t* const delta_ub,
03127                                                                         const real_t* const delta_xFX,
03128                                                                         const real_t* const delta_xFR,
03129                                                                         const real_t* const delta_yFX,
03130                                                                         int& BC_idx, SubjectToStatus& BC_status
03131                                                                         )
03132 {
03133         int i, ii;
03134         int nV = getNV( );
03135         int nFR = getNFR( );
03136         int nFX = getNFX( );
03137 
03138         int* FR_idx;
03139         int* FX_idx;
03140 
03141         bounds.getFree( )->getNumberArray( &FR_idx );
03142         bounds.getFixed( )->getNumberArray( &FX_idx );
03143 
03144         tau = 1.0;
03145         BC_idx = -1;
03146         BC_status = ST_UNDEFINED;
03147 
03148         int BC_idx_tmp = -1;
03149 
03150         real_t* num = new real_t[nV];
03151         real_t* den = new real_t[nV];
03152 
03153 
03154         /* I) DETERMINE MAXIMUM DUAL STEPLENGTH, i.e. ensure that
03155          *    active dual bounds remain valid (ignoring implicitly fixed variables): */
03156         for( i=0; i<nFX; ++i )
03157         {
03158                 ii = FX_idx[i];
03159                 num[i] = y[ii];
03160                 den[i] = -delta_yFX[i];
03161         }
03162 
03163         performRatioTest( nFX,FX_idx,&bounds, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
03164 
03165         if ( BC_idx_tmp >= 0 )
03166         {
03167                 BC_idx = BC_idx_tmp;
03168                 BC_status = ST_INACTIVE;
03169         }
03170 
03171 
03172         /* II) DETERMINE MAXIMUM PRIMAL STEPLENGTH, i.e. ensure that
03173          *     inactive bounds remain valid (ignoring unbounded variables). */
03174         /* 1) Inactive lower bounds. */
03175         if ( bounds.hasNoLower( ) == BT_FALSE )
03176         {
03177                 for( i=0; i<nFR; ++i )
03178                 {
03179                         ii = FR_idx[i];
03180                         num[i] = getMax( x[ii] - lb[ii],0.0 );
03181                         den[i] = delta_lb[ii] - delta_xFR[i];
03182                 }
03183 
03184                 performRatioTest( nFR,FR_idx,&bounds, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
03185 
03186                 if ( BC_idx_tmp >= 0 )
03187                 {
03188                         BC_idx = BC_idx_tmp;
03189                         BC_status = ST_LOWER;
03190                 }
03191         }
03192 
03193         /* 2) Inactive upper bounds. */
03194         if ( bounds.hasNoUpper( ) == BT_FALSE )
03195         {
03196                 for( i=0; i<nFR; ++i )
03197                 {
03198                         ii = FR_idx[i];
03199                         num[i] = getMax( ub[ii] - x[ii],0.0 );
03200                         den[i] = delta_xFR[i] - delta_ub[ii];
03201                 }
03202 
03203                 performRatioTest( nFR,FR_idx,&bounds, num,den, options.epsNum,options.epsDen, tau,BC_idx_tmp );
03204 
03205                 if ( BC_idx_tmp >= 0 )
03206                 {
03207                         BC_idx = BC_idx_tmp;
03208                         BC_status = ST_UPPER;
03209                 }
03210         }
03211 
03212         delete[] den;
03213         delete[] num;
03214 
03215 
03216         #ifndef __XPCTARGET__
03217         char messageString[80];
03218 
03219         if ( BC_status == ST_UNDEFINED )
03220                 snprintf( messageString,80,"Stepsize is %.10e!",tau );
03221         else
03222                 snprintf( messageString,80,"Stepsize is %.10e! (BC_idx = %d, BC_status = %d)",tau,BC_idx,BC_status );
03223 
03224         getGlobalMessageHandler( )->throwInfo( RET_STEPSIZE_NONPOSITIVE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
03225         #endif
03226 
03227 
03228         /* III) PERFORM STEP ALONG HOMOTOPY PATH */
03229         if ( tau > ZERO )
03230         {
03231                 /* 1) Perform step in primal und dual space. */
03232                 for( i=0; i<nFR; ++i )
03233                 {
03234                         ii = FR_idx[i];
03235                         x[ii] += tau*delta_xFR[i];
03236                 }
03237 
03238                 for( i=0; i<nFX; ++i )
03239                 {
03240                         ii = FX_idx[i];
03241                         x[ii] += tau*delta_xFX[i];
03242                         y[ii] += tau*delta_yFX[i];
03243                 }
03244 
03245                 /* 2) Shift QP data. */
03246                 for( i=0; i<nV; ++i )
03247                 {
03248                         g[i]  += tau*delta_g[i];
03249                         lb[i] += tau*delta_lb[i];
03250                         ub[i] += tau*delta_ub[i];
03251                 }
03252         }
03253         else
03254         {
03255                 /* print a warning if stepsize is zero */
03256                 #ifndef __XPCTARGET__
03257                 snprintf( messageString,80,"Stepsize is %.6e",tau );
03258                 getGlobalMessageHandler( )->throwWarning( RET_STEPSIZE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
03259                 #endif
03260         }
03261 
03262 
03263         return SUCCESSFUL_RETURN;
03264 }
03265 
03266 
03267 /*
03268  *      c h a n g e A c t i v e S e t
03269  */
03270 returnValue QProblemB::changeActiveSet( int BC_idx, SubjectToStatus BC_status )
03271 {
03272         char messageString[80];
03273 
03274         /* IV) UPDATE ACTIVE SET */
03275         switch ( BC_status )
03276         {
03277                 /* Optimal solution found as no working set change detected. */
03278                 case ST_UNDEFINED:
03279                         return RET_OPTIMAL_SOLUTION_FOUND;
03280 
03281 
03282                 /* Remove one variable from active set. */
03283                 case ST_INACTIVE:
03284                         #ifndef __XPCTARGET__
03285                         snprintf( messageString,80,"bound no. %d.", BC_idx );
03286                         getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
03287                         #endif
03288 
03289                         if ( removeBound( BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN )
03290                                 return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
03291 
03292                         y[BC_idx] = 0.0;
03293                         break;
03294 
03295 
03296                 /* Add one variable to active set. */
03297                 default:
03298                         #ifndef __XPCTARGET__
03299                         if ( BC_status == ST_LOWER )
03300                                 snprintf( messageString,80,"lower bound no. %d.", BC_idx );
03301                         else
03302                                 snprintf( messageString,80,"upper bound no. %d.", BC_idx );
03303                                 getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
03304                         #endif
03305 
03306                         if ( addBound( BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN )
03307                                 return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
03308                         break;
03309         }
03310 
03311         return SUCCESSFUL_RETURN;
03312 }
03313 
03314 
03315 
03316 /*
03317  * p e r f o r m D r i f t C o r r e c t i o n
03318  */
03319 returnValue QProblemB::performDriftCorrection( )
03320 {
03321         int i;
03322         int nV = getNV ();
03323 
03324         for ( i=0; i<nV; ++i )
03325         {
03326                 switch ( bounds.getType ( i ) )
03327                 {
03328                         case ST_BOUNDED:
03329                                 switch ( bounds.getStatus ( i ) )
03330                                 {
03331                                         case ST_LOWER:
03332                                                 lb[i] = x[i];
03333                                                 ub[i] = getMax (ub[i], x[i]);
03334                                                 y[i] = getMax (y[i], 0.0);
03335                                                 break;
03336                                         case ST_UPPER:
03337                                                 lb[i] = getMin (lb[i], x[i]);
03338                                                 ub[i] = x[i];
03339                                                 y[i] = getMin (y[i], 0.0);
03340                                                 break;
03341                                         case ST_INACTIVE:
03342                                                 lb[i] = getMin (lb[i], x[i]);
03343                                                 ub[i] = getMax (ub[i], x[i]);
03344                                                 y[i] = 0.0;
03345                                                 break;
03346                                         case ST_UNDEFINED:
03347                                                 break;
03348                                 }
03349                                 break;
03350                         case ST_EQUALITY:
03351                                 lb[i] = x[i];
03352                                 ub[i] = x[i];
03353                                 break;
03354                         case ST_UNBOUNDED:
03355                         case ST_UNKNOWN:
03356                                 break;
03357                 }
03358         }
03359 
03360         setupAuxiliaryQPgradient( );
03361 
03362         return SUCCESSFUL_RETURN;
03363 }
03364 
03365 
03366 /*
03367  *      s h a l l R e f a c t o r i s e
03368  */
03369 BooleanType QProblemB::shallRefactorise( const Bounds* const guessedBounds ) const
03370 {
03371         int i;
03372         int nV = getNV( );
03373 
03374         /* always refactorise if Hessian is not known to be positive definite */
03375         if ( getHessianType( ) == HST_SEMIDEF )
03376                 return BT_TRUE;
03377 
03378         /* 1) Determine number of bounds that have same status
03379          *    in guessed AND current bounds.*/
03380         int differenceNumber = 0;
03381 
03382         for( i=0; i<nV; ++i )
03383                 if ( guessedBounds->getStatus( i ) != bounds.getStatus( i ) )
03384                         ++differenceNumber;
03385 
03386         /* 2) Decide wheter to refactorise or not. */
03387         if ( 2*differenceNumber > guessedBounds->getNFX( ) )
03388                 return BT_TRUE;
03389         else
03390                 return BT_FALSE;
03391 }
03392 
03393 
03394 /*
03395  *      a d d B o u n d
03396  */
03397 returnValue QProblemB::addBound(        int number, SubjectToStatus B_status,
03398                                                                         BooleanType updateCholesky
03399                                                                         )
03400 {
03401         int i, j;
03402         int nV  = getNV( );
03403         int nFR = getNFR( );
03404 
03405 
03406         /* consistency check */
03407         if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
03408                  ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
03409                  ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
03410                  ( getStatus( ) == QPS_SOLVED )            )
03411         {
03412                 return THROWERROR( RET_UNKNOWN_BUG );
03413         }
03414 
03415         /* Perform cholesky updates only if QProblemB has been initialised! */
03416         if ( getStatus( ) == QPS_PREPARINGAUXILIARYQP )
03417         {
03418                 /* UPDATE INDICES */
03419                 if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
03420                         return THROWERROR( RET_ADDBOUND_FAILED );
03421 
03422                 return SUCCESSFUL_RETURN;
03423         }
03424 
03425 
03426         /* I) PERFORM CHOLESKY UPDATE: */
03427         if ( ( updateCholesky == BT_TRUE ) &&
03428                  ( hessianType != HST_ZERO )   && ( hessianType != HST_IDENTITY ) )
03429         {
03430                 /* 1) Index of variable to be added within the list of free variables. */
03431                 int number_idx = bounds.getFree( )->getIndex( number );
03432 
03433                 real_t c, s, nu;
03434 
03435                 /* 2) Use row-wise Givens rotations to restore upper triangular form of R. */
03436                 for( i=number_idx+1; i<nFR; ++i )
03437                 {
03438                         computeGivens( RR(i-1,i),RR(i,i), RR(i-1,i),RR(i,i),c,s );
03439                         nu = s/(1.0+c);
03440 
03441                         for( j=(1+i); j<nFR; ++j ) /* last column of R is thrown away */
03442                                 applyGivens( c,s,nu,RR(i-1,j),RR(i,j), RR(i-1,j),RR(i,j) );
03443                 }
03444 
03445                 /* 3) Delete <number_idx>th column and ... */
03446                 for( i=0; i<nFR-1; ++i )
03447                         for( j=number_idx+1; j<nFR; ++j )
03448                                 RR(i,j-1) = RR(i,j);
03449                 /* ... last column of R. */
03450                 for( i=0; i<nFR; ++i )
03451                         RR(i,nFR-1) = 0.0;
03452         }
03453 
03454         /* II) UPDATE INDICES */
03455         if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
03456                 return THROWERROR( RET_ADDBOUND_FAILED );
03457 
03458 
03459         return SUCCESSFUL_RETURN;
03460 }
03461 
03462 
03463 /*
03464  *      r e m o v e B o u n d
03465  */
03466 returnValue QProblemB::removeBound(     int number,
03467                                                                         BooleanType updateCholesky
03468                                                                         )
03469 {
03470         int i;
03471         int nV  = getNV( );
03472         int nFR = getNFR( );
03473 
03474 
03475         /* consistency check */
03476         if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
03477                  ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
03478                  ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
03479                  ( getStatus( ) == QPS_SOLVED )            )
03480         {
03481                 return THROWERROR( RET_UNKNOWN_BUG );
03482         }
03483 
03484         /* save index sets and decompositions for flipping bounds strategy */
03485         if ( options.enableFlippingBounds == BT_TRUE )
03486                 flipper.set( &bounds,R );
03487 
03488         /* I) UPDATE INDICES */
03489         if ( bounds.moveFixedToFree( number ) != SUCCESSFUL_RETURN )
03490                 return THROWERROR( RET_REMOVEBOUND_FAILED );
03491 
03492         /* Perform cholesky updates only if QProblemB has been initialised! */
03493         if ( getStatus( ) == QPS_PREPARINGAUXILIARYQP )
03494                 return SUCCESSFUL_RETURN;
03495 
03496 
03497         /* II) PERFORM CHOLESKY UPDATE */
03498         if ( ( updateCholesky == BT_TRUE ) &&
03499                  ( hessianType != HST_ZERO )   && ( hessianType != HST_IDENTITY ) )
03500         {
03501                 int* FR_idx;
03502                 bounds.getFree( )->getNumberArray( &FR_idx );
03503 
03504                 /* 1) Calculate new column of cholesky decomposition. */
03505                 real_t* rhs = new real_t[nFR+1];
03506                 real_t* r   = new real_t[nFR];
03507 
03508                 real_t r0;
03509                 switch ( hessianType )
03510                 {
03511                         case HST_ZERO:
03512                                 if ( usingRegularisation( ) == BT_FALSE )
03513                                         r0 = 0.0;
03514                                 else
03515                                         r0 = options.epsRegularisation;
03516                                 for( i=0; i<nFR; ++i )
03517                                         rhs[i] = 0.0;
03518                                 break;
03519 
03520                         case HST_IDENTITY:
03521                                 r0 = 1.0;
03522                                 for( i=0; i<nFR; ++i )
03523                                         rhs[i] = 0.0;
03524                                 break;
03525 
03526                         default:
03527                                 H->getRow(number, bounds.getFree(), 1.0, rhs);
03528                                 r0 = H->diag(number);
03529                                 break;
03530                 }
03531 
03532                 if ( backsolveR( rhs,BT_TRUE,BT_TRUE,r ) != SUCCESSFUL_RETURN )
03533                 {
03534                         delete[] rhs; delete[] r;
03535                         return THROWERROR( RET_REMOVEBOUND_FAILED );
03536                 }
03537 
03538                 for( i=0; i<nFR; ++i )
03539                         r0 -= r[i]*r[i];
03540 
03541                 /* 2) Store new column into R. */
03542                 for( i=0; i<nFR; ++i )
03543                         RR(i,nFR) = r[i];
03544 
03545                 if ( options.enableFlippingBounds == BT_TRUE )
03546                 {
03547                         if ( r0 > options.epsFlipping )
03548                                 RR(nFR,nFR) = sqrt( r0 );
03549                         else
03550                         {
03551                                 hessianType = HST_SEMIDEF;
03552 
03553                                 flipper.get( &bounds,R );
03554                                 bounds.flipFixed(number);
03555 
03556                                 switch (bounds.getStatus(number))
03557                                 {
03558                                         case ST_LOWER: lb[number] = ub[number]; break;
03559                                         case ST_UPPER: ub[number] = lb[number]; break;
03560                                         default: delete[] rhs; delete[] r; return THROWERROR( RET_MOVING_BOUND_FAILED );
03561                                 }
03562 
03563                         }
03564                 }
03565                 else
03566                 {
03567                         if ( r0 > ZERO )
03568                                 RR(nFR,nFR) = sqrt( r0 );
03569                         else
03570                         {
03571                                 delete[] rhs; delete[] r;
03572 
03573                                 hessianType = HST_SEMIDEF;
03574                                 return THROWERROR( RET_HESSIAN_NOT_SPD );
03575                         }
03576                 }
03577 
03578                 delete[] rhs; delete[] r;
03579         }
03580 
03581         return SUCCESSFUL_RETURN;
03582 }
03583 
03584 
03585 /*
03586  *      p r i n t I t e r a t i o n
03587  */
03588 returnValue QProblemB::printIteration(  int iteration,
03589                                                                                 int BC_idx,     SubjectToStatus BC_status
03590                                                                                 )
03591 {
03592         #ifndef __XPCTARGET__
03593         char myPrintfString[160];
03594 
03595         /* consistency check */
03596         if ( iteration < 0 )
03597                 return THROWERROR( RET_INVALID_ARGUMENTS );
03598 
03599         /* nothing to do */
03600         if ( options.printLevel != PL_MEDIUM )
03601                 return SUCCESSFUL_RETURN;
03602 
03603 
03604         /* 1) Print header at first iteration. */
03605         if ( iteration == 0 )
03606         {
03607                 snprintf( myPrintfString,160,"\n\n#################   qpOASES  --  QP NO. %3.0d   ##################\n\n", count );
03608                 myPrintf( myPrintfString );
03609 
03610                 myPrintf( "    Iter   |    StepLength    |       Info       |   nFX    \n" );
03611                 myPrintf( " ----------+------------------+------------------+--------- \n" );
03612         }
03613 
03614         /* 2) Print iteration line. */
03615         if ( BC_status == ST_UNDEFINED )
03616         {
03617                 if ( hessianType == HST_ZERO )
03618                         snprintf( myPrintfString,80,"   %5.1d   |   %1.6e   |    LP SOLVED     |  %4.1d   \n", iteration,tau,getNFX( ) );
03619                 else
03620                         snprintf( myPrintfString,80,"   %5.1d   |   %1.6e   |    QP SOLVED     |  %4.1d   \n", iteration,tau,getNFX( ) );
03621                 myPrintf( myPrintfString );
03622         }
03623         else
03624         {
03625                 char info[8];
03626 
03627                 if ( BC_status == ST_INACTIVE )
03628                         snprintf( info,8,"REM BND" );
03629                 else
03630                         snprintf( info,8,"ADD BND" );
03631 
03632                 snprintf( myPrintfString,80,"   %5.1d   |   %1.6e   |   %s %4.1d   |  %4.1d   \n", iteration,tau,info,BC_idx,getNFX( ) );
03633                 myPrintf( myPrintfString );
03634         }
03635         #endif
03636 
03637         return SUCCESSFUL_RETURN;
03638 }
03639 
03640 
03641 
03642 END_NAMESPACE_QPOASES
03643 
03644 
03645 /*
03646  *      end of file
03647  */

acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Thu Aug 27 2015 11:59:51