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


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