Пример #1
0
/* 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;
}
Пример #2
0
/* 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;
}
Пример #3
0
/* 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;
}
Пример #4
0
/* 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));
}
Пример #5
0
/* 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);
}
Пример #6
0
/* 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;
}
Пример #7
0
/* 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;
}
Пример #8
0
/* 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");
}
Пример #9
0
/* 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);
	}
}
Пример #10
0
/* 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;
}
Пример #11
0
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);
}
Пример #12
0
/* 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));
}
Пример #13
0
/* 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);
	
}
Пример #14
0
/* 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; */
}
Пример #15
0
/*
  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);
	}
}