retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
openpilot/phonelibs/qpoases/SRC/QProblem.cpp

3868 lines
90 KiB
C++
Raw Blame History

/*
* This file is part of qpOASES.
*
* qpOASES -- An Implementation of the Online Active Set Strategy.
* Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
*
* qpOASES is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* qpOASES is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with qpOASES; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
/**
* \file SRC/QProblem.cpp
* \author Hans Joachim Ferreau
* \version 1.3embedded
* \date 2007-2008
*
* Implementation of the QProblem class which is able to use the newly
* developed online active set strategy for parametric quadratic programming.
*/
#include <QProblem.hpp>
#include <stdio.h>
void printmatrix2(char *name, double *A, int m, int n) {
int i, j;
printf("%s = [...\n", name);
for (i = 0; i < m; i++) {
for (j = 0; j < n; j++)
printf(" % 9.4f", A[i*n+j]);
printf(",\n");
}
printf("];\n");
}
//#define __PERFORM_KKT_TEST__
/*****************************************************************************
* P U B L I C *
*****************************************************************************/
/*
* Q P r o b l e m
*/
QProblem::QProblem( ) : QProblemB( )
{
constraints.init( 0 );
sizeT = 0;
cyclingManager.init( 0,0 );
}
/*
* Q P r o b l e m
*/
QProblem::QProblem( int _nV, int _nC ) : QProblemB( _nV )
{
/* consistency checks */
if ( _nV <= 0 )
_nV = 1;
if ( _nC < 0 )
{
_nC = 0;
THROWERROR( RET_INVALID_ARGUMENTS );
}
constraints.init( _nC );
sizeT = _nC;
if ( _nC > _nV )
sizeT = _nV;
cyclingManager.init( _nV,_nC );
}
/*
* Q P r o b l e m
*/
QProblem::QProblem( const QProblem& rhs ) : QProblemB( rhs )
{
int i, j;
int _nV = rhs.bounds.getNV( );
int _nC = rhs.constraints.getNC( );
for( i=0; i<_nC; ++i )
for( j=0; j<_nV; ++j )
A[i*NVMAX + j] = rhs.A[i*NVMAX + j];
for( i=0; i<_nC; ++i )
lbA[i] = rhs.lbA[i];
for( i=0; i<_nC; ++i )
ubA[i] = rhs.ubA[i];
constraints = rhs.constraints;
for( i=0; i<(_nV+_nC); ++i )
y[i] = rhs.y[i];
sizeT = rhs.sizeT;
for( i=0; i<sizeT; ++i )
for( j=0; j<sizeT; ++j )
T[i*NVMAX + j] = rhs.T[i*NVMAX + j];
for( i=0; i<_nV; ++i )
for( j=0; j<_nV; ++j )
Q[i*NVMAX + j] = rhs.Q[i*NVMAX + j];
for( i=0; i<_nC; ++i )
Ax[i] = rhs.Ax[i];
cyclingManager = rhs.cyclingManager;
}
/*
* ~ Q P r o b l e m
*/
QProblem::~QProblem( )
{
}
/*
* o p e r a t o r =
*/
QProblem& QProblem::operator=( const QProblem& rhs )
{
int i, j;
if ( this != &rhs )
{
QProblemB::operator=( rhs );
int _nV = rhs.bounds.getNV( );
int _nC = rhs.constraints.getNC( );
for( i=0; i<_nC; ++i )
for( j=0; j<_nV; ++j )
A[i*NVMAX + j] = rhs.A[i*NVMAX + j];
for( i=0; i<_nC; ++i )
lbA[i] = rhs.lbA[i];
for( i=0; i<_nC; ++i )
ubA[i] = rhs.ubA[i];
constraints = rhs.constraints;
for( i=0; i<(_nV+_nC); ++i )
y[i] = rhs.y[i];
sizeT = rhs.sizeT;
for( i=0; i<sizeT; ++i )
for( j=0; j<sizeT; ++j )
T[i*NVMAX + j] = rhs.T[i*NVMAX + j];
for( i=0; i<_nV; ++i )
for( j=0; j<_nV; ++j )
Q[i*NVMAX + j] = rhs.Q[i*NVMAX + j];
for( i=0; i<_nC; ++i )
Ax[i] = rhs.Ax[i];
cyclingManager = rhs.cyclingManager;
}
return *this;
}
/*
* r e s e t
*/
returnValue QProblem::reset( )
{
int i, j;
int nV = getNV( );
int nC = getNC( );
/* 1) Reset bounds, Cholesky decomposition and status flags. */
if ( QProblemB::reset( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_RESET_FAILED );
/* 2) Reset constraints. */
constraints.init( nC );
/* 3) Reset TQ factorisation. */
for( i=0; i<sizeT; ++i )
for( j=0; j<sizeT; ++j )
T[i*NVMAX + j] = 0.0;
for( i=0; i<nV; ++i )
for( j=0; j<nV; ++j )
Q[i*NVMAX + j] = 0.0;
/* 4) Reset cycling manager. */
if ( cyclingManager.clearCyclingData( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_RESET_FAILED );
return SUCCESSFUL_RETURN;
}
/*
* i n i t
*/
returnValue QProblem::init( const real_t* const _H, const real_t* const _g, const real_t* const _A,
const real_t* const _lb, const real_t* const _ub,
const real_t* const _lbA, const real_t* const _ubA,
int& nWSR, const real_t* const yOpt, real_t* const cputime
)
{
/* 1) Setup QP data. */
if (setupQPdata(_H, 0, _g, _A, _lb, _ub, _lbA, _ubA) != SUCCESSFUL_RETURN)
return THROWERROR( RET_INVALID_ARGUMENTS );
/* 2) Call to main initialisation routine (without any additional information). */
return solveInitialQP( 0,yOpt,0,0, nWSR,cputime );
}
returnValue QProblem::init( const real_t* const _H, const real_t* const _R, const real_t* const _g, const real_t* const _A,
const real_t* const _lb, const real_t* const _ub,
const real_t* const _lbA, const real_t* const _ubA,
int& nWSR, const real_t* const yOpt, real_t* const cputime
)
{
/* 1) Setup QP data. */
if (setupQPdata(_H, _R, _g, _A, _lb, _ub, _lbA, _ubA) != SUCCESSFUL_RETURN)
return THROWERROR( RET_INVALID_ARGUMENTS );
/* 2) Call to main initialisation routine (without any additional information). */
return solveInitialQP( 0,yOpt,0,0, nWSR,cputime );
}
/*
* h o t s t a r t
*/
returnValue QProblem::hotstart( const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,
const real_t* const lbA_new, const real_t* const ubA_new,
int& nWSR, real_t* const cputime
)
{
int l;
/* consistency check */
if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
( getStatus( ) == QPS_PERFORMINGHOMOTOPY ) )
{
return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
}
/* start runtime measurement */
real_t starttime = 0.0;
if ( cputime != 0 )
starttime = getCPUtime( );
/* I) PREPARATIONS */
/* 1) Reset cycling and status flags and increase QP counter. */
cyclingManager.clearCyclingData( );
infeasible = BT_FALSE;
unbounded = BT_FALSE;
++count;
/* 2) Allocate delta vectors of gradient and (constraints') bounds. */
returnValue returnvalue;
BooleanType Delta_bC_isZero, Delta_bB_isZero;
int FR_idx[NVMAX];
int FX_idx[NVMAX];
int AC_idx[NCMAX_ALLOC];
int IAC_idx[NCMAX_ALLOC];
real_t delta_g[NVMAX];
real_t delta_lb[NVMAX];
real_t delta_ub[NVMAX];
real_t delta_lbA[NCMAX_ALLOC];
real_t delta_ubA[NCMAX_ALLOC];
real_t delta_xFR[NVMAX];
real_t delta_xFX[NVMAX];
real_t delta_yAC[NCMAX_ALLOC];
real_t delta_yFX[NVMAX];
real_t delta_Ax[NCMAX_ALLOC];
int BC_idx;
SubjectToStatus BC_status;
BooleanType BC_isBound;
#ifdef PC_DEBUG
char messageString[80];
#endif
/* II) MAIN HOMOTOPY LOOP */
for( l=0; l<nWSR; ++l )
{
status = QPS_PERFORMINGHOMOTOPY;
if ( printlevel == PL_HIGH )
{
#ifdef PC_DEBUG
sprintf( messageString,"%d ...",l );
getGlobalMessageHandler( )->throwInfo( RET_ITERATION_STARTED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
#endif
}
/* 1) Setup index arrays. */
if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_HOTSTART_FAILED );
if ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_HOTSTART_FAILED );
if ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_HOTSTART_FAILED );
if ( constraints.getInactive( )->getNumberArray( IAC_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_HOTSTART_FAILED );
/* 2) Detemination of shift direction of the gradient and the (constraints') bounds. */
returnvalue = hotstart_determineDataShift( FX_idx, AC_idx,
g_new,lbA_new,ubA_new,lb_new,ub_new,
delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,
Delta_bC_isZero, Delta_bB_isZero );
if ( returnvalue != SUCCESSFUL_RETURN )
{
nWSR = l;
THROWERROR( RET_SHIFT_DETERMINATION_FAILED );
return returnvalue;
}
/* 3) Determination of step direction of X and Y. */
returnvalue = hotstart_determineStepDirection( FR_idx,FX_idx,AC_idx,
delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,
Delta_bC_isZero, Delta_bB_isZero,
delta_xFX,delta_xFR,delta_yAC,delta_yFX
);
if ( returnvalue != SUCCESSFUL_RETURN )
{
nWSR = l;
THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
return returnvalue;
}
/* 4) Determination of step length TAU. */
returnvalue = hotstart_determineStepLength( FR_idx,FX_idx,AC_idx,IAC_idx,
delta_lbA,delta_ubA,delta_lb,delta_ub,
delta_xFX,delta_xFR,delta_yAC,delta_yFX,delta_Ax,
BC_idx,BC_status,BC_isBound
);
if ( returnvalue != SUCCESSFUL_RETURN )
{
nWSR = l;
THROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED );
return returnvalue;
}
/* 5) Realisation of the homotopy step. */
returnvalue = hotstart_performStep( FR_idx,FX_idx,AC_idx,IAC_idx,
delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,
delta_xFX,delta_xFR,delta_yAC,delta_yFX,delta_Ax,
BC_idx,BC_status,BC_isBound
);
if ( returnvalue != SUCCESSFUL_RETURN )
{
nWSR = l;
/* stop runtime measurement */
if ( cputime != 0 )
*cputime = getCPUtime( ) - starttime;
/* optimal solution found? */
if ( returnvalue == RET_OPTIMAL_SOLUTION_FOUND )
{
status = QPS_SOLVED;
if ( printlevel == PL_HIGH )
THROWINFO( RET_OPTIMAL_SOLUTION_FOUND );
#ifdef PC_DEBUG
if ( printIteration( l,BC_idx,BC_status,BC_isBound ) != SUCCESSFUL_RETURN )
THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
#endif
/* check KKT optimality conditions */
return checkKKTconditions( );
}
else
{
/* checks for infeasibility... */
if ( isInfeasible( ) == BT_TRUE )
{
status = QPS_HOMOTOPYQPSOLVED;
return THROWERROR( RET_HOTSTART_STOPPED_INFEASIBILITY );
}
/* ...unboundedness... */
if ( unbounded == BT_TRUE ) /* not necessary since objective function convex! */
return THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS );
/* ... and throw unspecific error otherwise */
THROWERROR( RET_HOMOTOPY_STEP_FAILED );
return returnvalue;
}
}
/* 6) Output information of successful QP iteration. */
status = QPS_HOMOTOPYQPSOLVED;
#ifdef PC_DEBUG
if ( printIteration( l,BC_idx,BC_status,BC_isBound ) != SUCCESSFUL_RETURN )
THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
#endif
}
/* stop runtime measurement */
if ( cputime != 0 )
*cputime = getCPUtime( ) - starttime;
/* if programm gets to here, output information that QP could not be solved
* within the given maximum numbers of working set changes */
if ( printlevel == PL_HIGH )
{
#ifdef PC_DEBUG
sprintf( messageString,"(nWSR = %d)",nWSR );
return getGlobalMessageHandler( )->throwWarning( RET_MAX_NWSR_REACHED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
#endif
}
/* Finally check KKT optimality conditions. */
returnValue returnvalueKKTcheck = checkKKTconditions( );
if ( returnvalueKKTcheck != SUCCESSFUL_RETURN )
return returnvalueKKTcheck;
else
return RET_MAX_NWSR_REACHED;
}
/*
* g e t N Z
*/
int QProblem::getNZ( )
{
/* nZ = nFR - nAC */
return bounds.getFree( )->getLength( ) - constraints.getActive( )->getLength( );
}
/*
* g e t D u a l S o l u t i o n
*/
returnValue QProblem::getDualSolution( real_t* const yOpt ) const
{
int i;
/* return optimal dual solution vector
* only if current QP has been solved */
if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
( getStatus( ) == QPS_SOLVED ) )
{
for( i=0; i<getNV( )+getNC( ); ++i )
yOpt[i] = y[i];
return SUCCESSFUL_RETURN;
}
else
{
return RET_QP_NOT_SOLVED;
}
}
/*****************************************************************************
* P R O T E C T E D *
*****************************************************************************/
/*
* s e t u p S u b j e c t T o T y p e
*/
returnValue QProblem::setupSubjectToType( )
{
int i;
int nC = getNC( );
/* I) SETUP SUBJECTTOTYPE FOR BOUNDS */
if ( QProblemB::setupSubjectToType( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUPSUBJECTTOTYPE_FAILED );
/* II) SETUP SUBJECTTOTYPE FOR CONSTRAINTS */
/* 1) Check if lower constraints' bounds are present. */
constraints.setNoLower( BT_TRUE );
for( i=0; i<nC; ++i )
{
if ( lbA[i] > -INFTY )
{
constraints.setNoLower( BT_FALSE );
break;
}
}
/* 2) Check if upper constraints' bounds are present. */
constraints.setNoUpper( BT_TRUE );
for( i=0; i<nC; ++i )
{
if ( ubA[i] < INFTY )
{
constraints.setNoUpper( BT_FALSE );
break;
}
}
/* 3) Determine implicit equality constraints and unbounded constraints. */
int nEC = 0;
int nUC = 0;
for( i=0; i<nC; ++i )
{
if ( ( lbA[i] < -INFTY + BOUNDTOL ) && ( ubA[i] > INFTY - BOUNDTOL ) )
{
constraints.setType( i,ST_UNBOUNDED );
++nUC;
}
else
{
if ( lbA[i] > ubA[i] - BOUNDTOL )
{
constraints.setType( i,ST_EQUALITY );
++nEC;
}
else
{
constraints.setType( i,ST_BOUNDED );
}
}
}
/* 4) Set dimensions of constraints structure. */
constraints.setNEC( nEC );
constraints.setNUC( nUC );
constraints.setNIC( nC - nEC - nUC );
return SUCCESSFUL_RETURN;
}
/*
* 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
*/
returnValue QProblem::setupCholeskyDecompositionProjected( )
{
int i, j, k, ii, kk;
int nV = getNV( );
int nFR = getNFR( );
int nZ = getNZ( );
/* 1) Initialises R with all zeros. */
for( i=0; i<nV; ++i )
for( j=0; j<nV; ++j )
R[i*NVMAX + j] = 0.0;
/* 2) Calculate Cholesky decomposition of projected Hessian Z'*H*Z. */
if ( hessianType == HST_IDENTITY )
{
/* if Hessian is identity, so is its Cholesky factor. */
for( i=0; i<nV; ++i )
R[i*NVMAX + i] = 1.0;
}
else
{
if ( nZ > 0 )
{
int FR_idx[NVMAX];
if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INDEXLIST_CORRUPTED );
#if 0
real_t HZ[NVMAX*NVMAX];
real_t ZHZ[NVMAX*NVMAX];
/* calculate H*Z */
for ( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
for ( j=0; j<nZ; ++j )
{
real_t sum = 0.0;
for ( k=0; k<nFR; ++k )
{
kk = FR_idx[k];
sum += H[ii*NVMAX + kk] * Q[kk*NVMAX + j];
}
HZ[i * NVMAX + j] = sum;
}
}
/* calculate Z'*H*Z */
for ( i=0; i<nZ; ++i )
for ( j=0; j<nZ; ++j )
{
real_t sum = 0.0;
for ( k=0; k<nFR; ++k )
{
kk = FR_idx[k];
sum += Q[kk*NVMAX + i] * HZ[k*NVMAX + j];
}
ZHZ[i * NVMAX + j] = sum;
}
/* R'*R = Z'*H*Z */
real_t sum, inv;
for( i=0; i<nZ; ++i )
{
/* j == i */
sum = ZHZ[i*NVMAX + i];
for( k=(i-1); k>=0; --k )
sum -= R[k*NVMAX + i] * R[k*NVMAX + i];
if ( sum > 0.0 )
{
R[i*NVMAX + i] = sqrt( sum );
inv = 1.0 / R[i * NVMAX + i];
}
else
{
hessianType = HST_SEMIDEF;
return THROWERROR( RET_HESSIAN_NOT_SPD );
}
for( j=(i+1); j<nZ; ++j )
{
sum = ZHZ[j*NVMAX + i];
for( k=(i-1); k>=0; --k )
sum -= R[k*NVMAX + i] * R[k*NVMAX + j];
R[i*NVMAX + j] = sum * inv;
}
}
#else
real_t HZ[NVMAX];
real_t ZHZ[NVMAX];
real_t sum, inv;
for (j = 0; j < nZ; ++j)
{
/* Cache one column of Z. */
for (i = 0; i < NVMAX; ++i)
ZHZ[i] = Q[i * NVMAX + j];
/* Create one column of the product H * Z. */
for (i = 0; i < nFR; ++i)
{
ii = FR_idx[i];
sum = 0.0;
for (k = 0; k < nFR; ++k)
{
kk = FR_idx[k];
sum += H[ii * NVMAX + kk] * ZHZ[kk];
}
HZ[ii] = sum;
}
/* Create one column of the product Z^T * H * Z. */
for (i = j; i < nZ; ++i)
ZHZ[ i ] = 0.0;
for (k = 0; k < nFR; ++k)
{
kk = FR_idx[k];
real_t q = HZ[kk];
for (i = j; i < nZ; ++i)
{
ZHZ[i] += Q[kk * NVMAX + i] * q;
}
}
/* Use the computed column to update the factorization. */
/* j == i */
sum = ZHZ[j];
for (k = (j - 1); k >= 0; --k)
sum -= R[k * NVMAX + j] * R[k * NVMAX + j];
if (sum > 0.0)
{
R[j * NVMAX + j] = sqrt(sum);
inv = 1.0 / R[j * NVMAX + j];
}
else
{
hessianType = HST_SEMIDEF;
return THROWERROR( RET_HESSIAN_NOT_SPD );
}
for (i = (j + 1); i < nZ; ++i)
{
sum = ZHZ[i];
for (k = (j - 1); k >= 0; --k)
sum -= R[k * NVMAX + j] * R[k * NVMAX + i];
R[j * NVMAX + i] = sum * inv;
}
}
#endif
}
}
return SUCCESSFUL_RETURN;
}
/*
* s e t u p T Q f a c t o r i s a t i o n
*/
returnValue QProblem::setupTQfactorisation( )
{
int i, j, ii;
int nV = getNV( );
int nFR = getNFR( );
int FR_idx[NVMAX];
if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INDEXLIST_CORRUPTED );
/* 1) Set Q to unity matrix. */
for( i=0; i<nV; ++i )
for( j=0; j<nV; ++j )
Q[i*NVMAX + j] = 0.0;
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
Q[ii*NVMAX + i] = 1.0;
}
/* 2) Set T to zero matrix. */
for( i=0; i<sizeT; ++i )
for( j=0; j<sizeT; ++j )
T[i*NVMAX + j] = 0.0;
return SUCCESSFUL_RETURN;
}
/*
* s o l v e I n i t i a l Q P
*/
returnValue QProblem::solveInitialQP( const real_t* const xOpt, const real_t* const yOpt,
const Bounds* const guessedBounds, const Constraints* const guessedConstraints,
int& nWSR, real_t* const cputime
)
{
int i;
/* some definitions */
int nV = getNV( );
int nC = getNC( );
/* start runtime measurement */
real_t starttime = 0.0;
if ( cputime != 0 )
starttime = getCPUtime( );
status = QPS_NOTINITIALISED;
/* I) ANALYSE QP DATA: */
/* 1) Check if Hessian happens to be the identity matrix. */
if ( checkForIdentityHessian( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED );
/* 2) Setup type of bounds and constraints (i.e. unbounded, implicitly fixed etc.). */
if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED );
/* 3) Initialise cycling manager. */
cyclingManager.clearCyclingData( );
status = QPS_PREPARINGAUXILIARYQP;
/* II) SETUP AUXILIARY QP WITH GIVEN OPTIMAL SOLUTION: */
/* 1) Setup bounds and constraints data structure. */
if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED );
if ( constraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED );
/* 2) Setup optimal primal/dual solution for auxiliary QP. */
if ( setupAuxiliaryQPsolution( xOpt,yOpt ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED );
/* 3) Obtain linear independent working set for auxiliary QP. */
static Bounds auxiliaryBounds;
auxiliaryBounds.init( nV );
static Constraints auxiliaryConstraints;
auxiliaryConstraints.init( nC );
if ( obtainAuxiliaryWorkingSet( xOpt,yOpt,guessedBounds,guessedConstraints,
&auxiliaryBounds,&auxiliaryConstraints ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED );
/* 4) Setup working set of auxiliary QP and setup matrix factorisations. */
if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED_TQ );
if ( setupAuxiliaryWorkingSet( &auxiliaryBounds,&auxiliaryConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED );
if ( ( getNAC( ) + getNFX( ) ) == 0 )
{
/* Factorise full Hessian if no bounds/constraints are active. */
if (hasCholesky == BT_FALSE)
if ( setupCholeskyDecomposition( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED_CHOLESKY );
/* ... else we use user provided Cholesky factorization. At the moment
* we can do that only for cold-started solver. */
}
else
{
/* Factorise projected Hessian if there active bounds/constraints. */
if ( setupCholeskyDecompositionProjected( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED_CHOLESKY );
/* TODO: use user-supplied Hessian decomposition. R_Z = R * Z. */
}
/* 5) Store original QP formulation... */
real_t g_original[NVMAX];
real_t lb_original[NVMAX];
real_t ub_original[NVMAX];
real_t lbA_original[NCMAX_ALLOC];
real_t ubA_original[NCMAX_ALLOC];
for( i=0; i<nV; ++i )
{
g_original[i] = g[i];
lb_original[i] = lb[i];
ub_original[i] = ub[i];
}
for( i=0; i<nC; ++i )
{
lbA_original[i] = lbA[i];
ubA_original[i] = ubA[i];
}
/* ... and setup QP data of an auxiliary QP having an optimal solution
* as specified by the user (or xOpt = yOpt = 0, by default). */
if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED );
if ( setupAuxiliaryQPbounds( &auxiliaryBounds,&auxiliaryConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INIT_FAILED );
status = QPS_AUXILIARYQPSOLVED;
/* III) SOLVE ACTUAL INITIAL QP: */
/* Use hotstart method to find the solution of the original initial QP,... */
returnValue returnvalue = hotstart( g_original,lb_original,ub_original,lbA_original,ubA_original, nWSR,0 );
/* ... check for infeasibility and unboundedness... */
if ( isInfeasible( ) == BT_TRUE )
return THROWERROR( RET_INIT_FAILED_INFEASIBILITY );
if ( isUnbounded( ) == BT_TRUE )
return THROWERROR( RET_INIT_FAILED_UNBOUNDEDNESS );
/* ... and internal errors. */
if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) &&
( returnvalue != RET_INACCURATE_SOLUTION ) && ( returnvalue != RET_NO_SOLUTION ) )
return THROWERROR( RET_INIT_FAILED_HOTSTART );
/* stop runtime measurement */
if ( cputime != 0 )
*cputime = getCPUtime( ) - starttime;
if ( printlevel == PL_HIGH )
THROWINFO( RET_INIT_SUCCESSFUL );
return returnvalue;
}
/*
* 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
*/
returnValue QProblem::obtainAuxiliaryWorkingSet( const real_t* const xOpt, const real_t* const yOpt,
const Bounds* const guessedBounds, const Constraints* const guessedConstraints,
Bounds* auxiliaryBounds, Constraints* auxiliaryConstraints
) const
{
int i = 0;
int nV = getNV( );
int nC = getNC( );
/* 1) Ensure that desiredBounds is allocated (and different from guessedBounds). */
if ( ( auxiliaryBounds == 0 ) || ( auxiliaryBounds == guessedBounds ) )
return THROWERROR( RET_INVALID_ARGUMENTS );
if ( ( auxiliaryConstraints == 0 ) || ( auxiliaryConstraints == guessedConstraints ) )
return THROWERROR( RET_INVALID_ARGUMENTS );
SubjectToStatus guessedStatus;
/* 2) Setup working set of bounds for auxiliary initial QP. */
if ( QProblemB::obtainAuxiliaryWorkingSet( xOpt,yOpt,guessedBounds, auxiliaryBounds ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
/* 3) Setup working set of constraints for auxiliary initial QP. */
if ( guessedConstraints != 0 )
{
/* If an initial working set is specific, use it!
* Moreover, add all equality constraints if specified. */
for( i=0; i<nC; ++i )
{
guessedStatus = guessedConstraints->getStatus( i );
if ( constraints.getType( i ) == ST_EQUALITY )
{
if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
}
else
{
if ( auxiliaryConstraints->setupConstraint( i,guessedStatus ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
}
}
}
else /* No initial working set specified. */
{
/* Obtain initial working set by "clipping". */
if ( ( xOpt != 0 ) && ( yOpt == 0 ) )
{
for( i=0; i<nC; ++i )
{
if ( Ax[i] <= lbA[i] + BOUNDTOL )
{
if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
continue;
}
if ( Ax[i] >= ubA[i] - BOUNDTOL )
{
if ( auxiliaryConstraints->setupConstraint( i,ST_UPPER ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
continue;
}
/* Moreover, add all equality constraints if specified. */
if ( constraints.getType( i ) == ST_EQUALITY )
{
if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
}
else
{
if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
}
}
}
/* Obtain initial working set in accordance to sign of dual solution vector. */
if ( ( xOpt == 0 ) && ( yOpt != 0 ) )
{
for( i=0; i<nC; ++i )
{
if ( yOpt[nV+i] > ZERO )
{
if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
continue;
}
if ( yOpt[nV+i] < -ZERO )
{
if ( auxiliaryConstraints->setupConstraint( i,ST_UPPER ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
continue;
}
/* Moreover, add all equality constraints if specified. */
if ( constraints.getType( i ) == ST_EQUALITY )
{
if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
}
else
{
if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
}
}
}
/* If xOpt and yOpt are null pointer and no initial working is specified,
* start with empty working set (or implicitly fixed bounds and equality constraints only)
* for auxiliary QP. */
if ( ( xOpt == 0 ) && ( yOpt == 0 ) )
{
for( i=0; i<nC; ++i )
{
/* Only add all equality constraints if specified. */
if ( constraints.getType( i ) == ST_EQUALITY )
{
if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
}
else
{
if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
}
}
}
}
return SUCCESSFUL_RETURN;
}
/*
* s e t u p A u x i l i a r y W o r k i n g S e t
*/
returnValue QProblem::setupAuxiliaryWorkingSet( const Bounds* const auxiliaryBounds,
const Constraints* const auxiliaryConstraints,
BooleanType setupAfresh
)
{
int i;
int nV = getNV( );
int nC = getNC( );
/* consistency checks */
if ( auxiliaryBounds != 0 )
{
for( i=0; i<nV; ++i )
if ( ( bounds.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryBounds->getStatus( i ) == ST_UNDEFINED ) )
return THROWERROR( RET_UNKNOWN_BUG );
}
else
{
return THROWERROR( RET_INVALID_ARGUMENTS );
}
if ( auxiliaryConstraints != 0 )
{
for( i=0; i<nC; ++i )
if ( ( constraints.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryConstraints->getStatus( i ) == ST_UNDEFINED ) )
return THROWERROR( RET_UNKNOWN_BUG );
}
else
{
return THROWERROR( RET_INVALID_ARGUMENTS );
}
/* I) SETUP CHOLESKY FLAG:
* Cholesky decomposition shall only be updated if working set
* shall be updated (i.e. NOT setup afresh!) */
BooleanType updateCholesky;
if ( setupAfresh == BT_TRUE )
updateCholesky = BT_FALSE;
else
updateCholesky = BT_TRUE;
/* II) REMOVE FORMERLY ACTIVE (CONSTRAINTS') BOUNDS (IF NECESSARY): */
if ( setupAfresh == BT_FALSE )
{
/* 1) Remove all active constraints that shall be inactive AND
* all active constraints that are active at the wrong bound. */
for( i=0; i<nC; ++i )
{
if ( ( constraints.getStatus( i ) == ST_LOWER ) && ( auxiliaryConstraints->getStatus( i ) != ST_LOWER ) )
if ( removeConstraint( i,updateCholesky ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
if ( ( constraints.getStatus( i ) == ST_UPPER ) && ( auxiliaryConstraints->getStatus( i ) != ST_UPPER ) )
if ( removeConstraint( i,updateCholesky ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
}
/* 2) Remove all active bounds that shall be inactive AND
* all active bounds that are active at the wrong bound. */
for( i=0; i<nV; ++i )
{
if ( ( bounds.getStatus( i ) == ST_LOWER ) && ( auxiliaryBounds->getStatus( i ) != ST_LOWER ) )
if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
if ( ( bounds.getStatus( i ) == ST_UPPER ) && ( auxiliaryBounds->getStatus( i ) != ST_UPPER ) )
if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
}
}
/* III) ADD NEWLY ACTIVE (CONSTRAINTS') BOUNDS: */
/* 1) Add all inactive bounds that shall be active AND
* all formerly active bounds that have been active at the wrong bound. */
for( i=0; i<nV; ++i )
{
if ( ( bounds.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE ) )
{
/* Add bound only if it is linearly independent from the current working set. */
if ( addBound_checkLI( i ) == RET_LINEARLY_INDEPENDENT )
{
if ( addBound( i,auxiliaryBounds->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
}
}
}
/* 2) Add all inactive constraints that shall be active AND
* all formerly active constraints that have been active at the wrong bound. */
for( i=0; i<nC; ++i )
{
if ( ( auxiliaryConstraints->getStatus( i ) == ST_LOWER ) || ( auxiliaryConstraints->getStatus( i ) == ST_UPPER ) )
{
/* formerly inactive */
if ( constraints.getStatus( i ) == ST_INACTIVE )
{
/* Add constraint only if it is linearly independent from the current working set. */
if ( addConstraint_checkLI( i ) == RET_LINEARLY_INDEPENDENT )
{
if ( addConstraint( i,auxiliaryConstraints->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
}
}
}
}
return SUCCESSFUL_RETURN;
}
/*
* s e t u p A u x i l i a r y Q P s o l u t i o n
*/
returnValue QProblem::setupAuxiliaryQPsolution( const real_t* const xOpt, const real_t* const yOpt
)
{
int i, j;
int nV = getNV( );
int nC = getNC( );
/* Setup primal/dual solution vector for auxiliary initial QP:
* if a null pointer is passed, a zero vector is assigned;
* old solution vector is kept if pointer to internal solution vevtor is passed. */
if ( xOpt != 0 )
{
if ( xOpt != x )
for( i=0; i<nV; ++i )
x[i] = xOpt[i];
for ( j=0; j<nC; ++j )
{
Ax[j] = 0.0;
for( i=0; i<nV; ++i )
Ax[j] += A[j*NVMAX + i] * x[i];
}
}
else
{
for( i=0; i<nV; ++i )
x[i] = 0.0;
for ( j=0; j<nC; ++j )
Ax[j] = 0.0;
}
if ( yOpt != 0 )
{
if ( yOpt != y )
for( i=0; i<nV+nC; ++i )
y[i] = yOpt[i];
}
else
{
for( i=0; i<nV+nC; ++i )
y[i] = 0.0;
}
return SUCCESSFUL_RETURN;
}
/*
* s e t u p A u x i l i a r y Q P g r a d i e n t
*/
returnValue QProblem::setupAuxiliaryQPgradient( )
{
int i, j;
int nV = getNV( );
int nC = getNC( );
/* Setup gradient vector: g = -H*x + [Id A]'*[yB yC]. */
for ( i=0; i<nV; ++i )
{
/* Id'*yB */
g[i] = y[i];
/* A'*yC */
for ( j=0; j<nC; ++j )
g[i] += A[j*NVMAX + i] * y[nV+j];
/* -H*x */
for ( j=0; j<nV; ++j )
g[i] -= H[i*NVMAX + j] * x[j];
}
return SUCCESSFUL_RETURN;
}
/*
* s e t u p A u x i l i a r y Q P b o u n d s
*/
returnValue QProblem::setupAuxiliaryQPbounds( const Bounds* const auxiliaryBounds,
const Constraints* const auxiliaryConstraints,
BooleanType useRelaxation
)
{
int i;
int nV = getNV( );
int nC = getNC( );
/* 1) Setup bound vectors. */
for ( i=0; i<nV; ++i )
{
switch ( bounds.getStatus( i ) )
{
case ST_INACTIVE:
if ( useRelaxation == BT_TRUE )
{
if ( bounds.getType( i ) == ST_EQUALITY )
{
lb[i] = x[i];
ub[i] = x[i];
}
else
{
/* If a bound is inactive although it was supposed to be
* active by the auxiliaryBounds, it could not be added
* due to linear dependence. Thus set it "strongly inactive". */
if ( auxiliaryBounds->getStatus( i ) == ST_LOWER )
lb[i] = x[i];
else
lb[i] = x[i] - BOUNDRELAXATION;
if ( auxiliaryBounds->getStatus( i ) == ST_UPPER )
ub[i] = x[i];
else
ub[i] = x[i] + BOUNDRELAXATION;
}
}
break;
case ST_LOWER:
lb[i] = x[i];
if ( bounds.getType( i ) == ST_EQUALITY )
{
ub[i] = x[i];
}
else
{
if ( useRelaxation == BT_TRUE )
ub[i] = x[i] + BOUNDRELAXATION;
}
break;
case ST_UPPER:
ub[i] = x[i];
if ( bounds.getType( i ) == ST_EQUALITY )
{
lb[i] = x[i];
}
else
{
if ( useRelaxation == BT_TRUE )
lb[i] = x[i] - BOUNDRELAXATION;
}
break;
default:
return THROWERROR( RET_UNKNOWN_BUG );
}
}
/* 2) Setup constraints vectors. */
for ( i=0; i<nC; ++i )
{
switch ( constraints.getStatus( i ) )
{
case ST_INACTIVE:
if ( useRelaxation == BT_TRUE )
{
if ( constraints.getType( i ) == ST_EQUALITY )
{
lbA[i] = Ax[i];
ubA[i] = Ax[i];
}
else
{
/* If a constraint is inactive although it was supposed to be
* active by the auxiliaryConstraints, it could not be added
* due to linear dependence. Thus set it "strongly inactive". */
if ( auxiliaryConstraints->getStatus( i ) == ST_LOWER )
lbA[i] = Ax[i];
else
lbA[i] = Ax[i] - BOUNDRELAXATION;
if ( auxiliaryConstraints->getStatus( i ) == ST_UPPER )
ubA[i] = Ax[i];
else
ubA[i] = Ax[i] + BOUNDRELAXATION;
}
}
break;
case ST_LOWER:
lbA[i] = Ax[i];
if ( constraints.getType( i ) == ST_EQUALITY )
{
ubA[i] = Ax[i];
}
else
{
if ( useRelaxation == BT_TRUE )
ubA[i] = Ax[i] + BOUNDRELAXATION;
}
break;
case ST_UPPER:
ubA[i] = Ax[i];
if ( constraints.getType( i ) == ST_EQUALITY )
{
lbA[i] = Ax[i];
}
else
{
if ( useRelaxation == BT_TRUE )
lbA[i] = Ax[i] - BOUNDRELAXATION;
}
break;
default:
return THROWERROR( RET_UNKNOWN_BUG );
}
}
return SUCCESSFUL_RETURN;
}
/*
* a d d C o n s t r a i n t
*/
returnValue QProblem::addConstraint( int number, SubjectToStatus C_status,
BooleanType updateCholesky
)
{
int i, j, ii;
/* consistency checks */
if ( constraints.getStatus( number ) != ST_INACTIVE )
return THROWERROR( RET_CONSTRAINT_ALREADY_ACTIVE );
if ( ( constraints.getNC( ) - getNAC( ) ) == constraints.getNUC( ) )
return THROWERROR( RET_ALL_CONSTRAINTS_ACTIVE );
if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
( getStatus( ) == QPS_SOLVED ) )
{
return THROWERROR( RET_UNKNOWN_BUG );
}
/* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET,
* i.e. remove a constraint or bound if linear dependence occurs. */
/* check for LI only if Cholesky decomposition shall be updated! */
if ( updateCholesky == BT_TRUE )
{
returnValue ensureLIreturnvalue = addConstraint_ensureLI( number,C_status );
switch ( ensureLIreturnvalue )
{
case SUCCESSFUL_RETURN:
break;
case RET_LI_RESOLVED:
break;
case RET_ENSURELI_FAILED_NOINDEX:
return THROWERROR( RET_ADDCONSTRAINT_FAILED_INFEASIBILITY );
case RET_ENSURELI_FAILED_CYCLING:
return THROWERROR( RET_ADDCONSTRAINT_FAILED_INFEASIBILITY );
default:
return THROWERROR( RET_ENSURELI_FAILED );
}
}
/* some definitions */
int nFR = getNFR( );
int nAC = getNAC( );
int nZ = getNZ( );
int tcol = sizeT - nAC;
int FR_idx[NVMAX];
if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_ADDCONSTRAINT_FAILED );
real_t aFR[NVMAX];
real_t wZ[NVMAX];
for( i=0; i<nZ; ++i )
wZ[i] = 0.0;
/* II) ADD NEW ACTIVE CONSTRAINT TO MATRIX T: */
/* 1) Add row [wZ wY] = aFR'*[Z Y] to the end of T: assign aFR. */
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
aFR[i] = A[number*NVMAX + ii];
}
/* calculate wZ */
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
for( j=0; j<nZ; ++j )
wZ[j] += aFR[i] * Q[ii*NVMAX + j];
}
/* 2) Calculate wY and store it directly into T. */
if ( nAC > 0 )
{
for( j=0; j<nAC; ++j )
T[nAC*NVMAX + tcol+j] = 0.0;
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
for( j=0; j<nAC; ++j )
T[nAC*NVMAX + tcol+j] += aFR[i] * Q[ii*NVMAX + nZ+j];
}
}
real_t c, s;
if ( nZ > 0 )
{
/* II) RESTORE TRIANGULAR FORM OF T: */
/* Use column-wise Givens rotations to restore reverse triangular form
* of T, simultanenous change of Q (i.e. Z) and R. */
for( j=0; j<nZ-1; ++j )
{
computeGivens( wZ[j+1],wZ[j], wZ[j+1],wZ[j],c,s );
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
applyGivens( c,s,Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j], Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j] );
}
if ( ( updateCholesky == BT_TRUE ) && ( hessianType != HST_IDENTITY ) )
{
for( i=0; i<=j+1; ++i )
applyGivens( c,s,R[i*NVMAX + 1+j],R[i*NVMAX + j], R[i*NVMAX + 1+j],R[i*NVMAX + j] );
}
}
T[nAC*NVMAX + tcol-1] = wZ[nZ-1];
if ( ( updateCholesky == BT_TRUE ) && ( hessianType != HST_IDENTITY ) )
{
/* III) RESTORE TRIANGULAR FORM OF R:
* Use row-wise Givens rotations to restore upper triangular form of R. */
for( i=0; i<nZ-1; ++i )
{
computeGivens( R[i*NVMAX + i],R[(1+i)*NVMAX + i], R[i*NVMAX + i],R[(1+i)*NVMAX + i],c,s );
for( j=(1+i); j<(nZ-1); ++j ) /* last column of R is thrown away */
applyGivens( c,s,R[i*NVMAX + j],R[(1+i)*NVMAX + j], R[i*NVMAX + j],R[(1+i)*NVMAX + j] );
}
/* last column of R is thrown away */
for( i=0; i<nZ; ++i )
R[i*NVMAX + nZ-1] = 0.0;
}
}
/* IV) UPDATE INDICES */
if ( constraints.moveInactiveToActive( number,C_status ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_ADDCONSTRAINT_FAILED );
return SUCCESSFUL_RETURN;
}
/*
* a d d C o n s t r a i n t _ c h e c k L I
*/
returnValue QProblem::addConstraint_checkLI( int number )
{
int i, j, jj;
int nFR = getNFR( );
int nZ = getNZ( );
int FR_idx[NVMAX];
if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_INDEXLIST_CORRUPTED );
/* Check if constraint <number> is linearly independent from the
the active ones (<=> is element of null space of Afr). */
real_t sum;
for( i=0; i<nZ; ++i )
{
sum = 0.0;
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
sum += Q[jj*NVMAX + i] * A[number*NVMAX + jj];
}
if ( getAbs( sum ) > 10.0*EPS )
return RET_LINEARLY_INDEPENDENT;
}
return RET_LINEARLY_DEPENDENT;
}
/*
* a d d C o n s t r a i n t _ e n s u r e L I
*/
returnValue QProblem::addConstraint_ensureLI( int number, SubjectToStatus C_status )
{
int i, j, ii, jj;
int nV = getNV( );
int nFR = getNFR( );
int nFX = getNFX( );
int nAC = getNAC( );
int nZ = getNZ( );
/* I) Check if new constraint is linearly independent from the active ones. */
returnValue returnvalueCheckLI = addConstraint_checkLI( number );
if ( returnvalueCheckLI == RET_INDEXLIST_CORRUPTED )
return THROWERROR( RET_ENSURELI_FAILED );
if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )
return SUCCESSFUL_RETURN;
/* II) NEW CONSTRAINT IS LINEARLY DEPENDENT: */
/* 1) Determine coefficients of linear combination,
* cf. M.J. Best. Applied Mathematics and Parallel Computing, chapter:
* An Algorithm for the Solution of the Parametric Quadratic Programming
* Problem, pages 57-76. Physica-Verlag, Heidelberg, 1996. */
int FR_idx[NVMAX];
if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_ENSURELI_FAILED );
int FX_idx[NVMAX];
if ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_ENSURELI_FAILED );
real_t xiC[NCMAX_ALLOC];
real_t xiC_TMP[NCMAX_ALLOC];
real_t xiB[NVMAX];
/* 2) Calculate xiC */
if ( nAC > 0 )
{
if ( C_status == ST_LOWER )
{
for( i=0; i<nAC; ++i )
{
xiC_TMP[i] = 0.0;
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
xiC_TMP[i] += Q[jj*NVMAX + nZ+i] * A[number*NVMAX + jj];
}
}
}
else
{
for( i=0; i<nAC; ++i )
{
xiC_TMP[i] = 0.0;
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
xiC_TMP[i] -= Q[jj*NVMAX + nZ+i] * A[number*NVMAX + jj];
}
}
}
if ( backsolveT( xiC_TMP, BT_TRUE, xiC ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_ENSURELI_FAILED_TQ );
}
/* 3) Calculate xiB. */
int AC_idx[NCMAX_ALLOC];
if ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_ENSURELI_FAILED );
if ( C_status == ST_LOWER )
{
for( i=0; i<nFX; ++i )
{
ii = FX_idx[i];
xiB[i] = A[number*NVMAX + ii];
for( j=0; j<nAC; ++j )
{
jj = AC_idx[j];
xiB[i] -= A[jj*NVMAX + ii] * xiC[j];
}
}
}
else
{
for( i=0; i<nFX; ++i )
{
ii = FX_idx[i];
xiB[i] = -A[number*NVMAX + ii];
for( j=0; j<nAC; ++j )
{
jj = AC_idx[j];
xiB[i] -= A[jj*NVMAX + ii] * xiC[j];
}
}
}
/* III) DETERMINE CONSTRAINT/BOUND TO BE REMOVED. */
real_t y_min = INFTY * INFTY;
int y_min_number = -1;
BooleanType y_min_isBound = BT_FALSE;
/* 1) Constraints. */
for( i=0; i<nAC; ++i )
{
ii = AC_idx[i];
if ( constraints.getStatus( ii ) == ST_LOWER )
{
if ( ( xiC[i] > ZERO ) && ( y[nV+ii] >= 0.0 ) )
{
if ( y[nV+ii]/xiC[i] < y_min )
{
y_min = y[nV+ii]/xiC[i];
y_min_number = ii;
}
}
}
else
{
if ( ( xiC[i] < -ZERO ) && ( y[nV+ii] <= 0.0 ) )
{
if ( y[nV+ii]/xiC[i] < y_min )
{
y_min = y[nV+ii]/xiC[i];
y_min_number = ii;
}
}
}
}
/* 2) Bounds. */
for( i=0; i<nFX; ++i )
{
ii = FX_idx[i];
if ( bounds.getStatus( ii ) == ST_LOWER )
{
if ( ( xiB[i] > ZERO ) && ( y[ii] >= 0.0 ) )
{
if ( y[ii]/xiB[i] < y_min )
{
y_min = y[ii]/xiB[i];
y_min_number = ii;
y_min_isBound = BT_TRUE;
}
}
}
else
{
if ( ( xiB[i] < -ZERO ) && ( y[ii] <= 0.0 ) )
{
if ( y[ii]/xiB[i] < y_min )
{
y_min = y[ii]/xiB[i];
y_min_number = ii;
y_min_isBound = BT_TRUE;
}
}
}
}
/* setup output preferences */
#ifdef PC_DEBUG
char messageString[80];
VisibilityStatus visibilityStatus;
if ( printlevel == PL_HIGH )
visibilityStatus = VS_VISIBLE;
else
visibilityStatus = VS_HIDDEN;
#endif
/* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */
if ( y_min_number >= 0 )
{
/* 1) Check for cycling due to infeasibility. */
if ( ( cyclingManager.getCyclingStatus( number,BT_FALSE ) == CYC_PREV_REMOVED ) &&
( cyclingManager.getCyclingStatus( y_min_number,y_min_isBound ) == CYC_PREV_ADDED ) )
{
infeasible = BT_TRUE;
return THROWERROR( RET_ENSURELI_FAILED_CYCLING );
}
else
{
/* set cycling data */
cyclingManager.clearCyclingData( );
cyclingManager.setCyclingStatus( number,BT_FALSE, CYC_PREV_ADDED );
cyclingManager.setCyclingStatus( y_min_number,y_min_isBound, CYC_PREV_REMOVED );
}
/* 2) Update Lagrange multiplier... */
for( i=0; i<nAC; ++i )
{
ii = AC_idx[i];
y[nV+ii] -= y_min * xiC[i];
}
for( i=0; i<nFX; ++i )
{
ii = FX_idx[i];
y[ii] -= y_min * xiB[i];
}
/* ... also for newly active constraint... */
if ( C_status == ST_LOWER )
y[nV+number] = y_min;
else
y[nV+number] = -y_min;
/* ... and for constraint to be removed. */
if ( y_min_isBound == BT_TRUE )
{
#ifdef PC_DEBUG
sprintf( messageString,"bound no. %d.",y_min_number );
getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
#endif
if ( removeBound( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
y[y_min_number] = 0.0;
}
else
{
#ifdef PC_DEBUG
sprintf( messageString,"constraint no. %d.",y_min_number );
getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
#endif
if ( removeConstraint( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
y[nV+y_min_number] = 0.0;
}
}
else
{
/* no constraint/bound can be removed => QP is infeasible! */
infeasible = BT_TRUE;
return THROWERROR( RET_ENSURELI_FAILED_NOINDEX );
}
return getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_HIDDEN );
}
/*
* a d d B o u n d
*/
returnValue QProblem::addBound( int number, SubjectToStatus B_status,
BooleanType updateCholesky
)
{
int i, j, ii;
/* consistency checks */
if ( bounds.getStatus( number ) != ST_INACTIVE )
return THROWERROR( RET_BOUND_ALREADY_ACTIVE );
if ( getNFR( ) == bounds.getNUV( ) )
return THROWERROR( RET_ALL_BOUNDS_ACTIVE );
if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
( getStatus( ) == QPS_SOLVED ) )
{
return THROWERROR( RET_UNKNOWN_BUG );
}
/* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET,
* i.e. remove a constraint or bound if linear dependence occurs. */
/* check for LI only if Cholesky decomposition shall be updated! */
if ( updateCholesky == BT_TRUE )
{
returnValue ensureLIreturnvalue = addBound_ensureLI( number,B_status );
switch ( ensureLIreturnvalue )
{
case SUCCESSFUL_RETURN:
break;
case RET_LI_RESOLVED:
break;
case RET_ENSURELI_FAILED_NOINDEX:
return THROWERROR( RET_ADDBOUND_FAILED_INFEASIBILITY );
case RET_ENSURELI_FAILED_CYCLING:
return THROWERROR( RET_ADDBOUND_FAILED_INFEASIBILITY );
default:
return THROWERROR( RET_ENSURELI_FAILED );
}
}
/* some definitions */
int nFR = getNFR( );
int nAC = getNAC( );
int nZ = getNZ( );
int tcol = sizeT - nAC;
/* I) SWAP INDEXLIST OF FREE VARIABLES:
* move the variable to be fixed to the end of the list of free variables. */
int lastfreenumber = bounds.getFree( )->getLastNumber( );
if ( lastfreenumber != number )
if ( bounds.swapFree( number,lastfreenumber ) != SUCCESSFUL_RETURN )
THROWERROR( RET_ADDBOUND_FAILED );
int FR_idx[NVMAX];
if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_ADDBOUND_FAILED );
real_t w[NVMAX];
/* II) ADD NEW ACTIVE BOUND TO TOP OF MATRIX T: */
/* 1) add row [wZ wY] = [Z Y](number) at the top of T: assign w */
for( i=0; i<nFR; ++i )
w[i] = Q[FR_idx[nFR-1]*NVMAX + i];
/* 2) Use column-wise Givens rotations to restore reverse triangular form
* of the first row of T, simultanenous change of Q (i.e. Z) and R. */
real_t c, s;
for( j=0; j<nZ-1; ++j )
{
computeGivens( w[j+1],w[j], w[j+1],w[j],c,s );
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
applyGivens( c,s,Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j], Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j] );
}
if ( ( updateCholesky == BT_TRUE ) && ( hessianType != HST_IDENTITY ) )
{
for( i=0; i<=j+1; ++i )
applyGivens( c,s,R[i*NVMAX + 1+j],R[i*NVMAX + j], R[i*NVMAX + 1+j],R[i*NVMAX + j] );
}
}
if ( nAC > 0 ) /* ( nAC == 0 ) <=> ( nZ == nFR ) <=> Y and T are empty => nothing to do */
{
/* store new column a in a temporary vector instead of shifting T one column to the left */
real_t tmp[NCMAX_ALLOC];
for( i=0; i<nAC; ++i )
tmp[i] = 0.0;
{
j = nZ-1;
computeGivens( w[j+1],w[j], w[j+1],w[j],c,s );
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
applyGivens( c,s,Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j], Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j] );
}
applyGivens( c,s,T[(nAC-1)*NVMAX + tcol],tmp[nAC-1], tmp[nAC-1],T[(nAC-1)*NVMAX + tcol] );
}
for( j=nZ; j<nFR-1; ++j )
{
computeGivens( w[j+1],w[j], w[j+1],w[j],c,s );
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
applyGivens( c,s,Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j], Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j] );
}
for( i=(nFR-2-j); i<nAC; ++i )
applyGivens( c,s,T[i*NVMAX + 1+tcol-nZ+j],tmp[i], tmp[i],T[i*NVMAX + 1+tcol-nZ+j] );
}
}
if ( ( updateCholesky == BT_TRUE ) && ( hessianType != HST_IDENTITY ) )
{
/* III) RESTORE TRIANGULAR FORM OF R:
* use row-wise Givens rotations to restore upper triangular form of R */
for( i=0; i<nZ-1; ++i )
{
computeGivens( R[i*NVMAX + i],R[(1+i)*NVMAX + i], R[i*NVMAX + i],R[(1+i)*NVMAX + i],c,s );
for( j=(1+i); j<nZ-1; ++j ) /* last column of R is thrown away */
applyGivens( c,s,R[i*NVMAX + j],R[(1+i)*NVMAX + j], R[i*NVMAX + j],R[(1+i)*NVMAX + j] );
}
/* last column of R is thrown away */
for( i=0; i<nZ; ++i )
R[i*NVMAX + nZ-1] = 0.0;
}
/* IV) UPDATE INDICES */
if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_ADDBOUND_FAILED );
return SUCCESSFUL_RETURN;
}
/*
* a d d B o u n d _ c h e c k L I
*/
returnValue QProblem::addBound_checkLI( int number )
{
int i;
/* some definitions */
int nZ = getNZ( );
/* Check if constraint <number> is linearly independent from the
the active ones (<=> is element of null space of Afr). */
for( i=0; i<nZ; ++i )
{
if ( getAbs( Q[number*NVMAX + i] ) > EPS )
return RET_LINEARLY_INDEPENDENT;
}
return RET_LINEARLY_DEPENDENT;
}
/*
* a d d B o u n d _ e n s u r e L I
*/
returnValue QProblem::addBound_ensureLI( int number, SubjectToStatus B_status )
{
int i, j, ii, jj;
int nV = getNV( );
int nFX = getNFX( );
int nAC = getNAC( );
int nZ = getNZ( );
/* I) Check if new constraint is linearly independent from the active ones. */
returnValue returnvalueCheckLI = addBound_checkLI( number );
if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )
return SUCCESSFUL_RETURN;
/* II) NEW BOUND IS LINEARLY DEPENDENT: */
/* 1) Determine coefficients of linear combination,
* cf. M.J. Best. Applied Mathematics and Parallel Computing, chapter:
* An Algorithm for the Solution of the Parametric Quadratic Programming
* Problem, pages 57-76. Physica-Verlag, Heidelberg, 1996. */
int FR_idx[NVMAX];
if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_ENSURELI_FAILED );
int FX_idx[NVMAX];
if ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_ENSURELI_FAILED );
int AC_idx[NCMAX_ALLOC];
if ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_ENSURELI_FAILED );
real_t xiC[NCMAX_ALLOC];
real_t xiC_TMP[NCMAX_ALLOC];
real_t xiB[NVMAX];
/* 2) Calculate xiC. */
if ( nAC > 0 )
{
if ( B_status == ST_LOWER )
{
for( i=0; i<nAC; ++i )
xiC_TMP[i] = Q[number*NVMAX + nZ+i];
}
else
{
for( i=0; i<nAC; ++i )
xiC_TMP[i] = -Q[number*NVMAX + nZ+i];
}
if ( backsolveT( xiC_TMP, BT_TRUE, xiC ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_ENSURELI_FAILED_TQ );
}
/* 3) Calculate xiB. */
for( i=0; i<nFX; ++i )
{
ii = FX_idx[i];
xiB[i] = 0.0;
for( j=0; j<nAC; ++j )
{
jj = AC_idx[j];
xiB[i] -= A[jj*NVMAX + ii] * xiC[j];
}
}
/* III) DETERMINE CONSTRAINT/BOUND TO BE REMOVED. */
real_t y_min = INFTY * INFTY;
int y_min_number = -1;
BooleanType y_min_isBound = BT_FALSE;
/* 1) Constraints. */
for( i=0; i<nAC; ++i )
{
ii = AC_idx[i];
if ( constraints.getStatus( ii ) == ST_LOWER )
{
if ( ( xiC[i] > ZERO ) && ( y[nV+ii] >= 0.0 ) )
{
if ( y[nV+ii]/xiC[i] < y_min )
{
y_min = y[nV+ii]/xiC[i];
y_min_number = ii;
}
}
}
else
{
if ( ( xiC[i] < -ZERO ) && ( y[nV+ii] <= 0.0 ) )
{
if ( y[nV+ii]/xiC[i] < y_min )
{
y_min = y[nV+ii]/xiC[i];
y_min_number = ii;
}
}
}
}
/* 2) Bounds. */
for( i=0; i<nFX; ++i )
{
ii = FX_idx[i];
if ( bounds.getStatus( ii ) == ST_LOWER )
{
if ( ( xiB[i] > ZERO ) && ( y[ii] >= 0.0 ) )
{
if ( y[ii]/xiB[i] < y_min )
{
y_min = y[ii]/xiB[i];
y_min_number = ii;
y_min_isBound = BT_TRUE;
}
}
}
else
{
if ( ( xiB[i] < -ZERO ) && ( y[ii] <= 0.0 ) )
{
if ( y[ii]/xiB[i] < y_min )
{
y_min = y[ii]/xiB[i];
y_min_number = ii;
y_min_isBound = BT_TRUE;
}
}
}
}
/* setup output preferences */
#ifdef PC_DEBUG
char messageString[80];
VisibilityStatus visibilityStatus;
if ( printlevel == PL_HIGH )
visibilityStatus = VS_VISIBLE;
else
visibilityStatus = VS_HIDDEN;
#endif
/* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */
if ( y_min_number >= 0 )
{
/* 1) Check for cycling due to infeasibility. */
if ( ( cyclingManager.getCyclingStatus( number,BT_TRUE ) == CYC_PREV_REMOVED ) &&
( cyclingManager.getCyclingStatus( y_min_number,y_min_isBound ) == CYC_PREV_ADDED ) )
{
infeasible = BT_TRUE;
return THROWERROR( RET_ENSURELI_FAILED_CYCLING );
}
else
{
/* set cycling data */
cyclingManager.clearCyclingData( );
cyclingManager.setCyclingStatus( number,BT_TRUE, CYC_PREV_ADDED );
cyclingManager.setCyclingStatus( y_min_number,y_min_isBound, CYC_PREV_REMOVED );
}
/* 2) Update Lagrange multiplier... */
for( i=0; i<nAC; ++i )
{
ii = AC_idx[i];
y[nV+ii] -= y_min * xiC[i];
}
for( i=0; i<nFX; ++i )
{
ii = FX_idx[i];
y[ii] -= y_min * xiB[i];
}
/* ... also for newly active bound ... */
if ( B_status == ST_LOWER )
y[number] = y_min;
else
y[number] = -y_min;
/* ... and for bound to be removed. */
if ( y_min_isBound == BT_TRUE )
{
#ifdef PC_DEBUG
sprintf( messageString,"bound no. %d.",y_min_number );
getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
#endif
if ( removeBound( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
y[y_min_number] = 0.0;
}
else
{
#ifdef PC_DEBUG
sprintf( messageString,"constraint no. %d.",y_min_number );
getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
#endif
if ( removeConstraint( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
y[nV+y_min_number] = 0.0;
}
}
else
{
/* no constraint/bound can be removed => QP is infeasible! */
infeasible = BT_TRUE;
return THROWERROR( RET_ENSURELI_FAILED_NOINDEX );
}
return getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_HIDDEN );
}
/*
* r e m o v e C o n s t r a i n t
*/
returnValue QProblem::removeConstraint( int number,
BooleanType updateCholesky
)
{
int i, j, ii, jj;
/* consistency check */
if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
( getStatus( ) == QPS_SOLVED ) )
{
return THROWERROR( RET_UNKNOWN_BUG );
}
/* some definitions */
int nFR = getNFR( );
int nAC = getNAC( );
int nZ = getNZ( );
int tcol = sizeT - nAC;
int number_idx = constraints.getActive( )->getIndex( number );
/* consistency checks */
if ( constraints.getStatus( number ) == ST_INACTIVE )
return THROWERROR( RET_CONSTRAINT_NOT_ACTIVE );
if ( ( number_idx < 0 ) || ( number_idx >= nAC ) )
return THROWERROR( RET_CONSTRAINT_NOT_ACTIVE );
int FR_idx[NVMAX];
if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
/* I) REMOVE <number>th ROW FROM T,
* i.e. shift rows number+1 through nAC upwards (instead of the actual
* constraint number its corresponding index within matrix A is used). */
if ( number_idx < nAC-1 )
{
for( i=(number_idx+1); i<nAC; ++i )
for( j=(nAC-i-1); j<nAC; ++j )
T[(i-1)*NVMAX + tcol+j] = T[i*NVMAX + tcol+j];
/* gimmick: write zeros into the last row of T */
for( j=0; j<nAC; ++j )
T[(nAC-1)*NVMAX + tcol+j] = 0.0;
/* II) RESTORE TRIANGULAR FORM OF T,
* use column-wise Givens rotations to restore reverse triangular form
* of T simultanenous change of Q (i.e. Y). */
real_t c, s;
for( j=(nAC-2-number_idx); j>=0; --j )
{
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 );
for( i=(nAC-j-1); i<(nAC-1); ++i )
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] );
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
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] );
}
}
}
else
{
/* gimmick: write zeros into the last row of T */
for( j=0; j<nAC; ++j )
T[(nAC-1)*NVMAX + tcol+j] = 0.0;
}
if ( ( updateCholesky == BT_TRUE ) && ( hessianType != HST_IDENTITY ) )
{
/* III) UPDATE CHOLESKY DECOMPOSITION,
* calculate new additional column (i.e. [r sqrt(rho2)]')
* of the Cholesky factor R. */
real_t Hz[NVMAX];
for ( i=0; i<nFR; ++i )
Hz[i] = 0.0;
real_t rho2 = 0.0;
/* 1) Calculate Hz = H*z, where z is the new rightmost column of Z
* (i.e. the old leftmost column of Y). */
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
for( i=0; i<nFR; ++i )
Hz[i] += H[jj*NVMAX + FR_idx[i]] * Q[jj*NVMAX + nZ];
}
if ( nZ > 0 )
{
real_t ZHz[NVMAX];
for ( i=0; i<nZ; ++i )
ZHz[i] = 0.0;
real_t r[NVMAX];
/* 2) Calculate ZHz = Z'*Hz (old Z). */
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
for( i=0; i<nZ; ++i )
ZHz[i] += Q[jj*NVMAX + i] * Hz[j];
}
/* 3) Calculate r = R^-T * ZHz. */
if ( backsolveR( ZHz,BT_TRUE,r ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
/* 4) Calculate rho2 = rho^2 = z'*Hz - r'*r
* and store r into R. */
for( i=0; i<nZ; ++i )
{
rho2 -= r[i]*r[i];
R[i*NVMAX + nZ] = r[i];
}
}
for( j=0; j<nFR; ++j )
rho2 += Q[FR_idx[j]*NVMAX + nZ] * Hz[j];
/* 5) Store rho into R. */
if ( rho2 > 0.0 )
R[nZ*NVMAX + nZ] = sqrt( rho2 );
else
{
hessianType = HST_SEMIDEF;
return THROWERROR( RET_HESSIAN_NOT_SPD );
}
}
/* IV) UPDATE INDICES */
if ( constraints.moveActiveToInactive( number ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
return SUCCESSFUL_RETURN;
}
/*
* r e m o v e B o u n d
*/
returnValue QProblem::removeBound( int number,
BooleanType updateCholesky
)
{
int i, j, ii, jj;
/* consistency checks */
if ( bounds.getStatus( number ) == ST_INACTIVE )
return THROWERROR( RET_BOUND_NOT_ACTIVE );
if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
( getStatus( ) == QPS_HOMOTOPYQPSOLVED ) ||
( getStatus( ) == QPS_SOLVED ) )
{
return THROWERROR( RET_UNKNOWN_BUG );
}
/* some definitions */
int nFR = getNFR( );
int nAC = getNAC( );
int nZ = getNZ( );
int tcol = sizeT - nAC;
/* I) UPDATE INDICES */
if ( bounds.moveFixedToFree( number ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_REMOVEBOUND_FAILED );
int FR_idx[NVMAX];
if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_REMOVEBOUND_FAILED );
/* I) APPEND <nFR+1>th UNITY VECOTR TO Q. */
int nnFRp1 = FR_idx[nFR];
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
Q[ii*NVMAX + nFR] = 0.0;
Q[nnFRp1*NVMAX + i] = 0.0;
}
Q[nnFRp1*NVMAX + nFR] = 1.0;
if ( nAC > 0 )
{
/* store new column a in a temporary vector instead of shifting T one column to the left and appending a */
int AC_idx[NCMAX_ALLOC];
if ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_REMOVEBOUND_FAILED );
real_t tmp[NCMAX_ALLOC];
for( i=0; i<nAC; ++i )
{
ii = AC_idx[i];
tmp[i] = A[ii*NVMAX + number];
}
/* II) RESTORE TRIANGULAR FORM OF T,
* use column-wise Givens rotations to restore reverse triangular form
* of T = [T A(:,number)], simultanenous change of Q (i.e. Y and Z). */
real_t c, s;
for( j=(nAC-1); j>=0; --j )
{
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 );
for( i=(nAC-j); i<nAC; ++i )
applyGivens( c,s,tmp[i],T[i*NVMAX + tcol+j],T[i*NVMAX + tcol+j],tmp[i] );
for( i=0; i<=nFR; ++i )
{
ii = FR_idx[i];
/* nZ+1+nAC = nFR+1 / nZ+(1) = nZ+1 */
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] );
}
}
}
if ( ( updateCholesky == BT_TRUE ) && ( hessianType != HST_IDENTITY ) )
{
/* III) UPDATE CHOLESKY DECOMPOSITION,
* calculate new additional column (i.e. [r sqrt(rho2)]')
* of the Cholesky factor R: */
real_t z2 = Q[nnFRp1*NVMAX + nZ];
real_t rho2 = H[nnFRp1*NVMAX + nnFRp1]*z2*z2; /* rho2 = h2*z2*z2 */
if ( nFR > 0 )
{
real_t Hz[NVMAX];
for( i=0; i<nFR; ++i )
Hz[i] = 0.0;
/* 1) Calculate R'*r = Zfr'*Hfr*z1 + z2*Zfr'*h1 =: Zfr'*Hz + z2*Zfr'*h1 =: rhs and
* rho2 = z1'*Hfr*z1 + 2*z2*h1'*z1 + h2*z2^2 - r'*r =: z1'*Hz + 2*z2*h1'*z1 + h2*z2^2 - r'r */
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
/* H * z1 */
Hz[i] += H[jj*NVMAX + ii] * Q[jj*NVMAX + nZ];
}
}
if ( nZ > 0 )
{
real_t r[NVMAX];
real_t rhs[NVMAX];
for( i=0; i<nZ; ++i )
rhs[i] = 0.0;
/* 2) Calculate rhs. */
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
for( i=0; i<nZ; ++i )
/* Zfr' * ( Hz + z2*h1 ) */
rhs[i] += Q[jj*NVMAX + i] * ( Hz[j] + z2 * H[nnFRp1*NVMAX + jj] );
}
/* 3) Calculate r = R^-T * rhs. */
if ( backsolveR( rhs,BT_TRUE,BT_TRUE,r ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_REMOVEBOUND_FAILED );
/* 4) Calculate rho2 = rho^2 = z'*Hz - r'*r
* and store r into R. */
for( i=0; i<nZ; ++i )
{
rho2 -= r[i]*r[i];
R[i*NVMAX + nZ] = r[i];
}
}
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
/* z1' * ( Hz + 2*z2*h1 ) */
rho2 += Q[jj*NVMAX + nZ] * ( Hz[j] + 2.0*z2*H[nnFRp1*NVMAX + jj] );
}
}
/* 5) Store rho into R. */
if ( rho2 > 0.0 )
R[nZ*NVMAX + nZ] = sqrt( rho2 );
else
{
hessianType = HST_SEMIDEF;
return THROWERROR( RET_HESSIAN_NOT_SPD );
}
}
return SUCCESSFUL_RETURN;
}
/*
* b a c k s o l v e R (CODE DUPLICATE OF QProblemB CLASS!!!)
*/
returnValue QProblem::backsolveR( const real_t* const b, BooleanType transposed,
real_t* const a
)
{
/* Call standard backsolve procedure (i.e. removingBound == BT_FALSE). */
return backsolveR( b,transposed,BT_FALSE,a );
}
/*
* b a c k s o l v e R (CODE DUPLICATE OF QProblemB CLASS!!!)
*/
returnValue QProblem::backsolveR( const real_t* const b, BooleanType transposed,
BooleanType removingBound,
real_t* const a
)
{
int i, j;
int nR = getNZ( );
real_t sum;
/* if backsolve is called while removing a bound, reduce nZ by one. */
if ( removingBound == BT_TRUE )
--nR;
/* nothing to do */
if ( nR <= 0 )
return SUCCESSFUL_RETURN;
/* Solve Ra = b, where R might be transposed. */
if ( transposed == BT_FALSE )
{
/* solve Ra = b */
for( i=(nR-1); i>=0; --i )
{
sum = b[i];
for( j=(i+1); j<nR; ++j )
sum -= R[i*NVMAX + j] * a[j];
if ( getAbs( R[i*NVMAX + i] ) > ZERO )
a[i] = sum / R[i*NVMAX + i];
else
return THROWERROR( RET_DIV_BY_ZERO );
}
}
else
{
/* solve R^T*a = b */
for( i=0; i<nR; ++i )
{
sum = b[i];
for( j=0; j<i; ++j )
sum -= R[j*NVMAX + i] * a[j];
if ( getAbs( R[i*NVMAX + i] ) > ZERO )
a[i] = sum / R[i*NVMAX + i];
else
return THROWERROR( RET_DIV_BY_ZERO );
}
}
return SUCCESSFUL_RETURN;
}
/*
* b a c k s o l v e T
*/
returnValue QProblem::backsolveT( const real_t* const b, BooleanType transposed, real_t* const a )
{
int i, j;
int nT = getNAC( );
int tcol = sizeT - nT;
real_t sum;
/* nothing to do */
if ( nT <= 0 )
return SUCCESSFUL_RETURN;
/* Solve Ta = b, where T might be transposed. */
if ( transposed == BT_FALSE )
{
/* solve Ta = b */
for( i=0; i<nT; ++i )
{
sum = b[i];
for( j=0; j<i; ++j )
sum -= T[i*NVMAX + sizeT-1-j] * a[nT-1-j];
if ( getAbs( T[i*NVMAX + sizeT-1-i] ) > ZERO )
a[nT-1-i] = sum / T[i*NVMAX + sizeT-1-i];
else
return THROWERROR( RET_DIV_BY_ZERO );
}
}
else
{
/* solve T^T*a = b */
for( i=0; i<nT; ++i )
{
sum = b[i];
for( j=0; j<i; ++j )
sum -= T[(nT-1-j)*NVMAX + tcol+i] * a[nT-1-j];
if ( getAbs( T[(nT-1-i)*NVMAX + tcol+i] ) > ZERO )
a[nT-1-i] = sum / T[(nT-1-i)*NVMAX + tcol+i];
else
return THROWERROR( RET_DIV_BY_ZERO );
}
}
return SUCCESSFUL_RETURN;
}
/*
* 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
*/
returnValue QProblem::hotstart_determineDataShift( const int* const FX_idx, const int* const AC_idx,
const real_t* const g_new, const real_t* const lbA_new, const real_t* const ubA_new,
const real_t* const lb_new, const real_t* const ub_new,
real_t* const delta_g, real_t* const delta_lbA, real_t* const delta_ubA,
real_t* const delta_lb, real_t* const delta_ub,
BooleanType& Delta_bC_isZero, BooleanType& Delta_bB_isZero
)
{
int i, ii;
int nC = getNC( );
int nAC = getNAC( );
/* I) DETERMINE DATA SHIFT FOR BOUNDS */
QProblemB::hotstart_determineDataShift( FX_idx,g_new,lb_new,ub_new, delta_g,delta_lb,delta_ub, Delta_bB_isZero );
/* II) DETERMINE DATA SHIFT FOR CONSTRAINTS */
/* 1) Calculate shift directions. */
for( i=0; i<nC; ++i )
{
/* if lower constraints' bounds do not exist, shift them to -infinity */
if ( lbA_new != 0 )
delta_lbA[i] = lbA_new[i] - lbA[i];
else
delta_lbA[i] = -INFTY - lbA[i];
}
for( i=0; i<nC; ++i )
{
/* if upper constraints' bounds do not exist, shift them to infinity */
if ( ubA_new != 0 )
delta_ubA[i] = ubA_new[i] - ubA[i];
else
delta_ubA[i] = INFTY - ubA[i];
}
/* 2) Determine if active constraints' bounds are to be shifted. */
Delta_bC_isZero = BT_TRUE;
for ( i=0; i<nAC; ++i )
{
ii = AC_idx[i];
if ( ( getAbs( delta_lbA[ii] ) > EPS ) || ( getAbs( delta_ubA[ii] ) > EPS ) )
{
Delta_bC_isZero = BT_FALSE;
break;
}
}
return SUCCESSFUL_RETURN;
}
/*
* 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
*/
returnValue QProblem::hotstart_determineStepDirection( const int* const FR_idx, const int* const FX_idx, const int* const AC_idx,
const real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA,
const real_t* const delta_lb, const real_t* const delta_ub,
BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero,
real_t* const delta_xFX, real_t* const delta_xFR,
real_t* const delta_yAC, real_t* const delta_yFX
)
{
int i, j, ii, jj;
int nFR = getNFR( );
int nFX = getNFX( );
int nAC = getNAC( );
int nZ = getNZ( );
/* initialise auxiliary vectors */
real_t HMX_delta_xFX[NVMAX];
real_t YFR_delta_xFRy[NVMAX];
real_t ZFR_delta_xFRz[NVMAX];
real_t HFR_YFR_delta_xFRy[NVMAX];
for( i=0; i<nFR; ++i )
{
delta_xFR[i] = 0.0;
HMX_delta_xFX[i] = 0.0;
YFR_delta_xFRy[i] = 0.0;
ZFR_delta_xFRz[i] = 0.0;
HFR_YFR_delta_xFRy[i] = 0.0;
}
real_t delta_xFRy[NCMAX_ALLOC];
real_t delta_xFRz[NVMAX];
for( i=0; i<nZ; ++i )
delta_xFRz[i] = 0.0;
/* I) DETERMINE delta_xFX */
if ( nFX > 0 )
{
for( i=0; i<nFX; ++i )
{
ii = FX_idx[i];
if ( bounds.getStatus( ii ) == ST_LOWER )
delta_xFX[i] = delta_lb[ii];
else
delta_xFX[i] = delta_ub[ii];
}
}
/* II) DETERMINE delta_xFR */
if ( nFR > 0 )
{
/* 1) Determine delta_xFRy. */
if ( nAC > 0 )
{
if ( ( Delta_bC_isZero == BT_TRUE ) && ( Delta_bB_isZero == BT_TRUE ) )
{
for( i=0; i<nAC; ++i )
delta_xFRy[i] = 0.0;
for( i=0; i<nFR; ++i )
delta_xFR[i] = 0.0;
}
else
{
/* auxillary variable */
real_t delta_xFRy_TMP[NCMAX_ALLOC];
for( i=0; i<nAC; ++i )
{
ii = AC_idx[i];
if ( constraints.getStatus( ii ) == ST_LOWER )
delta_xFRy_TMP[i] = delta_lbA[ii];
else
delta_xFRy_TMP[i] = delta_ubA[ii];
if ( Delta_bB_isZero == BT_FALSE )
{
for( j=0; j<nFX; ++j )
{
jj = FX_idx[j];
delta_xFRy_TMP[i] -= A[ii*NVMAX + jj] * delta_xFX[j];
}
}
}
if ( backsolveT( delta_xFRy_TMP, BT_FALSE, delta_xFRy ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_STEPDIRECTION_FAILED_TQ );
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
for( j=0; j<nAC; ++j )
YFR_delta_xFRy[i] += Q[ii*NVMAX + nZ+j] * delta_xFRy[j];
/* delta_xFR = YFR*delta_xFRy (+ ZFR*delta_xFRz) */
delta_xFR[i] = YFR_delta_xFRy[i];
}
}
}
/* 2) Determine delta_xFRz. */
if ( hessianType == HST_IDENTITY )
{
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
for( i=0; i<nZ; ++i )
delta_xFRz[i] -= Q[jj*NVMAX + i] * delta_g[jj];
}
if ( nZ > 0 )
{
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
for( j=0; j<nZ; ++j )
ZFR_delta_xFRz[i] += Q[ii*NVMAX + j] * delta_xFRz[j];
delta_xFR[i] += ZFR_delta_xFRz[i];
}
}
}
else
{
if ( Delta_bB_isZero == BT_FALSE )
{
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
for( j=0; j<nFX; ++j )
{
jj = FX_idx[j];
HMX_delta_xFX[i] += H[ii*NVMAX + jj] * delta_xFX[j];
}
}
}
if ( nAC > 0 )
{
if ( ( Delta_bC_isZero == BT_FALSE ) || ( Delta_bB_isZero == BT_FALSE ) )
{
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
HFR_YFR_delta_xFRy[i] += H[ii*NVMAX + jj] * YFR_delta_xFRy[j];
}
}
}
}
if ( nZ > 0 )
{
/* auxiliary variable */
real_t delta_xFRz_TMP[NVMAX];
real_t delta_xFRz_RHS[NVMAX];
if ( ( nAC > 0 ) && ( nFX > 0 ) && ( Delta_bB_isZero == BT_FALSE ) )
{
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
delta_xFRz_RHS[j] = delta_g[jj] + HFR_YFR_delta_xFRy[j] + HMX_delta_xFX[j];
}
}
else
{
if ( ( nAC == 0 ) && ( Delta_bB_isZero == BT_TRUE ) )
{
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
delta_xFRz_RHS[j] = delta_g[jj];
}
}
else
{
if ( nAC > 0 ) /* => Delta_bB_isZero == BT_TRUE, as BT_FALSE would imply nFX>0 */
{
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
delta_xFRz_RHS[j] = delta_g[jj] + HFR_YFR_delta_xFRy[j];
}
}
else /* Delta_bB_isZero == BT_FALSE, as nAC==0 */
{
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
delta_xFRz_RHS[j] = delta_g[jj] + HMX_delta_xFX[j];
}
}
}
}
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
for( i=0; i<nZ; ++i )
delta_xFRz[i] -= Q[jj*NVMAX + i] * delta_xFRz_RHS[j];
}
if ( backsolveR( delta_xFRz,BT_TRUE,delta_xFRz_TMP ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
if ( backsolveR( delta_xFRz_TMP,BT_FALSE,delta_xFRz ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
for( j=0; j<nZ; ++j )
ZFR_delta_xFRz[i] += Q[ii*NVMAX + j] * delta_xFRz[j];
delta_xFR[i] += ZFR_delta_xFRz[i];
}
}
}
}
/* III) DETERMINE delta_yAC */
if ( nAC > 0 ) /* => ( nFR = nZ + nAC > 0 ) */
{
/* auxiliary variables */
real_t delta_yAC_TMP[NCMAX_ALLOC];
for( i=0; i<nAC; ++i )
delta_yAC_TMP[i] = 0.0;
real_t delta_yAC_RHS[NVMAX];
for( i=0; i<nFR; ++i )
delta_yAC_RHS[i] = 0.0;
if ( hessianType == HST_IDENTITY )
{
/* delta_yAC = (T')^-1 * ( Yfr*delta_gFR + delta_xFRy ) */
for( j=0; j<nAC; ++j )
{
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
delta_yAC_TMP[j] += Q[ii*NVMAX + nZ+j] * delta_g[ii];
}
delta_yAC_TMP[j] += delta_xFRy[j];
}
}
else
{
if ( ( Delta_bC_isZero == BT_TRUE ) && ( Delta_bB_isZero == BT_TRUE ) )
{
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
delta_yAC_RHS[i] = delta_g[ii];
}
}
else
{
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
delta_yAC_RHS[i] = HFR_YFR_delta_xFRy[i] + delta_g[ii];
}
}
if ( nZ > 0 )
{
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
delta_yAC_RHS[i] += H[ii*NVMAX + jj] * ZFR_delta_xFRz[j];
}
}
}
if ( nFX > 0 )
{
if ( Delta_bB_isZero == BT_FALSE )
{
for( i=0; i<nFR; ++i )
delta_yAC_RHS[i] += HMX_delta_xFX[i];
}
}
for( i=0; i<nAC; ++i)
{
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
delta_yAC_TMP[i] += Q[jj*NVMAX + nZ+i] * delta_yAC_RHS[j];
}
}
}
if ( backsolveT( delta_yAC_TMP,BT_TRUE,delta_yAC ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_STEPDIRECTION_FAILED_TQ );
}
/* IV) DETERMINE delta_yFX */
if ( nFX > 0 )
{
for( i=0; i<nFX; ++i )
{
ii = FX_idx[i];
delta_yFX[i] = delta_g[ii];
for( j=0; j<nAC; ++j )
{
jj = AC_idx[j];
delta_yFX[i] -= A[jj*NVMAX + ii] * delta_yAC[j];
}
if ( hessianType == HST_IDENTITY )
{
delta_yFX[i] += delta_xFX[i];
}
else
{
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
delta_yFX[i] += H[ii*NVMAX + jj] * delta_xFR[j];
}
if ( Delta_bB_isZero == BT_FALSE )
{
for( j=0; j<nFX; ++j )
{
jj = FX_idx[j];
delta_yFX[i] += H[ii*NVMAX + jj] * delta_xFX[j];
}
}
}
}
}
return SUCCESSFUL_RETURN;
}
/*
* 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
*/
returnValue QProblem::hotstart_determineStepLength( const int* const FR_idx, const int* const FX_idx, const int* const AC_idx, const int* const IAC_idx,
const real_t* const delta_lbA, const real_t* const delta_ubA,
const real_t* const delta_lb, const real_t* const delta_ub,
const real_t* const delta_xFX, const real_t* const delta_xFR,
const real_t* const delta_yAC, const real_t* const delta_yFX,
real_t* const delta_Ax, int& BC_idx, SubjectToStatus& BC_status, BooleanType& BC_isBound
)
{
int i, j, ii, jj;
int nV = getNV( );
int nC = getNC( );
int nFR = getNFR( );
int nFX = getNFX( );
int nAC = getNAC( );
int nIAC = getNIAC( );
/* initialise maximum steplength array */
real_t maxStepLength[2*(NVMAX+NCMAX_ALLOC)];
for ( i=0; i<2*(nV+nC); ++i )
maxStepLength[i] = 1.0;
/* I) DETERMINE MAXIMUM DUAL STEPLENGTH: */
/* 1) Ensure that active dual constraints' bounds remain valid
* (ignoring inequality constraints). */
for( i=0; i<nAC; ++i )
{
ii = AC_idx[i];
if ( constraints.getType( ii ) != ST_EQUALITY )
{
if ( constraints.getStatus( ii ) == ST_LOWER )
{
/* active lower constraints' bounds */
if ( delta_yAC[i] < -ZERO )
{
if ( y[nV+ii] > 0.0 )
maxStepLength[nV+ii] = y[nV+ii] / ( -delta_yAC[i] );
else
maxStepLength[nV+ii] = 0.0;
}
}
else
{
/* active upper constraints' bounds */
if ( delta_yAC[i] > ZERO )
{
if ( y[nV+ii] < 0.0 )
maxStepLength[nV+ii] = y[nV+ii] / ( -delta_yAC[i] );
else
maxStepLength[nV+ii] = 0.0;
}
}
}
}
/* 2) Ensure that active dual bounds remain valid
* (ignoring implicitly fixed variables). */
for( i=0; i<nFX; ++i )
{
ii = FX_idx[i];
if ( bounds.getType( ii ) != ST_EQUALITY )
{
if ( bounds.getStatus( ii ) == ST_LOWER )
{
/* active lower bounds */
if ( delta_yFX[i] < -ZERO )
{
if ( y[ii] > 0.0 )
maxStepLength[ii] = y[ii] / ( -delta_yFX[i] );
else
maxStepLength[ii] = 0.0;
}
}
else
{
/* active upper bounds */
if ( delta_yFX[i] > ZERO )
{
if ( y[ii] < 0.0 )
maxStepLength[ii] = y[ii] / ( -delta_yFX[i] );
else
maxStepLength[ii] = 0.0;
}
}
}
}
/* II) DETERMINE MAXIMUM PRIMAL STEPLENGTH */
/* 1) Ensure that inactive constraints' bounds remain valid
* (ignoring unbounded constraints). */
real_t delta_x[NVMAX];
for( j=0; j<nFR; ++j )
{
jj = FR_idx[j];
delta_x[jj] = delta_xFR[j];
}
for( j=0; j<nFX; ++j )
{
jj = FX_idx[j];
delta_x[jj] = delta_xFX[j];
}
for( i=0; i<nIAC; ++i )
{
ii = IAC_idx[i];
if ( constraints.getType( ii ) != ST_UNBOUNDED )
{
delta_Ax[ii] = 0.0;
for( j=0; j<nV; ++j )
delta_Ax[ii] += A[ii*NVMAX + j] * delta_x[j]; // POSSIBLE SPEEDUP!
/* inactive lower constraints' bounds */
if ( constraints.isNoLower( ) == BT_FALSE )
{
if ( delta_lbA[ii] > delta_Ax[ii] )
{
if ( Ax[ii] > lbA[ii] )
maxStepLength[nV+ii] = ( Ax[ii] - lbA[ii] ) / ( delta_lbA[ii] - delta_Ax[ii] );
else
maxStepLength[nV+ii] = 0.0;
}
}
/* inactive upper constraints' bounds */
if ( constraints.isNoUpper( ) == BT_FALSE )
{
if ( delta_ubA[ii] < delta_Ax[ii] )
{
if ( Ax[ii] < ubA[ii] )
maxStepLength[nV+nC+nV+ii] = ( Ax[ii] - ubA[ii] ) / ( delta_ubA[ii] - delta_Ax[ii] );
else
maxStepLength[nV+nC+nV+ii] = 0.0;
}
}
}
}
/* 2) Ensure that inactive bounds remain valid
* (ignoring unbounded variables). */
/* inactive lower bounds */
if ( bounds.isNoLower( ) == BT_FALSE )
{
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
if ( bounds.getType( ii ) != ST_UNBOUNDED )
if ( delta_lb[ii] > delta_xFR[i] )
{
if ( x[ii] > lb[ii] )
maxStepLength[ii] = ( x[ii] - lb[ii] ) / ( delta_lb[ii] - delta_xFR[i] );
else
maxStepLength[ii] = 0.0;
}
}
}
/* inactive upper bounds */
if ( bounds.isNoUpper( ) == BT_FALSE )
{
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
if ( bounds.getType( ii ) != ST_UNBOUNDED )
if ( delta_ub[ii] < delta_xFR[i] )
{
if ( x[ii] < ub[ii] )
maxStepLength[nV+nC+ii] = ( x[ii] - ub[ii] ) / ( delta_ub[ii] - delta_xFR[i] );
else
maxStepLength[nV+nC+ii] = 0.0;
}
}
}
/* III) DETERMINE MAXIMUM HOMOTOPY STEPLENGTH */
real_t tau_new = 1.0;
BC_idx = 0;
BC_status = ST_UNDEFINED;
BC_isBound = BT_FALSE;
for ( i=0; i<nV; ++i )
{
/* 1) Consider lower/dual blocking bounds. */
if ( maxStepLength[i] < tau_new )
{
tau_new = maxStepLength[i];
BC_idx = i;
BC_isBound = BT_TRUE;
if ( bounds.getStatus( i ) == ST_INACTIVE ) /* inactive? */
BC_status = ST_LOWER;
else
BC_status = ST_INACTIVE;
}
/* 2) Consider upper blocking bounds. */
if ( maxStepLength[nV+nC+i] < tau_new )
{
tau_new = maxStepLength[nV+nC+i];
BC_idx = i;
BC_isBound = BT_TRUE;
BC_status = ST_UPPER;
}
}
for ( i=nV; i<nV+nC; ++i )
{
/* 3) Consider lower/dual blocking constraints. */
if ( maxStepLength[i] < tau_new )
{
tau_new = maxStepLength[i];
BC_idx = i-nV;
BC_isBound = BT_FALSE;
if ( constraints.getStatus( i-nV ) == ST_INACTIVE ) /* inactive? */
BC_status = ST_LOWER;
else
BC_status = ST_INACTIVE;
}
/* 4) Consider upper blocking constraints. */
if ( maxStepLength[nV+nC+i] < tau_new )
{
tau_new = maxStepLength[nV+nC+i];
BC_idx = i-nV;
BC_isBound = BT_FALSE;
BC_status = ST_UPPER;
}
}
/* IV) CLEAR CYCLING DATA
* if a positive step can be taken */
if ( tau_new > EPS )
cyclingManager.clearCyclingData( );
/* V) SET MAXIMUM HOMOTOPY STEPLENGTH */
tau = tau_new;
#ifdef PC_DEBUG
if ( printlevel == PL_HIGH )
{
char messageString[80];
if ( BC_status == ST_UNDEFINED )
sprintf( messageString,"Stepsize is %.6e!",tau );
else
sprintf( messageString,"Stepsize is %.6e! (BC_idx = %d, BC_isBound = %d, BC_status = %d)",tau,BC_idx,BC_isBound,BC_status );
getGlobalMessageHandler( )->throwInfo( RET_STEPSIZE_NONPOSITIVE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
}
#endif
return SUCCESSFUL_RETURN;
}
/*
* h o t s t a r t _ p e r f o r m S t e p
*/
returnValue QProblem::hotstart_performStep( const int* const FR_idx, const int* const FX_idx, const int* const AC_idx, const int* const IAC_idx,
const real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA,
const real_t* const delta_lb, const real_t* const delta_ub,
const real_t* const delta_xFX, const real_t* const delta_xFR,
const real_t* const delta_yAC, const real_t* const delta_yFX,
const real_t* const delta_Ax, int BC_idx, SubjectToStatus BC_status, BooleanType BC_isBound
)
{
int i, j, ii;
int nV = getNV( );
int nC = getNC( );
int nFR = getNFR( );
int nFX = getNFX( );
int nAC = getNAC( );
int nIAC = getNIAC( );
/* I) CHECK (CONSTRAINTS') BOUNDS' CONSISTENCY */
if ( areBoundsConsistent( delta_lb,delta_ub,delta_lbA,delta_ubA ) == BT_FALSE )
{
infeasible = BT_TRUE;
tau = 0.0;
return THROWERROR( RET_QP_INFEASIBLE );
}
/* II) GO TO ACTIVE SET CHANGE */
if ( tau > ZERO )
{
/* 1) Perform step in primal und dual space... */
for( i=0; i<nFR; ++i )
{
ii = FR_idx[i];
x[ii] += tau*delta_xFR[i];
}
for( i=0; i<nFX; ++i )
{
ii = FX_idx[i];
x[ii] += tau*delta_xFX[i];
y[ii] += tau*delta_yFX[i];
}
for( i=0; i<nAC; ++i )
{
ii = AC_idx[i];
y[nV+ii] += tau*delta_yAC[i];
}
/* ... also for Ax. */
for( i=0; i<nIAC; ++i )
{
ii = IAC_idx[i];
if ( constraints.getType( ii ) != ST_UNBOUNDED )
Ax[ii] += tau*delta_Ax[ii];
}
for( i=0; i<nAC; ++i )
{
ii = AC_idx[i];
Ax[ii] = 0.0;
for( j=0; j<nV; ++j )
Ax[ii] += A[ii*NVMAX + j] * x[j];
}
/* 2) Shift QP data. */
for( i=0; i<nV; ++i )
{
g[i] += tau*delta_g[i];
lb[i] += tau*delta_lb[i];
ub[i] += tau*delta_ub[i];
}
for( i=0; i<nC; ++i )
{
lbA[i] += tau*delta_lbA[i];
ubA[i] += tau*delta_ubA[i];
}
}
else
{
/* print a stepsize warning if stepsize is zero */
#ifdef PC_DEBUG
char messageString[80];
sprintf( messageString,"Stepsize is %.6e",tau );
getGlobalMessageHandler( )->throwWarning( RET_STEPSIZE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
#endif
}
/* setup output preferences */
#ifdef PC_DEBUG
char messageString[80];
VisibilityStatus visibilityStatus;
if ( printlevel == PL_HIGH )
visibilityStatus = VS_VISIBLE;
else
visibilityStatus = VS_HIDDEN;
#endif
/* III) UPDATE ACTIVE SET */
switch ( BC_status )
{
/* Optimal solution found as no working set change detected. */
case ST_UNDEFINED:
return RET_OPTIMAL_SOLUTION_FOUND;
/* Remove one variable from active set. */
case ST_INACTIVE:
if ( BC_isBound == BT_TRUE )
{
#ifdef PC_DEBUG
sprintf( messageString,"bound no. %d.", BC_idx );
getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
#endif
if ( removeBound( BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
y[BC_idx] = 0.0;
}
else
{
#ifdef PC_DEBUG
sprintf( messageString,"constraint no. %d.", BC_idx );
getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
#endif
if ( removeConstraint( BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
y[nV+BC_idx] = 0.0;
}
break;
/* Add one variable to active set. */
default:
if ( BC_isBound == BT_TRUE )
{
#ifdef PC_DEBUG
if ( BC_status == ST_LOWER )
sprintf( messageString,"lower bound no. %d.", BC_idx );
else
sprintf( messageString,"upper bound no. %d.", BC_idx );
getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
#endif
if ( addBound( BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
}
else
{
#ifdef PC_DEBUG
if ( BC_status == ST_LOWER )
sprintf( messageString,"lower constraint's bound no. %d.", BC_idx );
else
sprintf( messageString,"upper constraint's bound no. %d.", BC_idx );
getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
#endif
if ( addConstraint( BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
}
break;
}
return SUCCESSFUL_RETURN;
}
/*
* a r e B o u n d s C o n s i s t e n t
*/
BooleanType QProblem::areBoundsConsistent( const real_t* const delta_lb, const real_t* const delta_ub,
const real_t* const delta_lbA, const real_t* const delta_ubA
) const
{
int i;
/* 1) Check bounds' consistency. */
if ( QProblemB::areBoundsConsistent( delta_lb,delta_ub ) == BT_FALSE )
return BT_FALSE;
/* 2) Check constraints' consistency, i.e.
* check if delta_lb[i] is greater than delta_ub[i]
* for a component i whose bounds are already (numerically) equal. */
for( i=0; i<getNC( ); ++i )
if ( ( lbA[i] > ubA[i] - BOUNDTOL ) && ( delta_lbA[i] > delta_ubA[i] + EPS ) )
return BT_FALSE;
return BT_TRUE;
}
/*
* s e t u p Q P d a t a
*/
returnValue QProblem::setupQPdata( const real_t* const _H, const real_t* const _R, const real_t* const _g, const real_t* const _A,
const real_t* const _lb, const real_t* const _ub,
const real_t* const _lbA, const real_t* const _ubA
)
{
int i, j;
int nV = getNV( );
int nC = getNC( );
/* 1) Load Hessian matrix as well as lower and upper bounds vectors. */
if (QProblemB::setupQPdata(_H, _R, _g, _lb, _ub) != SUCCESSFUL_RETURN)
return THROWERROR( RET_INVALID_ARGUMENTS );
/* 2) Load constraint matrix. */
if ( ( nC > 0 ) && ( _A == 0 ) )
return THROWERROR( RET_INVALID_ARGUMENTS );
if ( nC > 0 )
{
for( i=0; i<nC; ++i )
for( j=0; j<nV; ++j )
A[i*NVMAX + j] = _A[i*nV + j];
/* 3) Setup lower constraints' bounds vector. */
if ( _lbA != 0 )
{
for( i=0; i<nC; ++i )
lbA[i] = _lbA[i];
}
else
{
/* if no lower constraints' bounds are specified, set them to -infinity */
for( i=0; i<nC; ++i )
lbA[i] = -INFTY;
}
/* 4) Setup upper constraints' bounds vector. */
if ( _ubA != 0 )
{
for( i=0; i<nC; ++i )
ubA[i] = _ubA[i];
}
else
{
/* if no upper constraints' bounds are specified, set them to infinity */
for( i=0; i<nC; ++i )
ubA[i] = INFTY;
}
}
// printmatrix2( "A",A,10,20 );
// printmatrix2( "lbA",lbA,1,nC );
// printmatrix2( "ubA",ubA,1,nC );
return SUCCESSFUL_RETURN;
}
#ifdef PC_DEBUG /* Define print functions only for debugging! */
/*
* p r i n t I t e r a t i o n
*/
returnValue QProblem::printIteration( int iteration,
int BC_idx, SubjectToStatus BC_status, BooleanType BC_isBound
)
{
char myPrintfString[160];
/* consistency check */
if ( iteration < 0 )
return THROWERROR( RET_INVALID_ARGUMENTS );
/* nothing to do */
if ( printlevel != PL_MEDIUM )
return SUCCESSFUL_RETURN;
/* 1) Print header at first iteration. */
if ( iteration == 0 )
{
sprintf( myPrintfString,"\n############## qpOASES -- QP NO.%4.1d ###############\n", count );
myPrintf( myPrintfString );
sprintf( myPrintfString," Iter | StepLength | Info | nFX | nAC \n" );
myPrintf( myPrintfString );
}
/* 2) Print iteration line. */
if ( BC_status == ST_UNDEFINED )
{
sprintf( myPrintfString," %4.1d | %1.5e | QP SOLVED | %4.1d | %4.1d \n", iteration,tau,getNFX( ),getNAC( ) );
myPrintf( myPrintfString );
}
else
{
char info[8];
if ( BC_status == ST_INACTIVE )
sprintf( info,"REM " );
else
sprintf( info,"ADD " );
if ( BC_isBound == BT_TRUE )
sprintf( &(info[4]),"BND" );
else
sprintf( &(info[4]),"CON" );
sprintf( myPrintfString," %4.1d | %1.5e | %s%4.1d | %4.1d | %4.1d \n", iteration,tau,info,BC_idx,getNFX( ),getNAC( ) );
myPrintf( myPrintfString );
}
return SUCCESSFUL_RETURN;
}
/*
* p r i n t I t e r a t i o n
*/
returnValue QProblem::printIteration( int iteration,
int BC_idx, SubjectToStatus BC_status
)
{
return printIteration( iteration,BC_idx,BC_status,BT_TRUE );
}
#endif /* PC_DEBUG */
/*
* c h e c k K K T c o n d i t i o n s
*/
returnValue QProblem::checkKKTconditions( )
{
#ifdef __PERFORM_KKT_TEST__
int i, j, jj;
int nV = getNV( );
int nC = getNC( );
int nAC = getNAC( );
real_t tmp;
real_t maxKKTviolation = 0.0;
int AC_idx[NCMAX_ALLOC];
constraints.getActive( )->getNumberArray( AC_idx );
/* 1) check for Hx + g - [yFX yAC]*[Id A]' = 0. */
for( i=0; i<nV; ++i )
{
tmp = g[i];
for( j=0; j<nV; ++j )
tmp += H[i*NVMAX + j] * x[j];
tmp -= y[i];
/* Only sum over active constraints as y is zero for all inactive ones. */
for( j=0; j<nAC; ++j )
{
jj = AC_idx[j];
tmp -= A[jj*NVMAX + i] * y[nV+jj];
}
if ( getAbs( tmp ) > maxKKTviolation )
maxKKTviolation = getAbs( tmp );
}
/* 2) Check for [lb lbA] <= [Id A]*x <= [ub ubA]. */
/* lbA <= Ax <= ubA */
for( i=0; i<nC; ++i )
{
if ( lbA[i] - Ax[i] > maxKKTviolation )
maxKKTviolation = lbA[i] - Ax[i];
if ( Ax[i] - ubA[i] > maxKKTviolation )
maxKKTviolation = Ax[i] - ubA[i];
}
/* lb <= x <= ub */
for( i=0; i<nV; ++i )
{
if ( lb[i] - x[i] > maxKKTviolation )
maxKKTviolation = lb[i] - x[i];
if ( x[i] - ub[i] > maxKKTviolation )
maxKKTviolation = x[i] - ub[i];
}
/* 3) Check for correct sign of y and for complementary slackness. */
/* bounds */
for( i=0; i<nV; ++i )
{
switch ( bounds.getStatus( i ) )
{
case ST_LOWER:
if ( -y[i] > maxKKTviolation )
maxKKTviolation = -y[i];
if ( getAbs( x[i] - lb[i] ) > maxKKTviolation )
maxKKTviolation = getAbs( x[i] - lb[i] );
break;
case ST_UPPER:
if ( y[i] > maxKKTviolation )
maxKKTviolation = y[i];
if ( getAbs( ub[i] - x[i] ) > maxKKTviolation )
maxKKTviolation = getAbs( ub[i] - x[i] );
break;
default: /* inactive */
if ( getAbs( y[i] ) > maxKKTviolation )
maxKKTviolation = getAbs( y[i] );
break;
}
}
/* constraints */
for( i=0; i<nC; ++i )
{
switch ( constraints.getStatus( i ) )
{
case ST_LOWER:
if ( -y[nV+i] > maxKKTviolation )
maxKKTviolation = -y[nV+i];
if ( getAbs( Ax[i] - lbA[i] ) > maxKKTviolation )
maxKKTviolation = getAbs( Ax[i] - lbA[i] );
break;
case ST_UPPER:
if ( y[nV+i] > maxKKTviolation )
maxKKTviolation = y[nV+i];
if ( getAbs( ubA[i] - Ax[i] ) > maxKKTviolation )
maxKKTviolation = getAbs( ubA[i] - Ax[i] );
break;
default: /* inactive */
if ( getAbs( y[nV+i] ) > maxKKTviolation )
maxKKTviolation = getAbs( y[nV+i] );
break;
}
}
if ( maxKKTviolation > CRITICALACCURACY )
return RET_NO_SOLUTION;
if ( maxKKTviolation > DESIREDACCURACY )
return RET_INACCURATE_SOLUTION;
#endif /* __PERFORM_KKT_TEST__ */
return SUCCESSFUL_RETURN;
}
/*
* end of file
*/
Reference in New Issue View Git Blame Copy Permalink