cddmex.c

CheckMate is a MATLAB-based tool for modeling, simulating and investigating properties of hybrid dyn

/* cddmex.c: Main program to call cdd library from Matlab (as a mex file).
   For details of proper Matlab call check accompanying cddmex.m file.

  (C) 2004 by M. Kvasnica, ETH Zurich
  (C) 2003 by M. Baotic, Zurich, October 14, 2003
  (C) 2002 by F. Torrisi and M. Baotic, Zurich, 2002
*/



#include "setoper.h"
#include "cdd.h"
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <string.h>
#include "mex.h"

#define CDDMEX_VERSION "1.0"

mxArray * MB_set_LPsol_MatrixPtr(const dd_LPPtr lp)
{
	mxArray * P;	
	mxArray * tmpx;
	mxArray * tmpy;	
	mxArray * tmpobj;
	mxArray * tmphow;
	mxArray * tmpactive;
	double * x;
	double * y;
	double * obj;
	double * how;
	double * active;
	int ii, jj;
	int nactive;
	

 	if (lp !=NULL) {
    		const char *f[] = {"xopt", "lambda", "how", "objlp", "active"};
    		int dims[] = {1};

    		P = mxCreateStructArray(1, dims, 5, f);
    		
    		/* store primal solution */
    		tmpx = mxCreateDoubleMatrix((lp->d)-1, 1, mxREAL);
    		x = mxGetPr(tmpx);
    		for (ii=1; ii<=(lp->d)-1; ii++)
    			x[ii-1]=(double)(lp->sol[ii])[0];
		mxSetField(P, 0, "xopt", tmpx);

    		/* store Lagrange multipliers (i.e. dual solution) */
    		tmpy = mxCreateDoubleMatrix((lp->m)-1, 1, mxREAL);
    		y = mxGetPr(tmpy);
    		for (ii=1; ii<lp->m; ii++)
    			y[ii-1]=0;
    		nactive=0;
		for (ii=1; ii<lp->d; ii++){
			if (lp->nbindex[ii+1]>0) {
				nactive++;
				y[lp->nbindex[ii+1]-1]=(lp->dsol[ii])[0];
			}
		}
		mxSetField(P, 0, "lambda", tmpy);

		/* store objective */
		tmphow = mxCreateDoubleMatrix(1, 1, mxREAL);
    		how = mxGetPr(tmphow);
		how[0]=lp->LPS;
      		mxSetField(P, 0, "how", tmphow);

		/* store objective value */
		tmpobj = mxCreateDoubleMatrix(1, 1, mxREAL);
    		obj = mxGetPr(tmpobj);
		obj[0]=(lp->optvalue)[0];
      		mxSetField(P, 0, "objlp", tmpobj);

		/* store nonbasis */
    		tmpactive = mxCreateDoubleMatrix(nactive, 1, mxREAL);
    		active = mxGetPr(tmpactive);
    		jj=0;
		for (ii=1; ii<lp->d; ii++){
			if (lp->nbindex[ii+1]>0){
				active[jj]=(double)lp->nbindex[ii+1];
				jj++;
			}
		}
		mxSetField(P, 0, "active", tmpactive);


      		return P;
	}
	return 0;
}

/* FT_get_H_MatrixPtr 
//
// Get a matrix from the input mxArray
//   - Copy the A matrix and b matrix into the aggregated matrix [b, -A].
//   - Copy the linearity indices into the data structure.
*/

dd_MatrixPtr FT_get_H_MatrixPtr(const mxArray * in)
{
    mxArray * tmpa;	
    mxArray * tmpb;	
    mxArray * tmpl;	
    dd_MatrixPtr A;
    int eqsize, m, n, i, j;
    double * a;
    double * b;
    double * lin;
	
    if ((tmpa = mxGetField(in, 0, "A")) &&  
        (tmpb = mxGetField(in, 0, "B")) &&
        (mxGetNumberOfDimensions(tmpa) == 2) && /* A must be a matrix */
        (mxGetNumberOfDimensions(tmpb) == 2) && /* B must be a matrix */
        (mxGetM(tmpa) == mxGetM(tmpb)) && /* Dimension must be compatible */
        (mxGetN(tmpb) == 1)) { /* B must be a column vector */
        m = mxGetM(tmpa);
        n = mxGetN(tmpa) + 1; /* Create [b, -A] */
        a = mxGetPr(tmpa);
        b = mxGetPr(tmpb);		
        A = dd_CreateMatrix(m, n);
        for (i = 0; i < m; i++) {
            dd_set_d(A->matrix[i][0],b[i]); /* A[i,0] <- b[i] */
            for (j = 0; j < n - 1; j++) {
                dd_set_d(A->matrix[i][j + 1],- a[j * m + i]); /* A[i,j+1] <- a[i,j] */
            }
        }
        A->representation = dd_Inequality; 		
        A->numbtype = dd_Real;
        /* set lineality if present */
        if ((tmpl = mxGetField(in, 0, "lin")) &&
            (mxGetNumberOfDimensions(tmpl) <= 2) && 
            (mxGetM(tmpl) == 1)) {
            lin = mxGetPr(tmpl);
            eqsize = mxGetN(tmpl);
            for (i = 0; i < eqsize; i++) {
                if (lin[i] <= (double) m) 
                    set_addelem(A->linset,(long) lin[i]); /* linset uses 1-based */
                else
                    mexWarnMsgTxt("Error in the lineality vector ");
            }
        }
        return A;
    } else {
        return 0;
    }
}


dd_LPPtr MB_get_LP_MatrixPtr(const mxArray * in)
{
	mxArray * tmpobj;
	dd_MatrixPtr A;
	int j;
	double * value;
	dd_ErrorType error=dd_NoError;
	dd_LPPtr lp;
	
	dd_set_global_constants(); /* First, this must be called once to use cddlib. */
	
	if (A=FT_get_H_MatrixPtr(in)) {
		/* set objective */
		if ((tmpobj = mxGetField(in, 0, "obj")) &&
		    (mxGetNumberOfDimensions(tmpobj) <= 2) && 
		    (mxGetM(tmpobj) == 1)) {
		    	value = mxGetPr(tmpobj);
  			A->objective = dd_LPmin;
  			dd_set_d(A->rowvec[0], 0.0); /* there is no constant term */
  			for (j = 1; j < A->colsize; j++)
  				dd_set_d(A->rowvec[j],value[j-1]);
  			lp=dd_Matrix2LP(A, &error);
  			dd_FreeMatrix(A);
			return lp;
  		}else{
  			mexErrMsgTxt("Error in the setting of LP objective.");
  		}
	}else{
		mexErrMsgTxt("Error in the setting of LP matrix.");
	}
	return 0;
}

mxArray * FT_set_V_MatrixPtr(const dd_MatrixPtr M)
{
	mxArray * P;
	int mr, mv, m, i, j;
	int ir, iv;
	mxArray * tmpv;	
	mxArray * tmpr;	
	mxArray * tmpvpos;	
	mxArray * tmprpos;	
	mxArray * tmpl;	
	
	double * v;
	double * r; 
	double * vpos; 
	double * rpos; 
	double * l; 

	int k=0;
	dd_rowrange ii;
		
 	if ((M !=NULL) &&
 	    (M->representation == dd_Generator)) {
    		const char *f[] = {"V", "R", "rpos", "vpos", "lin"};
    		int dims[] = {1};

    		P = mxCreateStructArray(1, dims, 5, f);
		if (set_card(M->linset)) {
			tmpl = mxCreateDoubleMatrix(1, set_card(M->linset), mxREAL);
			l = mxGetPr(tmpl);		
    			for (ii=1; ii<=M->rowsize; ii++) {
      				if (set_member(ii, M->linset)) {
      					l[k] = (double)ii;
      					k ++;
      				}
      			}
      			mxSetField(P, 0, "lin", tmpl);
		}
		/* Count the rays and vertices */
		mr = 0;
		mv = 0;
		for (i = 0 ; i < (int)(M->rowsize); i++) {
			if (dd_get_d(M->matrix[i][0]) == 0) {
				mr++;
			} else { 
				mv++;
			}
		}
		m = mr + mv;
		/* Allocate the space in MATLAB */
		tmpr = mxCreateDoubleMatrix(mr, M->colsize - 1, mxREAL);
		r = mxGetPr(tmpr);
		tmpv = mxCreateDoubleMatrix(mv, M->colsize - 1, mxREAL);
		v = mxGetPr(tmpv);
		tmprpos = mxCreateDoubleMatrix(mr, 1, mxREAL);
		rpos = mxGetPr(tmprpos);
		tmpvpos = mxCreateDoubleMatrix(mv, 1, mxREAL);
		vpos = mxGetPr(tmpvpos);
		
		ir = 0; iv = 0;
	  	for (i = 0 ; i < m; i++) {
			if (dd_get_d(M->matrix[i][0]) == 0) {
				/* This is a ray */
    				for (j = 0; j < (int)(M->colsize) - 1; j++) {
      					r[ir + j * mr] = dd_get_d(M->matrix[i][j + 1]);
      				}
      				rpos[ir] = i + 1;
				ir++;
			} else { 
				/* This is a vertex*/
    				for (j = 0; j < (int)(M->colsize) - 1; j++) {
      					v[iv + j * mv] = dd_get_d(M->matrix[i][j + 1]);
      				}
      				vpos[iv] = i + 1;
				iv++;
			}

      		}		
      		mxSetField(P, 0, "V", tmpv);
      		mxSetField(P, 0, "R", tmpr);
      		mxSetField(P, 0, "vpos", tmpvpos);
      		mxSetField(P, 0, "rpos", tmprpos);
      		return P;
	}
	return 0;
}


dd_MatrixPtr FT_get_V_MatrixPtr(const mxArray * in)
{
	mxArray * tmpv;	
	mxArray * tmpr;	
	dd_MatrixPtr V;
	int mr, m, n, i, j;
	double * v;
	double * r;
	
	if ((tmpv = mxGetField(in, 0, "V")) && 
	    (mxGetNumberOfDimensions(tmpv) <= 2)) {
	    	
		if ((tmpr = mxGetField(in, 0, "R")) && 
	    	    (mxGetNumberOfDimensions(tmpv) <= 2)) {
	    	    	mr = mxGetM(tmpr);
	    	    	r = mxGetPr(tmpr);
		} else mr = 0;
	    		    
		m = mxGetM(tmpv);
		n = mxGetN(tmpv) + 1;
		v = mxGetPr(tmpv);
		V = dd_CreateMatrix(m + mr, n);
		for (i = 0; i < m; i++) {
			dd_set_si(V->matrix[i][0],1);
			for (j = 0; j < n - 1; j++) {
				dd_set_d(V->matrix[i][j + 1], v[i + j * m]);
			}
		}
		for (i = m; i < m + mr; i++) {
			dd_set_si(V->matrix[i][0],0);
			for (j = 0; j < n - 1; j++) {
				dd_set_d(V->matrix[i][j + 1], r[(i - m) + j * mr]);
			}
		}		
		V->representation = dd_Generator;	
		V->numbtype = dd_Real;
		return V;
	}
	return 0;
}

mxArray * ZH_set_Vlist(const dd_SetFamilyPtr Inc, const dd_SetFamilyPtr Adj)
{
	mxArray *A, *row;
	double *r;
	dd_bigrange i, j;  /* unsigned long int */
	dd_bigrange cur;
	set_type s, adj;
	int count, total;
	int num;

	if(Inc){
		num = (int) Inc->famsize;
		A = mxCreateCellMatrix(num,1);
		for (i=0;i<Inc->famsize;i++){
			set_initialize(&s, Inc->set[i][0]);
			set_copy(s, Inc->set[i]);
			count=0;
			if (set_card(s)>0) {
				/*get the first element in the set*/
				for (j=1;j<=s[0];j++){
					if (set_member(j,s)) break;
				}
				cur = j;
				total = set_card(s);
				row = mxCreateDoubleMatrix(1,total,mxREAL);
				r = mxGetPr(row);
				while (count <= total){
					/* write to the output */
					set_delelem(s, cur);
					r[count++]=(double)cur;
					/* find its neighbor in list */
					adj = Adj->set[cur-1]; 
					for (j=1; j<=s[0]; j++){
						if ((set_member(j,adj)==1 )&&(set_member(j,s)==1)) {
							break;
						}
					}
					if (j<=s[0])
						cur = j;
					else
						break;
				}
				mxSetCell(A, i, row);
			}
		}
		return A;
	}
    return 0;  
}

mxArray * FT_set_SetFamilyPtr(const dd_SetFamilyPtr F)
{
	mxArray * A, * tmp;
	double * r;
	int i, j, k;
	int num;
	
	if (F) {
		num = (int)(F->famsize);
		A = mxCreateCellMatrix(num, 1); 
		for (i=0; i<(int)(F->famsize); i++) {
			num = (int)set_card(F->set[i]);
			tmp = mxCreateDoubleMatrix(1, num, mxREAL);
			r = mxGetPr(tmp);
			k = 0;
			for (j = 1; j <= *(F->set[i]); j++) {
				if (set_member(j,F->set[i]))
					r[k++] = j;
			}
			mxSetCell(A, i, tmp);
		}
		return A;
	}
	return 0;
}

mxArray * FT_set_Set(const set_type S)
{
	mxArray * tmp;
	double * r;
	int i, j;
	int num;
	
	if (S) {
		num = (int)(S[0]);
		j = 0;
		for (i = 1; i <= num; i++) {
			if (set_member(i,S)) {
				j++; 
			}
		}
		tmp = mxCreateDoubleMatrix(1, j, mxREAL);
		r = mxGetPr(tmp);
		j = 0;
		for (i = 1; i <= num; i++) {
			if (set_member(i,S)) {
				r[j++] = i;
			}
		}	
		return tmp;
	}
	return 0;
}

mxArray * FT_set_H_MatrixPtr(const dd_MatrixPtr M)
{
	mxArray * P;	
	mxArray * tmpa;	
	mxArray * tmpb;	
	mxArray * tmpl;
	int i, j;
	double * a;
	double * b;
	double * l;
	int k=0;

	dd_rowrange ii;

 	if ((M !=NULL) &&
 	    (M->representation == dd_Inequality)) {
    		const char *f[] = {"A", "B", "lin"};
    		int dims[] = {1};

    		P = mxCreateStructArray(1, dims, 3, f);
		if (set_card(M->linset)) {
			tmpl = mxCreateDoubleMatrix(1, set_card(M->linset), mxREAL);
			l = mxGetPr(tmpl);		
    			for (ii=1; ii<=M->rowsize; ii++) {
      				if (set_member(ii, M->linset)) {
      					l[k] = (int)ii;
      					k ++;
      				}
      			}
      			mxSetField(P, 0, "lin", tmpl);
		}

		tmpb = mxCreateDoubleMatrix(M->rowsize, 1, mxREAL);
		b = mxGetPr(tmpb);
		tmpa = mxCreateDoubleMatrix(M->rowsize, M->colsize - 1, mxREAL);
		a = mxGetPr(tmpa);
	  	for (i = 0 ; i < (int)(M->rowsize); i++) {
	  		b[i] = dd_get_d(M->matrix[i][0]);
    			for (j = 0; j < (int)(M->colsize) - 1; j++) {
      				a[i + j * (int)(M->rowsize)] = - dd_get_d(M->matrix[i][j + 1]);
      			}
      		}
      		mxSetField(P, 0, "A", tmpa);
      		mxSetField(P, 0, "B", tmpb);
      		return P;
	}
	return 0;
}

/* hull 
  Compute convex hull of a set of points
*/
void
hull(int nlhs, mxArray * plhs[], int nrhs, const mxArray * prhs[])
{
    dd_PolyhedraPtr P;
    dd_ErrorType err;
    dd_MatrixPtr H,V;
    dd_SetFamilyPtr GI,GA;
	
    if (nrhs  == 1 && nlhs <= 2 && mxIsStruct(prhs[0])) {
        V = FT_get_V_MatrixPtr(prhs[0]);		
        dd_set_global_constants();  /* First, this must be called. */

        P = dd_DDMatrix2Poly(V, &err); /* compute the second representation */
        if (err == dd_NoError) {
            H = dd_CopyInequalities(P);
            plhs[0] = FT_set_H_MatrixPtr(H);
                
            GI=dd_CopyIncidence(P);
            GA=dd_CopyInputAdjacency(P);
            plhs[1] = ZH_set_Vlist(GI,GA);

            dd_FreeSetFamily(GI);
            dd_FreeSetFamily(GA);
            dd_FreeMatrix(H);
        } else {
            dd_WriteErrorMessages(stdout,err);
            mexErrMsgTxt("CDD returned an error, see above(!) for details");
        }
        dd_FreeMatrix(V);
        dd_FreePolyhedra(P);
        return;
    } else {
        mexErrMsgTxt("hull expects a V input struct and produces an H output struct");
    }
}

void
v_hull_extreme(int nlhs, mxArray * plhs[], int nrhs, const mxArray * prhs[])
{
	dd_PolyhedraPtr P,P1;
	dd_ErrorType err;
	dd_MatrixPtr H,V,V1;

	
	if (nrhs  == 1 && nlhs == 1 && mxIsStruct(prhs[0])) {
		V = FT_get_V_MatrixPtr(prhs[0]);		
		dd_set_global_constants();  /* First, this must be called. */

		P = dd_DDMatrix2Poly(V, &err); /* compute the second representation */
		if (err == dd_NoError) {
			H = dd_CopyInequalities(P);
			P1 = dd_DDMatrix2Poly(H, &err); /* compute the second representation */


			if (err == dd_NoError) {
				V1 = dd_CopyGenerators(P1);
				plhs[0] = FT_set_V_MatrixPtr(V1);
				dd_FreeMatrix(V1);
			} else {
    				dd_WriteErrorMessages(stdout,err);
    				mexErrMsgTxt("CDD returned an error, see above(!) for details");    			
  			}
			dd_FreePolyhedra(P1);
			dd_FreeMatrix(H);
		} else {
    			dd_WriteErrorMessages(stdout,err);
    			mexErrMsgTxt("CDD returned an error, see above(!) for details");