/* 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;
}
/* Assumes column-major */
void Matrix_RemoveRow(PT_Matrix pA, ptrdiff_t row)
{
/* ASSERT(pA->rows > 0)*/
dcopy(&(Matrix_Cols(pA)),
&(C_SEL(pA,Matrix_Rows(pA)-1,0)), &(pA->rows_alloc),
&(C_SEL(pA,row,0)), &(pA->rows_alloc));
pA->rows = pA->rows - 1;
}
/* Assumes column-major */
void Matrix_RemoveCol(PT_Matrix pA, ptrdiff_t col)
{
ptrdiff_t incx = 1;
/* ASSERT(pA->cols > 0)*/
dcopy(&(Matrix_Rows(pA)),
&(C_SEL(pA,0,Matrix_Cols(pA)-1)), &incx,
&(C_SEL(pA,0,col)), &incx);
pA->cols = pA->cols - 1;
}
/* Replaces a row with the given vector */
void Matrix_ReplaceRow(PT_Matrix pA, ptrdiff_t krow, double *vec)
{
ptrdiff_t incx = 1;
/* ASSERT(krow < pA->rows)*/
dcopy(&(Matrix_Cols(pA)),
vec, &incx,
&(C_SEL(pA,krow,0)), &(pA->rows_alloc));
}
/* Replaces a column with the given vector */
void Matrix_ReplaceCol(PT_Matrix pA, ptrdiff_t kcol, double *vec)
{
ptrdiff_t incx = 1;
/* ASSERT(kcol < pA->cols)*/
dcopy(&(Matrix_Rows(pA)),
vec, &incx,
&(C_SEL(pA,0,kcol)), &incx);
}
/* Assumes column-major */
void Matrix_AddCol(PT_Matrix pA, double *col)
{
ptrdiff_t incx = 1;
/* ASSERT(pA->cols < pA->cols_alloc)*/
dcopy(&(Matrix_Rows(pA)),
col, &incx,
&(C_SEL(pA,0,pA->cols)), &incx);
pA->cols = pA->cols + 1;
}
/* Assumes column-major format */
void Matrix_AddRow(PT_Matrix pA, double *row)
{
ptrdiff_t incy = 1;
/* ASSERT(pA->rows < pA->rows_alloc)*/
dcopy(&(Matrix_Cols(pA)),
row, &incy,
&(C_SEL(pA,pA->rows,0)), &(pA->rows_alloc));
pA->rows = pA->rows + 1;
}
/* Column-major formulation */
void Matrix_Print_col(PT_Matrix pMat)
{
ptrdiff_t r,c;
printf("%s = [\n",pMat->name);
for(r=0;r<pMat->rows;r++)
{
printf(" ");
for(c=0;c<pMat->cols;c++)
print_num(C_SEL(pMat,r,c));
printf("\n");
}
printf("];\n");
}
/* Copies matrix B <- A column-wise */
void Matrix_Copy(PT_Matrix pA, PT_Matrix pB, double *w)
{
ptrdiff_t i, j;
memcpy(&(pB->rows), &(Matrix_Rows(pA)), 1*sizeof(ptrdiff_t));
memcpy(&(pB->cols), &(Matrix_Cols(pA)), 1*sizeof(ptrdiff_t));
memcpy(&(pB->rows_alloc), &(pA->rows_alloc), 1*sizeof(ptrdiff_t));
memcpy(&(pB->cols_alloc), &(pA->cols_alloc), 1*sizeof(ptrdiff_t));
for (i=0;i<Matrix_Cols(pA);i++) {
for (j=0;j<Matrix_Rows(pA);j++) {
w[j] = C_SEL(pA, j, i);
}
Matrix_ReplaceCol(pB, i, w);
}
}
/* The main function: pivot */
ptrdiff_t Basis_Pivot(PT_Basis pB, PT_Matrix pA, ptrdiff_t enter, ptrdiff_t leave)
{
ptrdiff_t m, nW, Wl, c, r;
PT_Index pW, pZ, pWc;
PT_Matrix pG, pX, pF;
ptrdiff_t left;
/* abbreviations */
pW = pB->pW;
pWc = pB->pWc;
pZ = pB->pZ;
pG = pB->pG;
pX = pB->pX;
pF = pB->pF;
/* get dimensions */
m = Matrix_Rows(pA);
nW = Index_Length(pW);
/* determine the leaving variable */
if(leave < Index_Length(pW))
{
left = Index_Get(pW,leave);
}
else
{
left = Index_Get(pZ,leave-Index_Length(pW))+m;
}
/* printf("\n ---------------------------------------\n"); */
/* printf("enter = %ld, leave = %ld\n", enter, leave); */
/* Four cases */
if(enter < m)
{
/* r = find(B.Wc == enter) */
r = Index_FindElement(pWc, enter);
if(leave < nW)
{
/* printf("case 1:\n"); */
Wl = Index_Get(pW,leave);
/* G_l,* = M_e,Z */
/* Basis_Print(pB); */
/* Update matrix G with the row corresponding to the entering variable */
Matrix_GetRow(pA, pB->y, enter, pZ);
Matrix_ReplaceRow(pG, leave, pB->y);
/* X_r,* = M_Wl,Z */
/* Update matrix X with the row corresponing to the leaving variable
from index set pW. Similarly, we must update matrix F as its gets overwritten
by dgesv routine. */
Matrix_GetRow(pA, pB->y, Wl, pZ);
Matrix_ReplaceRow(pF, r, pB->y);
Matrix_ReplaceRow(pX, r, pB->y);
/* smart update with LUmod */
LU_Replace_Row(pB->pLX, pB->pUX, r, pB->y, pB->z, pB->w);
/* Wc_r = Wl, W_l = e */
/* update index sets */
Index_Replace(pWc, r, Wl);
Index_Replace(pW, leave, enter);
}
else /* leave >= nW */
{
/* printf("case 2:\n"); */
Index_Add(pW,enter);
c = leave-nW;
/* Replace row r and column c with the last row and column of X
and remove last row/column. */
Matrix_RemoveRow(pX,r);
Matrix_RemoveCol(pX,c);
/* We must update matrix F as well */
Matrix_RemoveRow(pF,r);
Matrix_RemoveCol(pF,c);
/* smart update with LUmod */
LU_Shrink(pB->pLX, pB->pUX, r, c, pB->y, pB->z, pB->w);
/* Update indices */
Index_Remove(pWc, r);
Index_Remove(pZ, c);
/* Update G matrix */
Matrix_RemoveCol(pG, c);
Matrix_GetRow(pA, pB->y, enter, pZ);
Matrix_AddRow(pG, pB->y);
}
}
else /* enter >= m */
{
if(leave < nW)
{
/* printf("case 3:\n"); */
Wl = Index_Get(pW,leave);
/* Add row Wl and column e-m to X */
if(Index_Length(pZ) > 0)
{
Matrix_GetRow(pA, pB->y, Wl, pZ);
Matrix_GetCol(pA, pB->z, pWc, enter-m);
}
pB->y[Index_Length(pZ)] = C_SEL(pA,Wl,enter-m);
/* updating F */
Matrix_AddCol(pF, pB->z);
Matrix_AddRow(pF, pB->y);
/* updating X */
Matrix_AddCol(pX, pB->z);
Matrix_AddRow(pX, pB->y);
/* update with LUmod */
LU_Expand(pB->pLX, pB->pUX, pB->y, pB->z, pB->w);
/* printf("pX col=\n"); */
/* Matrix_Print_col(pX); */
/* Remove row Wl from G */
/* and add column e-m */
Matrix_RemoveRow(pG, leave);
Index_Remove(pW, leave);
Matrix_GetCol(pA, pB->z, pW, enter-m);
Matrix_AddCol(pG, pB->z);
/* Extend Wc and Z */
Index_Add(pWc, Wl);
Index_Add(pZ, enter-m);
}
else /* leave >= nW */
{
/* printf("case 4:\n"); */
/* Replace column */
c = leave - nW;
/* G_*,c = M_w,(e-m) */
Matrix_GetCol(pA, pB->y, pW, enter-m);
Matrix_ReplaceCol(pG, c, pB->y);
/* X_*,c = M_Wc,(e-m) */
Matrix_GetCol(pA, pB->z, pWc, enter-m);
/* Vector_Print_raw(pB->z,m); */
/* Basis_Print(pB); */
/* update matrices X, F by replacing column */
Matrix_ReplaceCol(pX, c, pB->z);
Matrix_ReplaceCol(pF, c, pB->z);
/* update for LUmod */
LU_Replace_Col(pB->pLX, pB->pUX, c, pB->y, pB->z, pB->w);
/* Update Z */
Index_Replace(pZ, c, enter-m);
}
}
/* create column-wise upper triangular matrix Ut from U*/
Matrix_Triu(pB->pUt, pB->pUX, pB->r);
return left;
}
void Matrix_GetCol(PT_Matrix pA, double *col, PT_Index row_index, ptrdiff_t col_index)
{
ptrdiff_t i;
for(i=0;i<Index_Length(row_index);i++)
col[i] = C_SEL(pA, Index_Get(row_index,i), col_index);
}
/* pA assumed column major */
void Matrix_GetRow(PT_Matrix pA, double *row, ptrdiff_t row_index, PT_Index col_index)
{
ptrdiff_t i;
for(i=0;i<Index_Length(col_index);i++)
row[i] = C_SEL(pA,row_index,Index_Get(col_index,i));
}
/* 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);
}
/* If something goes wrong, returned value contains LCP_flag (negative values). */
ptrdiff_t LexMinRatio(PT_Basis pB, PT_Matrix pA, double *v, T_Options options)
{
ptrdiff_t m, i, j, k, incx, info;
PT_Index pP;
double *rat, *q;
double minrat=0.0, alpha;
m = Matrix_Rows(pA);
pP = pB->pP;
rat = pB->y;
q = pB->w;
/* used to obtain an identity matrix in lex-pivot operation */
alpha = 0.0;
/* increment of each vectors is 1 */
incx = 1;
/* Compute the index set of positive v s */
Index_Length(pP) = 0;
for (j=0, i=0;i<m;i++)
{
if ( v[i] > options.zerotol )
{
Index_Add(pP, i);
/* The positive elements of 1/v are now the first |pP| elements */
v[j++] = v[i];
}
}
if(Index_Length(pP) == 0) /* Problem infeasible */
return LCP_INFEASIBLE;
/* q = A(:,m+1) */
memcpy(q,&(C_SEL(pA, 0, m+1)),m*sizeof(double));
for (i=0; i<m+1; i++)
{
/* Basis_Print(pB); */
/* Index_Print(pP); */
/* rat = iB*q */
info = Basis_Solve(pB, q, rat, options);
if (info!=0) {
#ifdef MATLAB_MEX_FILE
if (options.verbose) {
printf("Numerical problems with Basis_Solve in LexRatio.\n");
}
#endif
return LCP_CODEERROR;
}
/* printf("q after Basis_Solve in LexRatio=\n"); */
/* Vector_Print_raw(q,m); */
/* rat = rat(pP) / v(pP) */
/* find minimum ratio */
minrat = rat[Index_Get(pP,0)]/v[0];
for (j=0; j<Index_Length(pP); j++) {
rat[j] = rat[Index_Get(pP,j)]/v[j];
if (rat[j] < minrat) {
minrat = rat[j];
}
}
/* printf("minrat=%lf\n",minrat); */
/* printf("Ratio vector after=\n"); */
/* Vector_Print_raw(rat,m); */
/* printf("positive index set:\n"); */
/* Index_Print(pP); */
/* remove all indices that are greater than the minumum ratio with some tolerance */
/* Move all elements equal to minrat to the start of the list */
for (k=0, j=0;j<Index_Length(pP);j++)
{
/* printf("j=%ld, fabs(minrat - rat[lndex_Get(pP,j)]) = %f\n", j, fabs(minrat - rat[Index_Get(pP,j)])); */
if (fabs(minrat - rat[j]) < options.lextol) {
Index_Set(pP,k,Index_Get(pP,j));
/*pP is now a set of indices with equal ratio */
v[k++] = v[j];
}
}
pP->len = k;
/* printf("new index set:\n"); */
/* Index_Print(pP); */
if(Index_Length(pP) <= 0) {
/* Vector_Print_raw(v,m); */
/* Vector_Print_raw(rat,m); */
/* mexPrintf("minrat = %lf, length(pP) = %ld\n",minrat,Index_Length(pP)); */
/* Index_Print(pP); */
#ifdef MATLAB_MEX_FILE
if (options.verbose) {
printf("Numerical problems with finding lexicographic minimum. You can refine this using the 'lextol' feature in options.\n");
}
#endif
return LCP_CODEERROR;
}
if(Index_Length(pP) > 1)
{
/* printf("Taking lex-pivot\n"); */
/* Need to make a lex-comparison */
/* q = 0*q */
dscal(&m, &alpha, q, &incx);
q[i] = 1.0;
}
else
{
return Index_Get(pP,0);
}
}
/* if the lex-pivot failed, pick the largest element from v_i vector i=0, ..., k */
return Index_Get(pP, idamax(&k, v, &incx)-1);
/* mexErrMsgTxt("Did not find a lex-min. This should be impossible."); */
/* return LCP_INFEASIBLE; */
}
/*
The main LCP function
Goal: compute w,z > 0 s.t. w-Mz = q, w'z = 0
On Input:
pA = [-M -1 q]
Returns:
LCP_FEASIBLE 1
LCP_INFEASIBLE -1
LCP_UNBOUNDED -2
LCP_PRETERMINATED -3
LCP_CODEERROR -4
*/
int lcp(PT_Basis pB, PT_Matrix pA, ptrdiff_t *pivs, T_Options options)
{
ptrdiff_t enter, leave, left;
ptrdiff_t m, i, info;
int FirstLoop;
double *v, *t, total_time;
#ifdef MATLAB_MEX_FILE
clock_t t1,t2;
#endif
m = Matrix_Rows(pA);
v = pB->v;
t = pB->y;
/* Index of the artificial variable */
enter = 2*m;
#ifdef MATLAB_MEX_FILE
t1 = clock();
#endif
FirstLoop = 1;
(*pivs) = 0;
while(1)
{
if ( ++(*pivs) >= options.maxpiv ) {
#ifdef MATLAB_MEX_FILE
if (options.verbose) {
printf("Exceeded maximum number of pivots, returning current basis.\n");
}
#endif
/* take out the artificial variable from the basis */
left = Basis_Pivot(pB, pA, enter, Index_Length(pB->pW));
return LCP_PRETERMINATED;
}
#ifdef MATLAB_MEX_FILE
t2 = clock();
total_time = ((double)(t2-t1))/CLOCKS_PER_SEC;
#else
total_time = -1;
#endif
if ( total_time >= options.timelimit ) {
#ifdef MATLAB_MEX_FILE
if (options.verbose) {
printf("Maximum time limit was achieved, returning current basis.\n");
}
#endif
/* take out the artificial variable from the basis */
left = Basis_Pivot(pB, pA, enter, Index_Length(pB->pW));
return LCP_PRETERMINATED;
}
/******************************/
/* 1. Choose leaving variable */
/******************************/
if(FirstLoop == 1)
{
FirstLoop = 0;
for(i=0;i<m;i++) v[i]=1.0;
} else
{
/* Solve for v = inv(B)*Ae */
if(enter < m)
{
/* A(:,enter) is [0 ... 1 ... 0] */
for(i=0;i<m;i++) t[i]=0.0;
t[enter] = 1.0;
}
else
{
/* v = M(:,enter-m) */
memcpy(t,&(C_SEL(pA,0,enter-m)),sizeof(double)*m);
}
#ifdef MATLAB_MEX_FILE
if (options.verbose) {
printf("right hand side entering basis_solve:\n");
Vector_Print_raw(t,m);
}
#endif
info = Basis_Solve(pB, t, v, options);
if (info!=0) {
#ifdef MATLAB_MEX_FILE
if (options.verbose) {
printf("info=%ld\n", info);
printf("Numerical problems in lcp.\n");
}
#endif
return LCP_INFEASIBLE;
}
#ifdef MATLAB_MEX_FILE
/* print basis if required */
if (options.verbose) {
Basis_Print(pB);
printf("Basic solution:\n");
Vector_Print_raw(v,m);
}
#endif
}
/* printf("vector v:\n"); */
/* Vector_Print_raw(v, m); */
leave = LexMinRatio(pB, pA, v, options);
/* Test if leave <0 => no valid leaving var, returning LCP_flag */
if (leave < 0) {
return leave;
}
#ifdef MATLAB_MEX_FILE
if (options.verbose) {
printf("Lexicographic minimum found with leaving var=%ld\n",leave);
}
#endif
/******************************/
/* 2. Make the pivot */
/******************************/
#ifdef MATLAB_MEX_FILE
if (options.verbose) {
printf("Pivoting with variables: enter = %4ld leave = %4ld ...\n", enter, leave);
}
#endif
/* do the pivot */
left = Basis_Pivot(pB, pA, enter, leave);
/* for LUMOD we need to refactorize at every n-steps due numerical problems */
/* every n-steps refactorize L, U from X using lapack's routine dgetrf */
if ((options.routine==0) && (*(pivs) % options.nstepf == 0) ) {
LU_Refactorize(pB);
}
#ifdef MATLAB_MEX_FILE
if (options.verbose) {
printf("Leaving variable after pivot operation: left = %4ld\n", left);
}
#endif
/* Did the artificial variable leave? */
if(left == 2*m) {
return LCP_FEASIBLE;
}
/*******************************/
/* 3. Choose entering variable */
/*******************************/
/* The entering variable must be the
complement of the previous leaving variable */
enter = (left+m) % (2*m);
}
}