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-2008 by Hans Joachim Ferreau et al. All rights reserved.
00006  *
00007  *      qpOASES is free software; you can redistribute it and/or
00008  *      modify it under the terms of the GNU Lesser General Public
00009  *      License as published by the Free Software Foundation; either
00010  *      version 2.1 of the License, or (at your option) any later version.
00011  *
00012  *      qpOASES is distributed in the hope that it will be useful,
00013  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00015  *      Lesser General Public License for more details.
00016  *
00017  *      You should have received a copy of the GNU Lesser General Public
00018  *      License along with qpOASES; if not, write to the Free Software
00019  *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00020  *
00021  */
00022 
00023 
00035 #include <QProblemB.hpp>
00036 
00037 #include <stdio.h>
00038 
00039 void printmatrix(char *name, double *A, int m, int n) {
00040   int i, j;
00041 
00042   printf("%s = [...\n", name);
00043   for (i = 0; i < m; i++) {
00044     for (j = 0; j < n; j++)
00045         printf("  % 9.4f", A[i*n+j]);
00046     printf(",\n");
00047   }
00048   printf("];\n");
00049 }
00050 
00051 
00052 
00053 /*****************************************************************************
00054  *  P U B L I C                                                              *
00055  *****************************************************************************/
00056 
00057 
00058 /*
00059  *      Q P r o b l e m B
00060  */
00061 QProblemB::QProblemB( )
00062 {
00063         /* reset global message handler */
00064         getGlobalMessageHandler( )->reset( );
00065 
00066         hasHessian = BT_FALSE;
00067 
00068         bounds.init( 0 );
00069 
00070         hasCholesky = BT_FALSE;
00071 
00072         tau = 0.0;
00073 
00074         hessianType = HST_POSDEF_NULLSPACE; /* Hessian is assumed to be positive definite by default */
00075         infeasible = BT_FALSE;
00076         unbounded = BT_FALSE;
00077 
00078         status = QPS_NOTINITIALISED;
00079 
00080         #ifdef PC_DEBUG
00081         printlevel = PL_MEDIUM;
00082         setPrintLevel( PL_MEDIUM );
00083         #else
00084         printlevel = QPOASES_PRINTLEVEL;
00085         #endif
00086 
00087         count = 0;
00088 }
00089 
00090 
00091 /*
00092  *      Q P r o b l e m B
00093  */
00094 QProblemB::QProblemB( int _nV )
00095 {
00096         /* consistency check */
00097         if ( _nV <= 0 )
00098         {
00099                 _nV = 1;
00100                 THROWERROR( RET_INVALID_ARGUMENTS );
00101         }
00102 
00103         hasHessian = BT_FALSE;
00104 
00105         /* reset global message handler */
00106         getGlobalMessageHandler( )->reset( );
00107 
00108         bounds.init( _nV );
00109 
00110         hasCholesky = BT_FALSE;
00111 
00112         tau = 0.0;
00113 
00114         hessianType = HST_POSDEF_NULLSPACE; /* Hessian is assumed to be positive definite by default */
00115         infeasible = BT_FALSE;
00116         unbounded = BT_FALSE;
00117 
00118         status = QPS_NOTINITIALISED;
00119 
00120         #ifdef PC_DEBUG
00121         printlevel = PL_MEDIUM;
00122         setPrintLevel( PL_MEDIUM );
00123         #else
00124         printlevel = QPOASES_PRINTLEVEL;
00125         #endif
00126 
00127         count = 0;
00128 }
00129 
00130 
00131 /*
00132  *      Q P r o b l e m B
00133  */
00134 QProblemB::QProblemB( const QProblemB& rhs )
00135 {
00136         int i, j;
00137 
00138         int _nV = rhs.bounds.getNV( );
00139 
00140         for( i=0; i<_nV; ++i )
00141                 for( j=0; j<_nV; ++j )
00142                         H[i*NVMAX + j] = rhs.H[i*NVMAX + j];
00143 
00144         hasHessian = rhs.hasHessian;
00145 
00146         for( i=0; i<_nV; ++i )
00147                 g[i] = rhs.g[i];
00148 
00149         for( i=0; i<_nV; ++i )
00150                 lb[i] = rhs.lb[i];
00151 
00152         for( i=0; i<_nV; ++i )
00153                 ub[i] = rhs.ub[i];
00154 
00155 
00156         bounds = rhs.bounds;
00157 
00158         for( i=0; i<_nV; ++i )
00159                 for( j=0; j<_nV; ++j )
00160                         R[i*NVMAX + j] = rhs.R[i*NVMAX + j];
00161         hasCholesky = rhs.hasCholesky;
00162 
00163         for( i=0; i<_nV; ++i )
00164                 x[i] = rhs.x[i];
00165 
00166         for( i=0; i<_nV; ++i )
00167                 y[i] = rhs.y[i];
00168 
00169         tau = rhs.tau;
00170 
00171         hessianType = rhs.hessianType;
00172         infeasible = rhs.infeasible;
00173         unbounded = rhs.unbounded;
00174 
00175         status = rhs.status;
00176 
00177         printlevel = rhs.printlevel;
00178 
00179         count = rhs.count;
00180 }
00181 
00182 
00183 /*
00184  *      ~ Q P r o b l e m B
00185  */
00186 QProblemB::~QProblemB( )
00187 {
00188 }
00189 
00190 
00191 /*
00192  *      o p e r a t o r =
00193  */
00194 QProblemB& QProblemB::operator=( const QProblemB& rhs )
00195 {
00196         int i, j;
00197 
00198         if ( this != &rhs )
00199         {
00200                 int _nV = rhs.bounds.getNV( );
00201 
00202                 for( i=0; i<_nV; ++i )
00203                         for( j=0; j<_nV; ++j )
00204                                 H[i*NVMAX + j] = rhs.H[i*NVMAX + j];
00205 
00206                 hasHessian = rhs.hasHessian;
00207 
00208                 for( i=0; i<_nV; ++i )
00209                         g[i] = rhs.g[i];
00210 
00211                 for( i=0; i<_nV; ++i )
00212                         lb[i] = rhs.lb[i];
00213 
00214                 for( i=0; i<_nV; ++i )
00215                         ub[i] = rhs.ub[i];
00216 
00217                 bounds = rhs.bounds;
00218 
00219                 for( i=0; i<_nV; ++i )
00220                         for( j=0; j<_nV; ++j )
00221                                 R[i*NVMAX + j] = rhs.R[i*NVMAX + j];
00222                 hasCholesky = rhs.hasCholesky;
00223 
00224 
00225                 for( i=0; i<_nV; ++i )
00226                         x[i] = rhs.x[i];
00227 
00228                 for( i=0; i<_nV; ++i )
00229                         y[i] = rhs.y[i];
00230 
00231                 tau = rhs.tau;
00232 
00233                 hessianType = rhs.hessianType;
00234                 infeasible = rhs.infeasible;
00235                 unbounded = rhs.unbounded;
00236 
00237                 status = rhs.status;
00238 
00239                 printlevel = rhs.printlevel;
00240                 setPrintLevel( rhs.printlevel );
00241 
00242                 count = rhs.count;
00243         }
00244 
00245         return *this;
00246 }
00247 
00248 
00249 /*
00250  *      r e s e t
00251  */
00252 returnValue QProblemB::reset( )
00253 {
00254         int i, j;
00255         int nV = getNV( );
00256 
00258         hasHessian = BT_FALSE;
00259 
00260         /* 1) Reset bounds. */
00261         bounds.init( nV );
00262 
00263         /* 2) Reset Cholesky decomposition. */
00264         for( i=0; i<nV; ++i )
00265                 for( j=0; j<nV; ++j )
00266                         R[i*NVMAX + j] = 0.0;
00267         hasCholesky = BT_FALSE;
00268 
00269         /* 3) Reset steplength and status flags. */
00270         tau = 0.0;
00271 
00272         hessianType = HST_POSDEF_NULLSPACE; /* Hessian is assumed to be positive definite by default */
00273         infeasible = BT_FALSE;
00274         unbounded = BT_FALSE;
00275 
00276         status = QPS_NOTINITIALISED;
00277 
00278         return SUCCESSFUL_RETURN;
00279 }
00280 
00281 
00282 /*
00283  *      i n i t
00284  */
00285 returnValue QProblemB::init(    const real_t* const _H, const real_t* const _g,
00286                                                                 const real_t* const _lb, const real_t* const _ub,
00287                                                                 int& nWSR, const real_t* const yOpt, real_t* const cputime
00288                                                                 )
00289 {
00290         /* 1) Setup QP data. */
00291         if (setupQPdata(_H, 0, _g, _lb, _ub) != SUCCESSFUL_RETURN)
00292                 return THROWERROR( RET_INVALID_ARGUMENTS );
00293 
00294         /* 2) Call to main initialisation routine (without any additional information). */
00295         return solveInitialQP(0, yOpt, 0, nWSR, cputime);
00296 }
00297 
00298 returnValue QProblemB::init(    const real_t* const _H, const real_t* const _R, const real_t* const _g,
00299                                                                 const real_t* const _lb, const real_t* const _ub,
00300                                                                 int& nWSR, const real_t* const yOpt, real_t* const cputime
00301                                                                 )
00302 {
00303         /* 1) Setup QP data. */
00304         if (setupQPdata(_H, _R, _g, _lb, _ub) != SUCCESSFUL_RETURN)
00305                 return THROWERROR( RET_INVALID_ARGUMENTS );
00306 
00307         /* 2) Call to main initialisation routine (without any additional information). */
00308         return solveInitialQP(0, yOpt, 0, nWSR, cputime);
00309 }
00310 
00311 
00312 /*
00313  *      h o t s t a r t
00314  */
00315 returnValue QProblemB::hotstart(        const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,
00316                                                                         int& nWSR, real_t* const cputime
00317                                                                         )
00318 {
00319         int l;
00320 
00321         /* consistency check */
00322         if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
00323                  ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
00324                  ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
00325         {
00326                 return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
00327         }
00328 
00329         /* start runtime measurement */
00330         real_t starttime = 0.0;
00331         if ( cputime != 0 )
00332                 starttime = getCPUtime( );
00333 
00334 
00335         /* I) PREPARATIONS */
00336         /* 1) Reset status flags and increase QP counter. */
00337         infeasible = BT_FALSE;
00338         unbounded = BT_FALSE;
00339 
00340         ++count;
00341 
00342         /* 2) Allocate delta vectors of gradient and bounds. */
00343         returnValue returnvalue;
00344         BooleanType Delta_bB_isZero;
00345 
00346         int FR_idx[NVMAX];
00347         int FX_idx[NVMAX];
00348 
00349         real_t delta_g[NVMAX];
00350         real_t delta_lb[NVMAX];
00351         real_t delta_ub[NVMAX];
00352 
00353         real_t delta_xFR[NVMAX];
00354         real_t delta_xFX[NVMAX];
00355         real_t delta_yFX[NVMAX];
00356 
00357         int BC_idx;
00358         SubjectToStatus BC_status;
00359 
00360         #ifdef PC_DEBUG
00361         char messageString[80];
00362         #endif
00363 
00364         /* II) MAIN HOMOTOPY LOOP */
00365         for( l=0; l<nWSR; ++l )
00366         {
00367                 status = QPS_PERFORMINGHOMOTOPY;
00368 
00369                 if ( printlevel == PL_HIGH )
00370                 {
00371                         #ifdef PC_DEBUG
00372                         sprintf( messageString,"%d ...",l );
00373                         getGlobalMessageHandler( )->throwInfo( RET_ITERATION_STARTED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00374                         #endif
00375                 }
00376 
00377                 /* 1) Setup index arrays. */
00378                 if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
00379                         return THROWERROR( RET_HOTSTART_FAILED );
00380 
00381                 if ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
00382                         return THROWERROR( RET_HOTSTART_FAILED );
00383 
00384                 /* 2) Initialize shift direction of the gradient and the bounds. */
00385                 returnvalue = hotstart_determineDataShift(  FX_idx,
00386                                                                                                         g_new,lb_new,ub_new,
00387                                                                                                         delta_g,delta_lb,delta_ub,
00388                                                                                                         Delta_bB_isZero
00389                                                                                                         );
00390                 if ( returnvalue != SUCCESSFUL_RETURN )
00391                 {
00392                         nWSR = l;
00393                         THROWERROR( RET_SHIFT_DETERMINATION_FAILED );
00394                         return returnvalue;
00395                 }
00396 
00397                 /* 3) Determination of step direction of X and Y. */
00398                 returnvalue = hotstart_determineStepDirection(  FR_idx,FX_idx,
00399                                                                                                                 delta_g,delta_lb,delta_ub,
00400                                                                                                                 Delta_bB_isZero,
00401                                                                                                                 delta_xFX,delta_xFR,delta_yFX
00402                                                                                                                 );
00403                 if ( returnvalue != SUCCESSFUL_RETURN )
00404                 {
00405                         nWSR = l;
00406                         THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
00407                         return returnvalue;
00408                 }
00409 
00410 
00411                 /* 4) Determination of step length TAU. */
00412                 returnvalue = hotstart_determineStepLength(     FR_idx,FX_idx,
00413                                                                                                         delta_lb,delta_ub,
00414                                                                                                         delta_xFR,delta_yFX,
00415                                                                                                         BC_idx,BC_status );
00416                 if ( returnvalue != SUCCESSFUL_RETURN )
00417                 {
00418                         nWSR = l;
00419                         THROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED );
00420                         return returnvalue;
00421                 }
00422 
00423                 /* 5) Realization of the homotopy step. */
00424                 returnvalue = hotstart_performStep(     FR_idx,FX_idx,
00425                                                                                         delta_g,delta_lb,delta_ub,
00426                                                                                         delta_xFX,delta_xFR,delta_yFX,
00427                                                                                         BC_idx,BC_status
00428                                                                                         );
00429 
00430 
00431                 if ( returnvalue != SUCCESSFUL_RETURN )
00432                 {
00433                         nWSR = l;
00434 
00435                         /* stop runtime measurement */
00436                         if ( cputime != 0 )
00437                                 *cputime = getCPUtime( ) - starttime;
00438 
00439                         /* optimal solution found? */
00440                         if ( returnvalue == RET_OPTIMAL_SOLUTION_FOUND )
00441                         {
00442                                 status = QPS_SOLVED;
00443 
00444                                 if ( printlevel == PL_HIGH )
00445                                         THROWINFO( RET_OPTIMAL_SOLUTION_FOUND );
00446 
00447                                 #ifdef PC_DEBUG
00448                                 if ( printIteration( l,BC_idx,BC_status ) != SUCCESSFUL_RETURN )
00449                                         THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
00450                                 #endif
00451 
00452                                 /* check KKT optimality conditions */
00453                                 return checkKKTconditions( );
00454                         }
00455                         else
00456                         {
00457                                 /* checks for infeasibility... */
00458                                 if ( infeasible == BT_TRUE )
00459                                 {
00460                                         status = QPS_HOMOTOPYQPSOLVED;
00461                                         return THROWERROR( RET_HOTSTART_STOPPED_INFEASIBILITY );
00462                                 }
00463 
00464                                 /* ...unboundedness... */
00465                                 if ( unbounded == BT_TRUE ) /* not necessary since objective function convex! */
00466                                         return THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS );
00467 
00468                                 /* ... and throw unspecific error otherwise */
00469                                 THROWERROR( RET_HOMOTOPY_STEP_FAILED );
00470                                 return returnvalue;
00471                         }
00472                 }
00473 
00474                 /* 6) Output information of successful QP iteration. */
00475                 status = QPS_HOMOTOPYQPSOLVED;
00476 
00477                 #ifdef PC_DEBUG
00478                 if ( printIteration( l,BC_idx,BC_status ) != SUCCESSFUL_RETURN )
00479                         THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
00480                 #endif
00481         }
00482 
00483 
00484         /* stop runtime measurement */
00485         if ( cputime != 0 )
00486                 *cputime = getCPUtime( ) - starttime;
00487 
00488 
00489         /* if programm gets to here, output information that QP could not be solved
00490          * within the given maximum numbers of working set changes */
00491         if ( printlevel == PL_HIGH )
00492         {
00493                 #ifdef PC_DEBUG
00494                 sprintf( messageString,"(nWSR = %d)",nWSR );
00495                 return getGlobalMessageHandler( )->throwWarning( RET_MAX_NWSR_REACHED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00496                 #endif
00497         }
00498 
00499         /* Finally check KKT optimality conditions. */
00500         returnValue returnvalueKKTcheck = checkKKTconditions( );
00501 
00502         if ( returnvalueKKTcheck != SUCCESSFUL_RETURN )
00503                 return returnvalueKKTcheck;
00504         else
00505                 return RET_MAX_NWSR_REACHED;
00506 }
00507 
00508 
00509 /*
00510  *      g e t N Z
00511  */
00512 int QProblemB::getNZ( )
00513 {
00514         /* if no constraints are present: nZ=nFR */
00515         return bounds.getFree( )->getLength( );
00516 }
00517 
00518 
00519 /*
00520  *      g e t O b j V a l
00521  */
00522 real_t QProblemB::getObjVal( ) const
00523 {
00524         real_t objVal;
00525 
00526         /* calculated optimal objective function value
00527          * only if current QP has been solved */
00528         if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
00529                  ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
00530                  ( getStatus( ) == QPS_SOLVED ) )
00531         {
00532                 objVal = getObjVal( x );
00533         }
00534         else
00535         {
00536                 objVal = INFTY;
00537         }
00538 
00539         return objVal;
00540 }
00541 
00542 
00543 /*
00544  *      g e t O b j V a l
00545  */
00546 real_t QProblemB::getObjVal( const real_t* const _x ) const
00547 {
00548         int i, j;
00549         int nV = getNV( );
00550 
00551         real_t obj_tmp = 0.0;
00552 
00553         for( i=0; i<nV; ++i )
00554         {
00555                 obj_tmp += _x[i]*g[i];
00556 
00557                 for( j=0; j<nV; ++j )
00558                         obj_tmp += 0.5*_x[i]*H[i*NVMAX + j]*_x[j];
00559         }
00560 
00561         return obj_tmp;
00562 }
00563 
00564 
00565 /*
00566  *      g e t P r i m a l S o l u t i o n
00567  */
00568 returnValue QProblemB::getPrimalSolution( real_t* const xOpt ) const
00569 {
00570         int i;
00571 
00572         /* return optimal primal solution vector
00573          * only if current QP has been solved */
00574         if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
00575                  ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
00576                  ( getStatus( ) == QPS_SOLVED ) )
00577         {
00578                 for( i=0; i<getNV( ); ++i )
00579                         xOpt[i] = x[i];
00580 
00581                 return SUCCESSFUL_RETURN;
00582         }
00583         else
00584         {
00585                 return RET_QP_NOT_SOLVED;
00586         }
00587 }
00588 
00589 
00590 /*
00591  *      g e t D u a l S o l u t i o n
00592  */
00593 returnValue QProblemB::getDualSolution( real_t* const yOpt ) const
00594 {
00595         int i;
00596 
00597         /* return optimal dual solution vector
00598          * only if current QP has been solved */
00599         if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
00600                  ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
00601                  ( getStatus( ) == QPS_SOLVED ) )
00602         {
00603                 for( i=0; i<getNV( ); ++i )
00604                         yOpt[i] = y[i];
00605 
00606                 return SUCCESSFUL_RETURN;
00607         }
00608         else
00609         {
00610                 return RET_QP_NOT_SOLVED;
00611         }
00612 }
00613 
00614 
00615 /*
00616  *      s e t P r i n t L e v e l
00617  */
00618 returnValue QProblemB::setPrintLevel( PrintLevel _printlevel )
00619 {
00620         #ifndef __MATLAB__
00621         if ( ( printlevel >= PL_MEDIUM ) && ( printlevel != _printlevel ) )
00622                 THROWINFO( RET_PRINTLEVEL_CHANGED );
00623         #endif
00624 
00625         printlevel = _printlevel;
00626 
00627         /* update message handler preferences */
00628         switch ( printlevel )
00629         {
00630                 case PL_NONE:
00631                         getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_HIDDEN );
00632                         getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_HIDDEN );
00633                         getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN );
00634                         break;
00635 
00636                 case PL_LOW:
00637                         getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE );
00638                         getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_HIDDEN );
00639                         getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN );
00640                         break;
00641 
00642                 default: /* PL_MEDIUM, PL_HIGH */
00643                         getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE );
00644                         getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_VISIBLE );
00645                         getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_VISIBLE );
00646                         break;
00647         }
00648 
00649         return SUCCESSFUL_RETURN;
00650 }
00651 
00652 
00653 
00654 /*****************************************************************************
00655  *  P R O T E C T E D                                                        *
00656  *****************************************************************************/
00657 
00658 /*
00659  *      c h e c k F o r I d e n t i t y H e s s i a n
00660  */
00661 returnValue QProblemB::checkForIdentityHessian( )
00662 {
00663         int i, j;
00664         int nV = getNV( );
00665 
00666         /* nothing to do as status flag remains unaltered
00667          * if Hessian differs from identity matrix */
00668         if ( hessianType == HST_IDENTITY )
00669                 return SUCCESSFUL_RETURN;
00670 
00671         /* 1) If Hessian differs from identity matrix,
00672          *    return without changing the internal HessianType. */
00673         for ( i=0; i<nV; ++i )
00674                 if ( getAbs( H[i*NVMAX + i] - 1.0 ) > EPS )
00675                         return SUCCESSFUL_RETURN;
00676 
00677         for ( i=0; i<nV; ++i )
00678         {
00679                 for ( j=0; j<i; ++j )
00680                         if ( ( getAbs( H[i*NVMAX + j] ) > EPS ) || ( getAbs( H[j*NVMAX + i] ) > EPS ) )
00681                                 return SUCCESSFUL_RETURN;
00682         }
00683 
00684         /* 2) If this point is reached, Hessian equals the idetity matrix. */
00685         hessianType = HST_IDENTITY;
00686 
00687         return SUCCESSFUL_RETURN;
00688 }
00689 
00690 
00691 /*
00692  *      s e t u p S u b j e c t T o T y p e
00693  */
00694 returnValue QProblemB::setupSubjectToType( )
00695 {
00696         int i;
00697         int nV = getNV( );
00698 
00699 
00700         /* 1) Check if lower bounds are present. */
00701         bounds.setNoLower( BT_TRUE );
00702         for( i=0; i<nV; ++i )
00703                 if ( lb[i] > -INFTY )
00704                 {
00705                         bounds.setNoLower( BT_FALSE );
00706                         break;
00707                 }
00708 
00709         /* 2) Check if upper bounds are present. */
00710         bounds.setNoUpper( BT_TRUE );
00711         for( i=0; i<nV; ++i )
00712                 if ( ub[i] < INFTY )
00713                 {
00714                         bounds.setNoUpper( BT_FALSE );
00715                         break;
00716                 }
00717 
00718         /* 3) Determine implicitly fixed and unbounded variables. */
00719         int nFV = 0;
00720         int nUV = 0;
00721 
00722         for( i=0; i<nV; ++i )
00723                 if ( ( lb[i] < -INFTY + BOUNDTOL ) && ( ub[i] > INFTY - BOUNDTOL ) )
00724                 {
00725                         bounds.setType( i,ST_UNBOUNDED );
00726                         ++nUV;
00727                 }
00728                 else
00729                 {
00730                         if ( lb[i] > ub[i] - BOUNDTOL )
00731                         {
00732                                 bounds.setType( i,ST_EQUALITY );
00733                                 ++nFV;
00734                         }
00735                         else
00736                         {
00737                                 bounds.setType( i,ST_BOUNDED );
00738                         }
00739                 }
00740 
00741         /* 4) Set dimensions of bounds structure. */
00742         bounds.setNFV( nFV );
00743         bounds.setNUV( nUV );
00744         bounds.setNBV( nV - nFV - nUV );
00745 
00746         return SUCCESSFUL_RETURN;
00747 }
00748 
00749 
00750 /*
00751  *      c h o l e s k y D e c o m p o s i t i o n
00752  */
00753 returnValue QProblemB::setupCholeskyDecomposition( )
00754 {
00755         int i, j, k, ii, jj;
00756         int nV  = getNV( );
00757         int nFR = getNFR( );
00758 
00759         /* If Hessian flag is false, it means that H & R already contain Cholesky
00760          * factorization -- provided from outside. */
00761         if (hasHessian == BT_FALSE)
00762                 return SUCCESSFUL_RETURN;
00763 
00764         /* 1) Initialises R with all zeros. */
00765         for( i=0; i<nV; ++i )
00766                 for( j=0; j<nV; ++j )
00767                         R[i*NVMAX + j] = 0.0;
00768 
00769         /* 2) Calculate Cholesky decomposition of H (projected to free variables). */
00770         if ( hessianType == HST_IDENTITY )
00771         {
00772                 /* if Hessian is identity, so is its Cholesky factor. */
00773                 for( i=0; i<nFR; ++i )
00774                         R[i*NVMAX + i] = 1.0;
00775         }
00776         else
00777         {
00778                 if ( nFR > 0 )
00779                 {
00780                         int FR_idx[NVMAX];
00781                         if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
00782                                 return THROWERROR( RET_INDEXLIST_CORRUPTED );
00783 
00784                         /* R'*R = H */
00785                         real_t sum;
00786                         real_t inv;
00787 
00788                         for( i=0; i<nFR; ++i )
00789                         {
00790                                 /* j == i */
00791                                 ii = FR_idx[i];
00792                                 sum = H[ii*NVMAX + ii];
00793 
00794                                 for( k=(i-1); k>=0; --k )
00795                                         sum -= R[k*NVMAX + i] * R[k*NVMAX + i];
00796 
00797                                 if ( sum > 0.0 )
00798                                 {
00799                                         R[i*NVMAX + i] = sqrt( sum );
00800                                         inv = 1.0 / R[i*NVMAX + i];
00801                                 }
00802                                 else
00803                                 {
00804                                         hessianType = HST_SEMIDEF;
00805                                         return THROWERROR( RET_HESSIAN_NOT_SPD );
00806                                 }
00807 
00808                                 /* j > i */
00809                                 for( j=(i+1); j<nFR; ++j )
00810                                 {
00811                                         jj = FR_idx[j];
00812                                         sum = H[jj*NVMAX + ii];
00813 
00814                                         for( k=(i-1); k>=0; --k )
00815                                                 sum -= R[k*NVMAX + i] * R[k*NVMAX + j];
00816 
00817                                         R[i*NVMAX + j] = sum * inv;
00818                                 }
00819                         }
00820                 }
00821         }
00822 
00823         return SUCCESSFUL_RETURN;
00824 }
00825 
00826 
00827 /*
00828  *      s o l v e I n i t i a l Q P
00829  */
00830 returnValue QProblemB::solveInitialQP(  const real_t* const xOpt, const real_t* const yOpt,
00831                                                                                 const Bounds* const guessedBounds,
00832                                                                                 int& nWSR, real_t* const cputime
00833                                                                                 )
00834 {
00835         int i, nFR;
00836         int nV = getNV( );
00837 
00838 
00839         /* start runtime measurement */
00840         real_t starttime = 0.0;
00841         if ( cputime != 0 )
00842                 starttime = getCPUtime( );
00843 
00844 
00845         status = QPS_NOTINITIALISED;
00846 
00847         /* I) ANALYSE QP DATA: */
00848         /* 1) Check if Hessian happens to be the identity matrix. */
00849         if ( checkForIdentityHessian( ) != SUCCESSFUL_RETURN )
00850                 return THROWERROR( RET_INIT_FAILED );
00851 
00852         /* 2) Setup type of bounds (i.e. unbounded, implicitly fixed etc.). */
00853         if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
00854                 return THROWERROR( RET_INIT_FAILED );
00855 
00856         status = QPS_PREPARINGAUXILIARYQP;
00857 
00858 
00859         /* II) SETUP AUXILIARY QP WITH GIVEN OPTIMAL SOLUTION: */
00860         /* 1) Setup bounds data structure. */
00861         if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
00862                 return THROWERROR( RET_INIT_FAILED );
00863 
00864         /* 2) Setup optimal primal/dual solution for auxiliary QP. */
00865         if ( setupAuxiliaryQPsolution( xOpt,yOpt ) != SUCCESSFUL_RETURN )
00866                 return THROWERROR( RET_INIT_FAILED );
00867 
00868         /* 3) Obtain linear independent working set for auxiliary QP. */
00869 
00870         static Bounds auxiliaryBounds;
00871 
00872         auxiliaryBounds.init( nV );
00873 
00874         if ( obtainAuxiliaryWorkingSet( xOpt,yOpt,guessedBounds, &auxiliaryBounds ) != SUCCESSFUL_RETURN )
00875                 return THROWERROR( RET_INIT_FAILED );
00876 
00877         /* 4) Setup working set of auxiliary QP and setup cholesky decomposition. */
00878         if ( setupAuxiliaryWorkingSet( &auxiliaryBounds,BT_TRUE ) != SUCCESSFUL_RETURN )
00879                 return THROWERROR( RET_INIT_FAILED );
00880 
00881         nFR = getNFR();
00882         /* At the moment we can only provide a Cholesky of the Hessian if
00883          * the solver is cold-started. */
00884         if (hasCholesky == BT_FALSE || nFR != nV)
00885                 if (setupCholeskyDecomposition() != SUCCESSFUL_RETURN)
00886                         return THROWERROR( RET_INIT_FAILED_CHOLESKY );
00887 
00888         /* 5) Store original QP formulation... */
00889         real_t g_original[NVMAX];
00890         real_t lb_original[NVMAX];
00891         real_t ub_original[NVMAX];
00892 
00893         for( i=0; i<nV; ++i )
00894                 g_original[i] = g[i];
00895         for( i=0; i<nV; ++i )
00896                 lb_original[i] = lb[i];
00897         for( i=0; i<nV; ++i )
00898                 ub_original[i] = ub[i];
00899 
00900         /* ... and setup QP data of an auxiliary QP having an optimal solution
00901          * as specified by the user (or xOpt = yOpt = 0, by default). */
00902         if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
00903                 return THROWERROR( RET_INIT_FAILED );
00904 
00905         if ( setupAuxiliaryQPbounds( BT_TRUE ) != SUCCESSFUL_RETURN )
00906                 return THROWERROR( RET_INIT_FAILED );
00907 
00908         status = QPS_AUXILIARYQPSOLVED;
00909 
00910 
00911         /* III) SOLVE ACTUAL INITIAL QP: */
00912         /* Use hotstart method to find the solution of the original initial QP,... */
00913         returnValue returnvalue = hotstart( g_original,lb_original,ub_original, nWSR,0 );
00914 
00915 
00916         /* ... check for infeasibility and unboundedness... */
00917         if ( isInfeasible( ) == BT_TRUE )
00918                 return THROWERROR( RET_INIT_FAILED_INFEASIBILITY );
00919 
00920         if ( isUnbounded( ) == BT_TRUE )
00921                 return THROWERROR( RET_INIT_FAILED_UNBOUNDEDNESS );
00922 
00923         /* ... and internal errors. */
00924         if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED )  &&
00925              ( returnvalue != RET_INACCURATE_SOLUTION ) && ( returnvalue != RET_NO_SOLUTION ) )
00926                 return THROWERROR( RET_INIT_FAILED_HOTSTART );
00927 
00928 
00929         /* stop runtime measurement */
00930         if ( cputime != 0 )
00931                 *cputime = getCPUtime( ) - starttime;
00932 
00933         if ( printlevel == PL_HIGH )
00934                 THROWINFO( RET_INIT_SUCCESSFUL );
00935 
00936         return returnvalue;
00937 }
00938 
00939 
00940 /*
00941  *      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
00942  */
00943 returnValue QProblemB::obtainAuxiliaryWorkingSet(       const real_t* const xOpt, const real_t* const yOpt,
00944                                                                                                         const Bounds* const guessedBounds, Bounds* auxiliaryBounds
00945                                                                                                         ) const
00946 {
00947         int i = 0;
00948         int nV = getNV( );
00949 
00950 
00951         /* 1) Ensure that desiredBounds is allocated (and different from guessedBounds). */
00952         if ( ( auxiliaryBounds == 0 ) || ( auxiliaryBounds == guessedBounds ) )
00953                 return THROWERROR( RET_INVALID_ARGUMENTS );
00954 
00955 
00956         /* 2) Setup working set for auxiliary initial QP. */
00957         if ( guessedBounds != 0 )
00958         {
00959                 /* If an initial working set is specific, use it!
00960                  * Moreover, add all implictly fixed variables if specified. */
00961                 for( i=0; i<nV; ++i )
00962                 {
00963                         if ( bounds.getType( i ) == ST_EQUALITY )
00964                         {
00965                                 if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
00966                                         return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
00967                         }
00968                         else
00969                         {
00970                                 if ( auxiliaryBounds->setupBound( i,guessedBounds->getStatus( i ) ) != SUCCESSFUL_RETURN )
00971                                         return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
00972                         }
00973                 }
00974         }
00975         else    /* No initial working set specified. */
00976         {
00977                 if ( ( xOpt != 0 ) && ( yOpt == 0 ) )
00978                 {
00979                         /* Obtain initial working set by "clipping". */
00980                         for( i=0; i<nV; ++i )
00981                         {
00982                                 if ( xOpt[i] <= lb[i] + BOUNDTOL )
00983                                 {
00984                                         if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
00985                                                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
00986                                         continue;
00987                                 }
00988 
00989                                 if ( xOpt[i] >= ub[i] - BOUNDTOL )
00990                                 {
00991                                         if ( auxiliaryBounds->setupBound( i,ST_UPPER ) != SUCCESSFUL_RETURN )
00992                                                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
00993                                         continue;
00994                                 }
00995 
00996                                 /* Moreover, add all implictly fixed variables if specified. */
00997                                 if ( bounds.getType( i ) == ST_EQUALITY )
00998                                 {
00999                                         if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
01000                                                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01001                                 }
01002                                 else
01003                                 {
01004                                         if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
01005                                                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01006                                 }
01007                         }
01008                 }
01009 
01010                 if ( ( xOpt == 0 ) && ( yOpt != 0 ) )
01011                 {
01012                         /* Obtain initial working set in accordance to sign of dual solution vector. */
01013                         for( i=0; i<nV; ++i )
01014                         {
01015                                 if ( yOpt[i] > ZERO )
01016                                 {
01017                                         if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
01018                                                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01019                                         continue;
01020                                 }
01021 
01022                                 if ( yOpt[i] < -ZERO )
01023                                 {
01024                                         if ( auxiliaryBounds->setupBound( i,ST_UPPER ) != SUCCESSFUL_RETURN )
01025                                                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01026                                         continue;
01027                                 }
01028 
01029                                 /* Moreover, add all implictly fixed variables if specified. */
01030                                 if ( bounds.getType( i ) == ST_EQUALITY )
01031                                 {
01032                                         if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
01033                                                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01034                                 }
01035                                 else
01036                                 {
01037                                         if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
01038                                                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01039                                 }
01040                         }
01041                 }
01042 
01043                 /* If xOpt and yOpt are null pointer and no initial working is specified,
01044                  * start with empty working set (or implicitly fixed bounds only)
01045                  * for auxiliary QP. */
01046                 if ( ( xOpt == 0 ) && ( yOpt == 0 ) )
01047                 {
01048                         for( i=0; i<nV; ++i )
01049                         {
01050                                 /* Only add all implictly fixed variables if specified. */
01051                                 if ( bounds.getType( i ) == ST_EQUALITY )
01052                                 {
01053                                         if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
01054                                                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01055                                 }
01056                                 else
01057                                 {
01058                                         if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
01059                                                 return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
01060                                 }
01061                         }
01062                 }
01063         }
01064 
01065         return SUCCESSFUL_RETURN;
01066 }
01067 
01068 
01069 /*
01070  *      s e t u p A u x i l i a r y W o r k i n g S e t
01071  */
01072 returnValue QProblemB::setupAuxiliaryWorkingSet(        const Bounds* const auxiliaryBounds,
01073                                                                                                         BooleanType setupAfresh
01074                                                                                                         )
01075 {
01076         int i;
01077         int nV = getNV( );
01078 
01079         /* consistency checks */
01080         if ( auxiliaryBounds != 0 )
01081         {
01082                 for( i=0; i<nV; ++i )
01083                         if ( ( bounds.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryBounds->getStatus( i ) == ST_UNDEFINED ) )
01084                                 return THROWERROR( RET_UNKNOWN_BUG );
01085         }
01086         else
01087         {
01088                 return THROWERROR( RET_INVALID_ARGUMENTS );
01089         }
01090 
01091 
01092         /* I) SETUP CHOLESKY FLAG:
01093          *    Cholesky decomposition shall only be updated if working set
01094          *    shall be updated (i.e. NOT setup afresh!) */
01095         BooleanType updateCholesky;
01096         if ( setupAfresh == BT_TRUE )
01097                 updateCholesky = BT_FALSE;
01098         else
01099                 updateCholesky = BT_TRUE;
01100 
01101 
01102         /* II) REMOVE FORMERLY ACTIVE BOUNDS (IF NECESSARY): */
01103         if ( setupAfresh == BT_FALSE )
01104         {
01105                 /* Remove all active bounds that shall be inactive AND
01106                 *  all active bounds that are active at the wrong bound. */
01107                 for( i=0; i<nV; ++i )
01108                 {
01109                         if ( ( bounds.getStatus( i ) == ST_LOWER ) && ( auxiliaryBounds->getStatus( i ) != ST_LOWER ) )
01110                                 if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )
01111                                         return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
01112 
01113                         if ( ( bounds.getStatus( i ) == ST_UPPER ) && ( auxiliaryBounds->getStatus( i ) != ST_UPPER ) )
01114                                 if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )
01115                                         return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
01116                 }
01117         }
01118 
01119 
01120         /* III) ADD NEWLY ACTIVE BOUNDS: */
01121         /*      Add all inactive bounds that shall be active AND
01122          *      all formerly active bounds that have been active at the wrong bound. */
01123         for( i=0; i<nV; ++i )
01124         {
01125                 if ( ( bounds.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE ) )
01126                 {
01127                         if ( addBound( i,auxiliaryBounds->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
01128                                 return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
01129                 }
01130         }
01131 
01132         return SUCCESSFUL_RETURN;
01133 }
01134 
01135 
01136 /*
01137  *      s e t u p A u x i l i a r y Q P s o l u t i o n
01138  */
01139 returnValue QProblemB::setupAuxiliaryQPsolution(        const real_t* const xOpt, const real_t* const yOpt
01140                                                                                                         )
01141 {
01142         int i;
01143         int nV = getNV( );
01144 
01145 
01146         /* Setup primal/dual solution vectors for auxiliary initial QP:
01147          * if a null pointer is passed, a zero vector is assigned;
01148          * old solution vector is kept if pointer to internal solution vector is passed. */
01149         if ( xOpt != 0 )
01150         {
01151                 if ( xOpt != x )
01152                         for( i=0; i<nV; ++i )
01153                                 x[i] = xOpt[i];
01154         }
01155         else
01156         {
01157                 for( i=0; i<nV; ++i )
01158                         x[i] = 0.0;
01159         }
01160 
01161         if ( yOpt != 0 )
01162         {
01163                 if ( yOpt != y )
01164                         for( i=0; i<nV; ++i )
01165                                 y[i] = yOpt[i];
01166         }
01167         else
01168         {
01169                 for( i=0; i<nV; ++i )
01170                         y[i] = 0.0;
01171         }
01172 
01173         return SUCCESSFUL_RETURN;
01174 }
01175 
01176 
01177 /*
01178  *      s e t u p A u x i l i a r y Q P g r a d i e n t
01179  */
01180 returnValue QProblemB::setupAuxiliaryQPgradient( )
01181 {
01182         int i, j;
01183         int nV = getNV( );
01184 
01185 
01186         /* Setup gradient vector: g = -H*x + y'*Id. */
01187         for ( i=0; i<nV; ++i )
01188         {
01189                 /* y'*Id */
01190                 g[i] = y[i];
01191 
01192                 /* -H*x */
01193                 for ( j=0; j<nV; ++j )
01194                         g[i] -= H[i*NVMAX + j] * x[j];
01195         }
01196 
01197         return SUCCESSFUL_RETURN;
01198 }
01199 
01200 
01201 /*
01202  *      s e t u p A u x i l i a r y Q P b o u n d s
01203  */
01204 returnValue QProblemB::setupAuxiliaryQPbounds( BooleanType useRelaxation )
01205 {
01206         int i;
01207         int nV = getNV( );
01208 
01209 
01210         /* Setup bound vectors. */
01211         for ( i=0; i<nV; ++i )
01212         {
01213                 switch ( bounds.getStatus( i ) )
01214                 {
01215                         case ST_INACTIVE:
01216                                 if ( useRelaxation == BT_TRUE )
01217                                 {
01218                                         if ( bounds.getType( i ) == ST_EQUALITY )
01219                                         {
01220                                                 lb[i] = x[i];
01221                                                 ub[i] = x[i];
01222                                         }
01223                                         else
01224                                         {
01225                                                 lb[i] = x[i] - BOUNDRELAXATION;
01226                                                 ub[i] = x[i] + BOUNDRELAXATION;
01227                                         }
01228                                 }
01229                                 break;
01230 
01231                         case ST_LOWER:
01232                                 lb[i] = x[i];
01233                                 if ( bounds.getType( i ) == ST_EQUALITY )
01234                                 {
01235                                         ub[i] = x[i];
01236                                 }
01237                                 else
01238                                 {
01239                                         if ( useRelaxation == BT_TRUE )
01240                                                 ub[i] = x[i] + BOUNDRELAXATION;
01241                                 }
01242                                 break;
01243 
01244                         case ST_UPPER:
01245                                 ub[i] = x[i];
01246                                 if ( bounds.getType( i ) == ST_EQUALITY )
01247                                 {
01248                                         lb[i] = x[i];
01249                                 }
01250                                 else
01251                                 {
01252                                         if ( useRelaxation == BT_TRUE )
01253                                                 lb[i] = x[i] - BOUNDRELAXATION;
01254                                 }
01255                                 break;
01256 
01257                         default:
01258                                 return THROWERROR( RET_UNKNOWN_BUG );
01259                 }
01260         }
01261 
01262         return SUCCESSFUL_RETURN;
01263 }
01264 
01265 
01266 /*
01267  *      a d d B o u n d
01268  */
01269 returnValue QProblemB::addBound(        int number, SubjectToStatus B_status,
01270                                                                         BooleanType updateCholesky
01271                                                                         )
01272 {
01273         int i, j;
01274         int nFR = getNFR( );
01275 
01276 
01277         /* consistency check */
01278         if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
01279                  ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
01280                  ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
01281                  ( getStatus( ) == QPS_SOLVED )            )
01282         {
01283                 return THROWERROR( RET_UNKNOWN_BUG );
01284         }
01285 
01286         /* Perform cholesky updates only if QProblemB has been initialised! */
01287         if ( ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) || ( updateCholesky == BT_FALSE ) )
01288         {
01289                 /* UPDATE INDICES */
01290                 if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
01291                         return THROWERROR( RET_ADDBOUND_FAILED );
01292 
01293                 return SUCCESSFUL_RETURN;
01294         }
01295 
01296 
01297         /* I) PERFORM CHOLESKY UPDATE: */
01298         /* 1) Index of variable to be added within the list of free variables. */
01299         int number_idx = bounds.getFree( )->getIndex( number );
01300 
01301         real_t c, s;
01302 
01303         /* 2) Use row-wise Givens rotations to restore upper triangular form of R. */
01304         for( i=number_idx+1; i<nFR; ++i )
01305         {
01306                 computeGivens( R[(i-1)*NVMAX + i],R[i*NVMAX + i], R[(i-1)*NVMAX + i],R[i*NVMAX + i],c,s );
01307 
01308                 for( j=(1+i); j<nFR; ++j ) /* last column of R is thrown away */
01309                         applyGivens( c,s,R[(i-1)*NVMAX + j],R[i*NVMAX + j], R[(i-1)*NVMAX + j],R[i*NVMAX + j] );
01310         }
01311 
01312         /* 3) Delete <number_idx>th column and ... */
01313         for( i=0; i<nFR-1; ++i )
01314                 for( j=number_idx+1; j<nFR; ++j )
01315                         R[i*NVMAX + j-1] = R[i*NVMAX + j];
01316         /* ... last column of R. */
01317         for( i=0; i<nFR; ++i )
01318                 R[i*NVMAX + nFR-1] = 0.0;
01319 
01320 
01321         /* II) UPDATE INDICES */
01322         if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
01323                 return THROWERROR( RET_ADDBOUND_FAILED );
01324 
01325 
01326         return SUCCESSFUL_RETURN;
01327 }
01328 
01329 
01330 returnValue QProblemB::removeBound(     int number,
01331                                                                         BooleanType updateCholesky
01332                                                                         )
01333 {
01334         int i, ii;
01335         int nFR = getNFR( );
01336 
01337 
01338         /* consistency check */
01339         if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
01340                  ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
01341                  ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
01342                  ( getStatus( ) == QPS_SOLVED )            )
01343         {
01344                 return THROWERROR( RET_UNKNOWN_BUG );
01345         }
01346 
01347 
01348         /* I) UPDATE INDICES */
01349         if ( bounds.moveFixedToFree( number ) != SUCCESSFUL_RETURN )
01350                 return THROWERROR( RET_REMOVEBOUND_FAILED );
01351 
01352         /* Perform cholesky updates only if QProblemB has been initialised! */
01353         if ( ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) || ( updateCholesky == BT_FALSE ) )
01354                 return SUCCESSFUL_RETURN;
01355 
01356 
01357         /* II) PERFORM CHOLESKY UPDATE */
01358         int FR_idx[NVMAX];
01359         if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
01360                 return THROWERROR( RET_REMOVEBOUND_FAILED );
01361 
01362         /* 1) Calculate new column of cholesky decomposition. */
01363         real_t rhs[NVMAX];
01364         real_t r[NVMAX];
01365         real_t r0 = H[number*NVMAX + number];
01366 
01367         for( i=0; i<nFR; ++i )
01368         {
01369                 ii = FR_idx[i];
01370                 rhs[i] = H[number*NVMAX + ii];
01371         }
01372 
01373         if ( backsolveR( rhs,BT_TRUE,BT_TRUE,r ) != SUCCESSFUL_RETURN )
01374                 return THROWERROR( RET_REMOVEBOUND_FAILED );
01375 
01376         for( i=0; i<nFR; ++i )
01377                 r0 -= r[i]*r[i];
01378 
01379         /* 2) Store new column into R. */
01380         for( i=0; i<nFR; ++i )
01381                 R[i*NVMAX + nFR] = r[i];
01382 
01383         if ( r0 > 0.0 )
01384                 R[nFR*NVMAX + nFR] = sqrt( r0 );
01385         else
01386         {
01387                 hessianType = HST_SEMIDEF;
01388                 return THROWERROR( RET_HESSIAN_NOT_SPD );
01389         }
01390 
01391 
01392         return SUCCESSFUL_RETURN;
01393 }
01394 
01395 
01396 /*
01397  *      b a c k s o l v e R  (CODE DUPLICATED IN QProblem CLASS!!!)
01398  */
01399 returnValue QProblemB::backsolveR(      const real_t* const b, BooleanType transposed,
01400                                                                         real_t* const a
01401                                                                         )
01402 {
01403         /* Call standard backsolve procedure (i.e. removingBound == BT_FALSE). */
01404         return backsolveR( b,transposed,BT_FALSE,a );
01405 }
01406 
01407 
01408 /*
01409  *      b a c k s o l v e R  (CODE DUPLICATED IN QProblem CLASS!!!)
01410  */
01411 returnValue QProblemB::backsolveR(      const real_t* const b, BooleanType transposed,
01412                                                                         BooleanType removingBound,
01413                                                                         real_t* const a
01414                                                                         )
01415 {
01416         int i, j;
01417         int nR = getNZ( );
01418 
01419         real_t sum;
01420 
01421         /* if backsolve is called while removing a bound, reduce nZ by one. */
01422         if ( removingBound == BT_TRUE )
01423                 --nR;
01424 
01425         /* nothing to do */
01426         if ( nR <= 0 )
01427                 return SUCCESSFUL_RETURN;
01428 
01429 
01430         /* Solve Ra = b, where R might be transposed. */
01431         if ( transposed == BT_FALSE )
01432         {
01433                 /* solve Ra = b */
01434                 for( i=(nR-1); i>=0; --i )
01435                 {
01436                         sum = b[i];
01437                         for( j=(i+1); j<nR; ++j )
01438                                 sum -= R[i*NVMAX + j] * a[j];
01439 
01440                         if ( getAbs( R[i*NVMAX + i] ) > ZERO )
01441                                 a[i] = sum / R[i*NVMAX + i];
01442                         else
01443                                 return THROWERROR( RET_DIV_BY_ZERO );
01444                 }
01445         }
01446         else
01447         {
01448                 /* solve R^T*a = b */
01449                 for( i=0; i<nR; ++i )
01450                 {
01451                         sum = b[i];
01452 
01453                         for( j=0; j<i; ++j )
01454                                 sum -= R[j*NVMAX + i] * a[j];
01455 
01456                         if ( getAbs( R[i*NVMAX + i] ) > ZERO )
01457                                 a[i] = sum / R[i*NVMAX + i];
01458                         else
01459                                 return THROWERROR( RET_DIV_BY_ZERO );
01460                 }
01461         }
01462 
01463         return SUCCESSFUL_RETURN;
01464 }
01465 
01466 
01467 /*
01468  *      h o t s t a r t _ d e t e r m i n e D a t a S h i f t
01469  */
01470 returnValue QProblemB::hotstart_determineDataShift(     const int* const FX_idx,
01471                                                                                                         const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,
01472                                                                                                         real_t* const delta_g, real_t* const delta_lb, real_t* const delta_ub,
01473                                                                                                         BooleanType& Delta_bB_isZero
01474                                                                                                         )
01475 {
01476         int i, ii;
01477         int nV  = getNV( );
01478         int nFX = getNFX( );
01479 
01480 
01481         /* 1) Calculate shift directions. */
01482         for( i=0; i<nV; ++i )
01483                 delta_g[i]  = g_new[i]  - g[i];
01484 
01485         if ( lb_new != 0 )
01486         {
01487                 for( i=0; i<nV; ++i )
01488                         delta_lb[i] = lb_new[i] - lb[i];
01489         }
01490         else
01491         {
01492                 /* if no lower bounds exist, assume the new lower bounds to be -infinity */
01493                 for( i=0; i<nV; ++i )
01494                         delta_lb[i] = -INFTY - lb[i];
01495         }
01496 
01497         if ( ub_new != 0 )
01498         {
01499                 for( i=0; i<nV; ++i )
01500                         delta_ub[i] = ub_new[i] - ub[i];
01501         }
01502         else
01503         {
01504                 /* if no upper bounds exist, assume the new upper bounds to be infinity */
01505                 for( i=0; i<nV; ++i )
01506                         delta_ub[i] = INFTY - ub[i];
01507         }
01508 
01509         /* 2) Determine if active bounds are to be shifted. */
01510         Delta_bB_isZero = BT_TRUE;
01511 
01512         for ( i=0; i<nFX; ++i )
01513         {
01514                 ii = FX_idx[i];
01515 
01516                 if ( ( getAbs( delta_lb[ii] ) > EPS ) || ( getAbs( delta_ub[ii] ) > EPS ) )
01517                 {
01518                         Delta_bB_isZero = BT_FALSE;
01519                         break;
01520                 }
01521         }
01522 
01523         return SUCCESSFUL_RETURN;
01524 }
01525 
01526 
01527 /*
01528  *      a r e B o u n d s C o n s i s t e n t
01529  */
01530 BooleanType QProblemB::areBoundsConsistent(     const real_t* const delta_lb, const real_t* const delta_ub
01531                                                                                         ) const
01532 {
01533         int i;
01534 
01535         /* Check if delta_lb[i] is greater than delta_ub[i]
01536          * for a component i whose bounds are already (numerically) equal. */
01537         for( i=0; i<getNV( ); ++i )
01538                 if ( ( lb[i] > ub[i] - BOUNDTOL ) && ( delta_lb[i] > delta_ub[i] + EPS ) )
01539                         return BT_FALSE;
01540 
01541         return BT_TRUE;
01542 }
01543 
01544 
01545 /*
01546  *      s e t u p Q P d a t a
01547  */
01548 returnValue QProblemB::setupQPdata(     const real_t* const _H, const real_t* const _R, const real_t* const _g,
01549                                                                         const real_t* const _lb, const real_t* const _ub
01550                                                                         )
01551 {
01552         int i, j;
01553         int nV = getNV( );
01554 
01555         /* 1) Setup Hessian matrix and it's Cholesky factorization. */
01556         if (_H != 0)
01557         {
01558                 for( i=0; i<nV; ++i )
01559                         for( j=0; j<nV; ++j )
01560                                 H[i*NVMAX + j] = _H[i*nV + j];
01561                 hasHessian = BT_TRUE;
01562         }
01563         else
01564                 hasHessian = BT_FALSE;
01565 
01566         if (_R != 0)
01567         {
01568                 for( i=0; i<nV; ++i )
01569                         for( j=0; j<nV; ++j )
01570                                 R[i*NVMAX + j] = _R[i*nV + j];
01571                 hasCholesky = BT_TRUE;
01572 
01573                 /* If Hessian is not provided, store it's factorization in H, and that guy
01574                  * is going to be used for H * x products (R^T * R * x in this case). */
01575                 if (hasHessian == BT_FALSE)
01576                         for( i=0; i<nV; ++i )
01577                                 for( j=0; j<nV; ++j )
01578                                         H[i*NVMAX + j] = _R[i*nV + j];
01579         }
01580         else
01581                 hasCholesky = BT_FALSE;
01582 
01583         if (hasHessian == BT_FALSE && hasCholesky == BT_FALSE)
01584                 return THROWERROR( RET_INVALID_ARGUMENTS );
01585 
01586         /* 2) Setup gradient vector. */
01587         if ( _g == 0 )
01588                 return THROWERROR( RET_INVALID_ARGUMENTS );
01589 
01590         for( i=0; i<nV; ++i )
01591                 g[i] = _g[i];
01592 
01593         /* 3) Setup lower bounds vector. */
01594         if ( _lb != 0 )
01595         {
01596                 for( i=0; i<nV; ++i )
01597                         lb[i] = _lb[i];
01598         }
01599         else
01600         {
01601                 /* if no lower bounds are specified, set them to -infinity */
01602                 for( i=0; i<nV; ++i )
01603                         lb[i] = -INFTY;
01604         }
01605 
01606         /* 4) Setup upper bounds vector. */
01607         if ( _ub != 0 )
01608         {
01609                 for( i=0; i<nV; ++i )
01610                         ub[i] = _ub[i];
01611         }
01612         else
01613         {
01614                 /* if no upper bounds are specified, set them to infinity */
01615                 for( i=0; i<nV; ++i )
01616                         ub[i] = INFTY;
01617         }
01618 
01619         //printmatrix( "H",H,nV,nV );
01620         //printmatrix( "R",R,nV,nV );
01621         //printmatrix( "g",g,1,nV );
01622         //printmatrix( "lb",lb,1,nV );
01623         //printmatrix( "ub",ub,1,nV );
01624 
01625         return SUCCESSFUL_RETURN;
01626 }
01627 
01628 
01629 
01630 /*****************************************************************************
01631  *  P R I V A T E                                                            *
01632  *****************************************************************************/
01633 
01634 /*
01635  *      h o t s t a r t _ d e t e r m i n e S t e p D i r e c t i o n
01636  */
01637 returnValue QProblemB::hotstart_determineStepDirection( const int* const FR_idx, const int* const FX_idx,
01638                                                                                                                 const real_t* const delta_g, const real_t* const delta_lb, const real_t* const delta_ub,
01639                                                                                                                 BooleanType Delta_bB_isZero,
01640                                                                                                                 real_t* const delta_xFX, real_t* const delta_xFR,
01641                                                                                                                 real_t* const delta_yFX
01642                                                                                                                 )
01643 {
01644         int i, j, ii, jj;
01645         int nFR = getNFR( );
01646         int nFX = getNFX( );
01647 
01648 
01649         /* initialise auxiliary vectors */
01650         real_t HMX_delta_xFX[NVMAX];
01651         for( i=0; i<nFR; ++i )
01652                 HMX_delta_xFX[i] = 0.0;
01653 
01654 
01655         /* I) DETERMINE delta_xFX */
01656         if ( nFX > 0 )
01657         {
01658                 for( i=0; i<nFX; ++i )
01659                 {
01660                         ii = FX_idx[i];
01661 
01662                         if ( bounds.getStatus( ii ) == ST_LOWER )
01663                                 delta_xFX[i] = delta_lb[ii];
01664                         else
01665                                 delta_xFX[i] = delta_ub[ii];
01666                 }
01667         }
01668 
01669 
01670         /* II) DETERMINE delta_xFR */
01671         if ( nFR > 0 )
01672         {
01673                 /* auxiliary variables */
01674                 real_t delta_xFRz_TMP[NVMAX];
01675                 real_t delta_xFRz_RHS[NVMAX];
01676 
01677                 /* Determine delta_xFRz. */
01678                 if ( Delta_bB_isZero == BT_FALSE )
01679                 {
01680                         for( i=0; i<nFR; ++i )
01681                         {
01682                                 ii = FR_idx[i];
01683                                 for( j=0; j<nFX; ++j )
01684                                 {
01685                                         jj = FX_idx[j];
01686                                         HMX_delta_xFX[i] += H[ii*NVMAX + jj] * delta_xFX[j];
01687                                 }
01688                         }
01689                 }
01690 
01691                 if ( Delta_bB_isZero == BT_TRUE )
01692                 {
01693                         for( j=0; j<nFR; ++j )
01694                         {
01695                                 jj = FR_idx[j];
01696                                 delta_xFRz_RHS[j] = delta_g[jj];
01697                         }
01698                 }
01699                 else
01700                 {
01701                         for( j=0; j<nFR; ++j )
01702                         {
01703                                 jj = FR_idx[j];
01704                                 delta_xFRz_RHS[j] = delta_g[jj] + HMX_delta_xFX[j]; /* *ZFR */
01705                         }
01706                 }
01707 
01708                 for( i=0; i<nFR; ++i )
01709                         delta_xFR[i] = -delta_xFRz_RHS[i];
01710 
01711                 if ( backsolveR( delta_xFR,BT_TRUE,delta_xFRz_TMP ) != SUCCESSFUL_RETURN )
01712                         return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
01713 
01714                 if ( backsolveR( delta_xFRz_TMP,BT_FALSE,delta_xFR ) != SUCCESSFUL_RETURN )
01715                         return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
01716         }
01717 
01718 
01719         /* III) DETERMINE delta_yFX */
01720         if ( nFX > 0 )
01721         {
01722                 for( i=0; i<nFX; ++i )
01723                 {
01724                         ii = FX_idx[i];
01725 
01726                         delta_yFX[i] = 0.0;
01727                         for( j=0; j<nFR; ++j )
01728                         {
01729                                 jj = FR_idx[j];
01730                                 delta_yFX[i] += H[ii*NVMAX + jj] * delta_xFR[j];
01731                         }
01732 
01733                         if ( Delta_bB_isZero == BT_FALSE )
01734                         {
01735                                 for( j=0; j<nFX; ++j )
01736                                 {
01737                                         jj = FX_idx[j];
01738                                         delta_yFX[i] += H[ii*NVMAX + jj] * delta_xFX[j];
01739                                 }
01740                         }
01741 
01742                         delta_yFX[i] += delta_g[ii];
01743                 }
01744         }
01745 
01746         return SUCCESSFUL_RETURN;
01747 }
01748 
01749 
01750 /*
01751  *      h o t s t a r t _ d e t e r m i n e S t e p L e n g t h
01752  */
01753 returnValue QProblemB::hotstart_determineStepLength(    const int* const FR_idx, const int* const FX_idx,
01754                                                                                                                 const real_t* const delta_lb, const real_t* const delta_ub,
01755                                                                                                                 const real_t* const delta_xFR,
01756                                                                                                                 const real_t* const delta_yFX,
01757                                                                                                                 int& BC_idx, SubjectToStatus& BC_status
01758                                                                                                                 )
01759 {
01760         int i, ii;
01761         int nFR = getNFR( );
01762         int nFX = getNFX( );
01763 
01764         real_t tau_tmp;
01765         real_t tau_new = 1.0;
01766 
01767         BC_idx = 0;
01768         BC_status = ST_UNDEFINED;
01769 
01770 
01771         /* I) DETERMINE MAXIMUM DUAL STEPLENGTH, i.e. ensure that
01772          *    active dual bounds remain valid (ignoring implicitly fixed variables): */
01773         for( i=0; i<nFX; ++i )
01774         {
01775                 ii = FX_idx[i];
01776 
01777                 if ( bounds.getType( ii ) != ST_EQUALITY )
01778                 {
01779                         if ( bounds.getStatus( ii ) == ST_LOWER )
01780                         {
01781                                 /* 1) Active lower bounds. */
01782                                 if ( ( delta_yFX[i] < -ZERO ) && ( y[ii] >= 0.0 ) )
01783                                 {
01784                                         tau_tmp = y[ii] / ( -delta_yFX[i] );
01785                                         if ( tau_tmp < tau_new )
01786                                         {
01787                                                 if ( tau_tmp >= 0.0 )
01788                                                 {
01789                                                         tau_new = tau_tmp;
01790                                                         BC_idx = ii;
01791                                                         BC_status = ST_INACTIVE;
01792                                                 }
01793                                         }
01794                                 }
01795                         }
01796                         else
01797                         {
01798                                 /* 2) Active upper bounds. */
01799                                 if ( ( delta_yFX[i] > ZERO ) && ( y[ii] <= 0.0 ) )
01800                                 {
01801                                         tau_tmp = y[ii] / ( -delta_yFX[i] );
01802                                         if ( tau_tmp < tau_new )
01803                                         {
01804                                                 if ( tau_tmp >= 0.0 )
01805                                                 {
01806                                                         tau_new = tau_tmp;
01807                                                         BC_idx = ii;
01808                                                         BC_status = ST_INACTIVE;
01809                                                 }
01810                                         }
01811                                 }
01812                         }
01813                 }
01814         }
01815 
01816 
01817         /* II) DETERMINE MAXIMUM PRIMAL STEPLENGTH, i.e. ensure that
01818          *     inactive bounds remain valid (ignoring unbounded variables). */
01819         /* 1) Inactive lower bounds. */
01820         if ( bounds.isNoLower( ) == BT_FALSE )
01821         {
01822                 for( i=0; i<nFR; ++i )
01823                 {
01824                         ii = FR_idx[i];
01825 
01826                         if ( bounds.getType( ii ) != ST_UNBOUNDED )
01827                         {
01828                                 if ( delta_lb[ii] > delta_xFR[i] )
01829                                 {
01830                                         if ( x[ii] > lb[ii] )
01831                                                 tau_tmp = ( x[ii] - lb[ii] ) / ( delta_lb[ii] - delta_xFR[i] );
01832                                         else
01833                                                 tau_tmp = 0.0;
01834 
01835                                         if ( tau_tmp < tau_new )
01836                                         {
01837                                                 if ( tau_tmp >= 0.0 )
01838                                                 {
01839                                                         tau_new = tau_tmp;
01840                                                         BC_idx = ii;
01841                                                         BC_status = ST_LOWER;
01842                                                 }
01843                                         }
01844                                 }
01845                         }
01846                 }
01847         }
01848 
01849         /* 2) Inactive upper bounds. */
01850         if ( bounds.isNoUpper( ) == BT_FALSE )
01851         {
01852                 for( i=0; i<nFR; ++i )
01853                 {
01854                         ii = FR_idx[i];
01855 
01856                         if ( bounds.getType( ii ) != ST_UNBOUNDED )
01857                         {
01858                                 if ( delta_ub[ii] < delta_xFR[i] )
01859                                 {
01860                                         if ( x[ii] < ub[ii] )
01861                                                 tau_tmp = ( x[ii] - ub[ii] ) / ( delta_ub[ii] - delta_xFR[i] );
01862                                         else
01863                                                 tau_tmp = 0.0;
01864 
01865                                         if ( tau_tmp < tau_new )
01866                                         {
01867                                                 if ( tau_tmp >= 0.0 )
01868                                                 {
01869                                                         tau_new = tau_tmp;
01870                                                         BC_idx = ii;
01871                                                         BC_status = ST_UPPER;
01872                                                 }
01873                                         }
01874                                 }
01875                         }
01876                 }
01877         }
01878 
01879 
01880         /* III) SET MAXIMUM HOMOTOPY STEPLENGTH */
01881         tau = tau_new;
01882 
01883         if ( printlevel ==  PL_HIGH )
01884         {
01885                 #ifdef PC_DEBUG
01886                 char messageString[80];
01887 
01888                 if ( BC_status == ST_UNDEFINED )
01889                         sprintf( messageString,"Stepsize is %.6e!",tau );
01890                 else
01891                         sprintf( messageString,"Stepsize is %.6e! (BC_idx = %d, BC_status = %d)",tau,BC_idx,BC_status );
01892 
01893                 getGlobalMessageHandler( )->throwInfo( RET_STEPSIZE_NONPOSITIVE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
01894                 #endif
01895         }
01896 
01897         return SUCCESSFUL_RETURN;
01898 }
01899 
01900 
01901 /*
01902  *      h o t s t a r t _ p e r f o r m S t e p
01903  */
01904 returnValue QProblemB::hotstart_performStep(    const int* const FR_idx, const int* const FX_idx,
01905                                                                                                 const real_t* const delta_g, const real_t* const  delta_lb, const real_t* const delta_ub,
01906                                                                                                 const real_t* const delta_xFX, const real_t* const delta_xFR,
01907                                                                                                 const real_t* const delta_yFX,
01908                                                                                                 int BC_idx, SubjectToStatus BC_status
01909                                                                                                 )
01910 {
01911         int i, ii;
01912         int nV  = getNV( );
01913         int nFR = getNFR( );
01914         int nFX = getNFX( );
01915 
01916 
01917         /* I) CHECK BOUNDS' CONSISTENCY */
01918         if ( areBoundsConsistent( delta_lb,delta_ub ) == BT_FALSE )
01919         {
01920                 infeasible = BT_TRUE;
01921                 tau = 0.0;
01922 
01923                 return THROWERROR( RET_QP_INFEASIBLE );
01924         }
01925 
01926 
01927         /* II) GO TO ACTIVE SET CHANGE */
01928         if ( tau > ZERO )
01929         {
01930                 /* 1) Perform step in primal und dual space. */
01931                 for( i=0; i<nFR; ++i )
01932                 {
01933                         ii = FR_idx[i];
01934                         x[ii] += tau*delta_xFR[i];
01935                 }
01936 
01937                 for( i=0; i<nFX; ++i )
01938                 {
01939                         ii = FX_idx[i];
01940                         x[ii] += tau*delta_xFX[i];
01941                         y[ii] += tau*delta_yFX[i];
01942                 }
01943 
01944                 /* 2) Shift QP data. */
01945                 for( i=0; i<nV; ++i )
01946                 {
01947                         g[i]  += tau*delta_g[i];
01948                         lb[i] += tau*delta_lb[i];
01949                         ub[i] += tau*delta_ub[i];
01950                 }
01951         }
01952         else
01953         {
01954                 /* print a stepsize warning if stepsize is zero */
01955                 #ifdef PC_DEBUG
01956                 char messageString[80];
01957                 sprintf( messageString,"Stepsize is %.6e",tau );
01958                 getGlobalMessageHandler( )->throwWarning( RET_STEPSIZE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
01959                 #endif
01960         }
01961 
01962 
01963         /* setup output preferences */
01964         #ifdef PC_DEBUG
01965         char messageString[80];
01966         VisibilityStatus visibilityStatus;
01967 
01968         if ( printlevel == PL_HIGH )
01969                 visibilityStatus = VS_VISIBLE;
01970         else
01971                 visibilityStatus = VS_HIDDEN;
01972         #endif
01973 
01974 
01975         /* III) UPDATE ACTIVE SET */
01976         switch ( BC_status )
01977         {
01978                 /* Optimal solution found as no working set change detected. */
01979                 case ST_UNDEFINED:
01980                         return RET_OPTIMAL_SOLUTION_FOUND;
01981 
01982 
01983                 /* Remove one variable from active set. */
01984                 case ST_INACTIVE:
01985                         #ifdef PC_DEBUG
01986                         sprintf( messageString,"bound no. %d.", BC_idx );
01987                         getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
01988                         #endif
01989 
01990                         if ( removeBound( BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN )
01991                                 return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
01992 
01993                         y[BC_idx] = 0.0;
01994                         break;
01995 
01996 
01997                 /* Add one variable to active set. */
01998                 default:
01999                         #ifdef PC_DEBUG
02000                         if ( BC_status == ST_LOWER )
02001                                 sprintf( messageString,"lower bound no. %d.", BC_idx );
02002                         else
02003                                 sprintf( messageString,"upper bound no. %d.", BC_idx );
02004                                 getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
02005                         #endif
02006 
02007                         if ( addBound( BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN )
02008                                 return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
02009                         break;
02010         }
02011 
02012         return SUCCESSFUL_RETURN;
02013 }
02014 
02015 
02016 #ifdef PC_DEBUG  /* Define print functions only for debugging! */
02017 
02018 /*
02019  *      p r i n t I t e r a t i o n
02020  */
02021 returnValue QProblemB::printIteration(  int iteration,
02022                                                                                 int BC_idx,     SubjectToStatus BC_status
02023                                                                                 )
02024 {
02025         char myPrintfString[160];
02026 
02027         /* consistency check */
02028         if ( iteration < 0 )
02029                 return THROWERROR( RET_INVALID_ARGUMENTS );
02030 
02031         /* nothing to do */
02032         if ( printlevel != PL_MEDIUM )
02033                 return SUCCESSFUL_RETURN;
02034 
02035 
02036         /* 1) Print header at first iteration. */
02037         if ( iteration == 0 )
02038         {
02039                 sprintf( myPrintfString,"\n##############  qpOASES  --  QP NO.%4.1d  ###############\n", count );
02040                 myPrintf( myPrintfString );
02041 
02042                 sprintf( myPrintfString,"   Iter   |   StepLength    |       Info      |   nFX   \n" );
02043                 myPrintf( myPrintfString );
02044         }
02045 
02046         /* 2) Print iteration line. */
02047         if ( BC_status == ST_UNDEFINED )
02048         {
02049                 sprintf( myPrintfString,"   %4.1d   |   %1.5e   |    QP SOLVED    |  %4.1d   \n", iteration,tau,getNFX( ) );
02050                 myPrintf( myPrintfString );
02051         }
02052         else
02053         {
02054                 char info[8];
02055 
02056                 if ( BC_status == ST_INACTIVE )
02057                         sprintf( info,"REM BND" );
02058                 else
02059                         sprintf( info,"ADD BND" );
02060 
02061                 sprintf( myPrintfString,"   %4.1d   |   %1.5e   |   %s%4.1d   |  %4.1d   \n", iteration,tau,info,BC_idx,getNFX( ) );
02062                 myPrintf( myPrintfString );
02063         }
02064 
02065         return SUCCESSFUL_RETURN;
02066 }
02067 
02068 #endif  /* PC_DEBUG */
02069 
02070 
02071 
02072 /*
02073  *      c h e c k K K T c o n d i t i o n s
02074  */
02075 returnValue QProblemB::checkKKTconditions( )
02076 {
02077         #ifdef __PERFORM_KKT_TEST__
02078 
02079         int i, j;
02080         int nV = getNV( );
02081 
02082         real_t tmp;
02083         real_t maxKKTviolation = 0.0;
02084 
02085 
02086         /* 1) Check for Hx + g - y*A' = 0  (here: A = Id). */
02087         for( i=0; i<nV; ++i )
02088         {
02089                 tmp = g[i];
02090 
02091                 for( j=0; j<nV; ++j )
02092                         tmp += H[i*nV + j] * x[j];
02093 
02094                 tmp -= y[i];
02095 
02096                 if ( getAbs( tmp ) > maxKKTviolation )
02097                         maxKKTviolation = getAbs( tmp );
02098         }
02099 
02100         /* 2) Check for lb <= x <= ub. */
02101         for( i=0; i<nV; ++i )
02102         {
02103                 if ( lb[i] - x[i] > maxKKTviolation )
02104                         maxKKTviolation = lb[i] - x[i];
02105 
02106                 if ( x[i] - ub[i] > maxKKTviolation )
02107                         maxKKTviolation = x[i] - ub[i];
02108         }
02109 
02110         /* 3) Check for correct sign of y and for complementary slackness. */
02111         for( i=0; i<nV; ++i )
02112         {
02113                 switch ( bounds.getStatus( i ) )
02114                 {
02115                         case ST_LOWER:
02116                                 if ( -y[i] > maxKKTviolation )
02117                                         maxKKTviolation = -y[i];
02118                                 if ( getAbs( ( x[i] - lb[i] ) * y[i] ) > maxKKTviolation )
02119                                         maxKKTviolation = getAbs( ( x[i] - lb[i] ) * y[i] );
02120                                 break;
02121 
02122                         case ST_UPPER:
02123                                 if ( y[i] > maxKKTviolation )
02124                                         maxKKTviolation = y[i];
02125                                 if ( getAbs( ( ub[i] - x[i] ) * y[i] ) > maxKKTviolation )
02126                                         maxKKTviolation = getAbs( ( ub[i] - x[i] ) * y[i] );
02127                                 break;
02128 
02129                         default: /* inactive */
02130                         if ( getAbs( y[i] ) > maxKKTviolation )
02131                                         maxKKTviolation = getAbs( y[i] );
02132                                 break;
02133                 }
02134         }
02135 
02136         if ( maxKKTviolation > CRITICALACCURACY )
02137                 return RET_NO_SOLUTION;
02138 
02139         if ( maxKKTviolation > DESIREDACCURACY )
02140                 return RET_INACCURATE_SOLUTION;
02141 
02142         #endif /* __PERFORM_KKT_TEST__ */
02143 
02144         return SUCCESSFUL_RETURN;
02145 }
02146 
02147 
02148 
02149 /*
02150  *      end of file
02151  */


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