/* 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 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);
}
}
