Пример #1
0
void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[] )
{
  double *prec,*eoutr,*eouti;
  int     mrows,ncols;
  char *input_buf;
  char *w1,*w2;
  int   buflen,status;
  mpfr_t xr,xi,z,temp;
  mp_exp_t expptr;

  /* Check for proper number of arguments. */
  if(nrhs!=3) {
    mexErrMsgTxt("3 inputs required.");
  } else if(nlhs>1) {
    mexErrMsgTxt("Too many output arguments");
  }
  
  /* The input must be a noncomplex scalar double.*/
  mrows = mxGetM(prhs[0]);
  ncols = mxGetN(prhs[0]);
  if( !mxIsDouble(prhs[0]) || mxIsComplex(prhs[0]) ||
      !(mrows==1 && ncols==1) ) {
    mexErrMsgTxt("Input must be a noncomplex scalar double.");
  }

  /* Set precision and initialize mpfr variables */
  prec = mxGetPr(prhs[0]);
  mpfr_set_default_prec(*prec);
  mpfr_init(xr);  mpfr_init(xi);  
  mpfr_init(z);
  mpfr_init(temp);  
  
  /* Read the input strings into mpfr x and y */
  buflen = (mxGetM(prhs[1]) * mxGetN(prhs[1])) + 1;
  input_buf=mxCalloc(buflen, sizeof(char));
  status = mxGetString(prhs[1], input_buf, buflen);
  mpfr_set_str(xr,input_buf,10,GMP_RNDN);

  buflen = (mxGetM(prhs[2]) * mxGetN(prhs[2])) + 1;
  input_buf=mxCalloc(buflen, sizeof(char));
  status = mxGetString(prhs[2], input_buf, buflen);
  mpfr_set_str(xi,input_buf,10,GMP_RNDN);
  
  /* Mathematical operation */
  /* magnitude */
  mpfr_mul(temp,xr,xr,GMP_RNDN);
  mpfr_mul(z,xi,xi,GMP_RNDN);
  mpfr_add(z,temp,z,GMP_RNDN);
  mpfr_sqrt(z,z,GMP_RNDN);

  /* Retrieve results */
  mxFree(input_buf);
  input_buf=mpfr_get_str (NULL, &expptr, 10, 0, z, GMP_RNDN);
  w1=malloc(strlen(input_buf)+20);
  w2=malloc(strlen(input_buf)+20);
  if (strncmp(input_buf, "-", 1)==0){
    strcpy(w2,&input_buf[1]);
    sprintf(w1,"-.%se%i",w2,expptr);
  } else {
    strcpy(w2,&input_buf[0]);
    sprintf(w1,"+.%se%i",w2,expptr);
  }
  plhs[0] = mxCreateString(w1);
/*   plhs[1] = mxCreateDoubleMatrix(mrows,ncols, mxREAL); */
/*   eoutr=mxGetPr(plhs[1]); */
/*   *eoutr=expptr; */
  
  mpfr_clear(xr);  mpfr_clear(xi);
  mpfr_clear(z);
  mpfr_clear(temp);
  mpfr_free_str(input_buf);
  
  free(w1);
  free(w2);
}
Пример #2
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	PXL_RETURN_CODE nRetValue;		/* the return codes of the pixelink functions */
	mxArray *serialNumberField;
	U32		serialNumber = -1;		/* the serial number of the device */
	HANDLE deviceId;				/* the deviceId PimMegaInitialize returns */
	mxArray *lhs[1], *rhs[2];		/* to call fgdevices */
	double *px;
	int isOpen = 0;
	/* prhs[0] should be the device handle (struct) or serial number (int) */
	if (nrhs==1)
	{
		/* get the serialNumber */
		if (mxIsStruct(prhs[0]))
		{
			serialNumberField = mxGetField(prhs[0], 0, "HardwareVersion.SerialNumber");
			serialNumber = (U32)mxGetScalar(serialNumberField);
		}
		else if (mxIsDouble(prhs[0]))
		{
			serialNumber = (U32)mxGetScalar(prhs[0]);
		}
		else
		{
			mexPrintf("PixeLINK Interface error.\n");
			mexPrintf("plClose: Wrong arguments given. One argument needed (device handle or serial number).\n");
			mexErrMsgTxt("\n");
		}
		/* check whether the device has been opened */
		rhs[0] = mxCreateString("isopen");
		rhs[1] = mxCreateDoubleMatrix(1,1,mxREAL);
		px = mxGetPr(rhs[1]);
		px[0] = (U32)serialNumber;
		mexCallMATLAB(1, lhs, 2, rhs, "plDevices");
		isOpen = (int)mxGetScalar(lhs[0]);
		if (isOpen)
		{
			/* get the deviceId */
			rhs[0] = mxCreateString("get");
			rhs[1] = mxCreateDoubleMatrix(1,1,mxREAL);
			px = mxGetPr(rhs[1]);
			px[0] = (U32)serialNumber;
			mexCallMATLAB(1, lhs, 2, rhs, "plDevices");
			deviceId = (HANDLE *)(int)mxGetScalar(lhs[0]);
			/* uninitialize the device */
			nRetValue = PxLUninitialize(deviceId);
			if (plError(nRetValue, "closing the device"))
			{
				mexErrMsgTxt("\n");
			}
			/* update the open device array in plDevices */
			rhs[0] = mxCreateString("remove");
			rhs[1] = mxCreateDoubleMatrix(1,1,mxREAL);
			px = mxGetPr(rhs[1]);
			px[0] = (U32)serialNumber;
			mexCallMATLAB(1, lhs, 2, rhs, "plDevices");
		}
		else /* !isOpen */
		{
			mexPrintf("plClose: cannot close device with serial number %d.\n", serialNumber);
			mexPrintf("Device has not been opened.\n");
			mexErrMsgTxt("\n");
		}
	}
	else /* ! ((nrhs==1) && mxIsStruct(prhs[0])) */
	{
		mexPrintf("PixeLINK Interface error.\n");
		mexPrintf("plClose: Wrong arguments given. One argument needed (device handle or serial number).\n");
		mexErrMsgTxt("\n");
	}
}
Пример #3
0
void mexFunction(int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[])
{
	double *v, *x, sigma, *lambda, *pr; mwIndex *ir, *jc;
	char *mode; int imode;

    //Check Inputs
    if(nrhs < 1) {
        printInfo();        
        return;
    }   
    if(nrhs < 2) {
        mexErrMsgTxt("You must supply the callback mode and input vector.");
        return;
    }
    if(mxIsEmpty(prhs[0]) || !mxIsChar(prhs[0])) {
        mexErrMsgTxt("The mode must be a string!");
        return;
    }
    if(!mxIsEmpty(prhs[1])) {
        if(mxIsClass(prhs[1],"scipvar") || mxIsClass(prhs[1],"barvec")) {
            mexErrMsgTxt("SCIP and BARON cannot be used with this callback function - please specify 'mcode' via symbset as the cbmode.");
            return;
        }           
        if(!mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) || mxIsSparse(prhs[1])) {
            mexErrMsgTxt("The input vector must be a dense real double vector!");
            return;
        }
    }
    else {
        mexErrMsgTxt("The input vector must be a dense real double vector!");
        return;
    }
	//Check x input size
	if(mxGetNumberOfElements(prhs[1]) != getNoVar()) {
		mexErrMsgTxt("The input vector is not the right size!");
	}
	//Get x
	x = mxGetPr(prhs[1]);
	
	//Determine input mode and setup return variable
	mode = mxArrayToString(prhs[0]);
	lower(mode);
	if(!strcmp(mode,"obj")) {
		imode = 0;
		plhs[0] = mxCreateDoubleMatrix(1,1, mxREAL);
		v = mxGetPr(plhs[0]);
	}
	else if(!strcmp(mode,"grad")) {
		imode = 1;
		plhs[0] = mxCreateDoubleMatrix(1,getNoVar(), mxREAL);
		v = mxGetPr(plhs[0]);
	}
	else if(!strcmp(mode,"con")) {
		imode = 2;
		plhs[0] = mxCreateDoubleMatrix(getNoCon(),1, mxREAL);
		v = mxGetPr(plhs[0]);
	}
	else if(!strcmp(mode,"jac")) {
		imode = 3;
		plhs[0] = mxCreateSparse(getNoCon(),getNoVar(),getNNZJac(),mxREAL);   
		pr = mxGetPr(plhs[0]);
		ir = mxGetIr(plhs[0]);
		jc = mxGetJc(plhs[0]);
	}
	else if(!strcmp(mode,"hess")) {
		if(nrhs < 4) {
			mexErrMsgTxt("You must supply the callback mode, input vector, sigma and lambda for Hessian Evaluations.");
			return;
		}
		//Check length of Sigma
		if(mxIsEmpty(prhs[2]) || !mxIsDouble(prhs[2]) || mxIsComplex(prhs[2]) || mxGetNumberOfElements(prhs[2]) != 1)
			mexErrMsgTxt("Sigma must be a real, double scalar.");
		//Check length of Lambda
		if(!mxIsDouble(prhs[3]) || mxIsComplex(prhs[3]) || mxIsSparse(prhs[3]) || mxGetNumberOfElements(prhs[3]) != getNoCon())
			mexErrMsgTxt("Lambda must be a real, double, dense vector with ncon elements.");
		//Get Sigma, Lambda
		sigma = *mxGetPr(prhs[2]);
		lambda = mxGetPr(prhs[3]);
		imode = 4;
		plhs[0] = mxCreateSparse(getNoVar(),getNoVar(),getNNZHess(),mxREAL);   
		pr = mxGetPr(plhs[0]);
		ir = mxGetIr(plhs[0]);
		jc = mxGetJc(plhs[0]);
	}
	else
		mexErrMsgTxt("Unknown mode - options are 'obj', 'grad', 'con', 'jac', or 'hess'");
	mxFree(mode);
	
	
	//Call Req Callback
	switch(imode)
	{
		case 0: //objective
			*v = objective(x);
			break;
		case 1: //gradient
			gradient(x,v);
			break;
		case 2: //constraints
			constraints(x,v);
			break;
		case 3: //jacobian
			jacobian(x,pr,ir,jc);
			break;
		case 4: //hessian
			hessian(x,sigma,lambda,pr,ir,jc);
			break;
	}
}
Пример #4
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	mxClassID cid;
	const int *dim;
	const unsigned char *uc;
	const unsigned short *us;
	const unsigned int *ul;
	const double *dbl;
	bool forward = 1;
	int nd, ne, c = 0, cm, si;
	int odim[2] = {1, 2};
	double *odbl;

	if (nrhs < 1 || nrhs > 2 || nlhs > 2)
		mexErrMsgTxt("Bad number of input/output arguments.");
	cid = mxGetClassID(*prhs);
	dim = mxGetDimensions(*prhs);
	nd = mxGetNumberOfDimensions(*prhs);
	ne = mxGetNumberOfElements(*prhs);
	si = 0;
	if ((nrhs > 1) &&
	    mxIsDouble(prhs[1]) &&
	    (mxGetNumberOfElements(prhs[1]) == 1)) {
		si = (int) *((double*) mxGetPr(prhs[1]));
		if (si > 0) {
			--si;
			if (si >= ne)
				mexErrMsgTxt(ff_errbadindex);
		} else if (si < 0) {
			forward = 0;
			si = ne + si;
			if (si < 0)
				mexErrMsgTxt(ff_errbadindex);
		} else
			mexErrMsgTxt(ff_errbadindex);
	}

	switch (cid) {
		case mxLOGICAL_CLASS:
		case mxINT8_CLASS:
		case mxUINT8_CLASS:
			uc = (const unsigned char*) mxGetData(*prhs);
			uc = &uc[si];
			if (forward) {
				for (c = si; c < ne; ++c)
					if (*uc++ != 0)
						break;
			} else {
				for (c = si; c >= 0; --c)
					if (*uc-- != 0)
						break;
			}
			break;

		case mxINT16_CLASS:
		case mxUINT16_CLASS:
			us = (const unsigned short*) mxGetData(*prhs);
			us = &us[si];
			if (forward) {
				for (c = si; c < ne; ++c)
					if (*us++ != 0)
						break;
			} else {
				for (c = si; c >= 0; --c)
					if (*us-- != 0)
						break;
			}
			break;

		case mxINT32_CLASS:
		case mxUINT32_CLASS:
		case mxSINGLE_CLASS:
			ul = (const unsigned int*) mxGetData(*prhs);
			ul = &ul[si];
			if (forward) {
				for (c = si; c < ne; ++c)
					if (*ul++ != 0)
						break;
			} else {
				for (c = si; c >= 0; --c)
					if (*ul-- != 0)
						break;
			}
			break;

		case mxDOUBLE_CLASS:
			dbl = (const double*) mxGetData(*prhs);
			dbl = &dbl[si];
			if (forward) {
				for (c = si; c < ne; ++c)
					if (*dbl++ != 0.0)
						break;
			} else {
				for (c = si; c >= 0; --c)
					if (*dbl-- != 0.0)
						break;
			}
			break;

		default: mexErrMsgTxt("Invalid input class.");
			 break;
	}

	if (c < 0 || c >= ne) {

		*plhs = mxCreateNumericArray(2, ff_emptyarray, mxDOUBLE_CLASS, mxREAL);
		if (nlhs > 1)
			plhs[1] = mxCreateNumericArray(2, ff_emptyarray, mxDOUBLE_CLASS, mxREAL);

	} else {

		*plhs = mxCreateNumericArray(2, ff_scalararray, mxDOUBLE_CLASS, mxREAL);
		if (*plhs == NULL)
			mexErrMsgTxt("Error allocating 1x1 output argument.");
		odbl = (double *) mxGetData(*plhs);
		if (odbl == NULL)
			mexErrMsgTxt("Error addressing 1x1 output argument.");
		*odbl = (double) (c + 1);

		if (nlhs > 1) {
			odim[1] = nd;
			plhs[1] = mxCreateNumericArray(2, odim, mxDOUBLE_CLASS, mxREAL);
			if (plhs[1] == NULL)
				mexErrMsgTxt("Error allocating 1xN output argument.");
			odbl = (double *) mxGetData(plhs[1]);
			if (odbl == NULL)
				mexErrMsgTxt("Error addressing 1xN output argument.");
			for ( ; nd > 0; --nd) {
				cm = c % *dim;
				*odbl++ = (double) (cm + 1);
				c = (int) (((double) c) / ((double) *dim++));
			}
		}
	}
}
Пример #5
0
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) {
    
    // INPUT:
    double *pdX, *pdY1, *pdY2;
    
    // OUTPUT:
    double *pdDX;
    
    // Local:    
    const mxArray * pArg;
    mwSize nrowsX,ncolsX, nrowsY1, ncolsY1, nrowsY2, ncolsY2;

    long Nx, nshift, Nvecs, Ny;
        
    
    /* --------------- Check inputs ---------------------*/
    if (nrhs != 3)
        mexErrMsgTxt("3 inputs required");
    if (nlhs > 1)  
        mexErrMsgTxt("only one output allowed");
    
    /// ------------------- X ----------
	pArg = prhs[0];
    nrowsX = mxGetM(pArg); ncolsX = mxGetN(pArg);
	if (!mxIsNumeric(pArg) || !mxIsDouble(pArg) || mxIsEmpty(pArg) || mxIsComplex(pArg) )
            mexErrMsgTxt("Input 1 (V) must be a noncomplex matrix of doubles.");
	if ((ncolsX != 1) && (nrowsX != 1))
            mexErrMsgTxt("Input 1 (V) must be a vector");
    pdX  = mxGetPr(pArg);
    Nx = nrowsX*ncolsX;    

    /// ------------------- Y1 ----------
    pArg = prhs[1];
    nrowsY1 = mxGetM(pArg); ncolsY1 = mxGetN(pArg);
	if(!mxIsNumeric(pArg) || !mxIsDouble(pArg) || mxIsEmpty(pArg) || mxIsComplex(pArg) ) { 
            mexErrMsgTxt("Input 2 (Y1) must be a noncomplex double matrix.");
    }
    pdY1 = mxGetPr(prhs[1]);
    
    if ((nrowsY1 == 1) || (ncolsY1 == 1)) {
        Nvecs = 1;
        Ny = nrowsY1 * ncolsY1;
    } else {
        Ny = nrowsY1;
        Nvecs = ncolsY1;
    }
                
    if (Ny != Nx) {
        mexErrMsgTxt("Length of Y1 vectors must be the same as the length of X");
    }
        
    /// ------------------- Y2 ----------
    pArg = prhs[2];
    nrowsY2 = mxGetM(pArg); ncolsY2 = mxGetN(pArg);
	if(!mxIsNumeric(pArg) || !mxIsDouble(pArg) || mxIsEmpty(pArg) || mxIsComplex(pArg) ) { 
            mexErrMsgTxt("Input 3 (Y2) must be a noncomplex double matrix.");
    }
    if ((nrowsY1 != nrowsY2) || (ncolsY2 != ncolsY1)) {
        mexErrMsgTxt("Y1 and Y2 must be the same size");
    }
    pdY2 = mxGetPr(prhs[2]);
       
    
    

    /// ------------------- dX (OUTPUT)----------    
    plhs[0] = mxCreateDoubleMatrix(1, Nvecs, mxREAL);
    pdDX = mxGetPr(plhs[0]);

    bestCircShift(pdX, pdY1, pdY2, Nx, Nvecs, pdDX);    

}
Пример #6
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  size_t i, j, n;

  double  locval;

  double *M;
  double *L;
  double *U;
  double  sig;
  double *B;  /* Matlab will pass B_ = prhs[4] in as double, I think. */
  double *E; 

  double  val;
  double *Y;

  const mxArray *M_;
  const mxArray *L_;
  const mxArray *U_;
  const mxArray *sig_;
  const mxArray *B_;
  const mxArray *E_;

  const mxArray *val_;
  const mxArray *Y_;

  if( nrhs != 6 )
    mexErrMsgTxt("quadprob: Need six input arguments.");

  if( nlhs != 2 )
    mexErrMsgTxt("quadprob: Need two output arguments.");

  M_   = prhs[0];
  L_   = prhs[1];
  U_   = prhs[2];
  sig_ = prhs[3];
  B_   = prhs[4];
  E_   = prhs[5];

  n = mxGetM(M_) - 1;

  plhs[0] = mxCreateDoubleMatrix(1  , 1  , mxREAL);
  plhs[1] = mxCreateDoubleMatrix(1+n, 1+n, mxREAL);

  val_   = plhs[0];
  Y_     = plhs[1];

  if( mxGetM(M_) != 1+n || mxGetN(M_) != 1+n)
    mexErrMsgTxt("quadprob: Input 1 must be a square matrix of the appropriate size.");
  if( mxIsSparse(M_) )
    mexErrMsgTxt("quadprob: Input 1 should not be sparse.");
  if( mxIsDouble(M_) == 0 || mxIsComplex(M_) == 1 )
    mexErrMsgTxt("quadprob: Input 1 should be of type double.");

  if( mxGetM(L_) != 1+n || mxGetN(L_) != 1)
    mexErrMsgTxt("quadprob: Input 2 must be a column vector of the appropriate size.");
  if( mxIsSparse(L_) )
    mexErrMsgTxt("quadprob: Input 2 should not be sparse.");
  if( mxIsDouble(L_) == 0 || mxIsComplex(L_) == 1 )
    mexErrMsgTxt("quadprob: Input 2 should be of type double.");

  if( mxGetM(U_) != 1+n || mxGetN(U_) != 1)
    mexErrMsgTxt("quadprob: Input 3 must be a column vector of the appropriate size.");
  if( mxIsSparse(U_) )
    mexErrMsgTxt("quadprob: Input 3 should not be sparse.");
  if( mxIsDouble(U_) == 0 || mxIsComplex(U_) == 1 )
    mexErrMsgTxt("quadprob: Input 3 should be of type double.");

  if( mxGetM(sig_) * mxGetN(sig_) != 1)
    mexErrMsgTxt("quadprob: Input 4 must be a scalar.");
  if( mxIsSparse(sig_) )
    mexErrMsgTxt("quadprob: Input 4 should not be sparse.");
  if( mxIsDouble(sig_) == 0 || mxIsComplex(sig_) == 1 )
    mexErrMsgTxt("quadprob: Input 4 should be of type double.");

  if( mxGetM(B_) != 1+n || mxGetN(B_) != 1)
    mexErrMsgTxt("quadprob: Input 5 must be a column vector of the appropriate size.");
  if( mxIsSparse(B_) )
    mexErrMsgTxt("quadprob: Input 5 should not be sparse.");
  if( mxIsDouble(B_) == 0 || mxIsComplex(B_) == 1 )
    mexErrMsgTxt("quadprob: Input 5 should be of type double.");

  if( mxGetM(E_) != 1+n || mxGetN(E_) != 1+n)
    mexErrMsgTxt("quadprob: Input 6 must be a matrix of the appropriate size.");
  if( mxIsSparse(E_) )
    mexErrMsgTxt("quadprob: Input 6 should not be sparse.");
  if( mxIsDouble(E_) == 0 || mxIsComplex(E_) == 1 )
    mexErrMsgTxt("quadprob: Input 6 should be of type double.");

  M   =   mxGetPr(M_  ) ;
  L   =   mxGetPr(L_  ) ; 
  U   =   mxGetPr(U_  ) ; 
  sig = *(mxGetPr(sig_));
  B   =   mxGetPr(B_)   ;
  E   =   mxGetPr(E_)   ;

  Y = mxGetPr(Y_);

  if( sig < 0 )
    mexErrMsgTxt("quadprob: Input 4 should be nonegative.");


  /* L[0] = U[0] = 1.0; */ /* Code below assumes L[0] = U[0] = 1.0, which
                              is now handled in opt_dnn.m */

  /* Initialize val=0 */

  val = 0.0;

  /* Enforce Y_00=1; add to val */

  Y[0] = 1.0;
  val += M[0] + 0.5*sig;

  /* Optimize x and diag(X) portion of Y.
   * Also handle binary variables in B.
   * Add to val. */

  for(i = 1; i <= n; i++) {

    if(B[i] == 1) {

      /* This is tricky because we combine Y(i,i), Y(i,0), Y(0,i) together. */
      
      onedim(2*M[i] + M[i*(1+n)+i], 3*sig, L[i], U[i], &locval, Y+i);
      Y[i*(1+n)] = Y[i*(1+n)+i] = Y[i];
      val += locval;

    }
    else {

      onedim(M[i], sig, L[i], U[i], &locval, Y+i);
      Y[i*(1+n)] = Y[i];
      val += 2.0*locval;

      onedim(M[i*(1+n)+i], sig, L[i]*L[i], U[i]*U[i], &locval, Y+i*(1+n)+i);
      val += locval;

    }

  }

  /* Optimize strictly lower triangular part of X. */
  /* Handle complementarities. */
  /* Add to val. */

  for(j = 1; j <= n; j++) {
    for(i = j+1; i <= n; i++) {

      if(E[j*(1+n)+i] == 1.0) {
        Y[i*(1+n)+j] = Y[j*(1+n)+i] = 0.0;
      }
      else {
        onedim(M[j*(1+n)+i], sig, L[i]*L[j], U[i]*U[j], &locval, Y+j*(1+n)+i);
        Y[i*(1+n)+j] = Y[j*(1+n)+i];
        val += 2.0*locval;
      }

    }
  }

  *(mxGetPr(val_)) = val;

  return;

}
Пример #7
0
/*
 * The mex function runs a max-flow min-cut problem.
 */
void mexFunction(int nlhs, mxArray *plhs[],
                 int nrhs, const mxArray *prhs[])
{
    mwIndex i;

    mwIndex mrows, ncols;

    mwIndex n,nz;

    /* sparse matrix */
    mwIndex *ia, *ja;

    /* source/sink */
    mwIndex u;

    /* output data */
    double *d, *dt, *pred;
    int *int_d;
    int *int_dt;

    if (nrhs != 2)
    {
        mexErrMsgTxt("2 inputs required.");
    }

    /* The first input must be a sparse matrix. */
    mrows = mxGetM(prhs[0]);
    ncols = mxGetN(prhs[0]);
    if (mrows != ncols ||
            !mxIsSparse(prhs[0]))
    {
        mexErrMsgTxt("Input must be a square sparse matrix.");
    }

    n = mrows;

    /* The second input must be a scalar. */
    if (mxGetNumberOfElements(prhs[1]) > 1 || !mxIsDouble(prhs[1]))
    {
        mexErrMsgTxt("Invalid scalar.");
    }

    /* Get the sparse matrix */

    /* recall that we've transposed the matrix */
    ja = mxGetIr(prhs[0]);
    ia = mxGetJc(prhs[0]);

    nz = ia[n];

    /* Get the scalar */
    u = (mwIndex)mxGetScalar(prhs[1]);
    u = u-1;

    plhs[0] = mxCreateDoubleMatrix(n,1,mxREAL);
    plhs[1] = mxCreateDoubleMatrix(n,1,mxREAL);
    plhs[2] = mxCreateDoubleMatrix(1,n,mxREAL);

    d = mxGetPr(plhs[0]);
    dt = mxGetPr(plhs[1]);
    pred = mxGetPr(plhs[2]);

    int_d = (int*)d;
    int_dt = (int*)dt;

    for (i=0; i < n; i++)
    {
        int_d[i]=-1;
        int_dt[i]=-1;
    }

    int_d[u] = 0;

#ifdef _DEBUG
    mexPrintf("bfs...");
#endif
    breadth_first_search(n, ja, ia,
                         u, (int*)d, (int*)dt, (mbglIndex*)pred);
#ifdef _DEBUG
    mexPrintf("done!\n");
#endif


    expand_int_to_double((int*)d, d, n, 0.0);
    expand_int_to_double((int*)dt, dt, n, 0.0);
    expand_index_to_double_zero_equality((mwIndex*)pred, pred, n, 1.0);

#ifdef _DEBUG
    mexPrintf("return\n");
#endif
}
Пример #8
0
// =========================================================================
// mexFunction: this is the actual MEX function interface
// =========================================================================
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

	// working variables
	int i, j, k, nrows, ncols, iframe, nframes;
	double d, ang, angvel;
	
	// muscle variables
	double Lm[NMUS];				// muscle-tendon lengths
	double adot[NMUS];
	double Lcedot[NMUS];
	double force[NMUS];

	// multibody dynamics variables
	double *q, *qd;
	double qdd[NDOF];
	double mom[NDOF];
	double stick[2*NSTICK];
	double grf[6];
	
	// MEX function pointers to inputs and outputs
	double *x, *xdot, *u, *M;
	double *mforces_out, *mom_out;
	double *grf_out, *stick_out;
	
	// Initialize the model, if needed
	if (initialized != 1959) {
		printf("********************************************************************\n");
		printf("* GAIT2DE -- A Musculoskeletal Dynamics Model for Gait and Posture *\n");  
		printf("*         Version 1.0 -- Compiled: "__DATE__" "__TIME__"            *\n"); 
		printf("*              Licensed for non-commercial use only                *\n");
		printf("*                  (c) 2011 Orchard Kinetics LLC                   *\n");
		printf("*                                                                  *\n"); 
		printf("*  This software, and all associated files, may be distributed     *\n"); 
		printf("*  freely without restriction, provided that this text is not      *\n"); 
		printf("*  altered.  The latest version may be downloaded from             *\n");
        printf("*  http://www.orchardkinetics.com.                                 *\n"); 
		printf("*                                                                  *\n"); 
		printf("* THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED    *\n"); 
		printf("* BY APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE   *\n"); 
		printf("* COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM       *\n");
		printf("* \"AS IS\" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR        *\n");
		printf("* IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES   *\n");
		printf("* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE     *\n");
		printf("* ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS  *\n");
		printf("* WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE     *\n");
		printf("* COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.           *\n");	
		printf("********************************************************************\n");
		printf("Initializing model...\n");
		SetParameters();
		initialized = 1959;
	}
	
	if (nrhs<2 || nrhs>3) {
		mexErrMsgTxt("gait2d: must have 2 or 3 inputs.");
	}

	// State of model is the first input (required)
	nrows = mxGetM(prhs[0]);
	ncols = mxGetN(prhs[0]);
	if (!mxIsDouble(prhs[0]) || mxIsComplex(prhs[0]) )
		mexErrMsgTxt("gait2de: Incorrect type for state vector x, must be double.");
	if (nrows != NSTATES)
		mexErrMsgTxt("gait2de: Incorrect size for state vector x, must have 50 rows.");
	x = mxGetPr(prhs[0]);
	nframes = ncols;
		
	// Controls are second input (required)
	nrows = mxGetM(prhs[1]);
	ncols = mxGetN(prhs[1]);
	if (!mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) ) mexErrMsgTxt("gait2d: Incorrect type for u, must be double.");
	if (nrows != NMUS) mexErrMsgTxt("gait2de: Incorrect size for u, must have 16 rows.");
	if (ncols != nframes) mexErrMsgTxt("gait2de: Controls u must have same number of columns as state x.");
	u = mxGetPr(prhs[1]);
	
	// see if additional actuation was given
	if (nrhs == 3) {
		nrows = mxGetM(prhs[2]);
		ncols = mxGetN(prhs[2]);
		if (!mxIsDouble(prhs[2]) || mxIsComplex(prhs[2]) ) mexErrMsgTxt("gait2d: Incorrect type for M, must be double.");
		if (nrows != NDOF) mexErrMsgTxt("gait2de: Incorrect size for actuation M, must have 9 rows.");
		if (ncols != nframes) mexErrMsgTxt("gait2de: Actuation M must have same number of columns as state x.");
		M = mxGetPr(prhs[2]);
	}
	
	// Create matrix for the xdot output of the MEX function
	plhs[0] = mxCreateDoubleMatrix(NSTATES, nframes, mxREAL);
	xdot = mxGetPr(plhs[0]);

	// Create matrix for the optional GRF output of the MEX function
	if (nlhs > 1) {
		plhs[1] = mxCreateDoubleMatrix(6, nframes, mxREAL);
		grf_out = mxGetPr(plhs[1]);	
	}
	
	// Create matrix for the optional stick output of the MEX function
	if (nlhs > 2) {
		plhs[2] = mxCreateDoubleMatrix(NSTICK*2, nframes, mxREAL);
		stick_out = mxGetPr(plhs[2]);	
	}
	
	// Create matrix for the optional muscle forces output of the MEX function
	if (nlhs > 3) {
		plhs[3] = mxCreateDoubleMatrix(NMUS, nframes, mxREAL);
		mforces_out = mxGetPr(plhs[3]);	
	}

	// Create matrix for the optional joint moment output of the MEX function
	if (nlhs > 4) {
		plhs[4] = mxCreateDoubleMatrix(NMOM, nframes, mxREAL);
		mom_out = mxGetPr(plhs[4]);
	}
	
	for (iframe=0; iframe < nframes; iframe++) {

		// Compute the muscle Lcedot and muscle forces
		for(i=0; i<NMUS; i++) {	
			// Calculate da/dt from muscle activation model
			adot[i] = (u[i] - x[2*NDOF+NMUS+i]) * (u[i]/par_Tact + (1-u[i])/par_Tdeact);
		
			// Calculate muscle length Lm and derivatives dLm/dq from generalized coordinates in x
			Lm[i] = muscles[i].L0;			// muscle-tendon length when all angles are zero
			// Add contributions from joint angles, which are x[3+j]
			for (j=0; j<NMOM; j++) Lm[i] = Lm[i] - muscles[i].MA[j]*x[3+j];
			
			// Calculate Lcedot for the muscle
			Lcedot[i] = MuscleDynamics(&muscles[i],
				x[2*NDOF+NMUS+i],		// active state of muscle i
				x[2*NDOF+i],			// Lce of muscle i
				Lm[i],					// muscle length
				&force[i]);				// muscle force (output)
				
			// copy muscle forces to the MEX function output variable, if needed
			if (nlhs > 3) *(mforces_out++) = force[i];
		}

		// Compute the generalized forces for input to Autolev code
		for (i=0; i<NDOF; i++) {
			// if requested, use the extra actuation given as inputs
			if (nrhs == 3) {
				mom[i] = M[i];
			}
			else {
				mom[i] = 0.0;
			}
			
			// if this DOF is a joint angle, apply passive joint moments and muscle moments
			if (i>2) {
				ang = x[i];											// joint angle is one of the state variables
				angvel = x[NDOF+i];									// the corresponding angular velocity
				
				// is angle above upper limit of ROM?
				d = ang - par_MaxAngle[i-3];
				if (d > 0.0) {	
					mom[i] = -par_JointK2 * d*d;
				}
				
				// is angle below lower limit of ROM?
				d = ang - par_MinAngle[i-3];
				if (d < 0.0) {
					mom[i] = par_JointK2 * d*d;
				}
				
				// add a small amount of damping and overall stiffness
				mom[i] = mom[i] - par_JointB * angvel - par_JointK1 * ang;
					
				// add the muscle moments
				for (j=0; j<NMUS; j++) {
					mom[i] = mom[i] + muscles[j].MA[i-3] * force[j];
				}
				
				// copy joint moments to the MEX function output variable, if needed
				if (nlhs > 4) *(mom_out++) = mom[i];
			}			
		}
		
		// Call the C function that was generated by Autolev
		q = &x[0];
		qd = &x[NDOF];	
		// for (i=0; i<NDOF; i++) printf("mom[%d] = %f\n", i, mom[i]);
		gait2d_al(&param, q, qd, qdd, mom, grf, stick);
		
		// copy ground reaction forces to the MEX function output variable, if needed
		if (nlhs > 1) for (i=0; i<6; i++) *(grf_out++) = grf[i];

		// copy stick figure data to the MEX function output variable, if needed
		if (nlhs > 2) for (i=0; i<2*NSTICK; i++) *(stick_out++) = stick[i];

		// copy the elements of xdot into the MEX function output, columnwise
		for (i=0; i<NDOF; i++) *(xdot++) = x[NDOF+i];		// the first NDOF rows are: qdot = dq/dt
		for (i=0; i<NDOF; i++) *(xdot++) = qdd[i];			// the next NDOF rows are the equations of motion from Autolev
		for (i=0; i<NMUS; i++) *(xdot++) = Lcedot[i];		// the next NMUS rows are the muscle Lcedot values
		for (i=0; i<NMUS; i++) *(xdot++) = adot[i];			// the final NMUS rows are the muscle activation dynamics: da/dt = ...
	
		// advance the input pointers to the next frame
		x = x + NSTATES;
		u = u + NMUS;
		M = M + NDOF;
		
		
	
	}
	
	return;
	
}
Пример #9
0
// Interface function of matlab
// now assume prhs[0]: label prhs[1]: features
void mexFunction( int nlhs, mxArray *plhs[],
		int nrhs, const mxArray *prhs[] )
{
	const char *error_msg;
	// fix random seed to have same results for each run
	// (for cross validation)
	srand(1);

	// Transform the input Matrix to libsvm format
	if(nrhs > 2 && nrhs <= 6)
	{
		int err=0;

		if(!mxIsDouble(prhs[0]) || !mxIsDouble(prhs[1]) ) {
			mexPrintf("Error: weight vector, label vector matrix must be double\n");
			fake_answer(plhs);
			return;
		}
		if(!mxIsSingle(prhs[2])) {
			mexPrintf("Error: instance matrix must be single\n");
			fake_answer(plhs);
			return;
		}

		if(parse_command_line(nrhs, prhs, NULL))
		{
			exit_with_help();
			destroy_param(&param);
			fake_answer(plhs);
			return;
		}

#ifdef _DENSE_REP
		if(!mxIsSparse(prhs[2]))
			err = read_problem_sparse(prhs[0], prhs[1],prhs[2]);
		else
		{
			mexPrintf("Training_instance_matrix must be dense\n");
			destroy_param(&param);
			fake_answer(plhs);
			return;
		}
#else
		if(mxIsSparse(prhs[2]))
			err = read_problem_sparse(prhs[0], prhs[1],prhs[2]);
		else
		{
			mexPrintf("Training_instance_matrix must be sparse\n");
			destroy_param(&param);
			fake_answer(plhs);
			return;
		}
#endif

                // xren: delete the input instance matrix to free up space
                if (nrhs==6) {
                        mxArray* var=(mxArray*)prhs[5];
                        int status=mexCallMATLAB(0,NULL,1, &var, "clear");
                        if (status!=0) mexPrintf("Failed to delete variable %s\n",mxArrayToString(prhs[5]));
                        //mxDestroyArray( (mxArray*)prhs[1] );
                }

		// train's original code
		error_msg = check_parameter(&prob, &param);

		if(err || error_msg)
		{
			if (error_msg != NULL)
				mexPrintf("Error: %s\n", error_msg);
			destroy_param(&param);
			free(prob.y);
			free(prob.x);
                        if (!use_existing_space)
			free(x_space);
			fake_answer(plhs);
			return;
		}

		if(cross_validation_flag)
		{
			double *ptr;
			plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL);
			ptr = mxGetPr(plhs[0]);
			ptr[0] = do_cross_validation();
		}
		else
		{
			const char *error_msg;

			model_ = train(&prob, &param);
			error_msg = model_to_matlab_structure(plhs, model_);
			if(error_msg)
				mexPrintf("Error: can't convert libsvm model to matrix structure: %s\n", error_msg);
			free_and_destroy_model(&model_);
		}
		destroy_param(&param);
		free(prob.y);
		free(prob.x);
		free(prob.W);
                if (!use_existing_space)
		free(x_space);
	}
	else
	{
		exit_with_help();
		fake_answer(plhs);
		return;
	}
}
Пример #10
0
// Interface function of matlab
// now assume prhs[0]: label prhs[1]: features
void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[] )
{
    const char *error_msg;

    // fix random seed to have same results for each run
    // (for cross validation and probability estimation)
    srand(1);

    // Transform the input Matrix to libsvm format
    if(nrhs > 1 && nrhs < 4)
    {
        int err;

        if(!mxIsDouble(prhs[0]) || !mxIsDouble(prhs[1])) {
            mexPrintf("Error: label vector and instance matrix must be double\n");
            fake_answer(plhs);
            return;
        }

        if(parse_command_line(nrhs, prhs, NULL))
        {
            exit_with_help();
            svm_destroy_param(&param);
            fake_answer(plhs);
            return;
        }

        if(mxIsSparse(prhs[1]))
        {
            if(param.kernel_type == PRECOMPUTED)
            {
                // precomputed kernel requires dense matrix, so we make one
                mxArray *rhs[1], *lhs[1];

                rhs[0] = mxDuplicateArray(prhs[1]);
                if(mexCallMATLAB(1, lhs, 1, rhs, "full"))
                {
                    mexPrintf("Error: cannot generate a full training instance matrix\n");
                    svm_destroy_param(&param);
                    fake_answer(plhs);
                    return;
                }
                err = read_problem_dense(prhs[0], lhs[0]);
                mxDestroyArray(lhs[0]);
                mxDestroyArray(rhs[0]);
            }
            else
                err = read_problem_sparse(prhs[0], prhs[1]);
        }
        else
            err = read_problem_dense(prhs[0], prhs[1]);

        // svmtrain's original code
        error_msg = svm_check_parameter(&prob, &param);

        if(err || error_msg)
        {
            if (error_msg != NULL)
                mexPrintf("Error: %s\n", error_msg);
            svm_destroy_param(&param);
            free(prob.y);
            free(prob.x);
            free(x_space);
            fake_answer(plhs);
            return;
        }

        if(cross_validation)
        {
            double *ptr;
            plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL);
            ptr = mxGetPr(plhs[0]);
            ptr[0] = do_cross_validation();
        }
        else
        {
            int nr_feat = (int)mxGetN(prhs[1]);
            const char *error_msg;
            model = svm_train(&prob, &param);
            error_msg = model_to_matlab_structure(plhs, nr_feat, model);
            if(error_msg)
                mexPrintf("Error: can't convert libsvm model to matrix structure: %s\n", error_msg);
            svm_free_and_destroy_model(&model);
        }
        svm_destroy_param(&param);
        free(prob.y);
        free(prob.x);
        free(x_space);
    }
    else
    {
        exit_with_help();
        fake_answer(plhs);
        return;
    }
}
Пример #11
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
   double *x1, *x2, *y, gamma;

   int row, i, j, k, m, n, o, N;

   const mxArray *net;

   /* check number of input and output arguments */

   if (nrhs != 3)
   {
      mexErrMsgTxt("Wrong number input arguments.");
   }
   else if (nlhs > 1)
   {
      mexErrMsgTxt("Too many output arguments.");
   }

   /* get kernel structure */

   gamma = mxGetScalar(mxGetField(prhs[0],0,"gamma"));

   /* get input arguments */

   if (!mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]))
   {
      mexErrMsgTxt("x1 must be a double matrix.");
   }

   m  = mxGetM(prhs[1]);
   x1 = mxGetPr(prhs[1]);

   if (!mxIsDouble(prhs[2]) || mxIsComplex(prhs[2]))
   {
      mexErrMsgTxt("x2 must be a double matrix.");
   }

   n  = mxGetM(prhs[2]);
   o  = mxGetN(prhs[2]);
   x2 = mxGetPr(prhs[2]);

   /* allocate and initialise output matrix */

   plhs[0] = mxCreateDoubleMatrix(m, n, mxREAL);

   y = mxGetPr(plhs[0]);

   /* compute kernel matrix */

   for (i = 0; i < m; i++)
   {
      for (j = 0; j < n; j++)
      {
         for (k = 0; k < o; k++)
         {
            y[i+j*m] += square(x1[i+k*m] - x2[j+k*n]);
         }
      }
   }

   for (i = 0; i < m*n; i++)
   {
      y[i] = exp(-gamma*y[i]);
   }

   /* bye bye... */
}
Пример #12
0
void mexFunction(
         int nlhs,       mxArray *plhs[],
         int nrhs, const mxArray *prhs[]
         )
{
    /* Declare variable */
    int M, N, i, j, m, n;
    double *b, *S, *omega;
    int SORTED = true;
    
    /* Check for proper number of input and output arguments */    
    if ( (nrhs < 2) || (nrhs > 3) )  {
        printUsage();
        mexErrMsgTxt("Needs 2 or 3 input arguments");
    } 
    if ( nrhs == 3 ) SORTED = false;
    if(nlhs > 0){
        printUsage();
        mexErrMsgTxt("No output arguments!");
    }
    
    /* Check data type of input argument  */
    if (!(mxIsSparse(prhs[0])) || !((mxIsDouble(prhs[1]))) ){
        printUsage();
        mexErrMsgTxt("Input arguments wrong data-type (must be sparse, double).");
    }   

    /* Get the size and pointers to input data */
    /* Check second input */
    N = mxGetN( prhs[1] );
    M = mxGetM( prhs[1] );
    if ( (M>1) && (N>1) ) {
        printUsage();
        mexErrMsgTxt("Second argument must be a vector");
    }
    N = (N>M) ? N : M;

    
    /* Check first input */
    M = mxGetNzmax( prhs[0] );
    if ( M != N ) {
        printUsage();
        mexErrMsgTxt("Length of second argument must match nnz of first argument");
    }

    /* if 3rd argument provided, check that it agrees with 2nd argument */
    if (!SORTED) {
       m = mxGetM( prhs[2] );
       n = mxGetN( prhs[2] );
       if ( (m>1) && (n>1) ) {
           printUsage();
           mexErrMsgTxt("Third argument must be a vector");
       }
       n = (n>m) ? n : m;
       if ( n != N ) {
           printUsage();
           mexErrMsgTxt("Third argument must be same length as second argument");
       }
       omega = mxGetPr( prhs[2] );
    }

    
    b = mxGetPr( prhs[1] );
    S = mxGetPr( prhs[0] );

    if (SORTED) {
        /* And here's the really fancy part:  */
        for ( i=0 ; i < N ; i++ )
            S[i] = b[i];
    } else {
        for ( i=0 ; i < N ; i++ ) {
            /* this is a little slow, but I should really check
             * to make sure the index is not out-of-bounds, otherwise
             * Matlab could crash */
            j = (int)omega[i]-1; /* the -1 because Matlab is 1-based */
            if (j >= N){
                printUsage();
                mexErrMsgTxt("Third argument must have values < length of 2nd argument");
            }
/*             S[ j ] = b[i]; */  /* this is incorrect */
            S[ i ] = b[j];  /* this is the correct form */
        }
    }
}
Пример #13
0
void mexFunction(int nlhs, mxArray *plhs[],
                 int nrhs, const mxArray *prhs[])
{
  /* Declare variables. */ 

  double *obj,objective,*dX, *X, *dist,weight,curdist, *ind1,*ind2,*v1,*v2,*vo, *dummy;
  double pev=1.0,mev=1.0,*ev,*av,*bv;
  int m,p,n,inds;
  int oldi1=-1,i1,i2,j,i,r,c;
  int ione=1;
  char *chu="U"; 
  char *chl="L";
  char *chn2="T";
  char *chn="N";
  double minusone=-1.0,one=1.0, zero=0.0;



  /* Check for proper number of input and output arguments. */    
  if (nrhs!=4) mexErrMsgTxt("Exactly one or two input arguments required.");
 

  if (nlhs > 2) {
    mexErrMsgTxt("Too many output arguments.");
  }

  /* Check data type of input argument. */
  if (!(mxIsDouble(prhs[0]))) {
   mexErrMsgTxt("Input array must be of type double.");
  }
      
  n = mxGetN(Xp);
  m = mxGetM(Xp);
  c = mxGetNumberOfElements(distp);

  /* Get the data. */
  X  = mxGetPr(Xp);
  ind1  = mxGetPr(ip);
  ind2  = mxGetPr(jp);
  dist  = mxGetPr(distp);

  /* Create output matrix */
  plhs[0]=mxCreateDoubleMatrix(m,n,mxREAL);
  dX=mxGetPr(plhs[0]);
  plhs[1]=mxCreateDoubleMatrix(1,1,mxREAL);
  obj=mxGetPr(plhs[1]);

  /* create space for dummy vectors */
  dummy=malloc(m*sizeof(double));

  weight=0;
  oldi1=-1;
  v1=&X[0];v2=&X[0];vo=&dX[0];
  objective=0;
  for(i=0;i<c;i++){
    i1=(int) ind1[i]-1;
    i2=(int) ind2[i]-1;
    v2 =&X[(int) (i2*m)];

    if(i1!=oldi1){
      for(j=0;j<m;j++) {vo[j]=weight*v1[j]-dummy[j];dummy[j]=0;}
      weight=0;
     v1 =&X[(int) (i1*m)];
     vo=&dX[(int) (i1*m)];
     oldi1=i1;
    };
    curdist=0;
    for(j=0;j<m;j++) curdist=curdist+square(v1[j]-v2[j]);
    /*     curdist=(curdist-dist[i])/dist[i];objective+=square(curdist);*/
    curdist=(curdist-dist[i]);objective+=square(curdist);
    for(j=0;j<m;j++) dummy[j]+=curdist*v2[j];
    weight=weight+curdist;

  }

  for(j=0;j<m;j++) {vo[j]=weight*v1[j]-dummy[j];}

  obj[0]=objective;

  free(dummy);
}
Пример #14
0
void mexFunction(int nout, mxArray *out[],
                 int nin, const mxArray *in[]) {
    int M, N, S, H,K;
    const int* dimensions ;
    const double* P_pt ;
    const double* D_pt ;
    double* result ;
    enum {IN_P=0, IN_D} ;
    enum {OUT_Q=0} ;

    /* -----------------------------------------------------------------
     **                                               Check the arguments
     ** -------------------------------------------------------------- */
    if (nin < 2) {
        mexErrMsgTxt("At least 2 input arguments required.");
    } else if (nout > 1) {
        mexErrMsgTxt("Too many output arguments.");
    }
    if( !mxIsDouble(in[IN_D]) || mxGetNumberOfDimensions(in[IN_D]) != 3) {
        mexErrMsgTxt("G must be a three dimensional real array.") ;
    }

    dimensions = mxGetDimensions(in[IN_D]) ;
    M = dimensions[0] ;
    N = dimensions[1] ;
    S = dimensions[2] ;


    if(S < 3 || M < 3 || N < 3) {
        mexErrMsgTxt("All dimensions must be not less than 3.") ;
    }

    K = mxGetN(in[IN_P]) ;
    H = mxGetM(in[IN_P]) ;
    P_pt = mxGetPr(in[IN_P]) ;
    D_pt = mxGetPr(in[IN_D]) ;

    /* If the input array is empty, then output an empty array as well. */
    if( K == 0) {
        out[OUT_Q] = mxDuplicateArray(in[IN_P]) ;
        return ;
    }

    /* -----------------------------------------------------------------
     *                                                        Do the job
     * -------------------------------------------------------------- */
    {
        double* buffer = (double*) mxMalloc(K*H*sizeof(double)) ;
        double* buffer_iterator = buffer ;
        int p;
        const int yo = 1 ;
        const int xo = M ;
        const int so = M*N ;
        for(p = 0 ; p < K ; ++p) {
            double b[3] ;
            int x = ((int)*P_pt++) ;
            int y = ((int)*P_pt++) ;
            int s = ((int)*P_pt++) ;
            if(x < 1 || x > N-2 ||
                    y < 1 || y > M-2 ||
                    s < 1 || s > S-2) {
                continue ;
            }
            const double* pt = D_pt + y*yo + x*xo + s*so ;
            double Dx=0, Dy=0, Ds=0, Dxx=0, Dyy=0, Dss=0, Dxy=0, Dxs=0,Dys=0;
            double A[3*3] ;
            pt = D_pt + y*yo + x*xo + s*so ;

            /* Compute the gradient. */
            Dx = 0.5 * (at(+1, 0,0) - at(-1, 0,0)) ;
            Dy = 0.5 * (at(0, +1,0) - at(0, -1, 0));
            Ds = 0.5 * (at(0, 0,+1) - at(0, 0, -1)) ;

            /* Compute the Hessian. */
            Dxx = (at(+1, 0, 0) + at(-1, 0, 0) - 2.0 * at(0, 0, 0)) ;
            Dyy = (at(0, +1, 0) + at(0, -1, 0) - 2.0 * at(0, 0, 0)) ;
            Dss = (at(0, 0, +1) + at(0, 0, -1) - 2.0 * at(0, 0, 0)) ;

            Dxy = 0.25 * ( at(+1, +1,0) + at(-1, -1, 0) - at(-1, +1,0) - at(+1, -1,0) ) ;
            Dxs = 0.25 * ( at(+1, 0, +1) + at(-1, 0, -1) - at(-1, 0, +1) - at(+1, 0, -1) ) ;
            Dys = 0.25 * ( at(0, +1, +1) + at(0, -1,-1) - at(0, -1, +1) - at(0, +1, -1) ) ;
            /* Solve linear system. */
            Aat(0, 0) = Dxx ;
            Aat(1, 1) = Dyy ;
            Aat(2, 2) = Dss ;
            Aat(0, 1) = Aat(1, 0) = Dxy ;
            Aat(0, 2) = Aat(2, 0) = Dxs ;
            Aat(1, 2) = Aat(2, 1) = Dys ;
            b[0] = - Dx ;
            b[1] = - Dy ;
            b[2] = - Ds ;
            int ret = Gaussian_Elimination(A, 3, b);
            double xn = x  ;
            double yn = y ;
            double sn = s ;
            if(ret==0) {
                xn+=b[0];
                yn+=b[1];
                sn+=b[2];
            }
            *buffer_iterator++ = xn ;
            *buffer_iterator++ = yn ;
            *buffer_iterator++ = sn  ;
        }
        int NL = (buffer_iterator - buffer)/H ;
        out[OUT_Q] = mxCreateDoubleMatrix(H, NL, mxREAL) ;
        result = mxGetPr(out[OUT_Q]);
        memcpy(result, buffer, sizeof(double) * H * NL) ;
        mxFree(buffer) ;
    }
}
Пример #15
0
void mexFunction(int nlhs, mxArray *plhs[],
                 int nrhs, const mxArray *prhs[])
{
  /* Declare variables. */ 

  double *X, *dummy, *v1,*v2, *C;
  double *av,*bv;
  int m,p,n,inds;
  int j,i,r,c;
  int ione=1;
  char *chu="U"; 
  char *chl="L";
  char *chn2="T";
  char *chn="N";
  double w,minusone=-1.0,one=1.0, zero=0.0;
  double * weights;  


  /* Check for proper number of input and output arguments. */    
  if (nrhs != 4) {
    mexErrMsgTxt("Exactly four input arguments required.");
  } 

  if (nlhs > 1) {
    mexErrMsgTxt("Too many output arguments.");
  }

  /* Check data type of input argument. */
  if (!(mxIsDouble(prhs[0]))) {
   mexErrMsgTxt("Input array must be of type double.");
  }

  /* Get the number of elements in the input argument. */
  inds = mxGetNumberOfElements(prhs[1]);
  if(inds != mxGetNumberOfElements(prhs[2]))
    mexErrMsgTxt("ERROR: Both index vectors must have same length!\n");

 if(inds != mxGetNumberOfElements(prhs[3]))
  mexErrMsgTxt("ERROR: Weight  vector must have same length as index vectors!\n");

  n = mxGetN(prhs[0]);
  m = mxGetM(prhs[0]);

  /* Get the data. */
  X  = mxGetPr(prhs[0]);
  av  = mxGetPr(prhs[1]);
  bv  = mxGetPr(prhs[2]);
  weights  = mxGetPr(prhs[3]);
	

  /* Create output matrix */
  plhs[0]=mxCreateDoubleMatrix(m,m,mxREAL);
  C=mxGetPr(plhs[0]);
  memset(C,0,sizeof(double)*m*m);
/*  dummy=new double[m];*/
  dummy=(double*)malloc(m*sizeof(double));

 /* compute outer products and sum them up */
  for(i=0;i<inds;i++){
   /* Assign cols addresses */
   v1=&X[(int) (av[i]-1)*m];
   v2=&X[(int) (bv[i]-1)*m];
   w=weights[i];

   for(j=0;j<m;j++) dummy[j]=v1[j]-v2[j];

  j=0;	
   for(r=0;r<m;r++){
	 for(c=0;c<=r;c++) {C[j]+=dummy[r]*dummy[c]*w;j++;};
	 j+=m-r-1;
   }
  }


  /* fill in lower triangular part of C */
  if(inds>0)
   for(r=0;r<m;r++)
	 for(c=r+1;c<m;c++) C[c+r*m]=C[r+c*m]; 
 free(dummy);
}
Пример #16
0
void mexFunction(
  int nOUT, mxArray *pOUT[],
  int nINP, const mxArray *pINP[])
{
  
  double *t1;
  double *t2;
  double binsize;
  double *C, *cross, *B;
  double w, lbound, rbound;
  
  int nbins, nt1, nt2;
  int i1 = 0, i2 = 0, l, j, k;
  
  /* check number of arguments: expects 4 inputs, 1 or 2 outputs */
  if (nINP != 4)
    mexErrMsgTxt("Call with t1, t2, binsize and nbins  as inputs.");
  if (nOUT != 1 && nOUT != 2)
    mexErrMsgTxt("Requires one or two outputs.");

  /* check validity of inputs */
  if (mxGetM(pINP[0]) != 1 && mxGetN(pINP[0]) != 1)
    mexErrMsgTxt("t1 must be a row or column vector");
  if (mxGetM(pINP[1]) != 1 && mxGetN(pINP[1]) != 1)
    mexErrMsgTxt("t2 must be a row or column vector");
  if (mxGetM(pINP[2]) * mxGetN(pINP[2]) != 1)
    mexErrMsgTxt("binsize must be scalar");
  if (mxGetM(pINP[3]) * mxGetN(pINP[3]) != 1)
    mexErrMsgTxt("nbins must be scalar");

  /* unpack inputs */
  nt1 = mxGetM(pINP[0]) * mxGetN(pINP[0]);
  t1 = mxGetPr(pINP[0]);
  if (!t1)  mexErrMsgTxt("Memory allocation error t1");  // ADR 2014-11-25
  if (!mxIsDouble(pINP[0]))
	  mexErrMsgTxt("T1 input is not a double!");


  nt2 = mxGetM(pINP[1]) * mxGetN(pINP[1]);
  t2 = mxGetPr(pINP[1]);
  if (!t2)  mexErrMsgTxt("Memory allocation error t2");  // ADR 2014-11-25
  if (!mxIsDouble(pINP[1]))
	  mexErrMsgTxt("T2 input is not a double!");

  binsize = mxGetScalar(pINP[2]);
  nbins = (int)mxGetScalar(pINP[3]);
 
  /* we want nbins to be odd */
  if ((nbins / 2) * 2 == nbins)
    nbins++;

  pOUT[0] = mxCreateDoubleMatrix(nbins, 1, mxREAL);
  C = mxGetPr(pOUT[0]);
  if (!C)  mexErrMsgTxt("Memory allocation error C");  // ADR 2014-11-25

  if(nOUT == 2)
    {
      double m;
      
      pOUT[1] = mxCreateDoubleMatrix(nbins, 1, mxREAL);
      B =  mxGetPr(pOUT[1]);
      if (!B)  mexErrMsgTxt("Memory allocation error B");  // ADR 2014-11-25

      m = - binsize * (nbins / 2);
      
      for(j = 0; j < nbins; j++)
	B[j] = m + j * binsize;

    }
  
  /* cross correlations */
  
  w = (nbins / 2) * binsize;

  
  for(i1 = 0; i1 < nt1; i1++)
    {
      lbound = t1[i1] - w;
      while(t2[i2] < lbound && i2 < nt2 -1)
	i2++;
      while(t2[i2-1] > lbound && i2 > 1)
	i2--;
      rbound = lbound;
      l = i2;
      
      for(j = 0; j < nbins; j++)
	{
	  k = 0;
	  rbound += binsize;
	  while(t2[l] < rbound && l < nt2)
	    {
	      l++;
	      k++;
	    }

	  C[j] += k;
	}
    }
  
  
  for(j = 0; j < nbins; j++)
    C[j] /= nt1 * binsize / 10000;
  
      
}
Пример #17
0
void mexFunction(
   int nlhs, mxArray *plhs[],
   int nrhs, const mxArray *prhs[])
{
  mwSize n, nn, i; /*nn is mxGetNumberOfElements, n and i derive from nn.*/
  double *V, *Vend, *temp; /*mxGetPr, V, mxGetPr*/
  double *sigma, *S, *k, *r, *d, *T; /*mxGetPr,...,mxGetPr*/
  int nsigma, nS,nk,nr,nd,nT,nput, nv, nd1; /*1 or 0*/
  bool *putflag; /*true*/

  /* Error checking on inputs */  
  if (nrhs<6) mexErrMsgTxt("Not enough input arguments.");
  if (nrhs>7) mexErrMsgTxt("Too many input arguments.");
  if (nlhs>2) mexErrMsgTxt("Too many output arguments.");
  for (i=0; i<nrhs; i++)
  {
    if (!mxIsDouble(prhs[i]) && !mxIsSparse(prhs[i]))
      mexErrMsgTxt("Function not defined for variables of input class");
    if (mxIsComplex(prhs[i]))
      mexErrMsgTxt("X must be real.");
  }

  sigma=mxGetPr(prhs[0]);
  S=mxGetPr(prhs[1]);
  k=mxGetPr(prhs[2]);
  r=mxGetPr(prhs[3]);
  d=mxGetPr(prhs[4]);
  T=mxGetPr(prhs[5]);

  n=1;
  nn=mxGetNumberOfElements(prhs[0]);

  if (n!=nn)
    if (n==1) {n=nn; plhs[0]=mxDuplicateArray(prhs[0]);}  
    else if (nn>1) mexErrMsgTxt("Inputs are not size compatible");
  if (nn>1) nsigma=1; else nsigma=0;

  nn=mxGetNumberOfElements(prhs[1]);
  if (n!=nn)
    if (n==1) {n=nn; plhs[0]=mxDuplicateArray(prhs[1]);}  
    else if (nn>1) mexErrMsgTxt("Inputs are not size compatible");
  if (nn>1) nS=1; else nS=0;
 
  nn=mxGetNumberOfElements(prhs[2]);
  if (n!=nn)
    if (n==1) {n=nn; plhs[0]=mxDuplicateArray(prhs[2]);}  
    else if (nn>1) mexErrMsgTxt("Inputs are not size compatible");
  if (nn>1) nk=1; else nk=0;

  nn=mxGetNumberOfElements(prhs[3]);
  if (n!=nn)
    if (n==1) {n=nn; plhs[0]=mxDuplicateArray(prhs[3]);}  
    else if (nn>1) mexErrMsgTxt("Inputs are not size compatible");
  if (nn>1) nr=1; else nr=0;

  nn=mxGetNumberOfElements(prhs[4]);
  if (n!=nn)
    if (n==1)  {n=nn; plhs[0]=mxDuplicateArray(prhs[4]);} 
    else if (nn>1) mexErrMsgTxt("Inputs are not size compatible");
  if (nn>1) nd=1; else nd=0;

  nn=mxGetNumberOfElements(prhs[5]);
  if (n!=nn)
    if (n==1)  {n=nn; plhs[0]=mxDuplicateArray(prhs[5]);} 
    else if (nn>1) mexErrMsgTxt("Inputs are not size compatible");
  if (nn>1) nT=1; else nT=0;
    
  if (nrhs<7) {putflag=mxCalloc(1,sizeof(bool)); *putflag=false; nput=0;}
  else
  {
    nn=mxGetNumberOfElements(prhs[6]);
    if (n!=nn)
      if (n==1) {n=nn; plhs[0]=mxDuplicateArray(prhs[6]);}
      else if (nn>1) mexErrMsgTxt("Inputs are not size compatible");
    if (nn>1) nput=1; else nput=0;
    putflag=mxCalloc(nn,sizeof(bool));   
    temp=mxGetPr(prhs[6]);
    for (i=0;i<nn;i++)
      if (temp[i]==1) putflag[i]=true; else putflag[i]=false;
  }

  if (n==1) plhs[0]=mxCreateDoubleMatrix(1,1,mxREAL);

  V=mxGetPr(plhs[0]);
  Vend=V+n;
  for (;V<Vend;V++)
  {
    *V=bs(*sigma,*S,*k,*r,*d,*T,*putflag);
    sigma+=nsigma;
    S+=nS;
    k+=nk;
    r+=nr;
    d+=nd;
    T+=nT;
    putflag+=nput;
  }
}
Пример #18
0
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray*prhs[])

{
  double *A, *x, *y, *rows, *cols;
  mwSize m,n,m1,n1,m2,n2,rownum,colnum;
  mwIndex *row_ids,*col_ids,i,j;
  int colnumspecified=0;
  
  
  /* Check for proper number of arguments */
  
  if (nrhs!=3 && nrhs!=4) {
    mexErrMsgTxt("Invalid number of input arguments.");
  } else if (nlhs > 1) {
    mexErrMsgTxt("Too many output arguments.");
  }
  
  
  /* Check the input dimensions */
  
  m = mxGetM(A_IN);
  n = mxGetN(A_IN);
  if (!mxIsDouble(A_IN) || mxIsComplex(A_IN) || mxGetNumberOfDimensions(A_IN)>2) {
    mexErrMsgTxt("ROWLINCOMB requires that A be a double matrix.");
  }
  
  m1 = mxGetM(ROWS_IN);
  n1 = mxGetN(ROWS_IN);
  if (!mxIsDouble(ROWS_IN) || mxIsComplex(ROWS_IN) || (m1!=1 && n1!=1)) {
    mexErrMsgTxt("ROWLINCOMB requires that ROWS be an index vector of type double.");
  }
  rownum = (m1 > n1) ? m1 : n1;   /* the number of rows in the linear combination */
  
  m2 = mxGetM(X_IN);
  n2 = mxGetN(X_IN);
  if (!mxIsDouble(X_IN) || mxIsComplex(X_IN) || ((m2!=1) && (n2!=1))) {
    mexErrMsgTxt("ROWLINCOMB requires that X be a double vector.");
  }
  
  if (m2 != rownum && n2 != rownum) {
    mexErrMsgTxt("The length of X does not match the number of rows in ROWS.");
  }
  
  if (nrhs==4) {
    m1 = mxGetM(COLS_IN);
    n1 = mxGetN(COLS_IN);
    if (!mxIsDouble(COLS_IN) || mxIsComplex(COLS_IN) || (m1!=1 && n1!=1)) {
      mexErrMsgTxt("ROWLINCOMB requires that COLS be an index vector of type double.");
    }
    colnum = (m1 > n1) ? m1 : n1;   /* the number of columns */
    colnumspecified = 1;
    cols = mxGetPr(COLS_IN);
    
    Y_OUT = mxCreateDoubleMatrix(1, colnum, mxREAL);
  }
  else {
    cols = 0;
    Y_OUT = mxCreateDoubleMatrix(1, n, mxREAL);
  }
  
  
  /* Assign pointers to the various parameters */
  A = mxGetPr(A_IN);
  rows = mxGetPr(ROWS_IN);
  x = mxGetPr(X_IN);
  y = mxGetPr(Y_OUT);
  
  
  /* check row indices */
  
  row_ids = (mwIndex*)mxMalloc(rownum*sizeof(mwIndex));
  
  for (i=0; i<rownum; ++i) {
    row_ids[i] = (mwIndex)(rows[i]+0.1)-1;
    if (row_ids[i]<0 || row_ids[i]>=m) {
      mexErrMsgTxt("Row index in ROWS is out of range.");
    }
  }
  
  
  
  if (colnumspecified) {
    
    /* check column indices */
    col_ids = (mwIndex*)mxMalloc(colnum*sizeof(mwIndex));
    
    for (i=0; i<colnum; ++i) {
      col_ids[i] = (mwIndex)(cols[i]+0.1)-1;
      if (col_ids[i]<0 || col_ids[i]>=n) {
        mexErrMsgTxt("Column index in COLS is out of range.");
      }
    }
    
    /* Do the actual computation */
    for (j=0; j<colnum; ++j) {
      for (i=0; i<rownum; ++i) {
        y[j] += A[m*col_ids[j]+row_ids[i]]*x[i];
      }
    }
    
    mxFree(col_ids);
  }
  
  else {
    
    /* Do the actual computation */
    for (j=0; j<n; ++j) {
      for (i=0; i<rownum; ++i) {
        y[j] += A[m*j+row_ids[i]]*x[i];
      }
    }
  }
  
  
  mxFree(row_ids);
  
  return;
}
Пример #19
0
void mexFunction(int nlhs, mxArray *plhs[],
                 int nrhs, const mxArray *prhs[])
{

  /* [R, [n]] = general_radon_c( theta, rho, M, dx, dy [, interpolation, method, constraint, valid, rho_x] ) */

  /* INPUTS: */
  /* theta: vector of angles */
  /* rho: vector of (local) rho values */
  /* M: data matrix */
  /* dx,dy: sample size in x (row) and y (column) dimension */
  /* interpolation: interpolation method (0=nearest or 1=linear) */
  /* method: 0=integral, 1=sum, 2=mean, 3=slice, 4=product, 5=logsum */
  /* constraint: 0=no constraint, 1=loop over rows only (i.e. only one element per row contributes to a line), 2=loop over columns only */
  /* valid: 0/1 (valid lines are those lines that span the whole width or height of the input matrix) */
  /* rho_x: 0/1 (if 1, indicates that rho is specified as the intercept with the horizontal center line,
     rather than the distance from the origin, this could be useful if you want rho to be in the same units as the x dimension.
     Notice however, that in this mode, rho will go to infinity if theta approaches +/- pi ) */

  /* OUTPUTS: */
  /* if method is one of integral, sum, mean, product
  /* R: radon transform matrix if method is one of integral, sum, mean, product */
  /* n: number of elements in original matrix contributing to each line*/ 
  /* if method is slice */
  /* R: cell array of projections along lines specified by theta and rho pairs */
  /* n: number of elements in original matrix contributing to each line*/ 

  /* variables */
  int T,R,M,N;
  int t,r,m,n;
  double *theta, *rho, *g;
  double delta_x, delta_y;
  mxArray *out, *tmp, *out2;
  double *pout;
  short *pout2;
  double alpha, beta, betap, costheta, sintheta, rhooffset;
  int mmin, mmax, nmin, nmax, amax;
  double xmin, ymin;
  int rstart, rend;
  double sum;
  int count, method, lininterp, valid, rho_x, constraint;
  double eps=1e-9;
  int indx;
  double nfloat, w;
  int dims[3];
  const char *mclass;
  mxArray *mcopy;

  /* check optional input parameters */
  if (nrhs>5 && mxGetScalar(prhs[5]) ) {
    lininterp=1;
  } else {
    lininterp=0; /* default interpolation is nearest */
  }
  if (nrhs>6 ) {
    method=mxGetScalar(prhs[6]);
  } else {
    method=0; /* default method is integral */
  }
  if (nrhs>7) {
    constraint = (int) mxGetScalar(prhs[7]);
    if (constraint!=1 && constraint!=2)
      constraint = 0;
  } else {
    constraint = 0; /* by default no constraint */
  }
  if (nrhs>8 && mxGetScalar(prhs[8]) ) {
    valid = 1;
  } else {
    valid = 0; /* by default compute all lines */
  }
  if (nrhs>9 && mxGetScalar(prhs[9]) ) {
    rho_x = 1;
  } else {
    rho_x = 0; /* by default rho specifies distance from origin */
  }


  mclass = mxGetClassName(prhs[2]);
  if (!mxIsDouble(prhs[2])) {
    if ( mexCallMATLAB(1, &mcopy, 1, (mxArray**)&prhs[2], "double") )
      mexErrMsgTxt("Could not convert matrix to double precision");
    
    g = mxGetPr( mcopy );
  } else {
    g = mxGetPr( prhs[2] );
  }

  /* get input arguments */
  T = mxGetNumberOfElements( prhs[0] ); /* length theta vector */
  R = mxGetNumberOfElements( prhs[1] ); /* length rho vector */
  M = mxGetM( prhs[2] ); /* number of rows matrix M */
  N = mxGetN( prhs[2] ); /* number of columns matrix M */

  theta = mxGetPr( prhs[0] ); /* pointer to theta vector */
  rho = mxGetPr( prhs[1] ); /* pointer to rho vector */

  delta_x = mxGetScalar( prhs[3] ); /* sample size in x (row) dimension */
  delta_y = mxGetScalar( prhs[4] ); /* sample size in y (column) dimension */

  xmin = -delta_x*(M-1)/2; /* x of lower left corner of matrix M (local coordinates) */
  ymin = -delta_y*(N-1)/2; /* y of lower left corner of matrix M (local coordinates) */


  /* create output matrices */
  if (method==3) {
    out = mxCreateCellMatrix(T,1);
  } else {
    out = mxCreateDoubleMatrix( T,R,mxREAL);
    pout = mxGetPr( out );
  }

  if (nlhs>1) {
    if (method==3) {
      out2 = mxCreateNumericMatrix(T,2,mxINT16_CLASS,mxREAL);
    } else {
      dims[0]=T;
      dims[1]=R;
      dims[2]=2;      
      out2 = mxCreateNumericArray(3,(const int*) &dims, mxINT16_CLASS,mxREAL);
    }
    pout2 = (short*) mxGetPr( out2 );
  }

  /* (minimal) error checking */
  if (method==3 && T!=R)
    mexErrMsgTxt("Theta and rho vectors have incompatible lengths");


  for (t=0;t<T;t++) { /* loop through all angles */

    costheta = cos( theta[t] );
    sintheta = sin( theta[t] );
      
    /* decide whether to loop over samples in x or y dimension */
    if (constraint==1 || (constraint==0 && ( fabs(sintheta)>(delta_x/sqrt(delta_x*delta_x+delta_y*delta_y)) ) ) ) { /* loop over rows */
	
      rhooffset = xmin*costheta + ymin*sintheta;
      alpha = -(delta_x/delta_y)*(costheta/sintheta); /* alpha parameter of line */
	
      /* initialize rho start and end */
      if (method==3) {
	rstart = t;
	rend = rstart+1;
      } else {
	rstart = 0;
	rend = R;
      }
	
      /* loop over all rho values */
      for (r=rstart;r<rend;r++ ) {
	  
	/* compute beta parameter of line */
	if (rho_x)
	  beta = (rho[r]*costheta-rhooffset)/(delta_y*sintheta);
	else
	  beta = (rho[r]-rhooffset)/(delta_y*sintheta);
	  
	/* compute rows to loop over */
	if (lininterp) { 
	  if (alpha>eps) {
	    mmin=(int)ceil(-(beta-eps)/alpha);
	    mmax=1+(int)floor((N-beta-1-eps)/alpha);
	  } else if (alpha<-eps) {
	    mmin=(int)ceil((N-beta-1-eps)/alpha);
	    mmax=1+(int)floor(-(beta-eps)/alpha);
	  } else if (((beta-eps)>0) && ((beta+eps)<(N-1))) {
	    mmin=0;
	    mmax=M;
	  } else {
	    mmin=0;
	    mmax=-1;
	  }
	} else {
	  betap=beta+0.5;
	  if (alpha>eps) {
	    mmin=(int)ceil(-(betap-eps)/alpha);
	    mmax=1+(int)floor((N-betap-eps)/alpha);
	  } else if (alpha<-eps) {
	    mmin=(int)ceil((N-betap-eps)/alpha);
	    mmax=1+(int)floor(-(betap-eps)/alpha);
	  } else if (((betap-eps)>0) && ((betap+eps)<N)) {
	    mmin=0;
	    mmax=M;
	  } else {
	    mmin=0;
	    mmax=-1;
	  }
	}
	if (mmin<0) mmin=0;
	if (mmax>M) mmax=M;
	count=MAX(0,mmax-mmin); /* number of rows to loop over */
	  
	/* check if line is valid */
	if ( ( (valid) && count<M && fabs(alpha*count)<(N-2) ) || (count<=0) ) {
	  if (method==3) {
	    if (nlhs>1) {
	      pout2[t] = 0;
	      pout2[t+T] = 0;
	    }
	  } else {
	    pout[ t+T*r] = mxGetNaN();
	    if (nlhs>1) {
	      pout2[ t+T*r ] = 0;
	      pout2[ t+T*r+T*R ] = 0;
	    }
	  }
	  continue;
	}
	  
	if (method==3) { /* process slice method */
	    
	  tmp = mxCreateDoubleMatrix(count, 1, mxREAL);
	  mxSetCell(out, t, tmp );
	    
/* 	  if (alpha<-eps) */
/* 	    pout = mxGetPr(tmp) + count - 1; */
/* 	  else */
	  pout = mxGetPr(tmp);
	    
	  if (lininterp) {

	    for (m=mmin; m<mmax; m++) {
		
	      nfloat = alpha*m+beta;
	      n = floor(nfloat);
	      w = nfloat-n;
		
	      if (fabs(w)<eps) {
		(*pout)=(g[m+M*n]);
	      } else {
		(*pout)=(g[m+M*n]*(1-w)+g[m+M*(n+1)]*w);
	      }
		
/* 	      if (alpha<-eps) */
/* 		pout--; */
/* 	      else */
	      pout++;
	      
	    }
	  } else {
	    for (m=mmin; m<mmax; m++) {
	      indx = round(alpha*m+beta); /* nearest neighbour */
	      (*pout)= g[m+M*indx];
/* 	      if (alpha<-eps) */
/* 		pout--; */
/* 	      else */
	      pout++;
	    }
	  }
	    
	} else if (method==4) { /* process product method */
	    
	  /* initialize sum to one */
	  sum=1;

	  if (lininterp) {	    

	    for (m=mmin; m<mmax; m++) {
		
	      nfloat = alpha*m+beta;
	      n = floor(nfloat);
	      w = nfloat-n;
		
	      if (fabs(w)<eps) {
		sum*=g[m+M*n];
	      } else {
		sum*=g[m+M*n]*(1-w)+g[m+M*(n+1)]*w;
	      }
		
	    } 

	  } else {

	    for (m=mmin; m<mmax; m++) {

	      indx = round(alpha*m+beta); /* nearest neighbour */
	      sum *= g[m+M*indx];

	    }

	  }
	    
	  pout[t+T*r] = sum;

	} else if (method==5) { /* process log sum method */
	    
	  /* initialize sum to one */
	  sum=0;

	  if (lininterp) {	    

	    for (m=mmin; m<mmax; m++) {
		
	      nfloat = alpha*m+beta;
	      n = floor(nfloat);
	      w = nfloat-n;
		
	      if (fabs(w)<eps) {
		sum+=log(g[m+M*n]);
	      } else {
		sum+=log(g[m+M*n]*(1-w)+g[m+M*(n+1)]*w);
	      }
		
	    } 

	  } else {

	    for (m=mmin; m<mmax; m++) {

	      indx = round(alpha*m+beta); /* nearest neighbour */
	      sum += log(g[m+M*indx]);

	    }

	  }
	    
	  pout[t+T*r] = sum;
	    
	} else { /* process integral, sum and mean methods */
	    
	  /* initialize sum to zero */
	  sum = 0;

	  if (lininterp) {
	    
	    for (m=mmin; m<mmax; m++) {
		
	      nfloat = alpha*m+beta;
	      n = floor(nfloat);
	      w = nfloat-n;
		
	      if (fabs(w)<eps) {
		sum+=g[m+M*n];
	      } else {
		sum+=g[m+M*n]*(1-w)+g[m+M*(n+1)]*w;
	      }
	      
	    }

	  } else {

	    for (m=mmin; m<mmax; m++) {

	      indx = round(alpha*m+beta); /* nearest neighbour */
	      sum += g[m+M*indx];

	    }

	  }
	    
	  if (method==0) /* integral */
	    pout[ t+T*r ] = delta_x*sum/fabs(sintheta);
	  else if (method==1) /* sum */
	    pout[ t+T*r ] = sum;
	  else if (method==2) /* mean */
	    pout[ t+T*r ] = sum/count;
	    
	} /* end conditional on method */
	  
	if (nlhs>1) {
	    
	  if (method==3) {
	    pout2[t] = mmin+1;
	    pout2[t+T] = mmax;
	  } else {
	    pout2[t+T*r] = mmin+1;
	    pout2[t+T*r+T*R] = mmax;
	  }
	    
	}	  
	  
      } /* end loop over rho values */	
	
    } else { /* loop over columns */
	
      alpha = -(delta_y/delta_x)*(sintheta/costheta);
      rhooffset = xmin*costheta + ymin*sintheta;
	
      /* initialize rho start and end */
      if (method==3) {
	rstart = t;
	rend = rstart+1;
      } else {
	rstart = 0;
	rend = R;
      }
	
      /* loop over all rho values */
      for ( r=rstart;r<rend;r++ ) {
	  
	if (rho_x)
	  beta = (rho[r]*costheta-rhooffset)/(delta_x*costheta);
	else
	  beta = (rho[r]-rhooffset)/(delta_x*costheta);
	  
	/* compute columns to loop over */
	if (lininterp) {
	  if (alpha>eps) {
	    nmin=(int)ceil(-(beta-eps)/alpha);
	    nmax=1+(int)floor((M-beta-1-eps)/alpha);
	  } else if (alpha<-eps) {
	    nmin=(int)ceil((M-beta-1-eps)/alpha);
	    nmax=1+(int)floor(-(beta-eps)/alpha);
	  } else if (((beta-eps)>0) && ((beta+eps)<(M-1))) {
	    nmin=0;
	    nmax=N;
	  } else {
	    nmin=0;
	    nmax=-1;
	  }
	} else {
	  betap=beta+0.5;
	  if (alpha>eps) {
	    nmin=(int)ceil(-(betap-eps)/alpha);
	    nmax=1+(int)floor((M-betap-eps)/alpha);
	  } else if (alpha<-eps) {
	    nmin=(int)ceil((M-betap-eps)/alpha);
	    nmax=1+(int)floor(-(betap-eps)/alpha);
	  } else if (((betap-eps)>0) && ((betap+eps)<M)) {
	    nmin=0;
	    nmax=N;
	  } else {
	    nmin=0;
	    nmax=-1;
	  }
	}
	if (nmin<0) nmin=0;
	if (nmax>N) nmax=N;
	count=MAX(0,nmax-nmin); /* number of columns to loop over */
	  
	/* check if line is valid */
	if ( ( (valid) && count<N && fabs(alpha*count)<(M-2) ) || (count<=0) ) {
	  if (method==3) {
	    if (nlhs>1) {
	      pout2[t] = 0;
	      pout2[t+T] = 0;
	    }
	  } else {
	    pout[ t+T*r] = mxGetNaN();
	    if (nlhs>1) {
	      pout2[ t+T*r ] = 0;
	      pout2[ t+T*r+T*R ] = 0;
	    }
	  }	    
	  continue;
	}
	  
	if (method==3) { /* process slice method */
	    
	  tmp = mxCreateDoubleMatrix(count, 1, mxREAL);
	  mxSetCell(out, t, tmp );
	    
	  pout = mxGetPr(tmp);

	  if (lininterp) {
	    
	    for (n=nmin;n<nmax;n++) {
		
	      nfloat = alpha*n+beta;
	      m = floor(nfloat);
	      w = nfloat-m;
		
	      if (fabs(w)<eps) {
		(*pout)=(g[m+M*n]);
	      } else {
		(*pout)=(g[m+M*n]*(1-w)+g[m+1+M*n]*w);
	      }

	      pout++;
		
	    }

	  } else {

	    for (n=nmin;n<nmax;n++) {
		
	      indx = round(alpha*n+beta); /* nearest neighbour */
	      *(pout)=g[n*M+indx];
	      pout++;
		
	    }
	      
	  }
	    
	} else if (method==4) { /* process product */
	    
	  /* initialize sum to one */
	  sum=1;

	  if (lininterp) {
	    
	    for (n=nmin; n<nmax; n++) {
		
	      nfloat = alpha*n+beta;
	      m = floor(nfloat);
	      w = nfloat-m;
		
	      if (fabs(w)<eps) {
		sum*=g[m+M*n];
	      } else {
		sum*=g[m+M*n]*(1-w)+g[m+M*(n+1)]*w;
	      }
		
	    } 

	  } else {

	    for (n=nmin; n<nmax; n++) {
	      indx = round(alpha*n+beta); /* nearest neighbour */
	      sum *= g[n*M+indx];
	    }

	  }
	    
	  pout[t+T*r] = sum;

	} else if (method==5) { /* process log sum */
	    
	  /* initialize sum to one */
	  sum=0;

	  if (lininterp) {
	    
	    for (n=nmin; n<nmax; n++) {
		
	      nfloat = alpha*n+beta;
	      m = floor(nfloat);
	      w = nfloat-m;
		
	      if (fabs(w)<eps) {
		sum+=log(g[m+M*n]);
	      } else {
		sum+=log(g[m+M*n]*(1-w)+g[m+M*(n+1)]*w);
	      }
		
	    } 

	  } else {

	    for (n=nmin; n<nmax; n++) {
	      indx = round(alpha*n+beta); /* nearest neighbour */
	      sum += log(g[n*M+indx]);
	    }

	  }
	    
	  pout[t+T*r] = sum;
	    
	} else { /* process integral, sum and mean methods */
	    
	  /* initialize sum to zero */
	  sum = 0;

	  if (lininterp) {
	    
	    for (n=nmin;n<nmax;n++) {
		
	      nfloat = alpha*n+beta;
	      m = floor(nfloat);
	      w = nfloat-m;
		
	      if (fabs(w)<eps) {
		sum+=g[m+M*n];
	      } else {
		sum+=g[m+M*n]*(1-w)+g[m+1+M*n]*w;
	      }
		
	    }

	  } else {
	      
	    for (n=nmin;n<nmax;n++) {
	      indx = round(alpha*n+beta); /* nearest neighbour */
	      sum+=g[n*M+indx];
	    }	      

	  }
	      
	  if (method==0) /* integral */
	    pout[ t+T*r ] = delta_y*sum/fabs(costheta);
	  else if (method==1) /* sum */
	    pout[ t+T*r ] = sum;
	  else if (method==2) /* mean */
	    pout[ t+T*r ] = sum/count;
	    
	} /* end conditional on method */

	if (nlhs>1) {
	    
	  if (method==3) {
	    pout2[t] = nmin+1;
	    pout2[t+T] = nmax;
	  } else {
	    pout2[t+T*r] = nmin+1;
	    pout2[t+T*r+T*R] = nmax;
	  }
	    
	}
	  
      } /* end loop over rho values */
	
    } /* end conditional rows/columns */

  } /* end loop over angles */

  /* set output arguments */
  plhs[0] = out;
  
  if (nlhs>1)
    plhs[1] = out2;


} /* end mexFunction */
Пример #20
0
/* ----------------------------------------------------------------------- */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
/* ----------------------------------------------------------------------- */
{   const mxArray *atoms;
    const mxArray *fibers;
    const mxArray *values;
    const mxArray *D;

    mxArray *d;

    int
    nFibers, nTheta, nCoeffs, k, i;

    /*unsigned int   dims[2]; */

    /* Free memory and exit if no parameters are given */
    if (nrhs == 0)
    {   if (nlhs != 0) {
            mexErrMsgTxt("No output arguments expected.");
        }
        return ;
    }

    /* Check for proper number of arguments */
    if (nrhs != 5)  {
        mexErrMsgTxt("Five input arguments required.");
    }
    if ((nlhs  > 1)               ) {
        mexErrMsgTxt("Too many output arguments.");
    }

    /* Extract the arguments */
    atoms = (mxArray *)prhs[0];
    fibers = (mxArray *)prhs[1];
    values = (mxArray *)prhs[2];
    D = (mxArray *)prhs[3];

    nFibers = (int) mxGetScalar(prhs[4]);

    d = mxCreateNumericMatrix(nFibers, 1, mxDOUBLE_CLASS, mxREAL);

    /* Verify validity of input arguments */
    if (mxIsEmpty(atoms) || mxIsEmpty(fibers) || mxIsEmpty(values) || mxIsEmpty(D))
    {   /* If arguments are empty, simply return an empty weights */
        plhs[0] = mxCreateNumericMatrix(0, 0, mxDOUBLE_CLASS, mxREAL);
        mexWarnMsgTxt("Returning empty d.");
        return ;
    }

    /* Verify validity of input argument 'atoms' */
    if (atoms != NULL)
    {   if (!mxIsDouble(atoms)  || ((mxGetM(atoms) > 1) &&
                                    (mxGetN(atoms) > 1)) || (mxGetNumberOfDimensions(atoms) != 2))
        {   mexErrMsgTxt("Parameter 'atoms' has to be a double vector.");
        }
    }

    /* Verify validity of input argument 'fibers' */
    if (fibers != NULL)
    {   if (!mxIsDouble(fibers)  || ((mxGetM(fibers) > 1) &&
                                     (mxGetN(fibers) > 1)) || (mxGetNumberOfDimensions(fibers) != 2))
        {   mexErrMsgTxt("Parameter 'fibers' has to be a double vector.");
        }
    }

    /* Verify validity of input argument 'values' */
    if (values != NULL)
    {   if (!mxIsDouble(values)  || ((mxGetM(values) > 1) &&
                                     (mxGetN(values) > 1)) || (mxGetNumberOfDimensions(values) != 2))
        {   mexErrMsgTxt("Parameter 'values' has to be a double vector.");
        }
    }

    /* Verify validity of input argument 'D' */
    if (D != NULL)
    {   if (!mxIsDouble(D)  || ((mxGetM(D) > 1) &&
                                (mxGetN(D) < 2)) || (mxGetNumberOfDimensions(D) != 2))
        {   mexErrMsgTxt("Parameter 'D' has to be a double matrix.");
        }
    }

    /* Verify all vectors have the same lenght */
    if  ((mxGetNumberOfElements(atoms) != mxGetNumberOfElements(fibers)) || (mxGetNumberOfElements(atoms) != mxGetNumberOfElements(values)))
    {   mexErrMsgTxt("Parameters 'atoms' ,'fibers' and 'values' have to be of equal length.");
    }


    /* Call the core subroutine */
    nTheta = mxGetM(D);
    nCoeffs = mxGetM(atoms);
    compute_diag_sub( mxGetPr(d), mxGetPr(atoms), mxGetPr(fibers), mxGetPr(values), mxGetPr(D), nFibers, nTheta, nCoeffs );
    plhs[0] = d;

    return ;
}
Пример #21
0
int Options::loadConstraintBounds (const mxArray* ptr, double*& cl, 
				   double*& cu, double neginfty,
				   double posinfty, int &lin, int &nlin) {
  int m = 0;  // The return value is the number of constraints.
  int tm;
  //Defaults
  lin = 0; nlin = 0;

  // LOAD CONSTRAINT BOUNDS.
  // If the user has specified constraints bounds, then she must
  // specify *both* the lower and upper bounds.
  const mxArray* pl = mxGetField(ptr,0,"cl");
  const mxArray* pu = mxGetField(ptr,0,"cu");
  const mxArray* prl = mxGetField(ptr,0,"rl"); //linear constraint bounds
  const mxArray* pru = mxGetField(ptr,0,"ru");
  if (pl || pu || prl || pru) {

    // Check to make sure the constraint bounds are valid.
    if ((!pl ^ !pu) || (!prl ^ !pru))
      throw MatlabException("You must specify both lower and upper bounds on the constraints");
    if (pl && (!mxIsDouble(pl) || !mxIsDouble(pu) || (mxGetNumberOfElements(pl) != mxGetNumberOfElements(pu))))
      throw MatlabException("The nonlinear constraints lower and upper bounds must both be double-precision arrays with the same number of elements");
    if (prl && (!mxIsDouble(prl) || !mxIsDouble(pru) || (mxGetNumberOfElements(prl) != mxGetNumberOfElements(pru))))
      throw MatlabException("The linear constraints lower and upper bounds must both be double-precision arrays with the same number of elements");
    // Get the number of constraints.
    if(pl && prl) {
        lin = (int)mxGetNumberOfElements(prl);
        nlin = (int)mxGetNumberOfElements(pl);
        m = lin+nlin;
    }
    else if(pl) {
        lin = 0;
        nlin = (int)mxGetNumberOfElements(pl);
        m = nlin;        
    }
    else {
        lin = (int)mxGetNumberOfElements(prl);
        nlin = 0;
        m = lin;
    }

    // Load the lower bounds on the constraints and convert MATLAB's
    // convention of infinity to IPOPT's convention of infinity.
    cl = new double[m];
    cu = new double[m];
    if(pl && prl) {
        tm = (int)mxGetNumberOfElements(pl);
        copymemory(mxGetPr(pl),cl,tm);
        copymemory(mxGetPr(pu),cu,tm);
        copymemory(mxGetPr(prl),&cl[tm],(int)mxGetNumberOfElements(prl));
        copymemory(mxGetPr(pru),&cu[tm],(int)mxGetNumberOfElements(pru));
    }
    else if(pl) {
        copymemory(mxGetPr(pl),cl,m);
        copymemory(mxGetPr(pu),cu,m);
    }
    else {
        copymemory(mxGetPr(prl),cl,m);
        copymemory(mxGetPr(pru),cu,m);
    }

    // Convert MATLAB's convention of infinity to IPOPT's convention
    // of infinity.
    for (int i = 0; i < m; i++) {
      if (mxIsInf(cl[i])) cl[i] = neginfty;
      if (mxIsInf(cu[i])) cu[i] = posinfty;
    }
  }

  return m;
}
Пример #22
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])  {
    // This function is called like: [tSubsampledAndDoubledUp, ySubsampledAndDoubledUp]=minMaxDownsampleMex(t,y,r)
    //   t a double column vector of length nScans
    //   y a nScans x nChannels matrix of doubles
    //   r a double scalar holding a positive integer value, or empty
    //
    // If r is empty, is means "don't downsample", just return t and y as-is.

    // Load in the arguments, checking them thoroughly

    // prhs[0]: t
    if ( mxIsDouble(prhs[0]) && !mxIsComplex(prhs[0]) && mxGetNumberOfDimensions(prhs[0])==2 && mxGetN(prhs[0])==1 )  {
        // all is well
    }
    else  {
        mexErrMsgIdAndTxt("ws:minMaxDownSample:tNotRight", 
                          "Argument t must be a non-complex double column vector.");
    }
    mwSize nScans = mxGetM(prhs[0]) ;
    double *t = mxGetPr(prhs[0]);  // "Convert" to a C++ array
    
    // prhs[1]: y
    //bool isDouble = mxIsDouble(prhs[1]) ;
    //bool isReal = mxIsComplex(prhs[1]) ;
    //mwSize nDims = mxGetNumberOfDimensions(prhs[1]) ;
    //mwSize nRows = mxGetM(prhs[1]) ;
    if ( mxIsDouble(prhs[1]) && !mxIsComplex(prhs[1]) && mxGetNumberOfDimensions(prhs[1])==2 && mxGetM(prhs[1])==nScans )  {
        // all is well
    }
    else  {
        mexErrMsgIdAndTxt("ws:minMaxDownSample:yNotRight", 
                          "Argument y must be a non-complex double matrix with the same number of rows as t.");
    }
    mwSize nChannels = mxGetN(prhs[1]);
    double *y = mxGetPr(prhs[1]);  // "Convert" to a C++ array (sort of: it's still in col-major order)
    
    // prhs[1]: r
    bool rIsEmpty ;
    if ( mxIsEmpty(prhs[2]) )  {
        // this is always OK
        rIsEmpty = true ;
    } else {
        rIsEmpty = false ;
        // non-empty
        if ( mxIsDouble(prhs[2]) && !mxIsComplex(prhs[2]) && mxIsScalar(prhs[2]) )  {
            // this is ok
        }  
        else  {
            mexErrMsgIdAndTxt("ws:minMaxDownSample:rNotRight", 
                              "Argument r must be empty or be a non-complex double scalar.");
        }
    }

    double rAsDouble = -1.0 ;  // should not be used if rIsEmpty
    mwSize r = 0 ;  // should not be used if rIsEmpty
    if (!rIsEmpty)  {
        rAsDouble = mxGetScalar(prhs[2]) ;
        if ( floor(rAsDouble)!=ceil(rAsDouble) || rAsDouble<=0 )  {
            mexErrMsgIdAndTxt("ws:minMaxDownSample:rNotRight", 
                              "Argument r, if a scalar, must be a positive integer.");
        }
        r = (mwSize) rAsDouble ;
    }

    // At this point, all args have been read and validated

    int effectiveNLHS = (nlhs>1)?nlhs:1 ;  // even if nlhs==0, still safe to assign to plhs[0], and should do this, so ans gets assigned
    double* tSubsampledAndDoubled ;
    double* ySubsampledAndDoubled ;

    double* target ;
    double* targetEnd ;
    double* source ;

    if (rIsEmpty)  {
        // just copy the inputs to the outputs
        if ( effectiveNLHS>=1 )  {
            plhs[0] = mxCreateDoubleMatrix(nScans, (mwSize)1, mxREAL) ;
            tSubsampledAndDoubled = mxGetPr(plhs[0]);
            //for (mwSize i=0 ; i<nScans; ++i)  {
            //    tSubsampledAndDoubled[i] = t[i] ;
            //}
            target = tSubsampledAndDoubled ;
            source = t ;
            targetEnd = tSubsampledAndDoubled + nScans ;
            while (target!=targetEnd)  {
                *target = *source ;
                ++target ;
                ++source ;
            }
        }
        if ( effectiveNLHS>=2 )  {
            plhs[1] = mxCreateDoubleMatrix(nScans, nChannels, mxREAL) ;
            ySubsampledAndDoubled = mxGetPr(plhs[1]);
            target = ySubsampledAndDoubled ;
            source = y ;
            mwSize nElements = nChannels * nScans ;
            targetEnd = ySubsampledAndDoubled + nElements ;
            //for (mwSize i=0 ; i<nElements; ++i, ++source, ++target)  {
            //    *target = *source ;
            //}
            while (target!=targetEnd)  {
                *target = *source ;
                ++target ;
                ++source ;
            }
        }
        return ;
    }

    // If get here, r is a scalar, which is the interesting case

    mwSize nScansSubsampled = (mwSize) (ceil(((double)nScans)/rAsDouble)) ;

    // Create the subsampled timeline, decimating t by the factor r
    mxArray* tSubsampledMxArray = mxCreateDoubleMatrix(nScansSubsampled, (mwSize)1, mxREAL) ;
    double* tSubsampled = mxGetPr(tSubsampledMxArray) ;
    source = t ;
    target = tSubsampled ;
    //for (mwSize i=0 ; i<nScansSubsampled; ++i, source+=r, ++target) {
    //    *target = *source ;
    //}
    targetEnd = tSubsampled + nScansSubsampled ;
    while (target!=targetEnd)  {
        *target = *source ;
        ++target ;
        source+=r ;
    }

    // Now "double-up" time, with two copies of each time point
    mxArray* tSubsampledAndDoubledUpMxArray = mxCreateDoubleMatrix(2*nScansSubsampled, (mwSize)1, mxREAL) ;
    double* tSubsampledAndDoubledUp = mxGetPr(tSubsampledAndDoubledUpMxArray) ;
    target = tSubsampledAndDoubledUp ;
    source = tSubsampled ;
    targetEnd = tSubsampledAndDoubledUp + 2*nScansSubsampled ;
    //for (source = tSubsampled; source!=(tSubsampled+nScansSubsampled); ++source) {
    while (target!=targetEnd)  {
        double tSource = *source ;
        *target = tSource ;
        ++target ;
        *target = tSource ;
        ++target ;
        ++source ;
    }

    // Now set up for return (always assign this one)
    plhs[0] = tSubsampledAndDoubledUpMxArray ;

    // If that's all the return values the user wanted, return now
    if ( effectiveNLHS<2 )  {
        return ;
    }

    // Subsample y at each subsampled scan, getting the max and the min of the r samples for that point in the original y
    mxArray* ySubsampledMaxMxArray = mxCreateDoubleMatrix(nScansSubsampled, nChannels, mxREAL) ;
    double* ySubsampledMax = mxGetPr(ySubsampledMaxMxArray) ;
    mxArray* ySubsampledMinMxArray = mxCreateDoubleMatrix(nScansSubsampled, nChannels, mxREAL) ;
    double* ySubsampledMin = mxGetPr(ySubsampledMinMxArray) ;
    double maxSoFar ;
    double minSoFar ;
    mwSize iWithinTheR ;
    //mwSize iScanSubsampled ;
    double* minTarget ;
    double* maxTarget ;
    double* sourceEnd ;
    source = y ;
    double yThis ;  // for holding the current value of the y array
    for (mwSize iChannel=0 ; iChannel<nChannels; ++iChannel)  {
        maxTarget = ySubsampledMax + iChannel*nScansSubsampled ;
        minTarget = ySubsampledMin + iChannel*nScansSubsampled ;
        source = y + iChannel*nScans ;
        sourceEnd = source + nScans ;
        iWithinTheR = 0 ;
        while (source!=sourceEnd)  { 
            if (iWithinTheR==0)  {
                yThis = *source ;
                maxSoFar = yThis ;
                minSoFar = yThis ;
            } else {
                yThis = *source ;
                if (yThis>maxSoFar)  {
                    maxSoFar = yThis ;
                } else {
                    if (yThis<minSoFar)  {
                        minSoFar = yThis ;
                    }
                }
            }
            if (iWithinTheR+1 == r)  {
                // Write to the elements of the subsampled arrays
                *maxTarget = maxSoFar ;
                *minTarget = minSoFar ;
                ++minTarget ;
                ++maxTarget ;
                iWithinTheR = 0 ;
            } else {
                ++iWithinTheR ;
            }
            ++source ;
        }
        if ( iWithinTheR!=0 && nScansSubsampled>0 )  {  
            // This means r does not evenly divide nScans, so need to write the current minSoFar, maxSoFar to last el of ySubsampledMax, ySampledMin
            *maxTarget = maxSoFar ;
            *minTarget = minSoFar ;
        }
    }

    // Now for the y's, max, min, max, min, etc.
    mwSize nScansSubsampledAndDoubledUp = 2*nScansSubsampled ;
    mxArray* ySubsampledAndDoubledUpMxArray = mxCreateDoubleMatrix(nScansSubsampledAndDoubledUp, (mwSize)nChannels, mxREAL) ;
    double* ySubsampledAndDoubledUp = mxGetPr(ySubsampledAndDoubledUpMxArray) ;
    target = ySubsampledAndDoubledUp ;
    targetEnd = ySubsampledAndDoubledUp + nChannels*nScansSubsampledAndDoubledUp ;
    double* minSource = ySubsampledMin ;
    double* maxSource = ySubsampledMax ;
    //for (mwSize iChannel=0 ; iChannel<nChannels; ++iChannel)  {
    //    for (mwSize iScanSubsampled=0; iScanSubsampled<nScansSubsampled; ++iScanSubsampled)  {
    //        //*(ySubsampledAndDoubledUp+iChannel*nScansSubsampled+2*iScanSubsampled)   = *(ySubsampledMax+iChannel*nScansSubsampled+iScanSubsampled) ;
    //        *target = *maxSource ;
    //        ++maxSource ;
    //        ++target ;
    //        //*(ySubsampledAndDoubledUp+iChannel*nScansSubsampled+2*iScanSubsampled+1) = *(ySubsampledMin+iChannel*nScansSubsampled+iScanSubsampled) ;
    //        *target = *minSource ;
    //        ++minSource ;
    //        ++target ;
    //    }
    //}
    while (target!=targetEnd)  {
        *target = *maxSource ;
        ++maxSource ;
        ++target ;
        *target = *minSource ;
        ++minSource ;
        ++target ;
    }

    // Now set up for return
    plhs[1] = ySubsampledAndDoubledUpMxArray ;
}
Пример #23
0
// Interface function of matlab
// now assume prhs[0]: label prhs[1]: features prhs[2]: source models
void mexFunction( int nlhs, mxArray *plhs[],
		int nrhs, const mxArray *prhs[] )
{
	const char *error_msg;
	// fix random seed to have same results for each run
	// (for cross validation)
	srand(1);

	// Transform the input Matrix to libsvm format
    if(nrhs > 1 && nrhs < 7)
	{
		int err=0;

		if(!mxIsDouble(prhs[0]) || !mxIsDouble(prhs[1])) {
			mexPrintf("Error: label vector and instance matrix must be double\n");
			fake_answer(plhs);
			return;
		}

        if(parse_command_line(nrhs, prhs, NULL))
        {
            exit_with_help();
            destroy_param(&param);
            fake_answer(plhs);
            return;
        }

        //read in the source models
        const mxArray *cellModels = prhs[2];
        mwSize num_ms = mxGetNumberOfElements(cellModels);
        param.num_src = (int) num_ms;
        if(param.num_src > 0)
            src_models = Malloc(struct model*, param.num_src);

        for(int i=0; i< param.num_src; i++)
        {
            const mxArray *src_model_mat = mxGetCell(cellModels, i);
            src_models[i] = Malloc(struct model, 1);
            if((matlab_matrix_to_model(src_models[i], src_model_mat)) != NULL){
                mexPrintf("can't load source model\n");
                fake_answer(plhs);
                return;
            }
        }

        //read in the weight of the source models
        if(!mxIsDouble(prhs[3])) {
            mexPrintf("Error: weight vector must be double\n");
            fake_answer(plhs);
            return;
        }

        // weight of source models
        if (param.num_src > 0)
        {
            int num_row_src_weight = (int) mxGetM(prhs[3]);
            int num_col_src_weight = (int) mxGetN(prhs[3]);
            int dim_src_weight = (int) max(num_row_src_weight, num_col_src_weight);
            if(dim_src_weight != param.num_src) {
                mexPrintf("Error: lenght of weight vector must be equal to the number of source models!");
                fake_answer(plhs);
                return;
            }
            double *src_model_weight = (double *)mxGetPr(prhs[3]);
            param.src_weights = Malloc(double, param.num_src);
            for(int i=0; i< param.num_src; i++)
                param.src_weights[i] = src_model_weight[i];
        }
Пример #24
0
/* MATLAB interface */
void
mexFunction(int nlhs, mxArray *plhs[],
            int nrhs, const mxArray *prhs[])

{
  unsigned int m1, n1, m2, n2;
  double *s, *t, *z, *sdim, *eval_r, *eval_i, *info, *a, *b;
  double n;

  /* Check for proper number of arguments */

  if (nrhs < 2 || nrhs > 4 || nlhs == 0 || nlhs > 7)
    DYN_MEX_FUNC_ERR_MSG_TXT("MJDGGES: takes 2, 3 or 4 input arguments and between 1 and 7 output arguments.");

  /* Check that A and B are real matrices of the same dimension.*/

  m1 = mxGetM(prhs[0]);
  n1 = mxGetN(prhs[0]);
  m2 = mxGetM(prhs[1]);
  n2 = mxGetN(prhs[1]);
  if (!mxIsDouble(prhs[0]) || mxIsComplex(prhs[0])
      || !mxIsDouble(prhs[1]) || mxIsComplex(prhs[1])
      || (m1 != n1) || (m2 != n1) || (m2 != n2))
    DYN_MEX_FUNC_ERR_MSG_TXT("MJDGGES requires two square real matrices of the same dimension.");

  /* Create a matrix for the return argument */
  plhs[1] = mxCreateDoubleMatrix(n1, n1, mxREAL);
  plhs[2] = mxCreateDoubleMatrix(n1, n1, mxREAL);
  plhs[3] = mxCreateDoubleMatrix(n1, n1, mxREAL);
  plhs[4] = mxCreateDoubleMatrix(1, 1, mxREAL);
  plhs[5] = mxCreateDoubleMatrix(n1, 1, mxCOMPLEX);
  plhs[6] = mxCreateDoubleMatrix(1, 1, mxREAL);

  /* Assign pointers to the various parameters */
  s = mxGetPr(plhs[1]);
  t = mxGetPr(plhs[2]);
  z = mxGetPr(plhs[3]);
  sdim = mxGetPr(plhs[4]);
  eval_r = mxGetPr(plhs[5]);
  eval_i = mxGetPi(plhs[5]);
  info = mxGetPr(plhs[6]);

  a = mxGetPr(prhs[0]);
  b = mxGetPr(prhs[1]);

  /* set criterium for stable eigenvalues */
  if (nrhs >= 3 && mxGetM(prhs[2]) > 0)
    {
      criterium = *mxGetPr(prhs[2]);
    }
  else
    {
      criterium = 1+1e-6;
    }

  /* set criterium for 0/0 generalized eigenvalues */
  double zhreshold;
  if (nrhs == 4 && mxGetM(prhs[3]) > 0)
    {
      zhreshold = *mxGetPr(prhs[3]);
    }
  else
    {
      zhreshold = 1e-6;
    }


  /* keep a and b intact */
  memcpy(s, a, sizeof(double)*n1*n1);
  memcpy(t, b, sizeof(double)*n1*n1);

  n = n1;

  /* Do the actual computations in a subroutine */
  mjdgges(s, t, z, &n, sdim, eval_r, eval_i, zhreshold, info);

  plhs[0] = mxCreateDoubleScalar(0);
}
Пример #25
0
void mexFunction( int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[] )
{
  
  char strFileName[MAXSTRING];
  double record_offset;
  unsigned long file_size;
  FILE *fid=NULL;
  unsigned long nrecords = 0;
  char tval[6];
  double recid;
  uint32_t timestamp = 0;
  int i;
  uint32_t offset;

  /*create empty outputs*/
  for (i=0; i<nlhs; i++) {
    plhs[i] = mxCreateDoubleMatrix(0,0,mxREAL);
  }

  if (nrhs!=3)
    mexErrMsgTxt("Incorrect number of input arguments.");

/*   if (nlhs!=3) */
/*     mexErrMsgTxt("Incorrect number of output arguments."); */

  if (!mxIsChar(prhs[0]))
    mexErrMsgTxt("First argument should be a string");

  if (!mxIsDouble(prhs[1]))
    mexErrMsgTxt("Second argument should be a double");

  if (!mxIsDouble(prhs[2]))
    mexErrMsgTxt("Third argument should be a double");

  mxGetString(prhs[0], strFileName, MAXSTRING);
  if ( (fid = fopen(strFileName, "rb")) == NULL )
    mexErrMsgTxt("Unable to open file");

  recid = mxGetScalar(prhs[2]);
  record_offset = mxGetScalar(prhs[1]);

  fseek(fid, 0, 2);
  file_size = ftell(fid);

  if (record_offset > file_size)
    mexErrMsgTxt("Invalid record offset");

  fseek(fid, record_offset, 0);

  nrecords = 0;
  /*find the number of records in the file and store offsets*/
  while (1) {

    if (fread(tval, 1, 6, fid)==6) {
      offset = ftell(fid)-6;
      if (mxIsInf(recid)==0 && (nrecords == recid)) {
	  timestamp = *((uint32_t*)(&(tval[2])));
	  break;
      }
      nrecords++;
      fseek(fid, ((unsigned char*)tval)[0] * 3, SEEK_CUR);
    } else {
      if (nrecords == 0) {
	offset = record_offset;
	timestamp = 0;
     } else
	timestamp = *((uint32_t*)(&(tval[2])));
      break;
    }
  }

  if (nlhs>0)
    plhs[0] = mxCreateDoubleScalar( nrecords );
  if (nlhs>1)
    plhs[1] = mxCreateDoubleScalar( offset );
  if (nlhs>2)
    plhs[2] = mxCreateDoubleScalar( timestamp );

  fclose(fid);

}
Пример #26
0
/* void */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	DOUBLE *input, *output, *irr, *orr, *arr;
	int i, j, m, n, data_n, numofpoints;
	FIS *fis;

    DOUBLE *numofpointPt;
	int which_rule, which_output, which_mf;
	DOUBLE tmp, tmp_vec[2], mf_value;

	if (nrhs != 3)
	mexErrMsgTxt("Needs 3 input arguments.");
    if (!mxIsDouble(INPUT))
	mexErrMsgTxt("The first input must be a defined DOUBLE matrix");

    if (!mxIsStruct(FISMATRIX))
	mexErrMsgTxt("The Second input must be a structure.");


	/* Check the dimensions of input vector */
	/* This is permissive - granted as long as enough inputs
	   are there. So the user can pass the original training data
	   directly */
    m = (int) mxGetM(NUMOFPOINT);
    numofpointPt=mxGetPr(NUMOFPOINT);
    if (m > 0 && numofpointPt[0] > 101)  numofpoints=(int)numofpointPt[0];
    else  numofpoints=101;

	m = (int) mxGetM(INPUT);
	n = (int) mxGetN(INPUT);

/*=====================*/
    fis=matlab2cStr(prhs[1], numofpoints);
/*=====================*/
	if (!((n >= fis->in_n) || ((n == 1) && (m >= fis->in_n)))) {
		mexPrintf("The input vector is of size %dx%d,", m, n);
		mexPrintf("while expected input vector size is %d.\n", fis->in_n);
		mexErrMsgTxt("Exiting ...");
	}
	if ((n == 1) && (m == fis->in_n))
		data_n = 1;
	else
		data_n = m;

	/* signal error if multiple input vectors and mutiple output arguments */
	/*
	if ((data_n > 1) && (nlhs > 1))
		mexErrMsgTxt("Multiple output arguments can only be used with a single input vector.");
	*/


	/* Create matrices for returned arguments */
	OUTPUT = mxCreateDoubleMatrix(data_n, fis->out_n, mxREAL);

	for (i = 0; i < data_n; i++) {
	    /* Assign pointers to the various parameters */
		output = mxGetPr(OUTPUT);
		input = mxGetPr(INPUT);

		/* Dispatch the inputs */
		for (j = 0; j < fis->in_n; j++)
			fis->input[j]->value = input[j*data_n+i];
		/* Compute the output */
		fisEvaluate(fis, numofpoints);
		/* Collect the output */
		for (j = 0; j < fis->out_n; j++)
			output[j*data_n+i] = fis->output[j]->value;
	}

	/* take care of additonal output arguments */
	if (nlhs >= 2) {
		IRR = mxCreateDoubleMatrix(fis->rule_n, fis->in_n, mxREAL);
		irr = mxGetPr(IRR);
		for (i = 0; i < fis->rule_n; i++)
			for (j = 0; j < fis->in_n; j++) {
				which_mf = fis->rule_list[i][j];
				if (which_mf > 0)
					mf_value = fis->input[j]->mf[which_mf-1]->value;
				else if (which_mf == 0) {
				/* Don't care; mf_value depends on AND or OR */
					if (fis->and_or[i] == 1)	/* AND */
						mf_value = 1;
					else
						mf_value = 0;
					}
				else            /* Linguistic hedge NOT */
					mf_value = 1 - fis->input[j]->mf[-which_mf-1]->value;
				irr[j*fis->rule_n+i] = mf_value;
			}
	}

	if (nlhs >= 3) {
		if (strcmp(fis->type, "mamdani") == 0) {
			ORR = mxCreateDoubleMatrix(numofpoints, (fis->rule_n)*(fis->out_n), mxREAL);
			orr = mxGetPr(ORR);
			for (i = 0; i < numofpoints; i++)
				for (j = 0; j < (fis->rule_n)*(fis->out_n); j++) {
					which_rule = j%(fis->rule_n);	/* zero offset */
					which_output = j/(fis->rule_n);	/* zero offset */
					which_mf = fis->rule_list[which_rule][fis->in_n+which_output];
					if (which_mf > 0)
						tmp = fis->output[which_output]->mf[which_mf-1]->value_array[i];
					else if (which_mf == 0)
						tmp = 0;
					else
						tmp = 1-fis->output[which_output]->mf[-which_mf-1]->value_array[i];
					/*
					mexPrintf("rule = %d, output = %d, mf = %d\n", which_rule,
						which_output, which_mf);
					*/
					if (!fis->userDefinedImp) 
						orr[j*numofpoints+i] = (*fis->impFcn)(tmp,
							fis->firing_strength[which_rule]);
					else {
						tmp_vec[0] = tmp;
						tmp_vec[1] = fis->firing_strength[which_rule];
						orr[j*numofpoints+i] = 
							fisCallMatlabFcn(tmp_vec, 2, fis->impMethod);
        			}
				}
		}
		if (strcmp(fis->type, "sugeno") == 0) {
			ORR = mxCreateDoubleMatrix(fis->rule_n, fis->out_n, mxREAL);
			orr = mxGetPr(ORR);
			for (i = 0; i < fis->rule_n; i++)
				for (j = 0; j < fis->out_n; j++) {
					which_mf = fis->rule_list[i][fis->in_n+j]-1;
					/*
					mexPrintf("mf = %d\n", which_mf);
					*/
					if (which_mf == -1)	/* don't_care consequent */
						orr[j*fis->rule_n+i] = 0;
					else
						orr[j*fis->rule_n+i] = fis->output[j]->mf[which_mf]->value;
				}
		}
	}

	if (nlhs >= 4) {
		if (strcmp(fis->type, "mamdani") == 0) {
			ARR = mxCreateDoubleMatrix(numofpoints, fis->out_n, mxREAL);
			arr = mxGetPr(ARR);
			for (i = 0; i < numofpoints; i++)
				for (j = 0; j < fis->out_n; j++)
					arr[j*numofpoints+i] = fisFinalOutputMf(fis, j, i);
		}
		if (strcmp(fis->type, "sugeno") == 0) {
			ARR = mxCreateDoubleMatrix(fis->rule_n, fis->out_n, mxREAL);
			arr = mxGetPr(ARR);
			for (i = 0; i < fis->rule_n; i++)
				for (j = 0; j < fis->out_n; j++)
					arr[j*fis->rule_n+i] = fis->firing_strength[i];
		}
	}

	/* destroy FIS data structure */
	fisFreeFisNode(fis);
}
Пример #27
0
/* interface between MATLAB and the C function */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
	/* declare variables */
	int ns, n2, m, n, put = 0;
	char *str;
	char *pchData;
	HGLOBAL hClipboardData;

	if ( !OpenClipboard(NULL) )
		mexErrMsgTxt("CLIPBD ERROR: Error while opening clipboard.\n");

	put = nrhs;

	EmptyClipboard();

	if (put && mxIsChar(prhs[0])) {
		str = (char *)mxArrayToString(prhs[0]);
		ns = strlen(str) + 1;

		hClipboardData = GlobalAlloc(GMEM_DDESHARE, ns);

		pchData = (char*)GlobalLock(hClipboardData);
		  
		strcpy(pchData, str);
		  
		GlobalUnlock(hClipboardData);
		  
		SetClipboardData(CF_TEXT,hClipboardData);
		  
	}
	else if (put && mxIsNumeric(prhs[0])) {
		UINT format = RegisterClipboardFormat("000_FMT");
		int is_single = 0, is_double = 0;
		clip_matFormat data; 

		if (mxIsSingle(prhs[0]))
			is_single = 1;

		else if (mxIsDouble(prhs[0]))
			is_double = 1;

		data.ny = mxGetM(prhs[0]);	data.nx = mxGetN(prhs[0]);
		if (nrhs == 2) {
			if ( mxGetM(prhs[1]) == 9 || mxGetN(prhs[1]) == 9 )
				data.head = mxGetPr(prhs[1]);
		}

		if (is_single)
			data.z_s = (float *)mxGetData(prhs[0]);
		else
			data.z_d = mxGetPr(prhs[0]);

		hClipboardData = GlobalAlloc(GMEM_DDESHARE, sizeof(clip_matFormat));
		clip_matFormat *buffer = (clip_matFormat *)GlobalLock(hClipboardData);

		/* put the data into that memory */
		*buffer = data;

		/* Put it on the clipboard */
		GlobalUnlock(hClipboardData);
		SetClipboardData(format,hClipboardData);
		mexPrintf("FFSSS %d\n;", format);
		mexPrintf("FDS %d\n", IsClipboardFormatAvailable(format));
	}
	else if (!put) {
		UINT format = RegisterClipboardFormat("000_FMT");

		/* No idea why it is not working. The 'format' is no
		   longer recognized as a registered one .*/
		mexPrintf("FDS %d\n", IsClipboardFormatAvailable(format));
		if ( !IsClipboardFormatAvailable(format) )
			mexErrMsgTxt("fd-se.\n");

		clip_matFormat data; 
		HANDLE hData = GetClipboardData(format);
		clip_matFormat *buffer = (clip_matFormat *)GlobalLock(hData);

		/* make a local copy */
		data = *buffer;

		mexPrintf("mmm %d\n", (buffer->ny));

		GlobalUnlock( hData );
	}

	CloseClipboard();
}
Пример #28
0
/* The gateway routine */
void mexFunction(int nlhs, mxArray *plhs[],
                 int nrhs, const mxArray *prhs[])
{
/* ----------------- Variables ----------------- */
/* input variables */
/* mandatory input */
  int startIdx;
  int endIdx;
  double * high;
  double * low;
/* optional input */
  int	 optInTimePeriod;
/* output variables */ 
  int outBegIdx;
  int outNbElement;
  double*	 outReal;
/* input dimentions */ 
  int inSeriesRows;
  int inSeriesCols;
/* error handling */
  TA_RetCode retCode;

/* ----------------- input/output count ----------------- */  
  /*  Check for proper number of arguments. */
  if (nrhs < 2 || nrhs > 3) mexErrMsgTxt("#3 inputs possible #1 optional.");
  if (nlhs != 1) mexErrMsgTxt("#1 output required.");
/* ----------------- INPUT ----------------- */ 
  /* Create a pointer to the input matrix high. */
  high = mxGetPr(prhs[0]);
  /* Get the dimensions of the matrix input high. */
  inSeriesCols = mxGetN(prhs[0]);
  if (inSeriesCols != 1) mexErrMsgTxt("high only vector alowed.");
  /* Create a pointer to the input matrix low. */
  low = mxGetPr(prhs[1]);
  /* Get the dimensions of the matrix input low. */
  inSeriesCols = mxGetN(prhs[1]);
  if (inSeriesCols != 1) mexErrMsgTxt("low only vector alowed.");
  inSeriesRows = mxGetM(prhs[1]);  
  endIdx = inSeriesRows - 1;  
  startIdx = 0;

 /* Process optional arguments */ 
  if (nrhs >= 2+1) {
	if (!mxIsDouble(prhs[2]) || mxIsComplex(prhs[2]) ||
      mxGetN(prhs[2])*mxGetM(prhs[2]) != 1) 
    	mexErrMsgTxt("Input optInTimePeriod must be a scalar.");
   	/* Get the scalar input optInTimePeriod. */
   	optInTimePeriod = (int)  mxGetScalar(prhs[2]);
  } else {
  	optInTimePeriod = 14;
  }

/* ----------------- OUTPUT ----------------- */
  outReal = mxCalloc(inSeriesRows, sizeof(double));
/* -------------- Invocation ---------------- */

	retCode = TA_MINUS_DM(
                   startIdx, endIdx,
                   high,
                   low,
                   optInTimePeriod,
                   &outBegIdx, &outNbElement,
                   outReal);
/* -------------- Errors ---------------- */
   if (retCode) {
   	   mxFree(outReal);
       mexPrintf("%s%i","Return code=",retCode);
       mexErrMsgTxt(" Error!");
   }
  
   // Populate Output
  plhs[0] = mxCreateDoubleMatrix(outBegIdx+outNbElement,1, mxREAL);
  memcpy(((double *) mxGetData(plhs[0]))+outBegIdx, outReal, outNbElement*mxGetElementSize(plhs[0]));
  mxFree(outReal);  
} /* END mexFunction */
Пример #29
0
void matlabParse(int nrhs, const mxArray *prhs[], long *n_ad, long *m_ad, node **nodes_ad, arc **arcs_ad, long **cap_ad, node **source_ad, node **sink_ad, long *node_min_ad )


{

#define MAXLINE       100	/* max line length in the input file */
#define ARC_FIELDS      3	/* no of fields in arc line  */
#define NODE_FIELDS     2	/* no of fields in node line  */
#define P_FIELDS        3       /* no of fields in problem line */
#define PROBLEM_TYPE "max"      /* name of problem type*/

  long    n,                      /* internal number of nodes */
    node_min,               /* minimal no of node  */
    node_max,               /* maximal no of nodes */
    *arc_first,              /* internal array for holding
                                - node degree
                                - position of the first outgoing arc */
    *arc_tail,               /* internal array: tails of the arcs */
    source,                 /* no of the source */
    sink,                   /* no of the sink */
    /* temporary variables carrying no of nodes */
    head, tail, i;

  long    m,                      /* internal number of arcs */
    /* temporary variables carrying no of arcs */
    last, arc_num, arc_new_num;

  node    *nodes,                 /* pointer to the node structure */
    *head_p,
    *ndp;

  arc     *arcs,                  /* pointer to the arc structure */
    *arc_current,
    *arc_new,
    *arc_tmp;

  long    *acap,                  /* array of capasities */
    cap;                    /* capacity of the current arc */

  long    no_lines=0,             /* no of current input line */
    no_plines=0,            /* no of problem-lines */
    no_nslines=0,           /* no of node-source-lines */
    no_nklines=0,           /* no of node-source-lines */
    no_alines=0,            /* no of arc-lines */
    pos_current=0;          /* 2*no_alines */

  int     j,k;                  /* temporary */

  bool sparse;
  mwIndex *ir, *jc;
  double *cost, *ss;

  // check format of arguments
  if (mxIsSparse(prhs[0])) {
    sparse = true;
  } else if (mxIsDouble(prhs[0])&&!mxIsComplex(prhs[0])) {
    sparse = false;
  } else {
    mexErrMsgTxt("Cost matrix must be type double or sparse.");
  }
  n = mxGetM(prhs[0]);
  if (mxGetN(prhs[0]) != n) {
    mexErrMsgTxt("Cost matrix must be square.");
  }
  cost = mxGetPr(prhs[0]);
  if (sparse) {
    ir = (mwIndex*)mxGetIr(prhs[0]);
    jc = (mwIndex*)mxGetJc(prhs[0]);
    m = jc[n];
  } else {
    m = 0;
    for (i = 0; i < n*n; i++) {
      if (cost[i] != 0) {
        m++;
      }
    }
  }
  //mexPrint("%d nodes, %d arcs.\n",n,m);

  // determine source and sink
  if (nrhs == 1) {
    source = 1;
    sink = 2;
  } else {
    if (!mxIsDouble(prhs[1])||mxIsComplex(prhs[1])) {
      mexErrMsgTxt("Source and sink must be type double.");
    }
    if (mxGetNumberOfElements(prhs[1]) != 2) {
      mexErrMsgTxt("Source and sink must be two integers.");
    }
    ss = mxGetPr(prhs[1]);
    source = (long)(ss[0]);
    sink = (long)(ss[1]);
  }
  //mexPrint("Source = %d, sink = %d.\n",source,sink);
  //mexPrintf("Arguments parsed.\n");

  // now start setup
  nodes    = (node*) calloc ( n+2, sizeof(node) );
  arcs     = (arc*)  calloc ( 2*m+1, sizeof(arc) );
  arc_tail = (long*) calloc ( 2*m,   sizeof(long) ); 
  arc_first= (long*) calloc ( n+2, sizeof(long) );
  acap     = (long*) calloc ( 2*m, sizeof(long) );
  //for (i = 0; i < n+2; i++) {
  //	mexPrintf("%ld\n",arc_first[i]);
  //	arc_first[i] = 0;
  //}
  if ( nodes == NULL || arcs == NULL || arc_first == NULL || arc_tail == NULL ) {
    // memory is not allocated
    mexErrMsgTxt("Memory allocation failure.\n");
  }
  //mexPrint("Memory allocated.\n");
	
  // setting pointer to the first arc
  arc_current = arcs;

  //mexPrint("arc_current = %d, pos_current = %d, arcs = %d, nodes = %d.\n",arc_current,pos_current,arcs,nodes);
  node_max = 0;
  node_min = n;
  if (sparse) {
    for (j = 0; j < n; j++) {
      for (k = jc[j]; k < jc[j+1]; k++) {
        head = ir[k]+1;
        tail = j+1;
        cap = (long)(cost[k]);
        //mexPrint("Adding arc from %d to %d at cost %ld.\n",head,tail,cap);

        // no of arcs incident to node i is stored in arc_first[i+1]
        arc_first[tail + 1] ++; 
        arc_first[head + 1] ++;

        // storing information about the arc
        arc_tail[pos_current]        = tail;
        arc_tail[pos_current+1]      = head;
        arc_current       -> head    = nodes + head;
        arc_current       -> r_cap    = cap;
        arc_current       -> sister  = arc_current + 1;
        ( arc_current + 1 ) -> head    = nodes + tail;
        ( arc_current + 1 ) -> r_cap    = 0;
        ( arc_current + 1 ) -> sister  = arc_current;

        // searching minimum and maximum node
        if ( head < node_min ) node_min = head;
        if ( tail < node_min ) node_min = tail;
        if ( head > node_max ) node_max = head;
        if ( tail > node_max ) node_max = tail;

        no_alines   ++;
        arc_current += 2;
        pos_current += 2;
      }
    }
  } else {
    for (j = 0; j < n; j++) {
      for (k = 0; k < n; k++) {
        if (cost[j*n+k] != 0) {
          head = k+1;
          tail = j+1;
          cap = (long)(cost[k*n+j]);
          //mexPrint("Adding arc from %d to %d at cost %ld.\n",head,tail,cap);

          // no of arcs incident to node i is stored in arc_first[i+1]
          arc_first[tail + 1] ++; 
          arc_first[head + 1] ++;

          // storing information about the arc
          arc_tail[pos_current]        = tail;
          arc_tail[pos_current+1]      = head;
          arc_current       -> head    = nodes + head;
          arc_current       -> r_cap    = cap;
          arc_current       -> sister  = arc_current + 1;
          ( arc_current + 1 ) -> head    = nodes + tail;
          ( arc_current + 1 ) -> r_cap    = 0;
          ( arc_current + 1 ) -> sister  = arc_current;
          //mexPrintf("Sisters:  %d, %d (%d, %d).\n",arc_current->sister,(arc_current+1)->sister,arc_current->sister-arcs,(arc_current+1)->sister-arcs);

          // searching minimum and maximum node
          if ( head < node_min ) node_min = head;
          if ( tail < node_min ) node_min = tail;
          if ( head > node_max ) node_max = head;
          if ( tail > node_max ) node_max = tail;

          no_alines   ++;
          arc_current += 2;
          pos_current += 2;
        }
      }
    }
  }
  //mexPrint("Nodes added.\n");
  //mexPrint("arc_current = %d, pos_current = %d, arcs = %d, nodes = %d.\n",arc_current,pos_current,arcs,nodes);
  //mexPrint("Nodes range from %d to %d.\n",node_min,node_max);

  //********* ordering arcs - linear time algorithm ***********

  // first arc from the first node
  ( nodes + node_min ) -> first = arcs;

  // before below loop arc_first[i+1] is the number of arcs outgoing from i;
  // after this loop arc_first[i] is the position of the first 
  // outgoing from node i arcs after they would be ordered;
  // this value is transformed to pointer and written to node.first[i]
 
  for ( i = node_min + 1; i <= node_max + 1; i ++ ) {
    arc_first[i]          += arc_first[i-1];
    ( nodes + i ) -> first = arcs + arc_first[i];
  }

  for ( i = node_min; i < node_max; i ++ ) {	// scanning all the nodes except the last
    last = ( ( nodes + i + 1 ) -> first ) - arcs;
    // arcs outgoing from i must be cited    
    // from position arc_first[i] to the position
    // equal to initial value of arc_first[i+1]-1

    for ( arc_num = arc_first[i]; arc_num < last; arc_num ++ ) {
      tail = arc_tail[arc_num];

      while ( tail != i ) {
        // the arc no  arc_num  is not in place because arc cited here
        // must go out from i;
        // we'll put it to its place and continue this process
        // until an arc in this position would go out from i
        //mexPrint("Inner loop.\n");
        arc_new_num  = arc_first[tail];
        arc_current  = arcs + arc_num;
        arc_new      = arcs + arc_new_num;
				
        // arc_current must be cited in the position arc_new swapping these arcs:

        head_p               = arc_new -> head;
        arc_new -> head      = arc_current -> head;
        arc_current -> head  = head_p;

        cap                 = arc_new -> r_cap;
        arc_new -> r_cap     = arc_current -> r_cap;
        arc_current -> r_cap = cap;

        if ( arc_new != arc_current -> sister )	{
          arc_tmp                = arc_new -> sister;
          arc_new  -> sister     = arc_current -> sister;
          arc_current -> sister  = arc_tmp;

          ( arc_current -> sister ) -> sister = arc_current;
          ( arc_new     -> sister ) -> sister = arc_new;
        }

        arc_tail[arc_num] = arc_tail[arc_new_num];
        arc_tail[arc_new_num] = tail;

        // we increase arc_first[tail]
        arc_first[tail] ++ ;

        tail = arc_tail[arc_num];
      }
    }
    // all arcs outgoing from  i  are in place
  }       
  //mexPrint("Arcs ordered.\n");

  // -----------------------  arcs are ordered  -------------------------

  //----------- constructing lists ---------------

  for ( ndp = nodes + node_min; ndp <= nodes + node_max;  ndp ++ ) {
    ndp -> first = (arc*) NULL;
  }

  for ( arc_current = arcs + (2*m-1); arc_current >= arcs; arc_current -- ) {
    arc_num = arc_current - arcs;
    tail = arc_tail [arc_num];
    ndp = nodes + tail;
    arc_current -> next = ndp -> first;
    ndp -> first = arc_current;
    //mexPrintf("Arc %d:  tail = %d, next = %d.\n",arc_num,tail,arc_current->next);
  }

  // ----------- assigning output values ------------
  *m_ad = m;
  *n_ad = node_max - node_min + 1;
  *source_ad = nodes + source;
  *sink_ad   = nodes + sink;
  *node_min_ad = node_min;
  *nodes_ad = nodes + node_min;
  *arcs_ad = arcs;
  *cap_ad = acap;

  for ( arc_current = arcs, arc_num = 0; arc_num < 2*m; arc_current ++, arc_num ++) {
    acap [ arc_num ] = arc_current -> r_cap; 
  }

  // print out some results
  for (i = 0; i < 2*m; i++) {
    //mexPrint("Arc %d:  capacity %d, head %d, sister %d, next %d.\n",i,arcs[i].r_cap,arcs[i].head-nodes,arcs[i].sister-arcs,(arcs[i].next == NULL) ? 0:arcs[i].next-arcs);
    //mexPrint("  Additional info:  arc_tail = %d, acap = %d.\n",arc_tail[i],acap[i]);
  }
  for (i = 0; i < n+2; i++) {
    //mexPrint("Node %d:  arc_first = %d.\n",i,arc_first[i]);
  }

  //mexPrint("%d, %d, %d.\n",nodes,source,sink);
  //mexPrint("%d, %d.\n",(*source_ad) -> first,(*sink_ad) -> first);
  if ( (*source_ad) -> first == (arc*) NULL || (*sink_ad  ) -> first == (arc*) NULL    ) {
    mexErrMsgTxt("Either source or sink is disconnected.");		
  }

  // free internal memory
  free ( arc_first ); free ( arc_tail );
}
Пример #30
0
void gpc_read_polygon_MATLAB(const mxArray *prhs, gpc_polygon *p)
{

	int c, v;
	int read_hole_flag;
	int id_x, id_y, id_hole;
	int nx[2], ny[2];
	double *x, *y;
	const mxArray *field_x, *field_y, *field_hole;

/*  Checking if input is non empty Matlab-structure */
	if (!mxIsStruct(prhs))
        mexErrMsgTxt("Input needs to be structure.");
	if (!mxGetM(prhs) || !mxGetN(prhs))
        mexErrMsgTxt("Empty structure.");

/*  Checking field names and data type  */
    id_x = mxGetFieldNumber(prhs,"x");
    if (id_x==-1)
        mexErrMsgTxt("Input structure must contain a field 'x'.");

	field_x = mxGetFieldByNumber(prhs, 0, id_x);
    if (!mxIsDouble(field_x))
        mexErrMsgTxt("Structure field 'x' must be of type DOUBLE.");

    id_y = mxGetFieldNumber(prhs,"y");
    if (id_y==-1)
        mexErrMsgTxt("Input structure must contain a field 'y'.");
	field_y = mxGetFieldByNumber(prhs, 0, id_y);
    if (!mxIsDouble(field_y))
        mexErrMsgTxt("Structure field 'y' must be of type DOUBLE.");

	id_hole = mxGetFieldNumber(prhs,"hole");
	if (id_hole==-1)
		read_hole_flag = FALSE;
	else
		read_hole_flag = TRUE;



/*  Passing data: Matlab -> C++	*/
	p->num_contours = mxGetNumberOfElements(prhs);

	p->hole = mxMalloc(p->num_contours * sizeof(int));
	p->contour = mxMalloc(p->num_contours * sizeof(gpc_vertex_list));

	for (c=0; c < p->num_contours; c++) {

		/* Check vertices */
		field_x = mxGetFieldByNumber(prhs, c, id_x);
		field_y = mxGetFieldByNumber(prhs, c, id_y);

		nx[0] = mxGetM(field_x);
		nx[1] = mxGetN(field_x);
		ny[0] = mxGetM(field_y);
		ny[1] = mxGetN(field_y);
		if (Min(nx,2)!=1 || Min(ny,2)!=1 || Max(nx,2)!=Max(ny,2))
			mexErrMsgTxt("Structure fields x and y must be non empty vectors of the same length.");

		p->contour[c].num_vertices = Max(nx,2);
		p->contour[c].vertex = mxMalloc(p->contour[c].num_vertices * sizeof(gpc_vertex));

		x = mxGetPr(field_x);
		y = mxGetPr(field_y);
		for (v= 0; v < p->contour[c].num_vertices; v++) {
			p->contour[c].vertex[v].x = x[v];
			p->contour[c].vertex[v].y = y[v];
		}

		if (read_hole_flag) {
            field_hole = mxGetFieldByNumber(prhs, c, id_hole);
            if (field_hole != NULL)
                p->hole[c] = mxGetScalar(field_hole);
            else
                p->hole[c] = 0;
        }
		if (!read_hole_flag || p->hole[c]!=1)
			p->hole[c] = FALSE; /* Assume all contours to be external */

  }
}