Bounds::Ptr expand(const Bounds &bounds) const {
Bounds::Ptr pBounds;
switch (bounds.getType()) {
case Bounds::TYPE_PLANE:
pBounds = expand(dynamic_cast<const BoundingPlane&>(bounds));
break;
case Bounds::TYPE_SPHERE:
pBounds = expand(dynamic_cast<const BoundingSphere&>(bounds));
break;
case Bounds::TYPE_CYLINDER:
pBounds = expand(dynamic_cast<const BoundingCylinder&>(bounds));
break;
case Bounds::TYPE_BOX:
pBounds = expand(dynamic_cast<const BoundingBox&>(bounds));
break;
case Bounds::TYPE_CONVEX_MESH:
pBounds = expand(dynamic_cast<const BoundingConvexMesh&>(bounds));
break;
default:
break;
}
return pBounds;
}
/*
* s e t u p A u x i l i a r y Q P
*/
returnValue SQProblem::setupAuxiliaryQP ( SymmetricMatrix *H_new, Matrix *A_new,
const real_t *lb_new, const real_t *ub_new, const real_t *lbA_new, const real_t *ubA_new
)
{
int i;
int nV = getNV( );
int nC = getNC( );
returnValue returnvalue;
if ( ( getStatus( ) == QPS_NOTINITIALISED ) ||
( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
( getStatus( ) == QPS_PERFORMINGHOMOTOPY ) )
{
return THROWERROR( RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED );
}
status = QPS_PREPARINGAUXILIARYQP;
/* I) SETUP NEW QP MATRICES AND VECTORS: */
/* 1) Shift constraints' bounds vectors by (A_new - A)'*x_opt to ensure
* that old optimal solution remains feasible for new QP data. */
/* Firstly, shift by -A'*x_opt and ... */
if ( nC > 0 )
{
if ( A_new == 0 )
return THROWERROR( RET_INVALID_ARGUMENTS );
for ( i=0; i<nC; ++i )
{
lbA[i] = -Ax_l[i];
ubA[i] = Ax_u[i];
}
/* Set constraint matrix as well as ... */
setA( A_new );
/* ... secondly, shift by +A_new'*x_opt. */
for ( i=0; i<nC; ++i )
{
lbA[i] += Ax[i];
ubA[i] += Ax[i];
}
/* update constraint products. */
for ( i=0; i<nC; ++i )
{
Ax_u[i] = ubA[i] - Ax[i];
Ax_l[i] = Ax[i] - lbA[i];
}
}
/* 2) Set new Hessian matrix, determine Hessian type and
* regularise new Hessian matrix if necessary. */
/* a) Setup new Hessian matrix and determine its type. */
if ( H_new != 0 )
{
setH( H_new );
hessianType = HST_UNKNOWN;
if ( determineHessianType( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
/* b) Regularise new Hessian if necessary. */
if ( ( hessianType == HST_ZERO ) ||
( hessianType == HST_SEMIDEF ) ||
( usingRegularisation( ) == BT_TRUE ) )
{
regVal = 0.0; /* reset previous regularisation */
if ( regulariseHessian( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
}
}
else
{
if ( H != 0 )
return THROWERROR( RET_NO_HESSIAN_SPECIFIED );
}
/* 3) Setup QP gradient. */
if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
/* II) SETUP WORKING SETS AND MATRIX FACTORISATIONS: */
/* 1) Make a copy of current bounds/constraints ... */
Bounds oldBounds = bounds;
Constraints oldConstraints = constraints;
/* we're trying to find an active set with positive definite null
* space Hessian twice:
* - first for the current active set including all equalities
* - second after moving all inactive variables to a bound
* (depending on Options). This creates an empty null space and
* is guaranteed to succeed. Thus this loop will exit after n_try=1.
*/
int n_try;
for (n_try = 0; n_try < 2; ++n_try) {
if (n_try > 0) {
// the current active set leaves an indefinite null space Hessian
// move all inactive variables to a bound, creating an empty null space
for (int ii = 0; ii < nV; ++ii)
if (oldBounds.getStatus (ii) == ST_INACTIVE)
oldBounds.setStatus (ii, options.initialStatusBounds);
}
/* ... reset them ... */
bounds.init( nV );
constraints.init( nC );
/* ... and set them up afresh. */
if ( setupSubjectToType(lb_new,ub_new,lbA_new,ubA_new ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
if ( constraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
/* 2) Setup TQ factorisation. */
if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
// check for equalities that have become bounds ...
for (int ii = 0; ii < nC; ++ii) {
if (oldConstraints.getType (ii) == ST_EQUALITY && constraints.getType (ii) == ST_BOUNDED) {
if (oldConstraints.getStatus (ii) == ST_LOWER && y[nV+ii] < 0.0)
oldConstraints.setStatus (ii, ST_UPPER);
else if (oldConstraints.getStatus (ii) == ST_UPPER && y[nV+ii] > 0.0)
oldConstraints.setStatus (ii, ST_LOWER);
}
}
// ... and do the same also for the bounds!
for (int ii = 0; ii < nV; ++ii) {
if (oldBounds.getType(ii) == ST_EQUALITY
&& bounds.getType(ii) == ST_BOUNDED) {
if (oldBounds.getStatus(ii) == ST_LOWER && y[ii] < 0.0)
oldBounds.setStatus(ii, ST_UPPER);
else if (oldBounds.getStatus(ii) == ST_UPPER && y[ii] > 0.0)
oldBounds.setStatus(ii, ST_LOWER);
}
}
/* 3) Setup old working sets afresh (updating TQ factorisation). */
if ( setupAuxiliaryWorkingSet( &oldBounds,&oldConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
return THROWERROR( RET_SETUP_AUXILIARYQP_FAILED );
/* Factorise projected Hessian
* this now handles all special cases (no active bounds/constraints, no nullspace) */
returnvalue = computeProjectedCholesky( );
/* leave the loop if decomposition was successful, i.e. we have
* found an active set with positive definite null space Hessian */
if ( returnvalue == SUCCESSFUL_RETURN )
break;
}
/* adjust lb/ub if we changed the old active set in the second try
*/
if (n_try > 0) {
// as per setupAuxiliaryQPbounds assumptions ... oh the troubles
for (int ii = 0; ii < nC; ++ii)
Ax_l[ii] = Ax_u[ii] = Ax[ii];
setupAuxiliaryQPbounds (&bounds, &constraints, BT_FALSE);
}
status = QPS_AUXILIARYQPSOLVED;
return SUCCESSFUL_RETURN;
}