Ejemplo n.º 1
0
void mexFunction( int nlhs, mxArray *plhs[] , int nrhs, const mxArray *prhs[] )
{
	bool * front;
	double * M;
	unsigned int row, col;
	const mwSize  *dims;
    
	if(nrhs == 0 || nlhs > 1)
	{
	    printf("\nsynopsis:   front = paretofront(X)");
	    plhs[0]    = mxCreateDoubleMatrix(0 , 0 ,  mxREAL);
	    return;
	}
	
	M = mxGetPr(prhs[0]);
	dims = mxGetDimensions(prhs[0]);
	row = dims[0];
	col = dims[1];
	
	
	
	/* ----- output ----- */

	plhs[0]    = mxCreateLogicalMatrix (row , 1);
	front = (bool *) mxGetPr(plhs[0]);
	
	
	/* main call */
	paretofront(front,  M, row, col);
}
Ejemplo n.º 2
0
void MxArray::fromVector(const std::vector<bool>& v)
{
    p_ = mxCreateLogicalMatrix(1, v.size());
    if (!p_)
        mexErrMsgIdAndTxt("mexopencv:error", "Allocation error");
    std::copy(v.begin(), v.end(), mxGetLogicals(p_));
}
Ejemplo n.º 3
0
Archivo: mex.c Proyecto: a4a881d4/aloe
void save_output(mxArray **dst, void *src, int len) {
	double *out_real, *out_imag;
	char *out_bit;
	int i;
	float *f_src = src;
	_Complex float *c_src = src;
	char *b_src = src;

	if (output_sample_sz == sizeof(_Complex float)) {
		*dst = mxCreateDoubleMatrix(1, len, mxCOMPLEX);
	    out_real = mxGetPr(*dst);
        out_imag = mxGetPi(*dst);
        for (i=0;i<len;i++) {
        	out_real[i] = (double) __real__ c_src[i];
			out_imag[i] = (double) __imag__ c_src[i];
    	}
	} else if (output_sample_sz == sizeof(float)) {
		*dst = mxCreateDoubleMatrix(1, len, mxREAL);
		out_real = mxGetPr(*dst);
		for (i=0;i<len;i++) {
			out_real[i] = (double) f_src[i];
		}
	} else if (output_sample_sz == sizeof(char)) {
		*dst = mxCreateLogicalMatrix(1, len);
		out_bit = (char*) mxGetData(*dst);
		for (i=0;i<len;i++) {
			out_bit[i] = b_src[i];
		}
	}
}
Ejemplo n.º 4
0
mxArray *mxCreateLogicalScalar(mxLogical value)
{
    mxArray *ptr = mxCreateLogicalMatrix(1, 1);

    ((types::Bool *)ptr)->set(0, value);
    return ptr;
}
Ejemplo n.º 5
0
/*
    PsychSetStructArrayBooleanElement()
    
    Note: The variable "index" is zero-indexed.
*/                                    
void PsychSetStructArrayBooleanElement(	char *fieldName,
                                        int index,
                                        boolean state,
                                        PsychGenericScriptType *pStruct)
{
    int fieldNumber, numElements;
    boolean isStruct;
    mxArray *mxFieldValue;
	 char errmsg[256];


    //check for bogus arguments
    numElements=mxGetM(pStruct) *mxGetN(pStruct);
    if(index>=numElements)
        PsychErrorExitMsg(PsychError_internal, "Attempt to set a structure field at an out-of-bounds index");
    fieldNumber=mxGetFieldNumber(pStruct, fieldName);
    if(fieldNumber==-1) {
		  sprintf(errmsg, "Attempt to set a non-existent structure name field: %s", fieldName);
        PsychErrorExitMsg(PsychError_internal, errmsg);
	 }
    isStruct= mxIsStruct(pStruct);
    if(!isStruct)
        PsychErrorExitMsg(PsychError_internal, "Attempt to set a field within a non-existent structure.");
        
    //do stuff
    mxFieldValue=mxCreateLogicalMatrix(1, 1);
    mxGetLogicals(mxFieldValue)[0]= state;
    mxSetField(pStruct, index, fieldName, mxFieldValue); 
    if (PSYCH_LANGUAGE == PSYCH_OCTAVE) mxDestroyArray(mxFieldValue);
    
}
Ejemplo n.º 6
0
mxArray *emlrtMexFcnProperties(void)
{
  const char *mexProperties[] = {
    "Version",
    "ResolvedFunctions",
    "EntryPoints"};
  const char *epProperties[] = {
    "Name",
    "NumberOfInputs",
    "NumberOfOutputs",
    "ConstantInputs"};
  mxArray *xResult = mxCreateStructMatrix(1,1,3,mexProperties);
  mxArray *xEntryPoints = mxCreateStructMatrix(1,1,4,epProperties);
  mxArray *xInputs = NULL;
  xInputs = mxCreateLogicalMatrix(1, 6);
  mxSetFieldByNumber(xEntryPoints, 0, 0, mxCreateString("naivePerfusionSSEP2X4"));
  mxSetFieldByNumber(xEntryPoints, 0, 1, mxCreateDoubleScalar(6));
  mxSetFieldByNumber(xEntryPoints, 0, 2, mxCreateDoubleScalar(1));
  mxSetFieldByNumber(xEntryPoints, 0, 3, xInputs);
  mxSetFieldByNumber(xResult, 0, 0, mxCreateString("8.1.0.604 (R2013a)"));
  mxSetFieldByNumber(xResult, 0, 1, (mxArray*)emlrtMexFcnResolvedFunctionsInfo());
  mxSetFieldByNumber(xResult, 0, 2, xEntryPoints);

  return xResult;
}
Ejemplo n.º 7
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

	region_container* p = NULL;

	if( nrhs < 3 ) mexErrMsgTxt("One vector and two integer arguments required.");
	if( nlhs != 1 ) mexErrMsgTxt("Exactly one output argument required.");

	if (!MEX_TEST_VECTOR(0) || !MEX_TEST_DOUBLE(0)) mexErrMsgTxt("First argument must be a vector of type double");

    region_clear_flags(REGION_LEGACY_RASTERIZATION);
    if (nrhs > 3) {
	    char* codestr = get_string(prhs[3]);
	    if (strcmpi(codestr, "legacy") == 0) 
		    region_set_flags(REGION_LEGACY_RASTERIZATION);
	    free(codestr);
    }

	int width = getSingleInteger(prhs[1]);
	int height = getSingleInteger(prhs[2]);

	p = get_polygon(prhs[0]);
	float* tmp = p->data.polygon.x; p->data.polygon.x = p->data.polygon.y; p->data.polygon.y = tmp;

    plhs[0] = mxCreateLogicalMatrix(height, width);
    char *result = (char*) mxGetData(plhs[0]);
            
    region_mask(p, result, width, height);
    
    if (p) region_release(&p);

}
Ejemplo n.º 8
0
mxArray *emlrtMexFcnProperties()
{
  const char *mexProperties[] = {
    "Version",
    "ResolvedFunctions",
    "EntryPoints",
    "CoverageInfo",
    NULL };

  const char *epProperties[] = {
    "Name",
    "NumberOfInputs",
    "NumberOfOutputs",
    "ConstantInputs" };

  mxArray *xResult = mxCreateStructMatrix(1,1,4,mexProperties);
  mxArray *xEntryPoints = mxCreateStructMatrix(1,1,4,epProperties);
  mxArray *xInputs = NULL;
  xInputs = mxCreateLogicalMatrix(1, 2);
  mxSetFieldByNumber(xEntryPoints, 0, 0, mxCreateString("dummy"));
  mxSetFieldByNumber(xEntryPoints, 0, 1, mxCreateDoubleScalar(2));
  mxSetFieldByNumber(xEntryPoints, 0, 2, mxCreateDoubleScalar(1));
  mxSetFieldByNumber(xEntryPoints, 0, 3, xInputs);
  mxSetFieldByNumber(xResult, 0, 0, mxCreateString("8.5.0.197613 (R2015a)"));
  mxSetFieldByNumber(xResult, 0, 1, (mxArray*)emlrtMexFcnResolvedFunctionsInfo());
  mxSetFieldByNumber(xResult, 0, 2, xEntryPoints);
  return xResult;
}
Ejemplo n.º 9
0
/* main matab function */
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) {
 
    /* b = fast_inseg( seg, t ) */
    
    /* current t in current seg? start while block until out of seg, then next seg
     * current t before seg start? binsearch for next t in seg, start while block, next seg
     * current t after seg end? go to next seg
     */
    
    double *seg, *t;
    mxLogical *pout;
    
    int s, current_t, idx;
    int nseg, nt;
    
    if (nrhs!=2)
        mexErrMsgIdAndTxt("fast_inseg:invalidArguments", "Expecting two arguments");
    
    if ( mxGetNumberOfDimensions(prhs[0])!=2 || mxGetN(prhs[0])!=2 )
        mexErrMsgIdAndTxt("fast_inseg:invalidArguments", "Invalid segments");
    
    seg = mxGetPr( prhs[0] );
    t = mxGetPr( prhs[1] );
    
    nseg = mxGetM( prhs[0] );
    nt = mxGetNumberOfElements(prhs[1]);
    
    plhs[0] = mxCreateLogicalMatrix( nt, 1 );
    
    if (nseg==0 || nt==0)
        return;
    
    pout = mxGetLogicals( plhs[0] );

    current_t = 0;
    
    for (s=0; s<nseg; s++) {
     
        if ( t[current_t]<seg[s] ) {
            /* binsearch for next t in seg */
             idx = (int) ceil( bsearchi( &(t[current_t]), nt-current_t, seg[s] ) );
             if (idx==-1)
                 break;
             current_t += idx;
        } else if ( t[current_t]>seg[s+nseg] ) {
            continue;
        }
        
        while ( t[current_t]<=seg[s+nseg] && current_t<nt ) {
            pout[current_t] = 1;
            current_t++;
        }
     
        if (current_t>=nt)
            break;
                
    }
    
}
Ejemplo n.º 10
0
void CMatlabInterface::set_bool(bool scalar)
{
	mxArray* o=mxCreateLogicalMatrix(1, 1);
	if (!o)
		SG_ERROR("Couldn't create Logical.\n");

	bool* data=(bool*) mxGetData(o);
	data[0]=scalar;

	set_arg_increment(o);
}
Ejemplo n.º 11
0
/* ********************************************************** */
void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[] )
{
  int mrows, ncols, i, j, jcol;
  double *inputD64;
  unsigned char *inputU8;
  mxLogical *output;

  /* ************* ACCESS MATLAB VARIABLES ************* */
  mrows = mxGetM(prhs[0]);  ncols = mxGetN(prhs[0]);

  /* Create an array to hold the output. */
  plhs[0] = mxCreateLogicalMatrix(mrows, ncols);
  output = mxGetLogicals(plhs[0]);

  /* ********************* COMPUTE ********************* */

  switch (mxGetClassID(prhs[0])) {
    case (mxDOUBLE_CLASS):
      inputD64 = (double *) mxGetPr(prhs[0]);
      for (j = 0; j < ncols; j++) {
	jcol = j*mrows;
	output[0+jcol] = (inputD64[0+jcol] != 0);
	for (i = 1; i < mrows; i++) {
	  output[i+jcol] = inputD64[i+jcol] && !inputD64[(i-1)+jcol];
	}
      }
      break;
    case(mxUINT8_CLASS):
    case(mxLOGICAL_CLASS):
      inputU8 = (unsigned char *) mxGetData(prhs[0]);
      for (j = 0; j < ncols; j++) {
	jcol = j*mrows;
	output[0+jcol] = (inputU8[0+jcol] != 0);
	for (i = 1; i < mrows; i++) {
	  output[i+jcol] = inputU8[i+jcol] && !inputU8[(i-1)+jcol];
	}
      }
      break;
    default:
      mexErrMsgTxt("Invalid Data Type.\n");
  }
  

  /* ********************* CLEAN UP ******************** */

}
Ejemplo n.º 12
0
void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[] )
{
    mxLogical *image, *outImage;
    mwSize w, h;
	
	if (nrhs != 1){
		mexErrMsgIdAndTxt( "MATLAB:borderFill:wrongInputCount",
				"One input (image) expected.");
	}
	
	/* The input must be a logical.*/
	if(!mxIsLogical(prhs[0])){
		mexErrMsgIdAndTxt( "MATLAB:borderFill:inputNotLogical",
				"Input must be a logical.");
	}

    h = mxGetM(prhs[0]);
    w = mxGetN(prhs[0]);
    image = mxGetLogicals(prhs[0]);

    /* Create matrix for the return argument. */
    plhs[0] = mxCreateLogicalMatrix(h, w);
    outImage = mxGetLogicals(plhs[0]);
    
    /* first and last column */
    for (int y = 0; y < h; y++){
        if (!outImage[y]){
            fill(image, h, w, y, 0, image[y], outImage);
        }
        if (!outImage[h * (w - 1) + y]){
            fill(image, h, w, y, w - 1, image[h * (w - 1) + y], outImage);
        }
    }
    
    /* first and last row */
    for (int x = 1; x < w - 1; x++){
        if (!outImage[x * h]){
            fill(image, h, w, 0, x, image[x * h], outImage);
        }
        if (!outImage[x * h + h - 1]){
            fill(image, h, w, h - 1, x, image[x * h + h - 1], outImage);
        }
    }
}
Ejemplo n.º 13
0
void mexFunction(int nlhs_m, mxArray *plhs_m[], int nrhs_m, const mxArray *prhs_m[])
{
		
	if (nlhs_m > 1)
	{
		printf("Error, only one ouput parameter allowed!\n");
		return;
	}

	if (nrhs_m != 1)
	{
		printf("Error, only one input parameter allowed!\n");
		return;
	}
	
	plhs_m[0] = mxCreateLogicalMatrix(1,1);		
	
	*(mxGetLogicals(plhs_m[0])) = mxIsComplex(prhs_m[0]);

	return;
}
void mexFunction(
    int nlhs, mxArray *plhs[],
    int nrhs, const mxArray *prhs[])
{
  double *phi;
  mxLogical *contour;
  int iWidth, iHeight;
  
  /* The input must be a noncomplex scalar double.*/
  iHeight = mxGetM(prhs[0]);
  iWidth = mxGetN(prhs[0]);

  /* Create matrix for the return argument. */
  plhs[0] = mxCreateLogicalMatrix(iHeight,iWidth);
  
  /* Assign pointers to each input and output. */
  phi = mxGetPr(prhs[0]);
  contour = mxGetLogicals(plhs[0]);

	for (int i = 0; i < iWidth; i++) {
		for (int j = 0; j < iHeight; j++) {
			if (phi[i*iHeight+j] >= 0) {
        			bool bIsNeighbourBelowZero = false;
				for (int k = -1; k <= 1; k++)  {
          				for (int l = -1; l <= 1; l++) {
						if (i+k >= 0 && i+k < iWidth && j+l >= 0 && j+l < iHeight && phi[(i+k)*iHeight+j+l] < 0) {
              						if (-phi[(i+k)*iHeight+j+l] < phi[i*iHeight+j]) {
                						contour[(i+k)*iHeight+j+l] = true;
							}
							else {
								contour[i*iHeight+j] = true;
							}
						}
					}
				}
      			}
    		}
  	}
}
Ejemplo n.º 15
0
void 
mexFunction(int nlhs, mxArray *plhs[], 
	    int nrhs, const mxArray *prhs[])
{
   mxArray *par;               /* pointer to array structure */
   double *pda;                /* polynom data */
   mxLogical *pout;            /* pointer to output data */
   int ma;
   int k, Na;

   /* check argument pa */
   if ( !mxIsCell(prhs[0]) ) {
      mexErrMsgTxt("poly_iscwmex :  argument must be a cell array.");
   }
   Na = mxGetNumberOfElements(prhs[0]);
   if (!Na) {
      mexErrMsgTxt("poly_iscwmex :  no input polygons pa.");
   }

   /* create output array */
   plhs[0] = mxCreateLogicalMatrix(1, Na);
   pout = (mxLogical *)mxGetData(plhs[0]);

   /*
    * copy and prepare data
    */

   /* pa */
   for (k=0; k<Na; k++) {

      /* get the next polygon from the cell array */ 
      par = mxGetCell(prhs[0], k); /* ptr to mxArray */
      pda = mxGetData(par);        /* ptr to a data */     
      ma  = mxGetM(par);           /* rows = vertex number */

      /* calculate orientation */
      pout[k] = (mxLogical)clock_wise(pda, ma);
   }
}
mxArray *emlrtMexFcnProperties(void)
{
    const char *mexProperties[] = {
        "Version",
        "EntryPoints"};
    const char *epProperties[] = {
        "Name",
        "NumberOfInputs",
        "NumberOfOutputs",
        "ConstantInputs"};
    mxArray *xResult = mxCreateStructMatrix(1,1,2,mexProperties);
    mxArray *xEntryPoints = mxCreateStructMatrix(1,1,4,epProperties);
    mxArray *xInputs = NULL;
    xInputs = mxCreateLogicalMatrix(1, 6);
    mxSetFieldByNumber(xEntryPoints, 0, 0, mxCreateString("drr_SK_Simplified"));
    mxSetFieldByNumber(xEntryPoints, 0, 1, mxCreateDoubleScalar(6));
    mxSetFieldByNumber(xEntryPoints, 0, 2, mxCreateDoubleScalar(1));
    mxSetFieldByNumber(xEntryPoints, 0, 3, xInputs);
    mxSetFieldByNumber(xResult, 0, 0, mxCreateString("7.14.0.739 (R2012a)"));
    mxSetFieldByNumber(xResult, 0, 1, xEntryPoints);

    return xResult;
}
/* Gateway of insidepoly_dblengine */
void mexFunction(int nlhs, mxArray *plhs[],
                 int nrhs, const mxArray *prhs[]) {
                     
    TYPE *x, *y;
    TYPE dPx, dPy;
    TYPE *xx, *yy, *xlast;
    TYPE *Px1, *Py1, *Px2, *Py2;
    TYPE x1, y1, x2, y2;
    TYPE a, b, c, tol, onthreshold;
    TYPE l2;
    TYPE cmin, cmax;
    TYPE xmin, xmax, ymin, ymax;
    int08 *in, *on, *ii, *oo;
    mwSize n, nvertices, npnt;
    double *dfirst, *dlast;
    mwSignedIndex first, last;
    int presorting;
    
    nvertices = mxGetM(PX1);
    npnt = mxGetM(X);  
            
    x = (TYPE*)mxGetData(X);
    y = (TYPE*)mxGetData(Y);
    Px1 = (TYPE*)mxGetData(PX1);
    Py1 = (TYPE*)mxGetData(PY1);
    Px2 = (TYPE*)mxGetData(PX2);
    Py2 = (TYPE*)mxGetData(PY2);
    
    IN = mxCreateLogicalMatrix(npnt, 1);
    /* in and on is already prefilled with 0 */
    in = (int08*)mxGetData(IN);
    
    ON = mxCreateLogicalMatrix(npnt, 1);
    on = (int08*)mxGetData(ON);
    
    tol = *(TYPE*)mxGetData(TOL);
    
    /* FIRST and LAST are provided */
    presorting = nrhs>=9;
    if (presorting)
    {
        dfirst = mxGetPr(FIRST);
        dlast = mxGetPr(LAST);
    }
            
    /* Loop on segments */
    for (n=0; n<nvertices; n++) {
        
        x1 = *Px1++;
        y1 = *Py1++;
        x2 = *Px2++;
        y2 = *Py2++;
        
        if (presorting) {
            /* Remove one to convert Matlab to C zero-based indexing */
            first = (mwSignedIndex)(dfirst[n])-1;
            last = (mwSignedIndex)(dlast[n])-1;
        }
        else {
            /* Scan all points */
            first = 0;
            last = npnt-1;
        }
        
        if (first>last) continue; /* Nothing to do */
        
        /* Some quantities depend only on the edge */
        dPx = x2-x1;
        dPy = y2-y1;
        a = x1*y2 - x2*y1;
        l2 = dPx*dPx + dPy*dPy;
        onthreshold = (TYPE)sqrt(l2)*tol;
        b = x1*dPy - y1*dPx;
        cmin = b-onthreshold;
        cmax = b+onthreshold;
        
        /* Initlialize pointers to the begining of the arrays before looping */
        xx = x+first;
        yy = y+first;
        ii = in+first;
        oo = on+first;
        /* The last pointer scanned by xx */
        xlast = x+last;
                
        /* Branching */
        if (dPx>0) { /* x2>x1 */
            /* Loop on data points */
            while (xx<=xlast) {
                if (*xx>=x1 && *xx<x2) {
                    c = *xx*dPy - *yy*dPx;
                    *ii ^= (c >= a);
                    *oo |= (c>cmin && c<cmax);
                }
                xx++;
                yy++;
                ii++;
                oo++;
            } /* while */
        } else if (dPx<0) { /* x2<x1 */
            /* Loop on data points */
            while (xx<=xlast) {
                if (*xx>=x2 && *xx<x1) {
                    c = *xx*dPy - *yy*dPx;
                    *ii ^= (c <= a);
                    *oo |= (c>cmin && c<cmax);
                }
                xx++;
                yy++;
                ii++;
                oo++;
            }  /* while */
        }
        else { /* (dPx == 0.0) x1==x2 */
            xmin = x1-tol;
            xmax = x1+tol;
            /* Process special cases for on-boundary test */
            if (dPy>0.0) /* y2 > y1 */ {
                ymin = y1;
                ymax = y2;
            }
            else { /* y2 <= y1 */
                ymin = y2;
                ymax = y1;
            }
                                
            /* Loop on data points */
            while (xx<=xlast) {
                *oo |= (*yy>=ymin && *yy<=ymax && *xx>xmin && *xx<xmax);
                xx++;
                yy++;
                oo++;
            }  /* while */
        } /* if dPx */

    } /* for-loop segments*/

    return;
}
Ejemplo n.º 18
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray * prhs[]) {
  /* compute the infimum in CSFM, provided that the W
   * (which is 2 x nPoint x nFrame) is centered
   */
  double *wOri, *q, *res;
  int i, ii, j, k, l, m, n, p, iFrame, jFrame, nFrame, nPoint;
  mxArray *pIsDone;
  const mwSize *dim_array;
  double *errArray, *quaternionArray ;
  char *isDone;
  mxArray *pWPairwiseProd, *pTmp2By2, *pRTransposeR, *pDRTransposeR,
          *pQuaternionTmp, *pGradient, *pWPrimeWPrimeTranspose;
  double *wPairwiseProd, *tmp2By2, *RTransposeR, *dRTransposeR,
          *quaternionTmp, *gradient, *wPrimeWPrimeTranspose;
  
  /* Retrieve the useful data */
  wOri= mxGetPr(prhs[0]);
  dim_array = mxGetDimensions(prhs[0]);
  nPoint = dim_array[1];
  nFrame = dim_array[2];
  
  /* Create the output data */
  plhs[0] = mxCreateDoubleMatrix(nFrame, nFrame, mxREAL);
  errArray = mxGetPr(plhs[0]);
  
  plhs[1] = mxCreateDoubleMatrix(4, nFrame * nFrame, mxREAL);
  quaternionArray = mxGetPr(plhs[1]);
  
  pIsDone = mxCreateLogicalMatrix(nFrame, nFrame);
  isDone = (char*) mxGetPr(pIsDone);
  
  for (iFrame = 0, k = 0; iFrame < nFrame; ++iFrame)
    for (jFrame = 0; jFrame < nFrame; ++jFrame, ++k) {
    errArray[k] = 0;
    if (iFrame == jFrame) {
      /* set the rotation to identity */
      isDone[k] = 1;
      quaternionArray[4 * k + 0] = 1;
      for (i = 1; i < 4; ++i)
        quaternionArray[4 * k + i] = 0;
    } else
      isDone[k] = 0;
    }
  
  /* create the pairwise W multiplications between frames */
  pWPairwiseProd = mxCreateDoubleMatrix(2*nFrame, 2*nFrame, mxREAL);
  wPairwiseProd = mxGetPr(pWPairwiseProd);
  pTmp2By2 = mxCreateDoubleMatrix(2, 2, mxREAL);
  tmp2By2 = mxGetPr(pTmp2By2);
  
  for (i = 0; i < nFrame; ++i )
    for (j = 0; j <= i; ++j ) {
    matrixMultiply(wOri + i * 2 * nPoint, wOri + j * 2 * nPoint, tmp2By2, 2, nPoint, 2);
    for( k = 0; k < 2; ++k )
      for( l = 0; l < 2; ++l )
        wPairwiseProd[(2*i+k)*2*nFrame + 2*j + l] = tmp2By2[l+2*k];
    }
  
  /* symmetrize the matrix */
  for (i = 0; i < 2*nFrame; ++i )
    for (j = i+1; j < 2*nFrame; ++j )
      wPairwiseProd[i*2*nFrame+j] = wPairwiseProd[j*2*nFrame+i];
  
  /* create some cache some temporary matrices */
  pRTransposeR = mxCreateDoubleMatrix(4, 4, mxREAL);
  RTransposeR = mxGetPr(pRTransposeR);
  pDRTransposeR = mxCreateDoubleMatrix(4, 4*4, mxREAL);
  dRTransposeR = mxGetPr(pDRTransposeR);
  pQuaternionTmp = mxCreateDoubleMatrix(4, 1, mxREAL);
  quaternionTmp = mxGetPr(pQuaternionTmp);
  pGradient = mxCreateDoubleMatrix(4, 1, mxREAL);
  gradient = mxGetPr(pGradient);
  pWPrimeWPrimeTranspose = mxCreateDoubleMatrix(4, 4, mxREAL);
  wPrimeWPrimeTranspose = mxGetPr(pWPrimeWPrimeTranspose);
  
  /* do everything else */
  for (iFrame = 0; iFrame < nFrame; ++iFrame) {
    for (jFrame = iFrame+1; jFrame < nFrame; ++jFrame)
      gradientDescent(wPrimeWPrimeTranspose, iFrame, jFrame, wPairwiseProd, errArray, quaternionArray, isDone, nFrame, nPoint, RTransposeR, dRTransposeR, quaternionTmp, gradient);
    ++iFrame;
    for (jFrame = nFrame - 1; jFrame > iFrame; --jFrame)
      gradientDescent(wPrimeWPrimeTranspose, iFrame, jFrame, wPairwiseProd, errArray, quaternionArray, isDone, nFrame, nPoint, RTransposeR, dRTransposeR, quaternionTmp, gradient);
  }
  
  /* do everything else in a different order */
  for (jFrame = nFrame-1; jFrame > 0; --jFrame) {
    for (iFrame = jFrame+1; iFrame < nFrame; ++iFrame)
      gradientDescent(wPrimeWPrimeTranspose, iFrame, jFrame, wPairwiseProd, errArray, quaternionArray, isDone, nFrame, nPoint, RTransposeR, dRTransposeR, quaternionTmp, gradient);
    
    --jFrame;
    for (iFrame = nFrame - 1; iFrame > jFrame; --iFrame)
      gradientDescent(wPrimeWPrimeTranspose, iFrame, jFrame, wPairwiseProd, errArray, quaternionArray, isDone, nFrame, nPoint, RTransposeR, dRTransposeR, quaternionTmp, gradient);
  }
  
  /* Free memory */
  mxDestroyArray(pIsDone);
  mxDestroyArray(pWPrimeWPrimeTranspose);
  mxDestroyArray(pTmp2By2);
  mxDestroyArray(pWPairwiseProd);
  mxDestroyArray(pRTransposeR);
  mxDestroyArray(pDRTransposeR);
  mxDestroyArray(pQuaternionTmp);
  mxDestroyArray(pGradient);
}
Ejemplo n.º 19
0
/** the gateway function */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    /* [t, b] = ddm_rand_full(mu, sig2, b_lo, b_up, 
                              delta_t, n[, inv_leak[, seed]]) */

    /* Check argument number */
    if (nlhs != 2) {
        mexErrMsgIdAndTxt("ddm_rand_full:WrongOutputs", 
                          "Wrong number of output arguments");
    }
    if (nrhs < 6) {
        mexErrMsgIdAndTxt("ddm_rand_full:WrongInputs",
                          "Too few input arguments");
    }

    /* Process first 8 arguments */
    if (!MEX_ARGIN_IS_REAL_VECTOR(0))
        mexErrMsgIdAndTxt("ddm_rand_full:WrongInput",
                          "First input argument expected to be a vector");
    if (!MEX_ARGIN_IS_REAL_VECTOR(1))
        mexErrMsgIdAndTxt("ddm_rand_full:WrongInput",
                          "Second input argument expected to be a vector");
    if (!MEX_ARGIN_IS_REAL_VECTOR(2))
        mexErrMsgIdAndTxt("ddm_rand_full:WrongInput",
                          "Third input argument expected to be a vector");
    if (!MEX_ARGIN_IS_REAL_VECTOR(3))
        mexErrMsgIdAndTxt("ddm_rand_full:WrongInput",
                          "Fourth input argument expected to be a vector");
    if (!MEX_ARGIN_IS_REAL_DOUBLE(4))
        mexErrMsgIdAndTxt("ddm_rand_full:WrongInput",
                          "Fifth input argument expected to be a double");
    if (!MEX_ARGIN_IS_REAL_DOUBLE(5))
        mexErrMsgIdAndTxt("ddm_rand_full:WrongInput",
                          "Sixth input argument expected to be a double");
    if (nrhs >= 7 && !MEX_ARGIN_IS_REAL_DOUBLE(6))
        mexErrMsgIdAndTxt("ddm_rand_sym:WrongInput",
                          "Seventh input argument expected to be a double");
    if (nrhs >= 8 && !MEX_ARGIN_IS_REAL_DOUBLE(7))
        mexErrMsgIdAndTxt("ddm_rand_sym:WrongInput",
                          "Eighth input argument expected to be a double");
    int mu_size = std::max(mxGetN(prhs[0]), mxGetM(prhs[0]));
    int sig2_size = std::max(mxGetN(prhs[1]), mxGetM(prhs[1]));
    int b_lo_size = std::max(mxGetN(prhs[2]), mxGetM(prhs[2]));
    int b_up_size = std::max(mxGetN(prhs[3]), mxGetM(prhs[3]));
    ExtArray mu(ExtArray::shared_noowner(mxGetPr(prhs[0])), mu_size);
    ExtArray sig2(ExtArray::shared_noowner(mxGetPr(prhs[1])), sig2_size);
    ExtArray b_lo(ExtArray::shared_noowner(mxGetPr(prhs[2])), b_lo_size);
    ExtArray b_up(ExtArray::shared_noowner(mxGetPr(prhs[3])), b_up_size);
    ExtArray b_lo_deriv = ExtArray::const_array(0.0);
    ExtArray b_up_deriv = ExtArray::const_array(0.0);
    double delta_t = mxGetScalar(prhs[4]);
    int n = (int) mxGetScalar(prhs[5]);
    if (delta_t <= 0.0)
        mexErrMsgIdAndTxt("ddm_rand_full:WrongInput",
                          "delta_t needs to be larger than 0.0");
    if (n <= 0)
        mexErrMsgIdAndTxt("ddm_rand_full:WrongInput",
                          "n needs to be larger than 1");
    bool has_leak = false;
    double inv_leak = 0.0;
    if (nrhs >= 7) {
        inv_leak = mxGetScalar(prhs[6]);
        has_leak = (inv_leak != 0.0);
        if (inv_leak < 0.0)
            mexErrMsgIdAndTxt("ddm_rand_full:WrongInput",
                              "inv_leak needs to be non-negative");
    }
    int rngseed = 0;
    if (nrhs >= 8)
        rngseed = (int) mxGetScalar(prhs[7]);
    if (nrhs > 8)
        mexErrMsgIdAndTxt("ddm_rand_full:WrongInputs",
                          "Too many input arguments");

    /* reserve space for output */
    plhs[0] = mxCreateDoubleMatrix(1, n, mxREAL);
    plhs[1] = mxCreateLogicalMatrix(1, n);
    double* t = mxGetPr(plhs[0]);
    mxLogical* b = mxGetLogicals(plhs[1]);

    /* perform sampling */
    DMBase* dm = nullptr;
    if (has_leak)
        dm = DMBase::create(mu, sig2, b_lo, b_up, b_lo_deriv, b_up_deriv,
                            delta_t, inv_leak);
    else
        dm = DMBase::create(mu, sig2, b_lo, b_up, b_lo_deriv, b_up_deriv,
                            delta_t);
    DMBase::rngeng_t rngeng;
    if (rngseed == 0) rngeng.seed(std::random_device()());
    else rngeng.seed(rngseed);
    for (int i = 0; i < n; ++i) {
        DMSample s = dm->rand(rngeng);
        t[i] = s.t();
        b[i] = s.upper_bound() ? 1 : 0;
    }
    delete dm;
}
Ejemplo n.º 20
0
/* Gateway function with Matlab */
void mexFunction(int nlhs, mxArray *plhs[],
                 int nrhs, const mxArray *prhs[]) {
    char *cmd;
    /* If no input argument, print out the usage */
    if (nrhs == 0) {
        mexErrMsgTxt("Usage: messenger('init|listen|respond', extra1, extra2, ...)");
    }

    /* Get the input command */
    if(!(cmd = mxArrayToString(prhs[0]))) {
        mexErrMsgTxt("Cannot read the command");
    }

    /* Initialize a new server session */
    if (strcmp(cmd, "init") == 0) {
        char *socket_addr;
        mxLogical *p;

        /* Check if the input format is valid */
        if (nrhs != 2) {
            mexErrMsgTxt("Missing argument: socket address");
        }
        if (!(socket_addr = mxArrayToString(prhs[1]))) {
            mexErrMsgTxt("Cannot read socket address");
        }

        plhs[0] = mxCreateLogicalMatrix(1, 1);
        p = mxGetLogicals(plhs[0]);

        if (!initialized) {
            if (!initialize(socket_addr)) {
                p[0] = 1;
                mexPrintf("Socket created at: %s\n", socket_addr);
            } else {
                p[0] = 0;
                mexErrMsgTxt("Socket creation failed.");
            }
        } else {
            mexErrMsgTxt("One socket has already been initialized.");
        }

        return;

    /* Listen over an existing socket */
    } else if (strcmp(cmd, "listen") == 0) {
        char *recv_buffer = mxCalloc(BUFLEN, sizeof(char));

        int byte_recvd = listen_zmq(recv_buffer, BUFLEN);

        while (byte_recvd == -1 && errno == EAGAIN) {
        	mexCallMATLAB(0, NULL, 0, NULL, "drawnow");
        	byte_recvd = listen_zmq(recv_buffer, BUFLEN);
       	}

        /* Check if the received data is complete and correct */
        if ((byte_recvd > -1) && (byte_recvd <= BUFLEN)) {
            plhs[0] = mxCreateString(recv_buffer);
        } else if (byte_recvd > BUFLEN){
            mexErrMsgTxt("Receiver buffer overflow. Message truncated");
        } else {
        	sprintf(recv_buffer, "Failed to receive a message due to ZMQ error %s", strerror(errno));
            mexErrMsgTxt(recv_buffer);
        }

        return;

    /* Send a message out */
    } else if (strcmp(cmd, "respond") == 0) {
        size_t msglen;
        char *msg_out;
        mxLogical *p;

        /* Check if the input format is valid */
        if (nrhs != 2) {
            mexErrMsgTxt("Please provide the message to send");
        }

        msglen = mxGetNumberOfElements(prhs[1]);
        msg_out = mxArrayToString(prhs[1]);

        plhs[0] = mxCreateLogicalMatrix(1, 1);
        p = mxGetLogicals(plhs[0]);

        if (msglen == respond(msg_out, msglen)) {
            p[0] = 1;
        } else {
            p[0] = 0;
            mexErrMsgTxt("Failed to send message due to ZMQ error");
        }

        return;

    /* Close the socket and context */
    } else if (strcmp(cmd, "exit") == 0) {
        cleanup();

        return;
    } else {
        mexErrMsgTxt("Unidentified command");
    }
}
/* gateway Function */
void mexFunction(int nlhs,       mxArray * plhs[]
                ,int nrhs, const mxArray * prhs[])
{
    /* check for proper number of arguments */
    /* validate the input values */
    
    /* variable declaration */
    
    /* input variable declaration */
    double dX;
    double mRows;
    double nCols;
    double consideringCellsNo;
    /* 주의: mxArray 자료형 변수는 mexGetVariablePtr 함수를 이용하여
     * 호출함수의 작업공간에 있는 변수들의 포인터만 불러옴 */
    const mxArray * mxArray4;
    const mxArray * mxArray9;
    const mxArray * mxArray10;
    const mxArray * mxArray11;
    const mxArray * mxArray12;
    const mxArray * mxArray13;
    const mxArray * mxArray14;
    const mxArray * mxArray15;
    const mxArray * mxArray16;
    const mxArray * mxArray17;
    const mxArray * mxArray18;
    const mxArray * mxArray19;
    const mxArray * mxArray20;
    
    double * mexSortedIndicies;
    double * mexSDSNbrIndicies;
    double * flood;
    double * floodedRegionCellsNo;
    double * floodedRegionStorageVolume;
    double * bankfullWidth;
    double * transportCapacity;
    double * bedrockIncision;
    double * chanBedSed;
    double * sedimentThick;
    mxLogical * hillslope;
    double * transportCapacityForShallow;
    double * bedrockElev;
    
    /* output variable declaration */
    double * dSedimentThick;
    double * dBedrockElev;
    double * dChanBedSed;
    double * inputFlux;
    double * outputFlux;
    double * inputFloodedRegion;
    mxLogical * isFilled;

    /* create a pointer to the real data in the input matrix */
    dX                  = mxGetScalar(prhs[0]);
    mRows               = mxGetScalar(prhs[1]);
    nCols               = mxGetScalar(prhs[2]);
    consideringCellsNo  = mxGetScalar(prhs[3]);
    
    mxArray4 = mexGetVariablePtr("caller","mexSortedIndicies");    
    mxArray9 = mexGetVariablePtr("caller","mexSDSNbrIndicies");
    mxArray10 = mexGetVariablePtr("caller","flood");
    mxArray11 = mexGetVariablePtr("caller","floodedRegionCellsNo");
    mxArray12 = mexGetVariablePtr("caller","floodedRegionStorageVolume");
    mxArray13 = mexGetVariablePtr("caller","bankfullWidth");
    mxArray14 = mexGetVariablePtr("caller","transportCapacity");
    mxArray15 = mexGetVariablePtr("caller","bedrockIncision");
    mxArray16 = mexGetVariablePtr("caller","chanBedSed");
    mxArray17 = mexGetVariablePtr("caller","sedimentThick");
    mxArray18 = mexGetVariablePtr("caller","hillslope");
    mxArray19 = mexGetVariablePtr("caller","transportCapacityForShallow");
    mxArray20 = mexGetVariablePtr("caller","bedrockElev");
    
    mexSortedIndicies              = mxGetPr(mxArray4);
    mexSDSNbrIndicies              = mxGetPr(mxArray9);    
    flood                          = mxGetPr(mxArray10);    
    floodedRegionCellsNo           = mxGetPr(mxArray11);    
    floodedRegionStorageVolume     = mxGetPr(mxArray12);    
    bankfullWidth                  = mxGetPr(mxArray13);
    transportCapacity              = mxGetPr(mxArray14);    
    bedrockIncision                = mxGetPr(mxArray15);
    chanBedSed                     = mxGetPr(mxArray16);
    sedimentThick                  = mxGetPr(mxArray17);
    hillslope                      = mxGetLogicals(mxArray18);
    transportCapacityForShallow    = mxGetPr(mxArray19);
    bedrockElev                    = mxGetPr(mxArray20);

    /* prepare output data */
    /* create the output matrix */
    plhs[0] = mxCreateDoubleMatrix(mRows,nCols,mxREAL);
    plhs[1] = mxCreateDoubleMatrix(mRows,nCols,mxREAL);
    plhs[2] = mxCreateDoubleMatrix(mRows,nCols,mxREAL);
    plhs[3] = mxCreateDoubleMatrix(mRows,nCols,mxREAL);
    plhs[4] = mxCreateDoubleMatrix(mRows,nCols,mxREAL);
    plhs[5] = mxCreateDoubleMatrix(mRows,nCols,mxREAL);
    plhs[6] = mxCreateLogicalMatrix(mRows,nCols);
    
    /* get a pointer to the real data in the output matrix */
    dSedimentThick      = mxGetPr(plhs[0]);
    dBedrockElev        = mxGetPr(plhs[1]);
    dChanBedSed         = mxGetPr(plhs[2]);
    inputFlux           = mxGetPr(plhs[3]);
    outputFlux          = mxGetPr(plhs[4]);
    inputFloodedRegion  = mxGetPr(plhs[5]);
    isFilled            = mxGetLogicals(plhs[6]);
    
    /* 서브 루틴 수행 */
    EstimateDElevByFluvialProcessBySDS(
        dSedimentThick,
        dBedrockElev,
        dChanBedSed,
        inputFlux,
        outputFlux,
        inputFloodedRegion,
        isFilled,
        dX,
        consideringCellsNo,
        mexSortedIndicies,
        mexSDSNbrIndicies,
        flood,
        floodedRegionCellsNo,
        floodedRegionStorageVolume,
        bankfullWidth,
        transportCapacity,
        bedrockIncision,
        chanBedSed,
        sedimentThick,
        hillslope,
        transportCapacityForShallow,
        bedrockElev);

}
Ejemplo n.º 22
0
// [X,isValid] = rayTrace(D,C,vertices,faces);
//
// Inputs:
// D - 3xM matrix of direction vectors (single)
// C - 3x1 vector for camera center (single)
// vertices - 3xN matrix of vertices (single)
// faces - 3xK matrix of triangle face indices (int32)
//
// Outputs:
// X - 3xM matrix of 3D points that intersects the mesh (single)
// isValid - 1xM vector that indicates whether 3D point is valid intersection (logic)
void mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[]) {
  if(nrhs != 4) {
    mexErrMsgTxt("Error: 4 args. needed.");
    return;
  }

  // Get inputs:
  float *D = (float*)mxGetData(prhs[0]);
  float *C = (float*)mxGetData(prhs[1]);
  float *vertices = (float*)mxGetData(prhs[2]);
  int *faces = (int*)mxGetData(prhs[3]);

  int Nrays = mxGetN(prhs[0]);
  int Nvertices = mxGetN(prhs[2]);
  int Nfaces = mxGetN(prhs[3]);

  fprintf(stdout,"Nfaces: %d\n",Nfaces);
  fflush(stdout);

  // Allocate outputs:
  plhs[0] = mxCreateNumericMatrix(3,Nrays,mxSINGLE_CLASS,mxREAL);
  float *X = (float*)mxGetData(plhs[0]);
  plhs[1] = mxCreateLogicalMatrix(1,Nrays);
  bool *isValid = (bool*)mxGetData(plhs[1]);
  plhs[2] = mxCreateNumericMatrix(1,Nrays,mxINT32_CLASS,mxREAL);
  int *faceNdx = (int*)mxGetData(plhs[2]);

  // Allocate temporary memory:
  float *lambda_num = (float*)mxMalloc(Nfaces*sizeof(float));
  int *Uaxis = (int*)mxMalloc(Nfaces*sizeof(int));
  int *Vaxis = (int*)mxMalloc(Nfaces*sizeof(int));
  int *Waxis = (int*)mxMalloc(Nfaces*sizeof(int));
  float *Cu = (float*)mxMalloc(Nfaces*sizeof(float));
  float *Cv = (float*)mxMalloc(Nfaces*sizeof(float));
  float *bnu = (float*)mxMalloc(Nfaces*sizeof(float));
  float *bnv = (float*)mxMalloc(Nfaces*sizeof(float));
  float *cnu = (float*)mxMalloc(Nfaces*sizeof(float));
  float *cnv = (float*)mxMalloc(Nfaces*sizeof(float));
  float *vu = (float*)mxMalloc(Nfaces*sizeof(float));
  float *vv = (float*)mxMalloc(Nfaces*sizeof(float));
  float *nu = (float*)mxMalloc(Nfaces*sizeof(float));
  float *nv = (float*)mxMalloc(Nfaces*sizeof(float));

  // Pre-compute quantities on mesh:
  PreCompute(vertices,faces,Nfaces,C,Uaxis,Vaxis,Waxis,lambda_num,bnu,bnv,cnu,cnv,Cu,Cv,nu,nv,vu,vv);

  // Compute KD-tree:
  fprintf(stdout,"Creating KDtree...\n");
  fflush(stdout);
  int maxDepth = 20;//3;
  float bound_min[3] = {INF,INF,INF};
  float bound_max[3] = {-INF,-INF,-INF};
  int *leaves = (int*)mxMalloc(Nfaces*sizeof(int));
  for(int i = 0; i < Nfaces; i++) leaves[i] = i;
  for(int i = 0; i < Nvertices; i++) {
    for(int j = 0; j < 3; j++) {
      if(vertices[j+i*3]<bound_min[j]) bound_min[j] = vertices[j+i*3];
      if(vertices[j+i*3]>bound_max[j]) bound_max[j] = vertices[j+i*3];
    }
  }
//   for(int i = 0; i < 3; i++) fprintf(stdout,"Bounds %d: %f %f\n",i,bound_min[i],bound_max[i]);
  KDtree *root = CreateKDtree(vertices,faces,bound_min,bound_max,0,leaves,Nfaces,0,maxDepth);
//   mxFree(leaves);
  fprintf(stdout,"Finished creating KDtree...\n");
  fflush(stdout);

  int Nleaves = Nfaces;
  leaves = (int*)mxMalloc(Nleaves*sizeof(int));
  for(int i = 0; i < Nleaves; i++) leaves[i] = i;

  // Get 3D points:
  float maxLambda;
  float min_lambda,max_lambda;
//   float *Di;
  float Di[3];
  int j;
  float ll;
  int jmax;
  for(int i = 0; i < Nrays; i++) {
//     Di = D+i*3;
    // This was originally written for LH coordinates; change for RH coordinates
    Di[0] = -D[i*3];
    Di[1] = -D[1+i*3];
    Di[2] = -D[2+i*3];

    // Get bounds on ray:
//     min_lambda = -INF;
    max_lambda = -INF;
    for(j = 0; j < 3; j++) {
      ll = (bound_min[j]-C[j])/Di[j];
      if(ll<=0) {
	if((C[j]>=bound_min[j])&&(ll>max_lambda)) max_lambda = ll;
// 	if((C[j]<bound_min[j])&&(ll>min_lambda)) min_lambda = ll;
      }
      ll = (bound_max[j]-C[j])/Di[j];
      if(ll<=0) {
// 	if((C[j]>=bound_max[j])&&(ll>min_lambda)) min_lambda = ll;
	if((C[j]<bound_max[j])&&(ll>max_lambda)) max_lambda = ll;
      }
    }
    min_lambda = 0.0f;

//     fprintf(stdout,"Min/max lambda: (%f,%f)\n",min_lambda,max_lambda);
//     fflush(stdout);
    
    // Intersect triangles:
    maxLambda = IntersectTriangle_KDtree(Di,Uaxis,Vaxis,Waxis,lambda_num,bnu,bnv,cnu,cnv,Cu,Cv,nu,nv,vu,vv,root,min_lambda,max_lambda,C,&jmax);
//     maxLambda = IntersectTriangle(Di,Uaxis,Vaxis,Waxis,lambda_num,bnu,bnv,cnu,cnv,Cu,Cv,nu,nv,vu,vv,leaves,Nleaves);
    if(maxLambda != -INF) {
      isValid[i] = 1;
      faceNdx[i] = jmax+1;
      for(j = 0; j < 3; j++) X[j+i*3] = -D[j+i*3]*maxLambda+C[j];
    }
  }

  mxFree(leaves);

  // Free memory:
  mxFree(lambda_num);
  mxFree(Uaxis);
  mxFree(Vaxis);
  mxFree(Waxis);
  mxFree(Cu);
  mxFree(Cv);
  mxFree(bnu);
  mxFree(bnv);
  mxFree(cnu);
  mxFree(cnv);
  mxFree(vu);
  mxFree(vv);
  mxFree(nu);
  mxFree(nv);

  // Deallocate KDtree:
  FreeKDtree(root);
}
Ejemplo n.º 23
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  if ((nrhs == 0) || !mxIsChar(prhs[0])) {
    mexErrMsgTxt("BotParamClient: first argument must be command string");
  }
  if ((nrhs < 2) || !mxIsChar(prhs[1])) {
    mexErrMsgTxt("BotParamClient: second argument must be key");
  }

  std::string command = ::getString(prhs[0]);
  std::transform(command.begin(), command.end(), command.begin(), ::tolower);
  std::string key = ::getString(prhs[1]);

  if ((nrhs > 3) || ((nrhs == 3) && !::isSetCommand(command))) {
    mexErrMsgTxt("BotParamClient: too many input arguments");
  }

  if (isSetCommand(command) && (nrhs != 3)) {
    mexErrMsgTxt("BotParamClient: need value argument");
  }

  BotParam* param = BotParamClient::instance().getUnderlyingBotParam();
  if (param == NULL) {
    mexErrMsgTxt("BotParamClient: no param client; is server running?");
  }

  bool hasKey = (0 != bot_param_has_key(param, key.c_str()));
  if (!hasKey && !isSetCommand(command) && (command != "haskey")) {
    mexErrMsgTxt("BotParamClient: invalid key");
  }

  if (command == "haskey") {
    plhs[0] = mxCreateLogicalMatrix(1,1);
    mxLogical* out = mxGetLogicals(plhs[0]);
    out[0] = hasKey;
  }

  else if (command == "subkeys") {
    char** subkeysRaw = bot_param_get_subkeys(param, key.c_str());
    std::vector<std::string> subkeys;
    for (char** subkeyPtr = subkeysRaw; *subkeyPtr != NULL; ++subkeyPtr) {
      subkeys.push_back(std::string(*subkeyPtr));
    }
    bot_param_str_array_free(subkeysRaw);
    plhs[0] = mxCreateCellMatrix(1,subkeys.size());
    for (size_t i = 0; i < subkeys.size(); ++i) {
      mxSetCell(plhs[0], i, mxCreateString(subkeys[i].c_str()));
    }
  }

  else if (command == "getnum") {
    int len = bot_param_get_array_len(param, key.c_str());
    std::vector<double> vals;
    if (len <= 0) {
      double val;
      if (bot_param_get_double(param, key.c_str(), &val) != 0) {
        mexErrMsgTxt("BotParamClient: cannot find numeric");
      }
      vals.push_back(val);
    }
    else {
      vals.resize(len);
      if (bot_param_get_double_array(param, key.c_str(), vals.data(), len) != len) {
        mexErrMsgTxt("BotParamClient: non-numeric value(s)");
      }
    }
    plhs[0] = mxCreateDoubleMatrix(1,vals.size(),mxREAL);
    double* ptr = mxGetPr(plhs[0]);
    for (size_t i = 0; i < vals.size(); ++i) {
      ptr[i] = vals[i];
    }
  }

  else if (command == "getbool") {
    int len = bot_param_get_array_len(param, key.c_str());
    std::vector<bool> vals;
    if (len <= 0) {
      int val;
      if (bot_param_get_boolean(param, key.c_str(), &val) != 0) {
        mexErrMsgTxt("BotParamClient: cannot find bool");
      }
      vals.push_back(val!=0);
    }
    else {
      int valsRaw[len];
      if (bot_param_get_boolean_array(param, key.c_str(), valsRaw, len) != len) {
        mexErrMsgTxt("BotParamClient: non-boolean value(s)");
      }
      for (int i = 0; i < len; ++i) {
        vals.push_back(valsRaw[i]!=0);
      }
    }
    plhs[0] = mxCreateLogicalMatrix(1,vals.size());
    mxLogical* out = mxGetLogicals(plhs[0]);
    for (size_t i = 0; i < vals.size(); ++i) {
      out[i] = vals[i];
    }
  }

  else if (command == "getstr") {
    int len = bot_param_get_array_len(param, key.c_str());
    std::vector<std::string> vals;
    if (len <= 0) {
      char* val = NULL;
      if (bot_param_get_str(param, key.c_str(), &val) != 0) {
        mexErrMsgTxt("BotParamClient: cannot find string");
      }
      vals.push_back(std::string(val));
      free(val);
    }
    else {
      char** valsRaw = bot_param_get_str_array_alloc(param, key.c_str());
      for (char** valsPtr = valsRaw; *valsPtr != NULL; ++valsPtr) {
        vals.push_back(std::string(*valsPtr));
      }
      bot_param_str_array_free(valsRaw);
    }
    plhs[0] = mxCreateCellMatrix(1,vals.size());
    for (size_t i = 0; i < vals.size(); ++i) {
      mxSetCell(plhs[0], i, mxCreateString(vals[i].c_str()));
    }
  }

  else if (command == "setstr") {
    bot_param::set_t msg = constructSetMessage(param, key);
    std::string value;
    if (mxIsCell(prhs[2])) {
      int len = mxGetNumberOfElements(prhs[2]);
      for (int i = 0; i < len; ++i) {
        mxArray* cellVal = mxGetCell(prhs[2],i);
        if (!mxIsChar(cellVal)) {
          mexErrMsgTxt("BotParamClient: invalid string value argument");
        }
        std::string curVal = ::getString(cellVal);
        value += (curVal + std::string(","));
      }
      if (value.size() > 0) {
        value = value.substr(0,value.size()-1);
      }
      msg.entries[0].is_array = true;
    }
    else {
      if (!mxIsChar(prhs[2])) {
        mexErrMsgTxt("BotParamClient: invalid string value argument");
      }
      value = ::getString(prhs[2]);
    }
    msg.entries[0].value = value;
    BotParamClient::instance().getLcm()->publish("PARAM_SET", &msg);
  }

  else if (command == "setnum") {
    if (!mxIsDouble(prhs[2]) || mxIsComplex(prhs[2])) {
      mexErrMsgTxt("BotParamClient: third argument must be real value array");
    }
    bot_param::set_t msg = constructSetMessage(param, key);
    std::string value;
    int len = mxGetNumberOfElements(prhs[2]);
    double* valArray = mxGetPr(prhs[2]);
    for (int i = 0; i < len; ++i) {
      std::ostringstream oss;
      oss << valArray[i] << ",";
      value += oss.str();
    }
    if (value.size() > 0) {
      value = value.substr(0,value.size()-1);
    }
    msg.entries[0].value = value;
    msg.entries[0].is_array = (len > 1);
    BotParamClient::instance().getLcm()->publish("PARAM_SET", &msg);
  }

  else if (command == "print") {
    bot_param_write(param, stderr);
  }

  else {
    mexErrMsgTxt("BotParamClient: command must be haskey, subkeys, getnum, getbool, getstr, or setstr");
  }
}
Ejemplo n.º 24
0
void PPDEVWrite(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    int numAddresses = 1, numVals = 8, i, j, result, addrStrLen, port;
    uint8_T *data;
    mxArray *tmp;
    uint64_T val;
    mxLogical *out;
    uint8_T bitNum, regOffset, value = 0;
    unsigned char mask[NUM_REGISTERS] = { 0 }, vals[NUM_REGISTERS] = { 0 }, pos;
    bool writing = true;

    /* Check number of input arguments */
    if (nrhs != 2) {
        mexErrMsgTxt("Exactly 2 arguments required.");
    }

    /* The device number and value must be noncomplex scalar double */
    for (i = 0; i < nrhs; i++) {
/*        if (!mxIsDouble(prhs[i]) || mxIsComplex(prhs[i]) || ! mxIsScalar(prhs[i])) { */
        if (!mxIsDouble(prhs[i]) || mxIsComplex(prhs[i]) || !(mxGetN(prhs[i])==1 && mxGetM(prhs[i])==1)) {
            mexErrMsgTxt("Arguments must be noncomplex scalar double.");
        }
    }

    /* Assign pointers to each input */
    port = *mxGetPr(prhs[0]);
    if (port < 1 || port > MAX_DEVS) {
        mexErrMsgTxt("Port number out of range.");
    }
    port--;
    val = *mxGetPr(prhs[1]);
    if (val < 0 || val > 255) {
        mexErrMsgTxt("Value out of range.");
    }

    /* Convert value to data/bit matrix */
    tmp = mxCreateNumericMatrix(8, 3, mxUINT8_CLASS, mxREAL);
    data = mxGetData(tmp);
    for (i = 0; i < numVals; i++) {
        data[i] = i;
        data[i + numVals] = 0;
        data[i + 2 * numVals] = getBit(val, i);
    }

    /* Assign pointers to each output */
    if (DEBUG) printf("%d lhs\n",nlhs);
    switch (nlhs) {
        case 1:
            *plhs = mxCreateLogicalMatrix(numVals,numAddresses);
            if (*plhs == NULL) {
                mexErrMsgTxt("couldn't allocate output");
            }
            out = mxGetLogicals(*plhs);
            break;
        case 0:
            if (!writing) {
                mexErrMsgTxt("exactly 1 output argument required when reading");
            }
            out = NULL;
            break;
        default:
            mexErrMsgTxt("at most 1 output argument allowed");
            break;
    }

    if (DEBUG) printf("\n\ndata:\n");

    /* Convert data matrix for use with doPort */
    for (i = 0; i < numVals; i++) {
        bitNum    = data[i          ];
        regOffset = data[i+  numVals];
        if (writing) {
            value = data[i+2*numVals];
        }

        if (DEBUG) {
            printf("\t%d, %d", bitNum, regOffset);
            if (writing) printf(" %d", value);
            printf("\n");
        }

        if (bitNum>7 || regOffset>2 || value>1) {
            mexErrMsgTxt("bitNum must be 0-7, regOffset must be 0-2, value must be 0-1.");
        }

        pos = 1<<bitNum;
        mask[regOffset] |= pos;
        if (value) vals[regOffset] |= pos;
    }

    if (DEBUG) {
        for (j = 0; j < NUM_REGISTERS; j++) {
            printf("mask:");
            printBits(mask[j]);
            if (writing) {
                printf(" val:");
                printBits(vals[j]);
            }
            printf("\n");
        }
    }

    i = 0;
    if (ppdev_table[port] != NULL) {
        doPort(ppdev_table[port]->addr, ppdev_table[port]->parportfd, mask, vals, out, i, data, numVals, writing);
    } else {
        mexErrMsgTxt("Port not open.");
    }

}
Ejemplo n.º 25
0
void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[] )
{
    mxLogical *image;
	mxLogical *path;
	bool *track;
	double *dist, *idx;
    mwSize w, h, size, sizeIdx, i, x, y;
	long conn;
	double inf = mxGetInf();
	double nan = mxGetNaN();
	
	if (nrhs < 2){
		mexErrMsgIdAndTxt( "Image:distgeodesic:wrongInputCount",
				"At least two input (image, startPoints) expected.");
	}
	if (nrhs > 3){
		mexErrMsgIdAndTxt( "Image:distgeodesic:wrongInputCount",
				"Too may input parameter.");
	}
	
	if (!mxIsDouble(prhs[1])){
		mexErrMsgIdAndTxt( "Image:distgeodesic:wrongInputType",
				"Index parameter has to be doubles.");
	}
	
	if (nrhs < 3 || mxGetM(prhs[2]) * mxGetN(prhs[2]) == 0){
		conn = 6;
	}
	else {
		if (mxIsDouble(prhs[2])){
			if (mxGetM(prhs[2]) * mxGetN(prhs[2]) == 1){
				conn = (long) mxGetPr(prhs[2])[0];
			}
			else {
				mexErrMsgIdAndTxt( "Image:distgeodesic:wrongInputType",
					"Connection parameter has to be a scalar.");
			}
		}
		else {
			mexErrMsgIdAndTxt( "Image:distgeodesic:wrongInputType",
				"Connection parameter has to be a double.");
		}
	}
	
    h = mxGetM(prhs[0]);
    w = mxGetN(prhs[0]);
	size = h*w;
	/* The input must be a logical.*/
	if (!mxIsLogical(prhs[0])){
        image = mxGetLogicals(mxCreateLogicalMatrix(h, w));
        double *data = mxGetPr(prhs[0]);
        for (int i = 0; i < size; i += 1){
			image[i] = (mxLogical) data[i];
        }
	}
    else {
        image = mxGetLogicals(prhs[0]);
    }
	
	/* Create matrix for the return argument. */
	plhs[0] = mxCreateDoubleMatrix(h, w, mxREAL);
	dist = mxGetPr(plhs[0]);
	
	track = new bool[size];
	
	for (i = 0; i < size; i += 1){
		track[i] = false;
		dist[i] = image[i]? inf: nan;
	}
	
	idx = mxGetPr(prhs[1]);
	sizeIdx = mxGetM(prhs[1]) * mxGetN(prhs[1]);
	bool any = false;
	for (i = 0; i < sizeIdx; i += 1){
		mwSize startIdx = (long) idx[i];
		if (idx[i] != startIdx){
			mexErrMsgIdAndTxt( "Image:distgeodesic:idxNotIntegral",
					"Startindex has to be an integral number.");
		}
		if (startIdx <= 0 || startIdx > size){
			mexErrMsgIdAndTxt( "Image:distgeodesic:idxOutOfRange",
					"Startindex is out of range.");
		}
		track[startIdx - 1] = true;
		dist[startIdx - 1] = 0;
		any = true;
	}
	
	bool *trackCopy = new bool[size];
	while (any){
		any = false;
		for (i = 0; i < size; i += 1){
			trackCopy[i] = track[i];
		}
		
		for (i = 0; i < size; i += 1){ // cityblock
			if (trackCopy[i]){
				track[i] = false;
				y = i % h;
				x = (i - y) / h;
				setDist(x - 1, y, i, 1);
				setDist(x + 1, y, i, 1);
				setDist(x, y - 1, i, 1);
				setDist(x, y + 1, i, 1);
			}
		}
		if (conn == 6){ // chessboard
			for (i = 0; i < size; i += 1){
				if (trackCopy[i]){
					y = i % h;
					x = (i - y) / h;
					setDist(x - 1, y - 1, i, 1);
					setDist(x - 1, y + 1, i, 1);
					setDist(x + 1, y - 1, i, 1);
					setDist(x + 1, y + 1, i, 1);
				}
			}
		}
		if (conn == 8){ // quasi-euclidean
			for (i = 0; i < size; i += 1){
				if (trackCopy[i]){
					y = i % h;
					x = (i - y) / h;
					setDist(x - 1, y - 1, i, M_SQRT2);
					setDist(x - 1, y + 1, i, M_SQRT2);
					setDist(x + 1, y - 1, i, M_SQRT2);
					setDist(x + 1, y + 1, i, M_SQRT2);
				}
			}
		}
	}
	
	delete[] trackCopy;
	trackCopy = nullptr;
	
	
	/* free memory */
	delete[] track;
	track = nullptr;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    bool				isError;
    thread_policy_flavor_t		flavorConstant;
    int					flavorPolicySize, flavorPolicySizeBytes, kernError;
    task_t				threadID;
    thread_policy_t			threadPolicy;
    mach_msg_type_number_t		policySizeFilled;
    boolean_t				isDefault, getDefault;
    char				commandString[COMMAND_STRING_LENGTH];
    // for return structure
    //  ...outer
    const char *outerNames[] = 		{"threadID", "flavor", "policy", "policySize", "policyFillSize", "getDefault", "isDefault"};
    int	numOuterDims=2, numOuterFields=7;
    int	outerDims[2]={1,1};
    //	...inner
    const char *standardNames[] = 	{"no_data"};
    int numInnerFieldsStandard=		1;
    /*
    const char *extendedNames[] = 	{"timeshare"};
    int numInnerFieldsExtended=		1;
    */
    const char *timeConstraintNames[]=	{"period", "computation", "constraint", "preemptible"};
    int numInnerFieldsTimeConstraint=	4;
    const char *precedenceNames[]=	{"imporantance"};
    int numInnerFieldsPrecedence=	1;
    int	numInnerDims=2;
    int	innerDims[2]={1,1};
    // ...both
    mxArray  *tempFieldValue, *innerStruct, *outerStruct;
    

    

    //get the policy flavor constant specified by the user and the getDefault argument
    if(nrhs<2 || nrhs > 2)
        mexErrMsgTxt("MachGetPriorityMex requires two input arguments.  See help MachGetPriorityMex.");
    if(!mxIsChar(prhs[0]))
        mexErrMsgTxt("First input argument is not a string.  See help MachGetPriorityMex.");
    mxGetString(prhs[0], commandString, COMMAND_STRING_LENGTH);
    isError=GetFlavorConstantFromFlavorString(commandString, mxGetM(prhs[0]) * mxGetN(prhs[0]), &flavorConstant);  //case sensitive.  
    if(isError)
        mexErrMsgTxt("Unrecognized command.  See help MachGetPriorityMex.");
    if(!(mxIsDouble(prhs[1]) || mxIsLogical(prhs[1])) || mxGetN(prhs[1]) * mxGetM(prhs[1]) != 1)
        mexErrMsgTxt("Second argument must be 1x1 logical or double value.  See help MachGetPriorityMex.");
    if(mxIsLogical(prhs[1]))
        getDefault= (boolean_t)mxGetLogicals(prhs[1])[0];
    if(mxIsDouble(prhs[1]))
        getDefault= (boolean_t)mxGetPr(prhs[1])[0];


    //read the priority settings
    switch(flavorConstant){
        case THREAD_STANDARD_POLICY: 
            flavorPolicySizeBytes=sizeof(thread_standard_policy_data_t); 
            flavorPolicySize=THREAD_STANDARD_POLICY_COUNT; 
            break;
        case THREAD_TIME_CONSTRAINT_POLICY: 
            flavorPolicySizeBytes=sizeof(thread_time_constraint_policy_data_t); 
            flavorPolicySize=THREAD_TIME_CONSTRAINT_POLICY_COUNT; 
            break;
        case THREAD_PRECEDENCE_POLICY: 
            flavorPolicySizeBytes=sizeof(thread_precedence_policy_data_t); 
            flavorPolicySize=THREAD_PRECEDENCE_POLICY_COUNT; 
            break;
    }
    threadPolicy=(thread_policy_t)malloc(flavorPolicySizeBytes);		
    threadID= mach_thread_self();
    policySizeFilled=flavorPolicySize;
    isDefault=getDefault;
    kernError=thread_policy_get(threadID, flavorConstant, threadPolicy, &policySizeFilled, &isDefault);

    //create and populate the return structure
    outerStruct= mxCreateStructArray(numOuterDims, outerDims, numOuterFields, outerNames);
    
    tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
    mxGetPr(tempFieldValue)[0]=(double)threadID;
    mxSetField(outerStruct, 0, "threadID", tempFieldValue);
    
    tempFieldValue= mxCreateString(commandString);
    mxSetField(outerStruct, 0, "flavor", tempFieldValue);
    
    
    switch(flavorConstant){
        case THREAD_STANDARD_POLICY: 				
            innerStruct= mxCreateStructArray(numInnerDims, innerDims, numInnerFieldsStandard, standardNames);
            tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
            mxGetPr(tempFieldValue)[0]= (double)((thread_standard_policy_t)threadPolicy)->no_data;
            mxSetField(innerStruct, 0, "no_data", tempFieldValue);
            break;
       /* THREAD_EXTENDED_POLICY is equal to THREAD_STANDARD_POLICY.  Also,  THREAD_EXTENDED_POLICY is undocumented.  So we ignore it.  
        case THREAD_EXTENDED_POLICY: 		
            innerStruct= mxCreateStructArray(numInnerDims, innerDims, numInnerFieldsExtended, extendedNames);
            tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
            mxGetPr(tempFieldValue)[0]= (double)((thread_extended_policy_t)threadPolicy)->timeshare;
            mxSetField(innerStruct, 1, "timeshare", tempFieldValue);
            break;
        */
        case THREAD_TIME_CONSTRAINT_POLICY: 	
            innerStruct= mxCreateStructArray(numInnerDims, innerDims, numInnerFieldsTimeConstraint, timeConstraintNames);
            tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
            mxGetPr(tempFieldValue)[0]= (double)((thread_time_constraint_policy_t)threadPolicy)->period;
            mxSetField(innerStruct, 0, "period", tempFieldValue);
            tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
            mxGetPr(tempFieldValue)[0]= (double)((thread_time_constraint_policy_t)threadPolicy)->computation;
            mxSetField(innerStruct, 0, "computation", tempFieldValue);
            tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
            mxGetPr(tempFieldValue)[0]= (double)((thread_time_constraint_policy_t)threadPolicy)->constraint;
            mxSetField(innerStruct, 0, "constraint", tempFieldValue);
            tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
            mxGetPr(tempFieldValue)[0]= (double)((thread_time_constraint_policy_t)threadPolicy)->preemptible;
            mxSetField(innerStruct, 0, "preemptible", tempFieldValue);
            break;
        case THREAD_PRECEDENCE_POLICY:
            innerStruct= mxCreateStructArray(numInnerDims, innerDims, numInnerFieldsPrecedence, precedenceNames);
            tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
            mxGetPr(tempFieldValue)[0]= (double)((thread_precedence_policy_t)threadPolicy)->importance;
            mxSetField(innerStruct, 0, "imporantance", tempFieldValue);
            break;
    }
    mxSetField(outerStruct,0, "policy", innerStruct);
    
    tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
    mxGetPr(tempFieldValue)[0]=flavorPolicySize;
    mxSetField(outerStruct, 0, "policySize", tempFieldValue);
    
    tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
    mxGetPr(tempFieldValue)[0]=policySizeFilled;
    mxSetField(outerStruct, 0, "policyFillSize", tempFieldValue);
    
    tempFieldValue= mxCreateLogicalMatrix(1, 1);
    mxGetLogicals(tempFieldValue)[0]=(bool)getDefault;
    mxSetField(outerStruct, 0, "getDefault", tempFieldValue);
    
    tempFieldValue= mxCreateLogicalMatrix(1, 1);
    mxGetLogicals(tempFieldValue)[0]=(bool)isDefault;
    mxSetField(outerStruct, 0, "isDefault", tempFieldValue);
    
    plhs[0]=outerStruct;
    
    free((void*)threadPolicy);        
}
Ejemplo n.º 27
0
// Main function
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
    
    // Macros for output
    #define ITER        plhs[0]
    #define TIN_OUT     plhs[1]
    #define HOLD        plhs[2]
    #define DHOLD       plhs[3]
    #define D2HOLD      plhs[4]
    
    // Macros for input
    #define ITERMAX     prhs[0]
    #define TIN_IN      prhs[1]
    #define WBREAK      prhs[2]
    #define WORDER      prhs[3]
    #define WCOEFS      prhs[4]
    #define UIN         prhs[5]
    #define N_IN        prhs[6]
    #define SLAMBDA     prhs[7]
    #define TQIN        prhs[8]
    #define SCRRNG      prhs[9]
    #define CRIT        prhs[10]
    
    mxArray *WMAT   = call_eval_fd(TIN_IN, WFDOBJ, 0);
    mxArray *DWMAT  = call_eval_fd(TIN_IN, WFDOBJ, 1);
    mxArray *D2WMAT = call_eval_fd(TIN_IN, WFDOBJ, 2);
    double *wmat    = mxGetPr(WMAT);
    double *dwmat   = mxGetPr(DWMAT);
    double *d2wmat  = mxGetPr(D2WMAT);
    
    double emat, pmat, rmat;
    
    double *uin     = mxGetPr(UIN);
    
    long M          = mxGetM(WMAT);
    long N          = mxGetN(WMAT);
    long Nsubjects  = (long)mxGetPr(N_IN)[0];
    
    double *tin     = mxGetPr(TIN_IN);
    double *tqin    = mxGetPr(TQIN);
    
    double slambda  = mxGetPr(SLAMBDA)[0];
    
    HOLD            = mxCreateDoubleMatrix(M, 1, mxREAL);
    double *hold    = mxGetPr(HOLD);
    DHOLD           = mxCreateDoubleMatrix(M, 1, mxREAL);
    double *dhold   = mxGetPr(DHOLD);
    D2HOLD          = mxCreateDoubleMatrix(M, 1, mxREAL);
    double *d2hold  = mxGetPr(D2HOLD);
    
    double crit     = mxGetPr(CRIT)[0];
    
    long *activenew = (long *)calloc(M, sizeof(long));
    long nactivenew = 0;
    
    // Compute initial probability values for interior scores
    long m, n;
    for (m = 0; m < M; m++) {
        
        // Initial function, first and second derivative values  wrt theta
        hold[m]     = 0.0;
        dhold[m]    = 0.0;
        d2hold[m]   = 0.0;
        
        for (n = 0; n < N; n++) {
            long index  = m + M*n;
            
            emat        = exp(wmat[index]);
            pmat        = emat/(1 + emat);
            rmat        = uin[index] - pmat;
            
            hold[m]    += uin[index] * wmat[index] - log(1 + emat);
            dhold[m]   += rmat * dwmat[index];
            d2hold[m]  += ( (-d2wmat[index] * rmat) +
                           (pow(dwmat[index], 2) * pmat * (1 - pmat)) );
        }
        hold[m]     = -hold[m]/Nsubjects;
        dhold[m]    = -dhold[m]/Nsubjects;
        d2hold[m]   = d2hold[m]/Nsubjects;
        
        if (slambda > 0) {
            hold[m]    += slambda * pow(tin[m] - tqin[m], 2) / Nsubjects;
            dhold[m]   += (2 * slambda * (tin[m] - tqin[m])) / Nsubjects;
            d2hold[m]  += 2 * slambda / Nsubjects;
        }
        
        // Find cases where gradient size exceeds criterion and further optimization is required
        
        if (fabs(dhold[m]) > crit && d2hold[m] > 0) {
            activenew[nactivenew++] = m + 1;
        }
    }
    
    mxArray *ACTIVE = mxCreateDoubleMatrix(nactivenew, 1, mxREAL);
    double *active  = mxGetPr(ACTIVE);
    
    mxArray *TINA   = mxCreateDoubleMatrix(nactivenew, 1, mxREAL);
    double *tina    = mxGetPr(TINA);
    
    mxArray *UA     = mxCreateDoubleMatrix(nactivenew, N, mxREAL);
    double *ua      = mxGetPr(UA);
    
    for (m = 0; m < nactivenew; m++) {
        
        active[m]   = activenew[m];
        
        // Select active theta's and data
        
        tina[m]     = tin[activenew[m] - 1];
        
        for (n = 0; n < N; n++) {
            long newindex   = m + nactivenew*n;
            long index      = activenew[m] - 1 + Nsubjects*n;
            
            ua[newindex]    = uin[index];
        }
    }
    
    free(activenew);
    
    // Update scores in active set by a Newton-Raphson step
    
    ITER            = mxCreateDoubleMatrix(1, 1, mxREAL);
    double *iter    = mxGetPr(ITER);
    iter[0]         = 0;
    long itermax    = (long)mxGetPr(ITERMAX)[0];
    
    nactivenew = 1;
    
    TIN_OUT         = TIN_IN;
    
    while (nactivenew > 0 && iter[0] < itermax) {
        iter[0]++;
        
        WMAT            = call_eval_fd(TINA, WFDOBJ, NULL);
        DWMAT           = call_eval_fd(TINA, WFDOBJ, 1);
        D2WMAT          = call_eval_fd(TINA, WFDOBJ, 2);
        wmat            = mxGetPr(WMAT);
        dwmat           = mxGetPr(DWMAT);
        d2wmat          = mxGetPr(D2WMAT);
        
        M               = mxGetM(WMAT);
        N               = mxGetN(WMAT);
        
        ua              = mxGetPr(UA);
        
        double *step    = (double *)malloc(M * sizeof(double));
        double stepnew;
        
        tina            = mxGetPr(TINA);
        tqin            = mxGetPr(TQIN);
        active          = mxGetPr(ACTIVE);
        
        double scrmin   = mxGetPr(SCRRNG)[0];
        double scrmax   = mxGetPr(SCRRNG)[1];
        
        tin             = mxGetPr(TIN_IN);
        
        mxArray *TINANEW    = mxCreateDoubleMatrix(M, 1, mxREAL);
        double *tinanew     = mxGetPr(TINANEW);
        double tinanewfail;
        
        double *hanew   = (double *)malloc(M * sizeof(double));
        
        // Initial set of thetas failing to meet criterion
        mxArray *FAILINDEX  = mxCreateLogicalMatrix(M, 1);
        mxLogical *fail     = mxGetLogicals(FAILINDEX);
        
        long m, n;
        for (m = 0; m < M; m++) {
            long activeindex = (long)active[m] - 1;
            
            dhold[activeindex]    = 0.0;
            d2hold[activeindex]   = 0.0;
            
            // Compute probabilites and other quantities for active cases
            for (n = 0; n < N; n++) {
                long index  = m + M*n;
                
                emat       = exp(wmat[index]);
                pmat       = emat/(1 + emat);
                rmat       = ua[index] - pmat;
                
                dhold[activeindex]   += rmat * dwmat[index];
                d2hold[activeindex]  += ( (-d2wmat[index] * rmat) +
                                         (pow(dwmat[index], 2) * pmat * (1 - pmat)) );
            }
            
            dhold[activeindex]    = -dhold[activeindex]/Nsubjects;
            d2hold[activeindex]   = d2hold[activeindex]/Nsubjects;
            
            // Add regularization terms if needed
            if (slambda > 0) {
                dhold[activeindex]  += (2 * slambda * (tina[m] - tqin[activeindex])) / Nsubjects;
                d2hold[activeindex] += 2 * slambda / Nsubjects;
            }
            
            // Find a step size that reduces all fuction values in the active set
            // Computer the negative of initial step for each theta
            step[m] = dhold[activeindex] / d2hold[activeindex];
            if (step[m] < -2) step[m] = -2;
            if (step[m] >  2) step[m] = 2;
            
            // Ensure that initial step does not go to boundaries
            if (tina[m] - step[m] <= scrmin) step[m] = tina[m] - scrmin * 1.01;
            if (tina[m] - step[m] >= scrmax) step[m] = -(scrmax * 0.99 - tina[m]);
            
            tinanew[m]  = tin[activeindex];
            hanew[m]    = hold[activeindex];
            fail[m]     = true;
        }
        
        // Half fac as required to achieve the reduction of all function values
        double fac      = 1;
        
        long loopfail   = 1;
        
        long stepiter   = 0;
        while (loopfail > 0 && stepiter < 10){
            
            stepiter++;
            loopfail    = 0;
            
            long m;
            
            for (m = 0; m < M; m++) {
                // Compute current step size
                if (fail[m]) {
                    stepnew     = fac * step[m];
                    tinanewfail = tinanew[m];
                    if (tinanewfail - stepnew <= scrmin) stepnew = tinanewfail - scrmin * 1.01;
                    if (tinanewfail - stepnew >= scrmax) stepnew = -(scrmax * 0.99 - tinanewfail);
                    
                    // Update ability values
                    tinanew[m] -= stepnew;
                }
            }
            
            // Compute function values
            WMAT    = call_eval_fd(TINANEW, WFDOBJ, NULL);
            wmat    = mxGetPr(WMAT);
            N       = mxGetN(WMAT);
            
            // Halve fac in preparation for next step size iteration
            fac    /= 2;
            
            long n;
            for (m = 0; m < M; m++){
                long activeindex = (long)active[m] - 1;
                
                if (fail[m]){
                    
                    hanew[m] = 0.0;
                    
                    for (n = 0; n < N; n++){
                        long index  = m + M*n;
                        
                        emat       = exp(wmat[index]);
                        // Current vector of function values
                        hanew[m]   -= (ua[index] * wmat[index] - log(1 + emat));
                    }
                    
                    hanew[m] /= Nsubjects;
                    
                    if (slambda > 0) {
                        hanew[m]   += slambda * pow(tinanew[m] - tqin[activeindex], 2) / Nsubjects;
                    }
                }
                
                fail[m] = hold[activeindex] - hanew[m] < -crit;
                
                if (fail[m]) loopfail++;
            }
        }
        
        WMAT           = call_eval_fd(TINANEW, WFDOBJ, NULL);
        DWMAT          = call_eval_fd(TINANEW, WFDOBJ, 1);
        D2WMAT         = call_eval_fd(TINANEW, WFDOBJ, 2);
        wmat   = mxGetPr(WMAT);
        dwmat  = mxGetPr(DWMAT);
        d2wmat = mxGetPr(D2WMAT);
        
        long *activenew = (long *)calloc(M, sizeof(long));
        nactivenew      = 0;
        
        // Either all function values reduced or 10 halvings completed
        // Save current function values
        
        for (m = 0; m < M; m++) {
            long activeindex = (long)active[m] - 1;
            
            tin[activeindex]    = tinanew[m];
            
            dhold[activeindex]    = 0.0;
            d2hold[activeindex]   = 0.0;
            
            for (n = 0; n < N; n++) {
                long index  = m + M*n;
                
                emat       = exp(wmat[index]);
                pmat       = emat/(1 + emat);
                rmat       = ua[index] - pmat;
                
                dhold[activeindex]   += rmat * dwmat[index];
                d2hold[activeindex]  += ( (-d2wmat[index] * rmat) +
                                         (pow(dwmat[index], 2) * pmat * (1 - pmat)) );
            }
            
            dhold[activeindex]    = -dhold[activeindex]/Nsubjects;
            d2hold[activeindex]   = d2hold[activeindex]/Nsubjects;
            
            if (slambda > 0) {
                dhold[activeindex]  += (2 * slambda * (tinanew[m] - tqin[activeindex])) / Nsubjects;
                d2hold[activeindex] += 2 * slambda / Nsubjects;
            }
            
            hold[activeindex]   = hanew[m];
            
            if (fabs(dhold[activeindex]) > crit && d2hold[activeindex] > 0) {
                activenew[nactivenew++] = (long)active[m];
            }
        }
        ACTIVE              = mxCreateDoubleMatrix(nactivenew, 1, mxREAL);
        double *activeout   = mxGetPr(ACTIVE);
        
        TINA                = mxCreateDoubleMatrix(nactivenew, 1, mxREAL);
        double *tina_out    = mxGetPr(TINA);
        
        UA                  = mxCreateDoubleMatrix(nactivenew, N, mxREAL);
        double *ua_out      = mxGetPr(UA);
        double *uin         = mxGetPr(UIN);
        
        for (m = 0; m < nactivenew; m++) {
            
            activeout[m]    = activenew[m];
            tina_out[m]     = tin[activenew[m] - 1];
            
            for (n = 0; n < N; n++) {
                long newindex       = m + nactivenew*n;
                long index          = activenew[m] - 1 + Nsubjects*n;
                
                ua_out[newindex]    = uin[index];
            }
        }
        
        free(hanew);
        free(step);
        free(activenew);
        
    }
    
    return;
    
}
Ejemplo n.º 28
0
void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[] )
{
	unsigned int maxHoleSize;
    mxLogical *image, *fillImage, *outImage;
    mwSize w, h;
	
	if (nrhs == 0){
		mexErrMsgIdAndTxt( "MATLAB:fill:wrongInputCount",
				"At least one input (image) expected.");
	}
	if (nrhs == 1 || mxGetM(prhs[1]) * mxGetN(prhs[1]) == 0){
		maxHoleSize = 5;
	}
	else {
		maxHoleSize = (unsigned int) mxGetScalar(prhs[1]);
	}
	
	if (nrhs > 2){
		mexErrMsgIdAndTxt( "MATLAB:fill:wrongInputCount",
				"Too many input parameter.");
	}
	
	/* The input must be a logical.*/
	if(!mxIsLogical(prhs[0])){
		mexErrMsgIdAndTxt( "MATLAB:fill:inputNotLogical",
				"Input must be a logical.");
	}

    h = mxGetM(prhs[0]);
    w = mxGetN(prhs[0]);
    image = mxGetLogicals(mxDuplicateArray(prhs[0]));

    /* Create matrix for the return argument. */
    plhs[0] = mxDuplicateArray(prhs[0]);
    outImage = mxGetLogicals(plhs[0]);
	
	fillImage = mxGetLogicals(mxCreateLogicalMatrix(h, w));
	
	for (unsigned int x = 0; x < w; x += 1){
        unsigned int x_ = x * h;
		for (unsigned int y = 0; y < h; y += 1){
            unsigned int idx = x_ + y;
			
			if (!image[idx]){
				unsigned int
					minX = w,
					maxX = 0,
					minY = h,
					maxY = 0,
					count = 0;
				
				floodFill(
					image, fillImage,
					w, h, x, y,
					minX, maxX, minY, maxY,
					count
				);
				
				if (count <= maxHoleSize){
					copyFilling(
						fillImage, outImage,
						w, h,
						minX, maxX, minY, maxY
					);
				}
				copyFilling(
					fillImage, image,
					w, h,
					minX, maxX, minY, maxY
				);
				clearFilling(
					fillImage,
					w, h,
					minX, maxX, minY, maxY
				);
			}
		}
	}
}
Ejemplo n.º 29
0
int bytesToMx(mxArray **mx, const char *byteArray, int byteArrayLength) {
    int ii;
    int nInfoBytes=0, nFreeBytes=0, nBytesRead=0;
    mxGramInfo info;
    
    // for complex types
    int jj;
    int nElements;
    mxArray *elementData;
    mxArray *elementName;
    const char *elementByteArray;
    int elementBytesRead;
    char **fieldNames;
    
    mxArray *callMatlabError;
    
    info.gramBytes = (char *)byteArray;
    nInfoBytes = readInfoFieldsFromBytes(&info, byteArray, byteArrayLength);
    nBytesRead += nInfoBytes;
    
    info.dataBytes = (char *)byteArray + MX_GRAM_OFFSET_DATA;
    nFreeBytes = byteArrayLength - MX_GRAM_OFFSET_DATA;
    
    //printMxGramInfo(&info);
    //printBytes(info.gramBytes, info.gramLength);
    
    if (info.gramType==mxGramDouble) {
        *mx = mxCreateDoubleMatrix(info.dataM, info.dataN, mxREAL);
        nBytesRead += readMxDoubleDataFromBytes(*mx, &info, nFreeBytes);
        
    } else if (info.gramType==mxGramChar) {
        mwSize dims[2];
        dims[0] = info.dataM;
        dims[1] = info.dataN;
        *mx = mxCreateCharArray(2, dims);
        nBytesRead += readMxCharDataFromBytes(*mx, &info, nFreeBytes);
        
    } else if (info.gramType==mxGramLogical) {
        *mx = mxCreateLogicalMatrix(info.dataM, info.dataN);
        nBytesRead += readMxLogicalDataFromBytes(*mx, &info, nFreeBytes);
        
    } else if (info.gramType==mxGramCell) {
        *mx = mxCreateCellMatrix(info.dataM, info.dataN);
        
        nElements = info.dataM * info.dataN;
        elementByteArray = info.dataBytes;
        elementBytesRead = 0;
        
        for (ii=0; ii<nElements; ii++) {
            elementBytesRead = bytesToMx(&elementData, elementByteArray, nFreeBytes);
            if (elementBytesRead > 0) {
                mxSetCell(*mx, ii, elementData);
                nBytesRead += elementBytesRead;
                elementByteArray += elementBytesRead;
                nFreeBytes -= elementBytesRead;
            }
        }
        
    } else if (info.gramType==mxGramStruct) {
        
        // struct arrays may have size 1xn, with m fields
        nElements = info.dataM;
        fieldNames = mxMalloc(nElements*sizeof(char*));
        
        // recur to read out fieldNames
        elementByteArray = info.dataBytes;
        for (ii=0; ii<nElements; ii++) {
            elementBytesRead = bytesToMx(&elementName, elementByteArray, nFreeBytes);
            fieldNames[ii] = mxArrayToString(elementName);
            mxDestroyArray(elementName);
            
            nBytesRead += elementBytesRead;
            elementByteArray += elementBytesRead;
            nFreeBytes -= elementBytesRead;
        }
        
        *mx = mxCreateStructMatrix(1, info.dataN, info.dataM, (const char **)fieldNames);
        mxFree(fieldNames);
        
        // recur to fill in field data
        for (ii=0; ii<nElements; ii++) {
            for (jj=0; jj<info.dataN; jj++) {
                elementBytesRead = bytesToMx(&elementData, elementByteArray, nFreeBytes);
                mxSetFieldByNumber(*mx, jj, ii, elementData);
                
                nBytesRead += elementBytesRead;
                elementByteArray += elementBytesRead;
                nFreeBytes -= elementBytesRead;
            }
        }
        
    } else if (info.gramType==mxGramFunctionHandle) {
        // recur to read out stringified version of function
        elementBytesRead = bytesToMx(&elementData, info.dataBytes, nFreeBytes);
        if (elementBytesRead > 0) {
            nBytesRead += elementBytesRead;
            callMatlabError = mexCallMATLABWithTrap(1, mx, 1, &elementData, MX_GRAM_STRING_TO_FUNCTION);
        }
        
    } else {
        *mx = mxCreateDoubleScalar(-1);
        nBytesRead = 0;
    }
    
    return(nBytesRead);
}
void mexFunction(int nlhs,	/* number of expected outputs */
	mxArray *plhs[],					/* mxArray output pointer array */
	int nrhs, 								/* number of inputs */
	const mxArray *prhs[]			/* mxArray input pointer array */)
{
	unsigned int nodes;
	unsigned int pairs;
	bool *labels;
	unsigned int *source;
	unsigned int *destination;
	float *pairValue;
	float *unaryValue;
	float *flow;
	mxArray *sourceMxArray;
	mxArray *destinationMxArray;
	mxArray *pairValueMxArray;

	/** Input validation */
	if( nrhs != 2 )
		mexErrMsgTxt( USAGE_NOTIFICATION );
	
	const mxArray *A = prhs[ 0 ];
	const mxArray *T = prhs[ 1 ];
	
	if( !mxIsStruct( A ) )
	{
		mexErrMsgTxt( USAGE_NOTIFICATION );
	}
	else
	{
		sourceMxArray = mxGetField( A, 0, "source" );
		destinationMxArray = mxGetField( A, 0, "destination" );
		pairValueMxArray = mxGetField( A, 0, "value" );
		
		if( mxGetClassID( sourceMxArray ) != mxUINT32_CLASS ||
			mxGetClassID( destinationMxArray ) != mxUINT32_CLASS ||
			mxGetClassID( pairValueMxArray ) != mxSINGLE_CLASS )
		{
			mexErrMsgTxt( USAGE_NOTIFICATION );
		}
		
		if( mxIsComplex( sourceMxArray ) ||
			mxIsComplex( destinationMxArray ) ||
			mxIsComplex( pairValueMxArray ) )
		{
			mexErrMsgTxt( USAGE_NOTIFICATION );
		}
		
		pairs = mxGetNumberOfElements( sourceMxArray );
		if( pairs != mxGetNumberOfElements( destinationMxArray ) ||
			pairs != mxGetNumberOfElements( pairValueMxArray ) )
		{
			mexErrMsgTxt( USAGE_NOTIFICATION );
		}
	}
	
	if( mxGetClassID( T ) != mxSINGLE_CLASS )
	{
		mexErrMsgTxt( USAGE_NOTIFICATION );
	}
	else
	{
		if( mxIsComplex( T ) )
			mexErrMsgTxt( USAGE_NOTIFICATION );
	
		nodes = mxGetM( T );

		if( mxGetN( T ) != 2 )
			mexErrMsgTxt( USAGE_NOTIFICATION );
	}
	/** End of input validation */
	
	source = ( unsigned int * )mxGetData( sourceMxArray );
	destination = ( unsigned int * )mxGetData( destinationMxArray );
	pairValue = ( float * )mxGetData( pairValueMxArray );
	unaryValue = ( float * )mxGetData( T );
	
	typedef Graph< float, float, float > GraphType;
	GraphType *graph = new GraphType( nodes, pairs );
	graph->add_node( nodes );
	
	/** Add pairwise potentials */
	for( int edge = 0; edge < pairs; edge++ )
		graph->add_edge( source[ edge ], destination[ edge ], pairValue[ edge ], pairValue[ edge ] );
	
	for( int node = 0; node < nodes; node++ )
		graph->add_tweights( node, unaryValue[ node ], unaryValue[ node + nodes ] );
	
	/** Create outputs */
	plhs[ 0 ] = mxCreateNumericMatrix( 1, 1, mxSINGLE_CLASS, mxREAL );
	plhs[ 1 ] = mxCreateLogicalMatrix( nodes, 1 );
	
	flow = ( float * )mxGetData( plhs[ 0 ] );
	labels = ( bool * )mxGetData( plhs[ 1 ] );
	
	*flow = graph->maxflow();
	
	for( int node = 0; node < nodes; node++ )
		labels[ node ] = graph->what_segment( node );
	
	delete graph;
}