/* Prepare the matrices for the lcp call
pA = [-M -1 q]
*/
PT_Matrix lcp_Matrix_Init(ptrdiff_t m, double *M, double *q)
{
ptrdiff_t i,n,incx;
double tmp;
PT_Matrix pA;
/* Build column-major matrix A = [-M -1 q] */
pA = Matrix_Init(m,m+2,"A");
/* A(:,1:m) = M */
memcpy(pMAT(pA), M, m*m*sizeof(double));
/* A(:,1:m) = -A(:,1:m) */
tmp = -1.0;
incx = 1;
n = m*m;
dscal(&n, &tmp, pMAT(pA), &incx);
/* A(:,m+1) = -1 */
for(i=0;i<m;i++)
C_SEL(pA,i,m) = -1.0;
/* A(:,m+2) = q */
memcpy(&(C_SEL(pA,0,m+1)),q,m*sizeof(double));
return pA;
}
void LU_Refactorize(PT_Basis pB)
{
char L = 'L'; /* lower triangular */
char D = 'U'; /* unit triangular matrix (diagonals are ones) */
ptrdiff_t info, incx=1, incp;
/* Matrix_Print_row(pB->pLX); */
/* Matrix_Print_utril_row(pB->pUX); */
/* factorize using lapack */
dgetrf(&(Matrix_Rows(pB->pF)), &(Matrix_Rows(pB->pF)),
pMAT(pB->pF), &((pB->pF)->rows_alloc), pB->p, &info);
/* store upper triangular matrix (including the diagonal to Ut), i.e. copy Ut <- F */
/* lapack ignores remaining elements below diagonal when computing triangular solutions */
Matrix_Copy(pB->pF, pB->pUt, pB->w);
/* transform upper part of F (i.e. Ut) to triangular row major matrix UX*/
/* UX <- F */
Matrix_Full2RowTriangular(pB->pF, pB->pUX, pB->r);
/* invert lower triangular part */
dtrtri( &L, &D, &(Matrix_Rows(pB->pF)), pMAT(pB->pF),
&((pB->pF)->rows_alloc), &info);
/* set strictly upper triangular parts to zeros because L is a full matrix
* and we need zeros to compute proper permutation inv(L)*P */
Matrix_Uzeros(pB->pF);
/* transpose matrix because dlaswp operates rowwise and we need columnwise */
/* LX <- F' */
Matrix_Transpose(pB->pF, pB->pLX, pB->r);
/* interchange columns according to pivots in pB->p and write to LX*/
incp = -1; /* go backwards */
dlaswp( &(Matrix_Rows(pB->pLX)), pMAT(pB->pLX), &((pB->pLX)->rows_alloc),
&incx, &(Matrix_Rows(pB->pLX)) , pB->p, &incp);
/* Matrix_Print_col(pB->pX); */
/* Matrix_Print_row(pB->pLX); */
/* Matrix_Print_col(pB->pUt); */
/* Matrix_Print_utril_row(pB->pUX); */
/* matrix F after solution is factored in [L\U], we want the original format for the next call
to dgesv, thus create a copy F <- X */
Matrix_Copy(pB->pX, pB->pF, pB->w);
}
/* Return information if the solution was computed */
ptrdiff_t Basis_Solve(PT_Basis pB, double *q, double *x, T_Options options)
{
/* Basis B */
/* I = W union (Z+m) -> ordered */
/* X = M_/W,Z <- invertible */
/* G = M_W,Z */
/* Bx_B = q -> x = [w_W;z_Z] */
/* z_Z = iX*q_/W */
/* w_W = q_W - G*x_/W */
ptrdiff_t i, incx=1, nb, m, diml, info;
char T;
double alpha=-1.0, beta=1.0;
/* x(1:length(W)) = q(W) */
for(i=0;i<Index_Length(pB->pW);i++) x[i] = q[Index_Get(pB->pW, i)];
/* X = [] */
if(Index_Length(pB->pWc) == 0)
return 0;
/* z = q(B.Wc) */
for(i=0;i<Index_Length(pB->pWc);i++) pB->z[i] = q[Index_Get(pB->pWc, i)];
/* because the right hand side will be overwritten in dgesv, store it into
temporary vector r */
dcopy(&(Index_Length(pB->pWc)), pB->z, &incx, pB->r, &incx);
/* printf("x :\n"); */
/* Vector_Print_raw(pB->z,Index_Length(pB->pW)); */
/* printf("z :\n"); */
/* Vector_Print_raw(pB->z,Index_Length(pB->pWc)); */
/* x = [x1;x2] */
/* x2 = inv(X)*q(Wc) = inv(X)*z */
nb = 1; /* number of right hand side in A*x=b is 1 */
m = Matrix_Rows(pB->pF);
/* Find solution to general problem A*x=b using LAPACK
All the arguments have to be pointers. A and b will be altered on exit. */
switch (options.routine) {
case 1:
/* DGESV method implements LU factorization of A. */
dgesv(&m, &nb, pMAT(pB->pF), &((pB->pF)->rows_alloc),
pB->p, pB->r, &m, &info);
break;
case 2:
/* DGELS solves overdetermined or underdetermined real linear systems
involving an M-by-N matrix A, or its transpose, using a QR or LQ
factorization of A. It is assumed that A has full rank. */
T = 'n'; /* Not transposed */
diml = 2*m;
dgels(&T, &m, &m, &nb, pMAT(pB->pF), &((pB->pF)->rows_alloc),
pB->r, &m, pB->s, &diml, &info );
break;
default :
/* solve with factors F or U */
/* solve using LUMOD (no checks) */
/* LU_Solve0(pB->pLX, pB->pUX, pB->z, &(x[Index_Length(pB->pW)])); */
/* solve using LAPACK (contains also singularity checks) */
/* need to pass info as a pointer, otherwise weird numbers are assigned */
LU_Solve1(pB->pLX, pB->pUt, pB->z, &(x[Index_Length(pB->pW)]), &info);
/* if something went wrong, refactorize and solve again */
if (info!=0) {
/* printf("info=%ld, refactoring\n", info); */
/* Matrix_Print_row(pB->pLX); */
/* Matrix_Print_utril_row(pB->pUX); */
/* Matrix_Print_col(pB->pUt); */
LU_Refactorize(pB);
/* if this fails again, then no minimum ratio is found -> exit with
* numerical problems flag */
LU_Solve1(pB->pLX, pB->pUt, pB->z, &(x[Index_Length(pB->pW)]), &info);
}
}
/* x1 = -G*x2 + q(W) */
/* y = alpha*G*x + beta*y */
/* alpha = -1.0; */
/* beta = 1.0; */
T = 'n'; /* Not transposed */
if (options.routine>0) {
/* take LAPACK solution */
/* printf("lapack solution z:\n"); */
/* Vector_Print_raw(pB->r,Index_Length(pB->pWc)); */
/* matrix F after solution is factored in [L\U], we want the original format for the next call
to dgesv, thus create a copy F <- X */
Matrix_Copy(pB->pX, pB->pF, pB->w);
/* printf("x after lapack solution:\n"); */
/* Vector_Print_raw(x,Index_Length(pB->pW)+Index_Length(pB->pWc)); */
/* recompute the remaining variables according to basic solution */
dgemv(&T, &(Matrix_Rows(pB->pG)), &(Matrix_Cols(pB->pG)),
&alpha, pMAT(pB->pG), &((pB->pG)->rows_alloc),
pB->r, &incx, &beta, x, &incx);
/* append the basic solution at the end of x vector */
dcopy(&(Index_Length(pB->pWc)), pB->r, &incx, &(x[Index_Length(pB->pW)]), &incx);
} else {
/* take LUmod solution */
dgemv(&T, &(Matrix_Rows(pB->pG)), &(Matrix_Cols(pB->pG)),
&alpha, pMAT(pB->pG), &((pB->pG)->rows_alloc),
&(x[Index_Length(pB->pW)]), &incx,
&beta, x, &incx);
}
/* printf("y:\n"); */
/* Vector_Print_raw(x,Matrix_Rows(pB->pG)); */
return info;
}
/* Function: mdlOutputs =======================================================
* do the main optimization routine here
* no discrete states are considered
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
int_T i, j, Result, qinfeas=0;
real_T *z = ssGetOutputPortRealSignal(S,0);
real_T *w = ssGetOutputPortRealSignal(S,1);
real_T *I = ssGetOutputPortRealSignal(S,2);
real_T *exitflag = ssGetOutputPortRealSignal(S,3);
real_T *pivots = ssGetOutputPortRealSignal(S,4);
real_T *time = ssGetOutputPortRealSignal(S,5);
InputRealPtrsType M = ssGetInputPortRealSignalPtrs(S,0);
InputRealPtrsType q = ssGetInputPortRealSignalPtrs(S,1);
ptrdiff_t pivs, info, m, n, inc=1;
char_T T='N';
double total_time, alpha=1.0, tmp=-1.0, *x, *Mn, *qn, *r, *c, s=0.0, sn=0.0;
PT_Matrix pA; /* Problem data A = [M -1 q] */
PT_Basis pB; /* The basis */
T_Options options; /* options structure defined in lcp_matrix.h */
/* for RTW we do not need these variables */
#ifdef MATLAB_MEX_FILE
const char *fname;
mxArray *fval;
int_T nfields;
clock_t t1,t2;
#endif
UNUSED_ARG(tid); /* not used in single tasking mode */
/* default options */
options.zerotol = 1e-10; /* zero tolerance */
options.lextol = 1e-10; /* lexicographic tolerance - a small treshold to determine if values are equal */
options.maxpiv = INT_MAX; /* maximum number of pivots */
/* if LUMOD routine is chosen, this options refactorizes the basis after n steps using DGETRF
routine from lapack to avoid numerical problems */
options.nstepf = 50;
options.clock = 0; /* 0 or 1 - to print computational time */
options.verbose = 0; /* 0 or 1 - verbose output */
/* which routine in Basis_solve should solve a set of linear equations:
0 - corresponds to LUmod package that performs factorization in the form L*A = U. Depending on
the change in A factors L, U are updated. This is the fastest method.
1 - corresponds to DGESV simple driver
which solves the system AX = B by factorizing A and overwriting B with the solution X
2 - corresponds to DGELS simple driver
which solves overdetermined or underdetermined real linear systems min ||b - Ax||_2
involving an M-by-N matrix A, or its transpose, using a QR or LQ factorization of A. */
options.routine = 0;
options.timelimit = 3600; /* time limit in seconds to interrupt iterations */
options.normalize = 1; /* 0 or 1 - perform scaling of input matrices M, q */
options.normalizethres = 1e6;
/* If the normalize option is on, then the matrix scaling is performed
only if 1 norm of matrix M (maximum absolute column sum) is above this threshold.
This enforce additional control over normalization since it seems to be more
aggressive also for well-conditioned problems. */
#ifdef MATLAB_MEX_FILE
/* overwriting default options by the user */
if (ssGetSFcnParamsCount(S)==3) {
nfields = mxGetNumberOfFields(ssGetSFcnParam(S,2));
for(i=0; i<nfields; i++){
fname = mxGetFieldNameByNumber(ssGetSFcnParam(S,2), i);
fval = mxGetField(ssGetSFcnParam(S,2), 0, fname);
if ( strcmp(fname,"zerotol")==0 )
options.zerotol = mxGetScalar(fval);
if ( strcmp(fname,"lextol")==0 )
options.lextol = mxGetScalar(fval);
if ( strcmp(fname,"maxpiv")==0 ) {
if (mxGetScalar(fval)>=(double)INT_MAX)
options.maxpiv = INT_MAX;
else
options.maxpiv = (int_T)mxGetScalar(fval);
}
if ( strcmp(fname,"nstepf")==0 )
options.nstepf = (int_T)mxGetScalar(fval);
if ( strcmp(fname,"timelimit")==0 )
options.timelimit = mxGetScalar(fval);
if ( strcmp(fname,"clock")==0 )
options.clock = (int_T)mxGetScalar(fval);
if ( strcmp(fname,"verbose")==0 )
options.verbose = (int_T)mxGetScalar(fval);
if ( strcmp(fname,"routine")==0 )
options.routine = (int_T)mxGetScalar(fval);
if ( strcmp(fname,"normalize")==0 )
options.normalize = (int_T)mxGetScalar(fval);
if ( strcmp(fname, "normalizethres")==0 )
options.normalizethres = mxGetScalar(fval);
}
}
#endif
/* Normalize M, q to avoid numerical problems if possible
Mn = diag(r)*M*diag(c) , qn = diag(r)*q */
/* initialize Mn, qn */
Mn = (double *)ssGetPWork(S)[0];
qn = (double *)ssGetPWork(S)[1];
/* initialize vectors r, c */
r = (double *)ssGetPWork(S)[2];
c = (double *)ssGetPWork(S)[3];
/* initialize auxiliary vector x */
x = (double *)ssGetPWork(S)[4];
/* initialize to ones */
for (i=0; i<NSTATES(S); i++) {
r[i] = 1.0;
c[i] = 1.0;
}
m = NSTATES(S);
n = m*m;
/* write data to Mn = M */
memcpy(Mn, *M, n*sizeof(double));
/* write data to qn = q */
memcpy(qn, *q, m*sizeof(double));
/* check out the 1-norm of matrix M (maximum column sum) */
for (i=0; i<m; i++) {
sn = dasum(&m, &Mn[i*m], &inc);
if (sn>s) {
s = sn;
}
}
/* scale matrix M, q and write scaling factors to r (rows) and c (columns) */
if (options.normalize && s>options.normalizethres) {
NormalizeMatrix (m, m, Mn, qn, r, c, options);
}
/* Setup the problem */
pA = ssGetPWork(S)[5];
/* A(:,1:m) = M */
memcpy(pMAT(pA), Mn, n*sizeof(double));
/* A(:,1:m) = -A(:,1:m) */
dscal(&n, &tmp, pMAT(pA), &inc);
/* A(:,m+1) = -1 */
for(i=0;i<m;i++)
C_SEL(pA,i,m) = -1.0;
/* A(:,m+2) = q */
memcpy(&(C_SEL(pA,0,m+1)),qn,m*sizeof(double));
/* initialize basis */
pB = ssGetPWork(S)[6];
/* check if the problem is not feasible at the beginning */
for (i=0; i<m; i++) {
if (qn[i]<-options.zerotol) {
qinfeas = 1;
break;
}
}
/* Solve the LCP */
if (qinfeas) {
#ifdef MATLAB_MEX_FILE
t1 = clock();
#endif
/* main LCP rouinte */
Result = lcp(pB, pA, &pivs, options);
#ifdef MATLAB_MEX_FILE
t2 = clock();
total_time = ((double)(t2-t1))/CLOCKS_PER_SEC;
#else
total_time = -1;
#endif
} else {
pivs = 0;
total_time = 0;
Result = LCP_FEASIBLE;
}
#ifdef MATLAB_MEX_FILE
if (options.clock) {
printf("Time needed to perform pivoting:\n time= %i (%lf seconds)\n",
t2-t1,total_time);
printf("Pivots: %ld\n", pivs);
printf("CLOCKS_PER_SEC = %i\n",CLOCKS_PER_SEC);
}
#endif
/* initialize values to 0 */
for(i=0;i<NSTATES(S);i++)
{
w[i] = 0.0;
z[i] = 0.0;
I[i] = 0.0;
}
/* for a feasible basis, compute the solution */
if ( Result == LCP_FEASIBLE || Result == LCP_PRETERMINATED )
{
#ifdef MATLAB_MEX_FILE
t1 = clock();
#endif
info = Basis_Solve(pB, &(C_SEL(pA,0,m+1)), x, options);
for (j=0,i=0;i<Index_Length(pB->pW);i++,j++)
{
w[Index_Get(pB->pW,i)] = x[j];
/* add 1 due to matlab 1-indexing */
I[j] = Index_Get(pB->pW,i)+1;
}
for(i=0;i<Index_Length(pB->pZ);i++,j++)
{
/* take only positive values */
if (x[j] > options.zerotol ) {
z[Index_Get(pB->pZ,i)] = x[j];
}
/* add 1 due to matlab 1-indexing */
I[j] = Index_Get(pB->pZ, i)+m+1;
}
#ifdef MATLAB_MEX_FILE
t2 = clock();
total_time+=(double)(t2-t1)/(double)CLOCKS_PER_SEC;
if (options.clock) {
printf("Time in total needed to solve LCP: %lf seconds\n",
total_time + (double)(t2-t1)/(double)CLOCKS_PER_SEC);
}
#endif
if (options.normalize) {
/* do the backward normalization */
/* z = diag(c)*zn */
for (i=0; i<m; i++) {
z[i] = c[i]*z[i];
}
/* since the normalization does not compute w properly, we recalculate it from
* recovered z */
/* write data to Mn = M */
memcpy(Mn, *M, n*sizeof(double));
/* write data to qn = q */
memcpy(qn, *q, m*sizeof(double));
/* copy w <- q; */
dcopy(&m, qn, &inc, w, &inc);
/* compute w = M*z + q */
dgemv(&T, &m, &m, &alpha, Mn, &m, z, &inc, &alpha, w, &inc);
/* if w is less than eps, consider it as zero */
for (i=0; i<m; i++) {
if (w[i]<options.zerotol) {
w[i] = 0.0;
}
}
}
}
/* outputs */
*exitflag = (real_T )Result;
*pivots =(real_T)pivs;
*time = (real_T)total_time;
/* reset dimensions and values in basis for a recursive call */
Reinitialize_Basis(m, pB);
}
