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);
}
/* 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);
}
}
/* 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_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;
}
/* 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;
}
/************************************
Given the factorization LB = U for some B, solve the problem
Bx = vec for x
Solve using LAPACK functions.
************************************/
void LU_Solve1(PT_Matrix pL, PT_Matrix pUt, double *vec, double *x, ptrdiff_t *info)
{
ptrdiff_t n, incx=1;
char U='U', N='N',T='T';
double alpha=1.0, beta=0.0;
n = Matrix_Rows(pL);
/* solve using lapack */
/* compute x = L*vec */
dgemv(&T, &n, &n, &alpha, pL->A, &(pL->rows_alloc), vec, &incx, &beta, x, &incx);
/* solve U*xnew = x using lapack function that also checks for singularity */
dtrtrs(&U, &N, &N, &n, &incx, pUt->A, &(pUt->rows_alloc), x, &n, info);
/* printf("my x=\n"); */
/* Vector_Print_raw(x,n); */
}
/************************************
Given the factorization LB = U for some B, solve the problem
Bx = vec for x
Solve using LUMOD functions.
************************************/
void LU_Solve0(PT_Matrix pL, PT_Matrix pU, double *vec, double *x)
{
int mode;
ptrdiff_t n;
n = Matrix_Rows(pL);
/* solve using lumod */
/* solve for Bx = vec */
mode = 1;
/* due to 1-based indexing in Lprod, Usolve we need to shift vectors backwards */
/* Computes x = L*vec */
Lprod(mode, pL->rows_alloc, n, pL->A-1, vec-1, x-1);
/* Computes x_new s.t. U x_new = x */
Usolve(mode, pU->rows_alloc, n, pU->A-1, x-1);
/* Vector_Print_raw(x,n); */
}
/* 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;
}
/* 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;
}
/* 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);
}
}
