QPSolver.cpp File Reference

#include <stdio.h>
#include <memory.h>
#include "Matrix.h"
#include "tools.h"
#include "VectorChar.h"

Include dependency graph for QPSolver.cpp:

Go to the source code of this file.

Functions
void	QPReconstructLambda (Vector vLambda, Vector vLambdaScale)
void	simpleQPSolve (Matrix mH, Vector vG, Matrix mA, Vector vB, Vector vP, Vector vLambda, int *info)
void	restartSimpleQPSolve (Vector vBO, Vector vP)

Variables
Matrix	mQ_QP
MatrixTriangle	mR_QP
VectorInt	vi_QP
VectorInt	viPermut_QP
Vector	vLastLambda_QP
char	bQRFailed_QP

Function Documentation

QPReconstructLambda()

void QPReconstructLambda	(	Vector	vLambda,
		Vector	vLambdaScale
	)

Definition at line 84 of file QPSolver.cpp.

{
    int n=vi_QP.sz()-1, nc=vLambda.sz();
    int *ii=vi_QP;
    double *l=vLambda, *s=vLambdaScale;

    if (n>=0)
        while (nc--)
        {
            if (ii[n]==nc) 
            { 
//                l[nc]=mmax(0.0,l[n]/s[n]); 
                l[nc]=l[n]/s[n]; 
                n--; 
                if (n<0) break; 
            }
            else l[nc]=0.0;
        }
    if (nc) memset(l,0,nc*sizeof(double));
}

restartSimpleQPSolve()

void restartSimpleQPSolve	(	Vector	vBO,
		Vector	vP
	)

Definition at line 363 of file QPSolver.cpp.

{
    if (bQRFailed_QP) { vP.zero(); return; }
    int i,k=0, *ii=vi_QP, nc2=vi_QP.sz();
    double *lambda=vLastLambda_QP, *b=vBO;
    Vector vTmp(nc2);
    for (i=0; i<nc2; i++)
    {
        if (lambda[i]!=0.0)
        {
            b[k]=b[ii[i]];
            k++;
        }
    }
    vBO.setSize(k);
    vBO.permutIn(vTmp,viPermut_QP);
    mR_QP.solveInPlace(vTmp);
    mQ_QP.multiply(vP,vTmp);
}

simpleQPSolve()

void simpleQPSolve	(	Matrix	mH,
		Vector	vG,
		Matrix	mA,
		Vector	vB,
		Vector	vP,
		Vector	vLambda,
		int *	info
	)

Definition at line 105 of file QPSolver.cpp.

{
    const double tolRelFeasibility=1e-6;
//     int *info=NULL;
    int dim=mA.nColumn(), nc=mA.nLine(), ncr, i,j,k, lastJ=-2, *ii;
    MatrixTriangle M(dim);
    Matrix mAR, mZ(dim,dim-1), mHZ(dim,dim-1), mZHZ(dim-1,dim-1);
    Vector vTmp(mmax(nc,dim)), vYB(dim), vD(dim), vTmp2(dim), vTmp3(nc), vLast(dim), 
           vBR_QP, vLambdaScale(nc);
    VectorChar vLB(nc);
    double *lambda=vLambda, *br, *b=vB, violationMax, violationMax2,
           *rlambda,ax,al,dviolation, **a=mA, **ar=mAR, mymin, mymax, maxb, *llambda,
           **r,t, *scaleLambda=vLambdaScale;
    char finished=0, feasible=0, *lb=vLB;

    if (info) *info=0;

    // remove lines which are null
    k=0; vi_QP.setSize(nc); maxb=0.0;
    for (i=0; i<nc; i++)
    {
        mymin=INF; mymax=-INF;
        for (j=0; j<dim; j++)
        {
            mymin=mmin(mymin,a[i][j]);
            mymax=mmax(mymax,a[i][j]);
            if ((mymin!=mymax)||(mymin!=0.0)||(mymax!=0.0)) break;
        }
        if ((mymin!=mymax)||(mymin!=0.0)||(mymax!=0.0))
        {
            lambda[k]=lambda[i];
            maxb=mmax(maxb,condorAbs(b[i]));
            scaleLambda[k]=mA.euclidianNorm(i);
            vi_QP[k]=i;
            k++;
        }
    }
    nc=k; vi_QP.setSize(nc); ii=vi_QP; maxb=(1.0+maxb)*tolRelFeasibility;
    vLast.zero(); 
    
    for (i=0; i<nc; i++) if (lambda[i]!=0.0) lb[i]=2; else lb[i]=1;

    while (!finished)
    {
        finished=1;
        mAR.setSize(dim,dim); mAR.zero(); ar=mAR;
        vBR_QP.setSize(mmin(nc,dim)); br=vBR_QP;
        ncr=0;
        for (i=0; i<nc; i++)
            if (lambda[i]!=0.0)
            {
//                mAR.setLines(ncr,mA,ii[i],1);
                k=ii[i];
                t=scaleLambda[ncr];
                for (j=0; j<dim; j++) ar[ncr][j]=a[k][j]*t;
                br[ncr]=b[ii[i]]*t;
                ncr++;
            }
        mAR.setSize(ncr,dim);
        vBR_QP.setSize(ncr);
        vLastLambda_QP.copyFrom(vLambda); llambda=vLastLambda_QP;

        if (ncr==0)
        {
            // compute step
            vYB.copyFrom(vG);
            vYB.multiply(-1.0);
            if (mH.cholesky(M))
            {
                M.solveInPlace(vYB);
                M.solveTransposInPlace(vYB);
            } else
            {
                printf("warning: cholesky factorisation failed.\n");
                if (info) *info=2;
            }
            vLambda.zero();
        } else
        {
            Matrix mAR2=mAR.clone(), mQ2;
            MatrixTriangle mR2;
            mAR2.QR(mQ2,mR2);

            mAR.QR(mQ_QP,mR_QP, viPermut_QP); // content of mAR is destroyed here !
            bQRFailed_QP=0;

            r=mR_QP;
            for (i=0; i<ncr; i++)
                if (r[i][i]==0.0)
                {
                    // one constraint has been wrongly added.
                    bQRFailed_QP=1;
                    QPReconstructLambda(vLambda,vLambdaScale); vP.copyFrom(vLast); return; 
                }

            for (i=0; i<ncr; i++)
                if (viPermut_QP[i]!=i)
                {
                  //  printf("whoups.\n");
                }
            if (ncr<dim)
            {
                mQ_QP.getSubMatrix(mZ,0,ncr);

                // Z^t H Z
                mH.multiply(mHZ,mZ);
                mZ.transposeAndMultiply(mZHZ,mHZ);
                mQ_QP.setSize(dim,ncr);            
            }

            // form Yb
            vBR_QP.permutIn(vTmp,viPermut_QP);
            mR_QP.solveInPlace(vTmp);
            mQ_QP.multiply(vYB,vTmp);

            if (ncr<dim)
            {

                // ( vG + H vYB )^t Z : result in vD
                
                mH.multiply(vTmp,vYB);
                vTmp+=vG;
                vTmp.transposeAndMultiply(vD,mZ);

                // calculate current delta (result in vD)
                vD.multiply(-1.0);
                if (mZHZ.cholesky(M))
                {
                    M.solveInPlace(vD);
                    M.solveTransposInPlace(vD);
                }
                else
                {
                    printf("warning: cholesky factorisation failed.\n");
                    if (info) *info=2;
                };

                // evaluate vX* (result in vYB):
                mZ.multiply(vTmp, vD);
                vYB+=vTmp;
            }

            // evaluate vG* (result in vTmp2)
            mH.multiply(vTmp2,vYB);
            vTmp2+=vG;

            // evaluate lambda star (result in vTmp):
            mQ2.transposeAndMultiply(vTmp,vTmp2);
            mR2.solveTransposInPlace(vTmp);

            // evaluate lambda star (result in vTmp):
            mQ_QP.transposeAndMultiply(vTmp3,vTmp2);
            mR_QP.solveTransposInPlace(vTmp3);
            vTmp3.permutOut(vTmp,viPermut_QP);
            rlambda=vTmp;

            ncr=0;
            for (i=0; i<nc; i++)
                if (lambda[i]!=0.0)
                {
                    lambda[i]=rlambda[ncr];
                    ncr++;
                }
        } // end of test on ncr==0

        // find the most violated constraint j among non-active Linear constraints:
        j=-1;
        if (nc>0)
        {
            k=-1; violationMax=-INF; violationMax2=-INF;
            for (i=0; i<nc; i++)
            {
                if (lambda[i]<=0.0) // test to see if this constraint is not active 
                {
                    ax=mA.scalarProduct(ii[i],vYB);
                    dviolation=b[ii[i]]-ax;
                    if (llambda[i]==0.0)
                    {
                        // the constraint was not active this round
                        // thus, it can enter the competition for the next 
                        // active constraint

                        if (dviolation>maxb)
                        {
                            // the constraint should be activated
                            if (dviolation>violationMax2) 
                                { k=i; violationMax2=dviolation; }
                            al=mA.scalarProduct(ii[i],vLast)-ax; 
                            if (al>0.0) // test to see if we are going closer
                            {  
                                dviolation/=al;
                                if (dviolation>violationMax ) 
                                    { j=i; violationMax =dviolation; }
                            }
                        }
                    } else
                    {
                        lb[i]--;
                        if (feasible) 
                        {
                            if (lb[i]==0) 
                            {
                                vLambda.copyFrom(vLastLambda_QP);
                                if (lastJ>=0) lambda[lastJ]=0.0;
                                QPReconstructLambda(vLambda,vLambdaScale); 
                                vP.copyFrom(vYB); 
                                return; 
                            }
                        } else
                        {
                            if (lb[i]==0)
                            {
                                if (info) *info=1;
                                QPReconstructLambda(vLambda,vLambdaScale); 
                                vP.zero(); 
                                return;
                            }
                        }
                        finished=0;  // this constraint was wrongly activated.
                        lambda[i]=0.0;
                    }
                }
            }

            // !!! the order the tests is important here !!!
            if ((j==-1)&&(!feasible)) 
            { 
                feasible=1; 
                for (i=0; i<nc; i++) if (llambda[i]!=0.0) lb[i]=2; else lb[i]=1;
            }
            if (j==-1) { j=k; violationMax=violationMax2; } // change j to k after feasible is set
            if (ncr==mmin(dim,nc)) { 
                if (feasible) { // feasible must have been checked before
                    QPReconstructLambda(vLambda,vLambdaScale); vP.copyFrom(vYB); return; 
                } else
                {
                    if (info) *info=1;
                    QPReconstructLambda(vLambda,vLambdaScale); vP.zero(); return; 
                }
            }
            // activation of constraint only if ncr<mmin(dim,nc)
            if (j>=0) { lambda[j]=1e-5; finished=0; } // we need to activate a new constraint 
//            else if (ncr==dim){ 
//                QPReconstructLambda(vLambda); vP.copyFrom(vYB); return; }
        }

        // to prevent rounding error
        if (j==lastJ) { 
//        if (0) {
            QPReconstructLambda(vLambda,vLambdaScale); vP.copyFrom(vYB); return; }
        lastJ=j;

        vLast.copyFrom(vYB);
    }
    QPReconstructLambda(vLambda,vLambdaScale); vP.copyFrom(vYB); return;
}

Variable Documentation

bQRFailed_QP

char bQRFailed_QP

Definition at line 82 of file QPSolver.cpp.

mQ_QP

Matrix mQ_QP

Definition at line 78 of file QPSolver.cpp.

mR_QP

MatrixTriangle mR_QP

Definition at line 79 of file QPSolver.cpp.

vi_QP

VectorInt vi_QP

Definition at line 80 of file QPSolver.cpp.

viPermut_QP

VectorInt viPermut_QP

Definition at line 80 of file QPSolver.cpp.

vLastLambda_QP

Vector vLastLambda_QP

Definition at line 81 of file QPSolver.cpp.