USING_NAMESPACE_QPOASES
int sumOfSquares()
{
int i;
/* sum of first n squares */
const int N = 100;
real_t av[N];
real_t aT[N];
real_t bv[N];
real_t c;
for (i = 0; i < N; i++) av[i] = i+1;
for (i = 0; i < N; i++) aTv[i] = i+1;
for (i = 0; i < N; i++) bv[i] = i+1;
myStatic DenseMatrix a;
DenseMatrixCON( &a, 1, N, N, av);
myStatic DenseMatrix aT;
DenseMatrixCON( &aT, N, 1, 1, aTv);
a.times(1, 1.0, bv, N, 0.0, &c, 1);
fprintf(stdFile, "Dot product; Error in sum of first %d squares: %9.2e\n", N, c - (1.0/6.0)*N*(N+1)*(2*N+1));
aT.transTimes(1, 1.0, bv, N, 0.0, &c, 1);
fprintf(stdFile, "Transpose; Error in sum of first %d squares: %9.2e\n", N, c - (1.0/6.0)*N*(N+1)*(2*N+1));
return 0;
}
/*
* s e t u p C o n s t r a i n t M a t r i x
*/
returnValue setupConstraintMatrix( const mxArray* prhsA, int nV, int nC,
DenseMatrix* A
)
{
real_t *A_for = 0;
static real_t A_mem[NCMAX*NVMAX];
if ( prhsA == 0 )
return SUCCESSFUL_RETURN;
if ( mxIsSparse( prhsA ) != 0 )
{
myMexErrMsgTxt( "ERROR (qpOASES_e): Cannot handle sparse matrices!" );
return RET_INVALID_ARGUMENTS;
}
else
{
/* make a deep-copy in order to avoid modifying input data when regularising */
A_for = (real_t*) mxGetPr( prhsA );
convertFortranToC( A_for, nV,nC,A_mem );
DenseMatrixCON( A, nC,nV,nV, A_mem );
}
return SUCCESSFUL_RETURN;
}
/*
* s e t u p H e s s i a n M a t r i x
*/
returnValue setupHessianMatrix( const mxArray* prhsH, int nV,
DenseMatrix* H
)
{
real_t *H_for = 0;
/*static real_t H_mem[NVMAX*NVMAX];*/
if ( prhsH == 0 )
return SUCCESSFUL_RETURN;
if ( mxIsSparse( prhsH ) != 0 )
{
myMexErrMsgTxt( "ERROR (qpOASES_e): Cannot handle sparse matrices!" );
return RET_INVALID_ARGUMENTS;
}
else
{
/* make a deep-copy in order to avoid modifying input data when regularising */
H_for = (real_t*) mxGetPr( prhsH );
/*qpOASES_printM( H_for,nV,nV );*/
/*convertFortranToC( H_for, nV,nV,H_mem ); not neccessary as H is symmetric! */
DenseMatrixCON( H, nV,nV,nV, H_for );
}
return SUCCESSFUL_RETURN;
}
int hilbert()
{
int i, j;
real_t d, err;
/* permuted 4x4 Hilbert matrix, row major format */
real_t _Av[] = {1.0/3.0, 1.0, 0.5, 0.25,
0.25, 0.5, 1.0/3.0, 0.2,
0.2, 1.0/3.0, 0.25, 1.0/6.0,
1.0/6.0, 0.25, 0.2, 1.0/7.0};
/* and its inverse, column major format */
real_t Bv[] = {240, 16, -120, -140,
-2700, -120, 1200, 1680,
6480, 240, -2700, -4200,
-4200, -140, 1680, 2800};
/* result */
real_t *Av = new real_t[4*4];
real_t *Cv = new real_t[4*4];
myStatic DenseMatrix A;
DenseMatrixCON( &A, 4, 4, 4, Av );
for (i = 0; i < 16; i++) Av[i] = _Av[i];
A.times(4, 1.0, Bv, 4, 0.0, Cv, 4);
err = 0.0;
for (j = 0; j < 4; j++)
{
for (i = 0; i < 4; i++)
{
d = fabs(Cv[j*4+i] - static_cast<real_t>(i == j));
if (d > err) err = d;
}
}
fprintf(stdFile, "Hilbert; Deviation from identity: %9.2e\n", err);
delete[] Cv;
A.free (); // or delete[] Av;
return 0;
}
int submatrix()
{
int i, j;
real_t d, err;
/* 2x3 transposed submatrix */
real_t _Asubv[] = {1.0/3.0, 0.25,
1.0, 0.5,
0.5, 1.0/3.0,
0.25, 0.2};
real_t Bsubv[] = {240, 16, -120, -140,
-2700, -120, 1200, 1680};
real_t Csubv[2*2];
real_t *Asubv = new real_t[2*4];
for (i = 0; i < 8; i++) Asubv[i] = _Asubv[i];
myStatic DenseMatrix Asub;
DenseMatrixCON( &Asub, 4, 2, 2, Asubv );
Asub.transTimes(2, 1.0, Bsubv, 4, 0.0, Csubv, 2);
err = 0.0;
for (j = 0; j < 2; j++)
{
for (i = 0; i < 2; i++)
{
d = fabs(Csubv[j*2+i] - static_cast<real_t>(i == j));
if (d > err) err = d;
}
}
fprintf(stdFile, "Submatrix transpose; Deviation from identity: %9.2e\n", err);
Asub.free (); // or delete[] Asubv;
return 0;
}
int indexDenseSubmatrix()
{
/* dense submatrices via index lists */
const int M = 20, N = 15, K = 5;
int i, j, k, m, n;
Indexlist rows(N), cols(N);
int *rNum, *cNum;
real_t err = 0.0;
real_t *Av, *X, *Y, *x, *y, *xc, *yc;
// prepare index lists
m = (M-3)/4;
n = (N-3)/4;
for (i = 0; i < m; i++) { rows.addNumber(M-1 - 2*i); rows.addNumber(2*i); }
for (i = 0; i < n; i++) { cols.addNumber(N-1 - 2*i); cols.addNumber(2*i); }
m *= 2;
n *= 2;
rows.getNumberArray(&rNum);
fprintf(stdFile, "Rows: ");
for (i = 0; i < m; i++) fprintf(stdFile, " %2d", rNum[i]);
fprintf(stdFile, "\n");
cols.getNumberArray(&cNum);
fprintf(stdFile, "Cols: ");
for (i = 0; i < n; i++) fprintf(stdFile, " %2d", cNum[i]);
fprintf(stdFile, "\n");
// prepare input matrices
Av = new real_t[M*N];
X = new real_t[n*K];
Y = new real_t[m*K];
x = new real_t[n*K];
y = new real_t[m*K];
xc = new real_t[n*K];
yc = new real_t[m*K];
myStatic DenseMatrix A;
DenseMatrixCON( &A, M, N, N, Av );
for (i = 0; i < M*N; i++) Av[i] = -0.5*N*M + (real_t)i;
for (i = 0; i < n*K; i++) X[i] = 1.0 / (real_t)(i+1);
for (i = 0; i < m*K; i++) Y[i] = 1.0 / (real_t)(i+1);
// multiply
A.times(&rows, &cols, K, 1.0, X, n, 0.0, y, m);
// check result
for (j = 0; j < m; j++)
{
for (k = 0; k < K; k++)
{
yc[j+k*m] = -y[j+k*m];
for (i = 0; i < n; i++)
yc[j+k*m] += Av[cNum[i]+rNum[j]*N] * X[i+k*n];
if (fabs(yc[j+k*m]) > err) err = fabs(yc[j+k*m]);
}
}
fprintf(stdFile, "Indexlist submatrix; error: %9.2e\n", err);
// transpose multiply
A.transTimes(&rows, &cols, K, 1.0, Y, m, 0.0, x, n);
// check result
err = 0.0;
for (j = 0; j < n; j++)
{
for (k = 0; k < K; k++)
{
xc[j+k*n] = -x[j+k*n];
for (i = 0; i < m; i++)
xc[j+k*n] += Av[cNum[j]+rNum[i]*N] * Y[i+k*m];
if (fabs(xc[j+k*n]) > err) err = fabs(xc[j+k*n]);
}
}
fprintf(stdFile, "Indexlist transpose submatrix; error: %9.2e\n", err);
// check result
// clean up
delete[] yc;
delete[] xc;
delete[] y;
delete[] x;
delete[] Y;
delete[] X;
A.free (); // or delete[] Av;
return 0;
}
/*
* s o l v e O Q P b e n c h m a r k
*/
returnValue solveOQPbenchmarkB( int nQP, int nV,
real_t* _H, const real_t* const g,
const real_t* const lb, const real_t* const ub,
BooleanType isSparse, BooleanType useHotstarts,
const Options* options, int maxAllowedNWSR,
real_t* maxNWSR, real_t* avgNWSR, real_t* maxCPUtime, real_t* avgCPUtime,
real_t* maxStationarity, real_t* maxFeasibility, real_t* maxComplementarity
)
{
int k;
myStatic QProblemB qp;
returnValue returnvalue;
/* I) SETUP AUXILIARY VARIABLES: */
/* 1) Keep nWSR and store current and maximum number of
* working set recalculations in temporary variables */
int nWSRcur;
real_t CPUtimeLimit = *maxCPUtime;
real_t CPUtimeCur = CPUtimeLimit;
real_t stat, feas, cmpl;
/* 2) Pointers to data of current QP ... */
const real_t* gCur;
const real_t* lbCur;
const real_t* ubCur;
/* 3) Vectors for solution obtained by qpOASES. */
myStatic real_t x[NVMAX];
myStatic real_t y[NVMAX];
/* 4) Prepare matrix objects */
DenseMatrix *H;
myStatic DenseMatrix HH;
DenseMatrixCON( &HH, nV, nV, nV, _H );
H = &HH;
*maxNWSR = 0;
*avgNWSR = 0;
*maxCPUtime = 0.0;
*avgCPUtime = 0.0;
*maxStationarity = 0.0;
*maxFeasibility = 0.0;
*maxComplementarity = 0.0;
/* II) SETUP QPROBLEM OBJECT */
QProblemBCON( &qp,nV,HST_UNKNOWN );
QProblemB_setOptions( &qp,*options );
/*QProblemB_setPrintLevel( &qp,PL_LOW );*/
/* III) RUN BENCHMARK SEQUENCE: */
for( k=0; k<nQP; ++k )
{
/* 1) Update pointers to current QP data. */
gCur = &( g[k*nV] );
lbCur = &( lb[k*nV] );
ubCur = &( ub[k*nV] );
/* 2) Set nWSR and maximum CPU time. */
nWSRcur = maxAllowedNWSR;
CPUtimeCur = CPUtimeLimit;
/* 3) Solve current QP. */
if ( ( k == 0 ) || ( useHotstarts == BT_FALSE ) )
{
/* initialise */
returnvalue = QProblemB_initM( &qp,H,gCur,lbCur,ubCur, &nWSRcur,&CPUtimeCur );
if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
return THROWERROR( returnvalue );
}
else
{
/* hotstart */
returnvalue = QProblemB_hotstart( &qp,gCur,lbCur,ubCur, &nWSRcur,&CPUtimeCur );
if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED ) )
return THROWERROR( returnvalue );
}
/* 4) Obtain solution vectors and objective function value ... */
QProblemB_getPrimalSolution( &qp,x );
QProblemB_getDualSolution( &qp,y );
/* 5) Compute KKT residuals */
qpOASES_getKktViolationSB( nV, _H,gCur,lbCur,ubCur, x,y, &stat,&feas,&cmpl );
/* 6) update maximum values. */
if ( nWSRcur > *maxNWSR )
*maxNWSR = nWSRcur;
if (stat > *maxStationarity) *maxStationarity = stat;
if (feas > *maxFeasibility) *maxFeasibility = feas;
if (cmpl > *maxComplementarity) *maxComplementarity = cmpl;
if ( CPUtimeCur > *maxCPUtime )
*maxCPUtime = CPUtimeCur;
*avgNWSR += nWSRcur;
*avgCPUtime += CPUtimeCur;
}
*avgNWSR /= nQP;
*avgCPUtime /= ((double)nQP);
return SUCCESSFUL_RETURN;
}
