sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
openpilot is an open source driver assistance system. openpilot performs the functions of Automated Lane Centering and Adaptive Cruise Control for over 200 supported car makes and models.
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1156 Commits
113 Branches
85 Tags
6.2 GiB
 
 
 
 
 
 
Tag: Branch: Tree: ed7e568b70
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'ed7e568b70'
${ noResults }
openpilot_comma/phonelibs/qpoases/SRC/QProblem.cpp

3867 lines
90 KiB
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
*/