Ejemplo n.º 1
0
static void mdlOutputs(SimStruct *S, int_T tid)
{
    PCONFIG_DATA config;
    const real_T         *in_xdot;
    const real_T         *in_x0;
    InputRealPtrsType    pin_params;
    const boolean_T      *in_reset;
    real_T               *out_x;
    real_T               *out_t;
    real_T               *xd;
    real_T               *xd_temp;
    real_T               *xd_temp2;
    time_T               initial_time;
    time_T               final_time;
    quaternion           phi;
    quaternion           q;
    vector               omegadot_temp;
    const real_T         *pomegadot;
    int_T                i;

    /* Retrieve C object from the pointers vector */
    config = ssGetPWork(S)[0];

    xd = (real_T*) ssGetDWork(S,0);
    xd_temp = (real_T*) ssGetDWork(S,1);
    if( config->nq ) xd_temp2 = (real_T*) ssGetDWork(S,2);
    in_xdot = ssGetInputPortRealSignal(S, config->idxin_xdot);
    if( config->idxin_x0 ) in_x0 = ssGetInputPortRealSignal(S, config->idxin_x0);
    if( config->idxin_params ) pin_params = ssGetInputPortRealSignalPtrs(S, config->idxin_params);
    if( config->idxin_reset ) in_reset = ((InputBooleanPtrsType) ssGetInputPortSignalPtrs(S, config->idxin_reset))[0];
    out_x = ssGetOutputPortRealSignal(S, 1);
    if( config->idxout_time ) out_t = ssGetOutputPortRealSignal(S, config->idxout_time);

    switch( intval(mxGetScalar(paramSpecificationsSource)) )
    {
        case 1:
            initial_time = config->initial_time;
            final_time   = ssGetTaskTime(S,0);
            break;
        case 2:
            initial_time = mxGetScalar(paramInitialTime);
            final_time   = mxGetScalar(paramFinalTime);
            break;
        case 3:
            initial_time = *(pin_params[0]);
            final_time   = *(pin_params[1]);
            break;
        default:
            ssSetErrorStatus(S,"Wrong integration algorithm selected");
            return;
    }
    
/*    ssPrintf("ti=%f, tf=%f\r\n", initial_time, final_time); */

    /* Reset the states */
    if( ssGetIWorkValue(S, 0) || (config->idxin_reset && *in_reset) || (intval(mxGetScalar(paramSpecificationsSource)) > 1) )
    {
        ssSetIWorkValue(S, 0, 0);
        if( intval(mxGetScalar(paramInitialConditionSource)) == 1 )
        {
            /* Internal initial conditions */
            for( i=0; i<config->nstates; i++ )
            {
                xd[i] = mxGetPr(paramInitialCondition)[(mxGetNumberOfElements(paramInitialCondition) == 1 ? 0 : i)];
            }
        }
        else
        {
            /* External initial conditions */
            memcpy(xd, in_x0, config->nstates*sizeof(real_T));
        }
        memcpy(out_x, xd, config->nstates*sizeof(real_T));
        if( config->idxout_time ) out_t[0] = initial_time;
    }
    
    if( final_time > initial_time )
    {
        if( intval(mxGetScalar(paramIntegrationAlgorithm)) == 1 )
        {
            /* Euler algorithm */
            if( !ssCallSystemWithTid(S,0,tid) ) return;
            i = 0;
            while( i<config->nstates )
            {
                if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
                {
                    pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
                    QuaternionIntegralEulerChange( final_time-initial_time, &in_xdot[i], pomegadot, &xd[i], phi );
                    QuaternionProd(&xd[i], phi, &xd[i]);
                    QuaternionNormalize(&xd[i]);
                    i += 4;
                }
                else
                {
                    xd[i] += in_xdot[i] * (final_time-initial_time);
                    i++;
                }
            }
        }
        
        if( intval(mxGetScalar(paramIntegrationAlgorithm)) == 2 )
        {
            /* Runge-Kutta algorithm */
            /* f1 */
            if( !ssCallSystemWithTid(S,0,tid) ) return;
            i = 0;
            while( i<config->nstates )
            {
                if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
                {
                    pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
                    omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
                    QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], &xd_temp[i] );
                    QuaternionProd(&xd_temp[i], &xd_temp[i], q);
                    QuaternionProd(&xd_temp[i], q, phi);
                    QuaternionProd(&xd[i], phi, &out_x[i]);
                    i += 4;
                }
                else
                {
                    xd_temp[i] = in_xdot[i];
                    out_x[i] = xd[i] + 0.5*(final_time-initial_time)*in_xdot[i];
                    i++;
                }
            }
            if( config->idxout_time ) out_t[0] = initial_time + 0.5*(final_time-initial_time);
            
            /* f2 */
            if( !ssCallSystemWithTid(S,0,tid) ) return;
            i = 0;
            while( i<config->nstates )
            {
                if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
                {
                    pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
                    omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
                    QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
                    QuaternionProd(q, q, phi);
                    QuaternionProd(&xd_temp[i], phi, &xd_temp[i]);
                    QuaternionProd(phi, q, phi);
                    QuaternionProd(&xd[i], phi, &out_x[i]);
                    i += 4;
                }
                else
                {
                    xd_temp[i] += 2*in_xdot[i];
                    out_x[i] = xd[i] + 0.5*(final_time-initial_time)*in_xdot[i];
                    i++;
                }
            }
            if( config->idxout_time ) out_t[0] = initial_time + 0.5*(final_time-initial_time);
            
            /* f3 */
            if( !ssCallSystemWithTid(S,0,tid) ) return;
            i = 0;
            while( i<config->nstates )
            {
                if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
                {
                    pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
                    omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
                    QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
                    QuaternionProd(q, q, phi);
                    QuaternionProd(&xd_temp[i], phi, &xd_temp[i]);
                    QuaternionProd(phi, q, phi);
                    QuaternionProd(phi, phi, phi);
                    QuaternionProd(&xd[i], phi, &out_x[i]);
                    i += 4;
                }
                else
                {
                    xd_temp[i] += 2*in_xdot[i];
                    out_x[i] = xd[i] + (final_time-initial_time)*in_xdot[i];
                    i++;
                }
            }
            if( config->idxout_time ) out_t[0] = final_time;
            
            /* f4 */
            if( !ssCallSystemWithTid(S,0,tid) ) return;
            i = 0;
            while( i<config->nstates )
            {
                if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
                {
                    pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
                    omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
                    QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
                    QuaternionProd(&xd_temp[i], q, &xd_temp[i]);
                    QuaternionProd(&xd[i], &xd_temp[i], &xd[i]);
                    QuaternionNormalize(&xd[i]);
                    i += 4;
                }
                else
                {
                    xd_temp[i] += in_xdot[i];
                    xd[i] += 1.0/6*(final_time-initial_time)*xd_temp[i];
                    i++;
                }
            }
        }
    }
    else
    {
        if( !ssCallSystemWithTid(S,0,tid) ) return;
    }

    config->initial_time = final_time;

    /* Update the outputs */
    memcpy(out_x, xd, config->nstates*sizeof(real_T));
    if( config->idxout_time ) out_t[0] = final_time;
}
Ejemplo n.º 2
0
void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[])
{

  if(strcmp(glp_version(),"4.36")<0){
	mexErrMsgTxt("This MEX interface is compatible only with GLPK version 4.36 or higher.");
	}

  if (nrhs != 9){
      mexPrintf("MEX interface to GLPK Version %s\n",glp_version());
      mexPrintf("Internal interface for the GNU GLPK library.\n");
      mexPrintf("You should use the 'glpk' function instead.\n\n");
      mexPrintf("SYNTAX: [xopt, fmin, status, extra] = glpk(c, a, b, lb, ub, ctype, vartype, sense, param)\n");
      return;
  }

  //-- 1nd Input. A column array containing the objective function
  //--            coefficients.
  int mrowsc = mxGetM(C_IN);

  double *c=mxGetPr(C_IN);
  if (c == NULL) mexErrMsgTxt("glpk: invalid value of C");


  //-- 2nd Input. A matrix containing the constraints coefficients.
  // If matrix A is NOT a sparse matrix
  double *A = mxGetPr(A_IN); // get the matrix
  if(A==NULL) mexErrMsgTxt("glpk: invalid value of A");

  int mrowsA = mxGetM(A_IN);

  int *rn;
  int *cn;
  double *a;
  int nz = 0;

  if(!mxIsSparse(A_IN)){
     rn=(int *)mxCalloc(mrowsA*mrowsc+1,sizeof(int));
     cn=(int *)mxCalloc(mrowsA*mrowsc+1,sizeof(int));
	   a=(double *)mxCalloc(mrowsA*mrowsc+1,sizeof(double));

     for (int i = 0; i < mrowsA; i++){
      for (int j = 0; j < mrowsc; j++){
	     if (A[i+j*mrowsA] != 0){
	      nz++;
	      rn[nz] = i + 1;
	      cn[nz] = j + 1;
	      a[nz] = A[i+j*mrowsA];
	    }
	   }
     }
  }else{
	  /* NOTE: nnz is the actual number of nonzeros and is stored as the
         last element of the jc array where the size of the jc array is the
         number of columns + 1 */
	  nz = *(mxGetJc(A_IN) + mrowsc);

      mwIndex *jc = mxGetJc(A_IN);
      mwIndex *ir = mxGetIr(A_IN);

      double *pr = mxGetPr(A_IN);

      rn=(int *)mxCalloc(nz+1,sizeof(int));
	  cn=(int *)mxCalloc(nz+1,sizeof(int));
	  a=(double *)mxCalloc(nz+1,sizeof(double));

      int nelc,count,row;
      count=0; row=0;
	    for(int i=1;i<=mrowsc;i++){
	      nelc=jc[i]-jc[i-1];
	      for(int j=0;j<nelc;j++){
		      count++;
		      rn[count]=ir[row]+1;
		      cn[count]=i;
		      a[count]=pr[row];
		      row++;
	      }
	    }
  }

  //-- 3rd Input. A column array containing the right-hand side value
  //	           for each constraint in the constraint matrix.
  double *b = mxGetPr(B_IN);

  if (b==NULL) mexErrMsgTxt("glpk: invalid value of b");


  //-- 4th Input. An array of length mrowsc containing the lower
  //--            bound on each of the variables.
  double *lb = mxGetPr(LB_IN);

  if (lb==NULL) mexErrMsgTxt("glpk: invalid value of lb");


  //-- LB argument, default: Free
  int *freeLB=(int *)mxCalloc(mrowsc,sizeof(int));
  for (int i = 0; i < mrowsc; i++) {
    if (lb[i]==-mxGetInf()){
      freeLB[i] = 1;
	 	}else freeLB[i] = 0;
  }

  //-- 5th Input. An array of at least length numcols containing the upper
  //--            bound on each of the variables.
  double *ub = mxGetPr(UB_IN);

  if (ub==NULL) mexErrMsgTxt("glpk: invalid value of ub");

  int *freeUB=(int *)mxCalloc(mrowsc,sizeof(int));
  for (int i = 0; i < mrowsc; i++)
  {
    if (ub[i]==mxGetInf())
		{
	     freeUB[i] = 1;
	  }else freeUB[i] = 0;
  }

  //-- 6th Input. A column array containing the sense of each constraint
  //--            in the constraint matrix.
  int size = mxGetNumberOfElements(CTYPE_IN) + 1;
  if (size==0) mexErrMsgTxt("glpk: invalid value of ctype");

  /* Allocate enough memory to hold the converted string. */
  char *ctype =(char *)mxCalloc(size, sizeof (char));

  /* Copy the string data from string_array_ptr and place it into buf. */
  if (mxGetString(CTYPE_IN, ctype, size) != 0)  mexErrMsgTxt("Could not convert string data.");


  //-- 7th Input. A column array containing the types of the variables.
  size = mxGetNumberOfElements(VARTYPE_IN)+1;

  char *vtype = (char *)mxCalloc(size, sizeof (char));
  int *vartype = (int *)mxCalloc(size, sizeof (int));

  if (size==0) mexErrMsgTxt("glpk: invalid value of vartype");

  // Copy the string data from string_array_ptr and place it into buf.
  if (mxGetString(VARTYPE_IN, vtype, size) != 0)
	  mexErrMsgTxt("Could not convert string data.");

  int isMIP = 0;
  for (int i = 0; i < mrowsc ; i++)
  {
    switch (vtype[i]){
      case 'I': vartype[i] = GLP_IV; isMIP = 1; break;
      case 'B': vartype[i] = GLP_BV; isMIP = 1; break;
      default: vartype[i] = GLP_CV;
    }
  }

  //-- 8th Input. Sense of optimization.
  int sense;

  double *tmp = mxGetPr(SENSE_IN);

  if (*tmp >= 0) sense = 1;
  else sense = -1;

  //-- 9th Input. A structure containing the control parameters.

  //-- ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //-- Integer parameters
  //-- ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  //-- Level of messages output by the solver
  GLPK_GET_INT_PARAM (PARAM, "msglev", glpIntParam[0]);
  if (glpIntParam[0] < 0 || glpIntParam[0] > 3)
    {
      mexErrMsgTxt("glpk: param.msglev must be 0 (no output [default]) or 1 (error messages only) or 2 (normal output) or 3 (full output)");
    }

  //-- scaling option
  GLPK_GET_INT_PARAM (PARAM, "scale", glpIntParam[1]);
  if (glpIntParam[1] < 0 || glpIntParam[1] > 4)
    {
      mexErrMsgTxt("glpk: param.scale must be 0 (no scaling) or 1 (equilibration scaling [default]) or 2 (geometric mean scaling) or 3 (geometric then equilibration scaling) or 4 (rounds scale factors to nearest power of 2)");
    }

  //-- Dual dimplex option
  GLPK_GET_INT_PARAM (PARAM, "dual", glpIntParam[2]);
  if (glpIntParam[2] < 0 || glpIntParam[2] > 2)
    {
      mexErrMsgTxt("glpk: param.dual must be 0 (do NOT use dual simplex [default]) or 1 (use dual simplex) or 2 (two phase dual simplex, switch to primal simplexi if it fails");
    }

  //-- Pricing option
  GLPK_GET_INT_PARAM (PARAM, "price", glpIntParam[3]);
  if (glpIntParam[3] < 0 || glpIntParam[3] > 1)
    {
      mexErrMsgTxt("glpk: param.price must be 0 (textbook pricing) or 1 (steepest edge pricing [default])");
    }

  //-- Ratio test option
  GLPK_GET_INT_PARAM (PARAM, "r_test", glpIntParam[20]);
  if (glpIntParam[20] < 0 || glpIntParam[20] > 1)
    {
      mexErrMsgTxt("glpk: param.r_test must be 0 (textbook) or 1 (Harris's Two pass ratio test)");
    }

  //-- Solution rounding option
  GLPK_GET_INT_PARAM (PARAM, "round", glpIntParam[4]);
  if (glpIntParam[4] < 0 || glpIntParam[4] > 1)
    {
      mexErrMsgTxt("glpk: param.round must be 0 (report all primal and dual values [default]) or 1 (replace tiny primal and dual values by exact zero)");
    }

  //-- Simplex iterations limit
  GLPK_GET_INT_PARAM (PARAM, "itlim", glpIntParam[5]);

  //-- Simplex iterations count
  GLPK_GET_INT_PARAM (PARAM, "itcnt", glpIntParam[6]);

  //-- Output frequency, in iterations
  GLPK_GET_INT_PARAM (PARAM, "outfrq", glpIntParam[7]);

  //-- Branching heuristic option
  GLPK_GET_INT_PARAM (PARAM, "branch", glpIntParam[14]);
  if (glpIntParam[14] < 0 || glpIntParam[14] > 3)
    {
      mexErrMsgTxt("glpk: param.branch must be (MIP only) 0 (branch on first variable) or 1 (branch on last variable) or 2 (most fractional variable)  or 3 (branch using a heuristic by Driebeck and Tomlin [default]");
    }

  //-- Backtracking heuristic option
  GLPK_GET_INT_PARAM (PARAM, "btrack", glpIntParam[15]);
  if (glpIntParam[15] < 0 || glpIntParam[15] > 3)
    {
      mexErrMsgTxt("glpk: param.btrack must be (MIP only) 0 (depth first search) or 1 (breadth first search) or 2 ( best local bound ) or 3 (backtrack using the best projection heuristic [default]");
    }

  //-- Presolver option
  GLPK_GET_INT_PARAM (PARAM, "presol", glpIntParam[16]);
  if (glpIntParam[16] < 0 || glpIntParam[16] > 1)
    {
      mexErrMsgTxt("glpk: param.presol must be 0 (do NOT use LP presolver) or 1 (use LP presolver [default])");
    }

  //-- Generating cuts
  GLPK_GET_INT_PARAM (PARAM, "usecuts", glpIntParam[17]);
  if (glpIntParam[17] < 0 || glpIntParam[17] > 5)
    {
      mexErrMsgTxt("glpk: param.usecuts must be 0 (do NOT generate cuts), 1 (generate Gomory's cuts [default]), 2 (mir), 3 (cov) or 4 (clq cuts), 5( all cuts)");
    }

 //-- PrePocessing
 GLPK_GET_INT_PARAM (PARAM, "pprocess", glpIntParam[18]);
  if (glpIntParam[18] < 0 || glpIntParam[18] > 2)
    {
      mexErrMsgTxt("glpk: param.pprocess must be 0 (disable preprocessing), 1 (preprocess root only) or 2 (preprocess all levels)");
    }

 //-- Binarize
 GLPK_GET_INT_PARAM (PARAM, "binarize", glpIntParam[19]);
  if (glpIntParam[19] < 0 || glpIntParam[19] > 1)
    {
      mexErrMsgTxt("glpk: param.binarize must be 0 (do use binarization) or 1 (replace general integer variable by binary ones)");
    }

  //-- LPsolver option
  int lpsolver = 1;
  GLPK_GET_INT_PARAM (PARAM, "lpsolver", lpsolver);
  if (lpsolver < 1 || lpsolver > 3)
    {
      mexErrMsgTxt("glpk: param.lpsolver must be 1 (simplex method) or 2 (interior point method) or 3 (LP in exact arithmetic)");
    }

  //-- Save option
  int save_pb = 0;
  char *save_filename = NULL;
  char *filetype = NULL;
  GLPK_GET_INT_PARAM (PARAM, "save", save_pb);
  save_pb = (save_pb != 0);
  if (save_pb){
    // -- Look for the name --
    mxArray *mxtmp=mxGetField(PARAM,0,"savefilename");
    if ( mxtmp != NULL ){
      int nl=mxGetNumberOfElements(mxtmp)+1;
      nl=nl+4; // increase size to consider then extension .xxx
      save_filename=(char *)mxCalloc(nl,sizeof(char));
      if (mxGetString(mxtmp, save_filename, nl) != 0)
        mexErrMsgTxt("glpk: Could not load file name to save.");
    }else{
      // Default file name
      save_filename= (char *)mxCalloc(9, sizeof(char));
      strcpy(save_filename,"outpb");
    }

    // -- Look for the type --
    char save_filetype[5];
    mxArray *txtmp=mxGetField(PARAM,0,"savefiletype");
    if ( txtmp != NULL ){
      int nl=mxGetNumberOfElements(txtmp)+1;
      filetype=(char *)mxCalloc(nl,sizeof(char));
      if (mxGetString(txtmp, filetype, nl) != 0)
        mexErrMsgTxt("glpk: Could not load file type.");
      if (!strcmp(filetype,"fixedmps") || !strcmp(filetype,"freemps")){
        strcpy(save_filetype,".mps");
      } else {
        if (!strcmp(filetype,"cplex")) strcpy(save_filetype,".lp");
        else {
          if (!strcmp(filetype,"plain")) strcpy(save_filetype,".txt");
        }
      }
    }else{
      filetype= (char *)mxCalloc(5, sizeof(char));
      strcpy(filetype,"cplex");
      strcpy(save_filetype,".lp"); // Default file type
    }
    strcat(save_filename,save_filetype); // name.extension
  }

  // MPS parameters
  //-- mpsinfo
  GLPK_GET_INT_PARAM (PARAM, "mpsinfo", glpIntParam[8]);
  //-- mpsobj
  GLPK_GET_INT_PARAM (PARAM, "mpsobj", glpIntParam[9]);
  if (glpIntParam[9] < 0 || glpIntParam[9] > 2)
  {
    mexErrMsgTxt("glpk: param.mpsobj must be 0 (never output objective function row) or 1 (always output objective function row ) or 2 [default](output objective function row if the problem has no free rows)");
  }
  //-- mpsorig
  GLPK_GET_INT_PARAM (PARAM, "mpsorig", glpIntParam[10]);
  //-- mpswide
  GLPK_GET_INT_PARAM (PARAM, "mpswide", glpIntParam[11]);
  //-- mpsfree
  GLPK_GET_INT_PARAM (PARAM, "mpsfree", glpIntParam[12]);



  //-- ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //-- Real parameters
  //-- ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  //-- Ratio test option
  GLPK_GET_REAL_PARAM (PARAM, "relax", 0);

  //-- Relative tolerance used to check if the current basic solution
  //-- is primal feasible
  GLPK_GET_REAL_PARAM (PARAM, "tolbnd", 1);

  //-- Absolute tolerance used to check if the current basic solution
  //-- is dual feasible
  GLPK_GET_REAL_PARAM (PARAM, "toldj", 2);

  //-- Relative tolerance used to choose eligible pivotal elements of
  //--	the simplex table in the ratio test
  GLPK_GET_REAL_PARAM (PARAM, "tolpiv", 3);

  GLPK_GET_REAL_PARAM (PARAM, "objll", 4);

  GLPK_GET_REAL_PARAM (PARAM, "objul", 5);

  GLPK_GET_REAL_PARAM (PARAM, "tmlim", 6);

  GLPK_GET_REAL_PARAM (PARAM, "outdly", 7);

  GLPK_GET_REAL_PARAM (PARAM, "tolint", 8);

  GLPK_GET_REAL_PARAM (PARAM, "tolobj", 9);

  GLPK_GET_REAL_PARAM (PARAM, "mipgap", 10);

  //-- Assign pointers to the output parameters
  const char **extranames=(const char **)mxCalloc(4,sizeof(*extranames));
  extranames[0]="lambda";
  extranames[1]="redcosts";
  extranames[2]="time";
  extranames[3]="memory";

  XMIN_OUT   = mxCreateDoubleMatrix(mrowsc, 1, mxREAL);
  FMIN_OUT   = mxCreateDoubleMatrix(1, 1, mxREAL);
  STATUS_OUT = mxCreateDoubleMatrix(1, 1, mxREAL);

  double *xmin   = mxGetPr(XMIN_OUT);
  double *fmin   = mxGetPr(FMIN_OUT);
  double *status = mxGetPr(STATUS_OUT);

  EXTRA_OUT  = mxCreateStructMatrix(1, 1, 4, extranames);
  mxArray *mxlambda   = mxCreateDoubleMatrix(mrowsA, 1, mxREAL);
  mxArray *mxredcosts = mxCreateDoubleMatrix(mrowsc, 1, mxREAL);
  mxArray *mxtime     = mxCreateDoubleMatrix(1, 1, mxREAL);
  mxArray *mxmem      = mxCreateDoubleMatrix(1, 1, mxREAL);

  double *lambda = mxGetPr(mxlambda);
  double *redcosts= mxGetPr(mxredcosts);
  double *time   = mxGetPr(mxtime);
  double *mem    = mxGetPr(mxmem);

  int jmpret = setjmp (mark);

  if (jmpret == 0)
    glpk (sense, mrowsc, mrowsA, c, nz, rn,
	       cn, a, b, ctype, freeLB, lb, freeUB,
	       ub, vartype, isMIP, lpsolver, save_pb, save_filename, filetype,
	       xmin, fmin, status, lambda,
	       redcosts, time, mem);

  if (! isMIP)
    {
      mxSetField(EXTRA_OUT,0,extranames[0],mxlambda);
      mxSetField(EXTRA_OUT,0,extranames[1],mxredcosts);
    }

  mxSetField(EXTRA_OUT,0,extranames[2],mxtime);
  mxSetField(EXTRA_OUT,0,extranames[3],mxmem);

  mxFree(rn);
  mxFree(cn);
  mxFree(a);
  mxFree(freeLB);
  mxFree(freeUB);
  mxFree(ctype);
  mxFree(vartype);
  mxFree(vtype);
  mxFree(extranames);
  mxFree(save_filename);
  mxFree(filetype);

  return;
}
void mexFunction( int nlhs, mxArray *plhs[],
        int nrhs, const mxArray*prhs[] )
{
    
    // [is_string,is_esc_char,is_quote] = parse_json_helper(string)
    
    //see mxmalloc for example ...
    
    char* string;
    size_t string_byte_length;
    mwSize string_length;
    mwSize index;
    mxLogical* is_string;
    mxLogical* is_esc_char;
    mxLogical* is_quote;
    bool in_string = false;
    
    
    //This seems wasteful, I thought I was working with the string
    //directly ... Might be able to improve this ...
    string_byte_length = mxGetNumberOfElements(prhs[0])*sizeof(mxChar)+1;
    string_length = (mwSize)mxGetNumberOfElements(prhs[0]);
    string        = mxMalloc(string_length);
    mxGetString(prhs[0],string,(mwSize)string_length);
    
    //NOTE: This initializes everything to false, we could eventually remove the initialization
    plhs[0] = mxCreateLogicalMatrix(1,string_length);
    is_string = mxGetLogicals(plhs[0]);
    plhs[1] = mxCreateLogicalMatrix(1,string_length);
    is_esc_char = mxGetLogicals(plhs[1]);
    plhs[2] = mxCreateLogicalMatrix(1,string_length);
    is_quote = mxGetLogicals(plhs[2]);
    
    //TODO: Add check on empty string and return ...
    
    //NOTE: We assume the first character is not a quote
    //as this would be invalid JSON
    
    //is_string[0]   = false;
    //is_esc_char[0] = false;
    //is_quote[0]    = false;
    
    /* Copy data into the mxArray */
    for ( index = 1; index < string_length; index++ ) {
        if (!is_esc_char[index-1])
        {
            if (string[index] == '"')
            {      //'"'
                is_quote[index] = true;
                in_string = !in_string;
            }
            else if (string[index] == '\\') //'\'
            {   
                is_esc_char[index] = true;
                is_string[index]   = in_string;
            }
            else 
            {
                is_string[index] = in_string;
            }
        } 
        else 
        {
            is_string[index] = in_string;
        }
    }
    
    mxFree(string);
    
}
Ejemplo n.º 4
0
int main(int ac, char *av[])
{
    
    /* Input parameters:
     *
     * iterations: Number of points to draw in the triangle
     * draw: If true, draw the triangle in a figure window before returning.
     */
    mxArray *iterations = NULL, *draw = NULL;

    /* The Sierpinski function returns the X and Y coordinates of the points
     * forming the pattern in the triangle.
     */
    mxArray *x = NULL, *y = NULL;

    /* Default number of iterations */
    int num_points = 7500;

    /* Validate the number of inputs */
    if (ac < 1 || ac > 2)
    {
	fprintf(stderr, "Expecting 0 or 1 input(s). Found %d\n", ac);
	usage(av[0]);
    }

    /* If we have the right number of inputs (1), try to convert the input
     * string to an integer.
     */
    if (ac == 2)
        num_points = atoi(av[1]);

    /* Type check on input argument -- atoi() will fail if the input is
     * not an integer.
     */
    if (num_points == 0)
    {
	fprintf(stderr, "First argument must be an integer.\n");
	usage(av[0]);
    }

    /* Create the input data */
    iterations = mxCreateDoubleScalar(num_points);
    draw = mxCreateLogicalScalar(1);
    
    /* Call the library intialization routine and make sure that the
     * library was initialized properly
     */
    mclInitializeApplication(NULL,0);
    if (!libtriangleInitialize())
    {
        fprintf(stderr,"could not initialize the triangle library properly\n");
        return -1;
    }

    /* Call the library function */
    mlfSierpinski(2, &x, &y, iterations, draw);

    /* Display the return value of the library function */
    printf("Calculated %d points\n", mxGetNumberOfElements(x));

    /* Free the memory used by the return values. */
    mxDestroyArray(x); x=NULL;
    mxDestroyArray(y); y=NULL;

    /* Call the library termination routine */
    libtriangleTerminate();
    
    /* Free the memory used by the input variables */
    mxDestroyArray(iterations); iterations=0;
    mxDestroyArray(draw); draw = 0;

    /* Shut down all MCR instances */
    mclTerminateApplication();

    /* Success */
    return 0;
}
Ejemplo n.º 5
0
// Function definitions. 
// -----------------------------------------------------------------
void mexFunction (int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) 
{
    //Input Args
    double *x0;          

    //Outputs Args
    double *x, *fval, *exitflag, *iter, *feval;
    
    //Internal Vars
    size_t ndec;  
    int printLevel = 0;
    
    //M1QN3 Vars
    int m = 5, n, indic, ndz;
    int uiparm[2] = {1,0};  //user integer array [citer, printLevel]
    double *g, dxmin = 1e-8, df1, epsg = 1e-6, *dz;
    char *normtype = "dfn";
    int impres = 0, io = 0, omode = 0, reverse = 0;
    int imode[3] = {0,0,0}; //DIS, cold start, no SIMUL with indic = 1
    int iz[5];
    float *rzs = NULL; double *dzs = NULL; //dummy args
    //Defaults
    int maxfev = 1500;
    int maxiter = 1000;
    maxtime = 1000;
    iterF.enabled = false;

    if (nrhs < 1) {
        if(nlhs < 1)
            printSolverInfo();
        else
            plhs[0] = mxCreateString(M1QN3_VERSION);
            
        return;
    }

    //Check user inputs
    checkInputs(prhs,nrhs);

    //Get Sizes
    ndec = mxGetNumberOfElements(pX0);
    //Get Objective Function Handle
    if (mxIsChar(pFUN)) {
        CHECK(mxGetString(pFUN, fun.f, FLEN) == 0,"error reading objective name string");
        fun.nrhs = 1;
        fun.xrhs = 0;
    } else {
        fun.prhs[0] = (mxArray*)pFUN;
        strcpy(fun.f, "feval");
        fun.nrhs = 2;
        fun.xrhs = 1;
    }
    fun.prhs[fun.xrhs] = mxCreateDoubleMatrix(ndec, 1, mxREAL); //x0
    //Get Gradient Function Handle 
    if (mxIsChar(pGRAD)) {
        CHECK(mxGetString(pGRAD, fun.g, FLEN) == 0,"error reading gradient name string");
        fun.nrhs_g = 1;
        fun.xrhs_g = 0;
    } else {
        fun.prhs_g[0] = (mxArray*)pGRAD;
        strcpy(fun.g, "feval");
        fun.nrhs_g = 2;
        fun.xrhs_g = 1;
    }   
    fun.prhs_g[fun.xrhs_g] = mxCreateDoubleMatrix(ndec, 1, mxREAL); //x0   

    //Get x0
    x0 = mxGetPr(pX0);
    
    //Get Options if specified
    if(nrhs > eOPTS) {
        if(mxGetField(pOPTS,0,"display"))
            printLevel = (int)*mxGetPr(mxGetField(pOPTS,0,"display"));
        if(mxGetField(pOPTS,0,"maxfeval"))
            maxfev = (int)*mxGetPr(mxGetField(pOPTS,0,"maxfeval"));
        if(mxGetField(pOPTS,0,"maxiter"))
            maxiter = (int)*mxGetPr(mxGetField(pOPTS,0,"maxiter"));
        if(mxGetField(pOPTS,0,"maxtime"))
            maxtime = *mxGetPr(mxGetField(pOPTS,0,"maxtime"));
        if(mxGetField(pOPTS,0,"tolafun"))
            epsg = *mxGetPr(mxGetField(pOPTS,0,"tolafun")); //not function tolerance (gradient)
        if(mxGetField(pOPTS,0,"nupdates"))
            m = (int)*mxGetPr(mxGetField(pOPTS,0,"nupdates")); //number of l-bfgs updates
        if(mxGetField(pOPTS,0,"iterfun") && !mxIsEmpty(mxGetField(pOPTS,0,"iterfun")))
        {
            iterF.prhs[0] = (mxArray*)mxGetField(pOPTS,0,"iterfun");
            strcpy(iterF.f, "feval");
            iterF.enabled = true;  
            iterF.prhs[1] = mxCreateNumericMatrix(1,1,mxINT32_CLASS,mxREAL);
            iterF.prhs[2] = mxCreateDoubleMatrix(1,1,mxREAL);
            iterF.prhs[3] = mxCreateDoubleMatrix(ndec,1,mxREAL);
        }
    }       
    
    //Create Outputs
    plhs[0] = mxCreateDoubleMatrix(ndec,1, mxREAL);
    plhs[1] = mxCreateDoubleMatrix(1,1, mxREAL);
    plhs[2] = mxCreateDoubleMatrix(1,1, mxREAL);
    plhs[3] = mxCreateDoubleMatrix(1,1, mxREAL);
    plhs[4] = mxCreateDoubleMatrix(1,1, mxREAL);
    x = mxGetPr(plhs[0]); 
    fval = mxGetPr(plhs[1]); 
    exitflag = mxGetPr(plhs[2]);    
    iter = mxGetPr(plhs[3]);
    feval = mxGetPr(plhs[4]);
    
    //Copy initial guess to x
    memcpy(x,x0,ndec*sizeof(double));
    
    //Print Header
    if(printLevel) {
        mexPrintf("\n------------------------------------------------------------------\n");
        mexPrintf(" This is M1QN3 v%s\n",M1QN3_VERSION);  
        mexPrintf(" Authors: Jean Charles Gilbert, Claude Lemarechal, INRIA\n MEX Interface J. Currie 2012\n\n");
        mexPrintf(" Problem Properties:\n");
        mexPrintf(" # Decision Variables:     %4d\n",ndec);

        mexPrintf("------------------------------------------------------------------\n");
    }
    
    //Assign Arguments
    n = (int)ndec;
    indic = 4;
    g = (double*)mxCalloc(n,sizeof(double)); //allocate memory for gradient
    ndz = 4*n + m*(2*n + 1);
    dz = (double*)mxCalloc(ndz,sizeof(double));
    
    //Start timer
    start = clock();
    
    //Initialization Call
    SIMUL(&indic, &n, x, fval, g, uiparm, NULL, NULL);    
    //Set df1 (initial estiamte of f reduction)
    df1 = *fval;
    
    //MEX Options
    uiparm[0] = 1;
    uiparm[1] = printLevel;
    
    //Call Algorithm
    M1QN3(SIMUL,EUCLID,CTONBE,CTCABE,&n,x,fval,g,&dxmin,&df1,&epsg,normtype,
          &impres,&io,imode,&omode,&maxiter,&maxfev,iz,dz,&ndz,&reverse,&indic,
          uiparm,rzs,dzs);

    //Save Status & Iterations
    *exitflag = (double)omode;
    *iter = maxiter;
    *feval = maxfev;
    
    //Check if maxtime exceeded
    if(((double)(end-start))/CLOCKS_PER_SEC > maxtime)
        *exitflag = 8;
    
    //Print Header
    if(printLevel){            
        //Termination Detected
        switch((int)*exitflag)
        {
            //Success
            case 1:
                mexPrintf("\n *** SUCCESSFUL TERMINATION ***\n *** gradient convergence |gk|/|g1| < epsg ***\n"); break;
            //Error
            case 5:
                mexPrintf("\n *** MAXIMUM FUNCTION EVALUATIONS REACHED ***\n"); break;
            case 4:
                mexPrintf("\n *** MAXIMUM ITERATIONS REACHED ***\n"); break;                       
            case 8:
                mexPrintf("\n *** MAXIMUM TIME REACHED ***\n"); break;  
            case 2:
                mexPrintf("\n *** ERROR: one of the input arguments is not well initialized ***\n"); break;
            case 3:
                mexPrintf("\n *** ERROR: the line-search is blocked on tmax = 10^20 ***\n"); break;    
            case 6:
                mexPrintf("\n *** ERROR: stop dxmin during the line-search ***\n"); break;       
            case 7:
                mexPrintf("\n *** ERROR: either (g,d) is nonnegative or (y,s) is nonpositive ***\n"); break;
            //Early Exit
            case 0:
                mexPrintf("\n *** TERMINATION: USER EXITED ***\n"); break;
            //Other Error
            default:
                mexPrintf("\n *** ERROR: internal error code %d ***\n",omode); break;
        }
        
        if(*exitflag==1)
            mexPrintf("\n Final fval: %12.5g\n In %3.0f iterations\n",*fval,*iter);

        mexPrintf("------------------------------------------------------------------\n\n");
    }
    
    //Free Memory
    mxFree(g);
    mxFree(dz);
}
Ejemplo n.º 6
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

	/* Check number of input arguments */
	if (nrhs > 13) {
		ERROR("Thirteen or fewer input arguments are required.");
    } else if (nrhs < 5) {
		ERROR("At least five input arguments are required.");
    }

	/* Check number of output arguments */
	if (nlhs > 1) {
		ERROR("Too many output arguments.");
    }

	INT numPoints = (INT) * (DOUBLE *) mxGetData(prhs[0]);
	CHAR distLabelName;
	if (mxGetNumberOfElements(prhs[1]) != 0) {
		if (!mxIsChar(prhs[1])) {
			ERROR("Second argument must be of type CHAR.");
		}
		distLabelName = (CHAR)*(CHAR*) mxGetData(prhs[1]);
	} else {
		distLabelName = 'R';
	}
	DOUBLE *matlabConstraintMat = (DOUBLE*) mxGetData(prhs[2]);
	DOUBLE *weights = (DOUBLE*) mxGetData(prhs[3]);
	DOUBLE *tau = (DOUBLE*) mxGetData(prhs[4]);

	INT numConstraints = (INT) mxGetM(prhs[2]);
	INT numRepeats = (INT) mxGetNumberOfElements(prhs[4]);

	DOUBLE tolerance;
	if ((nrhs >= 6) && (mxGetNumberOfElements(prhs[5]) != 0)) {
		tolerance = *(DOUBLE*) mxGetData(prhs[5]);
	} else {
		tolerance = 0.00001;
	}

	DOUBLE delta;
	if ((nrhs >= 7) && (mxGetNumberOfElements(prhs[6]) != 0)) {
		delta = *(DOUBLE*) mxGetData(prhs[6]);
	} else {
		delta = 0.1;
	}

	INT numIters;
	if ((nrhs >= 8) && (mxGetNumberOfElements(prhs[7]) != 0)) {
		numIters = (INT)*(DOUBLE*) mxGetData(prhs[7]);
	} else {
		numIters = 50000;
	}

	INT rankIncrement;
	if ((nrhs >= 9) && (mxGetNumberOfElements(prhs[8]) != 0)) {
		rankIncrement = (INT)*(DOUBLE*) mxGetData(prhs[8]);
	} else {
		rankIncrement = - 1;
	}

	INT lineSearchFlag;
	if ((nrhs >= 10) && (mxGetNumberOfElements(prhs[9]) != 0)) {
		lineSearchFlag = (INT)*(DOUBLE*) mxGetData(prhs[9]);
	} else {
		lineSearchFlag = 0;
	}

	DOUBLE eta;
	if ((nrhs >= 11) && (mxGetNumberOfElements(prhs[10]) != 0)) {
		eta = *(DOUBLE*) mxGetData(prhs[10]);
	} else {
		eta = 1.1;
	}

	DOUBLE tauMultiplier;
	if ((nrhs >= 12) && (mxGetNumberOfElements(prhs[11]) != 0)) {
		tauMultiplier = *(DOUBLE*) mxGetData(prhs[11]);
	} else {
		tauMultiplier = 10000;
	}

	DOUBLE tauRate;
	if ((nrhs >= 13) && (mxGetNumberOfElements(prhs[12]) != 0)) {
		tauRate = *(DOUBLE*) mxGetData(prhs[12]);
	} else {
		tauRate = 0.9;
	}

	if ((mxGetNumberOfElements(prhs[3]) != 0) && \
		(mxGetNumberOfElements(prhs[3]) != numConstraints)) {
		ERROR("First dimension of vector weights does not match number of constraints (first dimension of constraint matrix).");
	}

	INT *constraintMat;
	DOUBLE *betaVec;
	DIST_LABEL_TYPE distLabelType = convertDistLabelName(distLabelName);
	if (distLabelType == DIST_LABEL_TARGETS) {
		if (mxGetN(prhs[2]) != 3) {
			ERROR("Constraint matrix does not meet specified format: second dimension not equal to three.");
		}
		constraintMat = (INT *) MALLOC(numConstraints * 2 * sizeof(INT));
		betaVec = (DOUBLE *) MALLOC(numConstraints * 1 * sizeof(DOUBLE));
	} else if (distLabelType == DIST_LABEL_BOUNDS) {
		if (mxGetN(prhs[2]) != 4) {
			ERROR("Constraint matrix does not meet specified format: second dimension not equal to four.");
		}
		constraintMat = (INT *) MALLOC(numConstraints * 3 * sizeof(INT));
		betaVec = (DOUBLE *) MALLOC(numConstraints * 1 * sizeof(DOUBLE));
	} else if ((distLabelType == DIST_LABEL_RELATIONAL) \
				|| (distLabelType == DIST_LABEL_SQRHINGE) \
				|| (distLabelType == DIST_LABEL_HUBERHINGE)) {
		if (mxGetN(prhs[2]) != 5) {
			ERROR("Constraint matrix does not meet specified format: second dimension not equal to five.");
		}
		constraintMat = (INT *) MALLOC(numConstraints * 4 * sizeof(INT));
		betaVec = (DOUBLE *) MALLOC(numConstraints * 1 * sizeof(DOUBLE));
	}
	convertDistanceLabelMat(constraintMat, betaVec, distLabelType, \
					matlabConstraintMat, numConstraints, numPoints);

	plhs[0] = mxCreateNumericMatrix(numPoints, numPoints, MXPRECISION_CLASS, mxREAL);
	DOUBLE *K = (DOUBLE *) mxGetData(plhs[0]);

	if (numRepeats > 1) {
		nrkl_apg(K, distLabelType, constraintMat, betaVec, weights, tau, delta, \
				numIters, tolerance, lineSearchFlag, eta, numPoints, \
				numConstraints, numRepeats);
	} else {
		nrkl_apg_continuation(K, distLabelType, constraintMat, betaVec, weights, \
				*tau, delta, numIters, tolerance, lineSearchFlag, eta, \
				tauMultiplier, tauRate, numPoints, numConstraints);
	}

	FREE(constraintMat);
	FREE(betaVec);
}
Ejemplo n.º 7
0
// Function definitions. 
// -----------------------------------------------------------------
void mexFunction (int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) 
{
    //Input Args         
    double *x0 = NULL, *lb = NULL, *ub = NULL, *A = NULL, *b = NULL;  
    
    //Outputs Args
    double *x, *fval, *exitflag, *iter, *feval;
    
    //Internal Vars
    double *llb, *lub;
    size_t ndec;   
    int i, exit_code;    
    
    //PSwarm Vars
    double *sol = NULL;
    int lincon = 0;

    if (nrhs < 1) {
        if(nlhs < 1)
            printSolverInfo();
        else
            plhs[0] = mxCreateString(PSWARM_VERSION);
        return;
    }
    
    //Check user inputs
    checkInputs(prhs,nrhs,&lincon);
    
    //Set Defaults    
    printLevel = 0;
    maxtime = 1000;
    noFeval = 0;
    ctrlCExit = false;
    iterF.enabled = false;
    //Reset Stats
    stats.solveriters = 0;
    stats.objfunctions = 0;
    stats.pollsteps = 0;
    stats.sucpollsteps = 0;
    //Set PSwarm Defaults
    opt.s = 42;
    opt.mu = 0.5; opt.nu = 0.5;
    opt.maxvfactor = 0.5; opt.maxiter = 1500;
    opt.maxf = 10000;       
    opt.iweight = 0.9; opt.fweight = 0.4;
    opt.n2grd = 0.5;
    opt.blim = 10;
    opt.tol = 1e-5;
    opt.delta = Inf; opt.fdelta = 5.0; opt.idelta = 2.0; opt.ddelta = 0.5;
    opt.pollbasis = 0; opt.EpsilonActive = 0.1;
    opt.IPrint = 10;    
    opt.vectorized = 0;     //important, otherwise will pass whole swarm
    opt.outfcn = &iterfcn;  //iteration + ctrl c

    //Get Sizes
    ndec = mxGetNumberOfElements(prhs[1]);
    //Get Objective Function Handle
    if (mxIsChar(prhs[0])) {
        CHECK(mxGetString(prhs[0], fun.f, FLEN) == 0,"error reading objective name string");
        fun.nrhs = 1;
        fun.xrhs = 0;
    } else {
        fun.prhs[0] = (mxArray*)prhs[0];
        strcpy(fun.f, "feval");
        fun.nrhs = 2;
        fun.xrhs = 1;
    }    

    //Get x0
    x0 = mxGetPr(prhs[1]);   
    
    //Get Bounds
    //LB
    if(!mxIsEmpty(prhs[2])){
        llb = mxGetPr(prhs[2]);
        lb = mxCalloc(ndec,sizeof(double));
        memcpy(lb,llb,ndec*sizeof(double));
        for(i=0;i<ndec;i++) {
            if(mxIsInf(lb[i]))
                lb[i] = -1e19;
        }
    }
    else {
        lb = mxCalloc(ndec,sizeof(double));
        for(i=0;i<ndec;i++)
            lb[i] = -1e19;
    }
    //UB
    if(nrhs > 3 && !mxIsEmpty(prhs[3])){
        lub = mxGetPr(prhs[3]);
        ub = mxCalloc(ndec,sizeof(double));
        memcpy(ub,lub,ndec*sizeof(double));
        for(i=0;i<ndec;i++) {
            if(mxIsInf(ub[i]))
                ub[i] = 1e19;
        }
    }
    else {
        ub = mxCalloc(ndec,sizeof(double));
        for(i=0;i<ndec;i++)
            ub[i] = 1e19;
    }
    
    //Get Linear Inequality Constraints
    if(nrhs > 4) {
        if(!mxIsEmpty(prhs[4]) && !mxIsEmpty(prhs[5])) {
            A = mxGetPr(prhs[4]);
            b = mxGetPr(prhs[5]);
        }
    }
    
    //Get Options if specified
    if(nrhs > 6) {
        if(mxGetField(prhs[6],0,"display"))
            printLevel = (int)*mxGetPr(mxGetField(prhs[6],0,"display"));
        if(mxGetField(prhs[6],0,"maxiter"))
            opt.maxiter = (int)*mxGetPr(mxGetField(prhs[6],0,"maxiter"));
        if(mxGetField(prhs[6],0,"maxtime"))
            maxtime = *mxGetPr(mxGetField(prhs[6],0,"maxtime"));
        if(mxGetField(prhs[6],0,"tolfun"))
            opt.tol = *mxGetPr(mxGetField(prhs[6],0,"tolfun"));
        if(mxGetField(prhs[6],0,"maxfeval"))
            opt.maxf = (int)*mxGetPr(mxGetField(prhs[6],0,"maxfeval"));
        if(mxGetField(prhs[6],0,"swarm_size"))
            opt.s = (int)*mxGetPr(mxGetField(prhs[6],0,"swarm_size"));
        if(mxGetField(prhs[6],0,"vectorized"))
            opt.vectorized = (int)*mxGetPr(mxGetField(prhs[6],0,"vectorized"));
        if(mxGetField(prhs[6],0,"mu"))
            opt.mu = *mxGetPr(mxGetField(prhs[6],0,"mu"));
        if(mxGetField(prhs[6],0,"nu"))
            opt.nu = *mxGetPr(mxGetField(prhs[6],0,"nu"));
        if(mxGetField(prhs[6],0,"iweight"))
            opt.iweight = *mxGetPr(mxGetField(prhs[6],0,"iweight"));
        if(mxGetField(prhs[6],0,"fweight"))
            opt.fweight = *mxGetPr(mxGetField(prhs[6],0,"fweight"));
        if(mxGetField(prhs[6],0,"delta"))
            opt.delta = *mxGetPr(mxGetField(prhs[6],0,"delta"));
        if(mxGetField(prhs[6],0,"idelta"))
            opt.idelta = *mxGetPr(mxGetField(prhs[6],0,"idelta"));
        if(mxGetField(prhs[6],0,"ddelta"))
            opt.ddelta = *mxGetPr(mxGetField(prhs[6],0,"ddelta"));
        if(mxGetField(prhs[6],0,"iterfun") && !mxIsEmpty(mxGetField(prhs[6],0,"iterfun")))
        {
            iterF.prhs[0] = (mxArray*)mxGetField(prhs[6],0,"iterfun");
            strcpy(iterF.f, "feval");
            iterF.enabled = true;  
            iterF.prhs[1] = mxCreateNumericMatrix(1,1,mxINT32_CLASS,mxREAL);
            iterF.prhs[2] = mxCreateDoubleMatrix(1,1,mxREAL);
            iterF.prhs[3] = mxCreateDoubleMatrix(ndec,1,mxREAL);
        }
    }       
    
    //If not vectorized, we can create x now, otherwise must be done in callback
    if(!opt.vectorized)
        fun.prhs[fun.xrhs] = mxCreateDoubleMatrix(ndec, 1, mxREAL);
    
    //Create Outputs
    plhs[0] = mxCreateDoubleMatrix(ndec,1, mxREAL);
    plhs[1] = mxCreateDoubleMatrix(1,1, mxREAL);
    plhs[2] = mxCreateDoubleMatrix(1,1, mxREAL);
    plhs[3] = mxCreateDoubleMatrix(1,1, mxREAL);
    plhs[4] = mxCreateDoubleMatrix(1,1, mxREAL);
    x = mxGetPr(plhs[0]); 
    fval = mxGetPr(plhs[1]); 
    exitflag = mxGetPr(plhs[2]);    
    iter = mxGetPr(plhs[3]);
    feval = mxGetPr(plhs[4]);

    //Print Header
    if(printLevel) {
        mexPrintf("\n------------------------------------------------------------------\n");
        mexPrintf(" This is PSwarm v1.5\n");
            
        mexPrintf(" Authors: A.I.F Vaz and L.N. Vicente\n MEX Interface J. Currie 2012\n\n");
        mexPrintf(" Problem Properties:\n");
        mexPrintf(" # Decision Variables:     %4d\n",ndec);
        mexPrintf(" # Linear Constraints:     %4d\n",lincon);

        mexPrintf("------------------------------------------------------------------\n");
    }
    
    //Run PSwarm
    start = clock();
    exit_code = PSwarm((int)ndec, &func, lb, ub, lincon, A, b, &sol, fval, x0);
    
    if(exit_code == 0) {
        //Copy Solution
        memcpy(x,sol,ndec*sizeof(double));
        free(sol);
        *iter = (double)stats.solveriters;
        *feval = (double)stats.objfunctions;
    }

    //Save Status & Iterations
    *exitflag = getStatus(exit_code,stats.solveriters,stats.objfunctions); 
    
    //Print Header
    if(printLevel){            
        //Termination Detected
        switch((int)*exitflag)
        {
            //Success
            case 1:
                mexPrintf("\n *** SUCCESSFUL TERMINATION ***\n *** Normal Exit ***\n"); break;
            //Error
            case -1:
                mexPrintf("\n *** ERROR: Abnormal Exit ***\n"); break;
            case 0:
                if(stats.solveriters >= (opt.maxiter-5))
                    mexPrintf("\n *** MAXIMUM ITERATIONS REACHED ***\n"); 
                else if(((double)(end-start))/CLOCKS_PER_SEC > maxtime)
                    mexPrintf("\n *** MAXIMUM TIME EXCEEDED ***\n");
                else
                    mexPrintf("\n *** MAXIMUM FUNCTION EVALUATIONS REACHED ***\n"); 
                
                break;
            //Early Exit
            case -2:
                mexPrintf("\n *** TERMINATION: EARLY EXIT ***\n *** CAUSE: Failed to allocate memory ***\n"); break;
            case -3:
                mexPrintf("\n *** TERMINATION: EARLY EXIT ***\n *** CAUSE: Unable to initialize population - check constraints are feasible ***\n"); break;
            case -5:
                mexPrintf("\n *** TERMINATION: EARLY EXIT ***\n *** CAUSE: User Exit (Ctrl C) ***\n"); break;
        }

        if(*exitflag==1)
            mexPrintf("\n Final Objective: %12.5g\n In %3d iterations and\n   %4d function evaluations\n",*fval,stats.solveriters,stats.objfunctions);

        mexPrintf("------------------------------------------------------------------\n\n");
    }
    
    //Free Memory
    if(lb) mxFree(lb);
    if(ub) mxFree(ub);    
}
//=========================================================================
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    //Documentation of the calling forms is given within each if clause

    if (!locked)
    {
        

        
        //NOTE: If we run clear all this will clear the definition of
        //this file and depending on whether or not we had open references
        //could cause Matlab to crash. By locking this file we prevent
        //clearing the file (which means we can't recompile it unless we
        //close Matlab) but we also prevent Matlab from crashing.
        //
        //TODO: Implement allowing unlock by reference counting
        //TODO: Alternatively we could pass in a command to unlock
        mexLock();
        locked = 1;
    }
    
    
    ADI_FileHandle fileH(0);
    
    
    //Setup output result code
    //-----------------------------------------------
    //Each function returns a result code, indicating if the function call
    //worked or not.
    ADIResultCode result;
    int *out_result; //Each function will return a result code as well as
    //possibly other values ..
    plhs[0]    = mxCreateNumericMatrix(1,1,mxINT32_CLASS,mxREAL);
    out_result = (int *) mxGetData(plhs[0]);
    
    
    //Which function to call
    double function_option = mxGetScalar(prhs[0]);
    
    //Function list
    //--------------------------------------
    // 0  ADI_OpenFile
    // 1  ADI_GetNumberOfRecords
    // 2  ADI_GetNumberOfChannels
    // 3  ADI_GetNumTicksInRecord
    // 4  ADI_GetRecordTickPeriod
    // 5  ADI_GetNumSamplesInRecord
    // 6  ADI_CreateCommentsAccessor
    // 7  ADI_CloseCommentsAccessor
    // 8  ADI_GetCommentInfo
    // 9  ADI_NextComment
    // 10 ADI_GetSamples
    // 11 ADI_GetUnitsName
    // 12 ADI_GetChannelName
    // 13 ADI_CloseFile
    // 14 ADI_GetErrorMessage
    // 15 ADI_GetRecordSamplePeriod
    // 16 ADI_GetRecordTime
    // 17 ADI_CreateFile
    
    
    if (function_option == 0)
    {
        //  ADI_OpenFile   <>   openFile
        //  ===================================================
        //  [result_code,file_h] = sdk_mex(0,file_path)
        
        wchar_t *w_file_path = (wchar_t *)mxGetData(prhs[1]);

        //long openMode  = getLongInput(prhs,2);
        
        result        = ADI_OpenFile(w_file_path, &fileH, kOpenFileForReadOnly);
        out_result[0] = result;

        setFileHandle(plhs,result,fileH);
    }
    else if (function_option == 0.5)
    {
		//This is a call to open the file for reading and writing
		//
        //TODO: Replace this with an input to function 0
        
        wchar_t *w_file_path = (wchar_t *)mxGetData(prhs[1]);

        result        = ADI_OpenFile(w_file_path, &fileH, kOpenFileForReadAndWrite);
        out_result[0] = result;

        setFileHandle(plhs,result,fileH);
    }
    else if (function_option == 1)
    {
        //  ADI_GetNumberOfRecords   <>   getNumberOfRecords
        //  ======================================================
        //  [result_code,n_records] = sdk_mex(1,file_handle)
        
        long nRecords = 0;
        fileH         = getFileHandle(prhs);
        
        //ADIResultCode ADI_GetNumberOfRecords(ADI_FileHandle fileH, long* nRecords);
        result        = ADI_GetNumberOfRecords(fileH,&nRecords);
        out_result[0] = result;
        setLongOutput(plhs,1,nRecords);
        
    }
    else if (function_option == 2)
    {
        //  ADI_GetNumberOfChannels   <>   getNumberOfChannels
        //  ========================================================
        //  [result_code,n_channels] = sdk_mex(2,file_handle)
        
        long nChannels = 0;
        fileH = getFileHandle(prhs);
        
        //ADIResultCode ADI_GetNumberOfChannels(ADI_FileHandle fileH, long* nChannels);
        result         = ADI_GetNumberOfChannels(fileH,&nChannels);
        out_result[0]  = result;
        setLongOutput(plhs,1,nChannels);
    }
    else if (function_option == 3)
    {
        //  ADI_GetNumTicksInRecord   <>   getNTicksInRecord
        //  ======================================================
        //  [result,n_ticks] = sdk_mex(3,file_handle,record_idx_0b)
        
        fileH = getFileHandle(prhs);
        
        //0 or 1 based ...
        long record = getLongInput(prhs,2);
        long nTicks = 0;
        
        //ADIResultCode ADI_GetNumTicksInRecord(ADI_FileHandle fileH, long record, long* nTicks);
        result         = ADI_GetNumTicksInRecord(fileH,record,&nTicks);
        out_result[0]  = result;
        setLongOutput(plhs,1,nTicks);
    }
    else if (function_option == 4)
    {
        //  ADI_GetRecordTickPeriod  <>  getTickPeriod
        //  =========================================================
        //  [result,s_per_tick] = sdk_mex(4,file_handle,record_idx_0b,channel_idx_0b)
        
        fileH          = getFileHandle(prhs);
        
        long record  = getLongInput(prhs,2);
        long channel = getLongInput(prhs,3);
        double secsPerTick = 0;
        
        //ADIResultCode ADI_GetRecordTickPeriod(ADI_FileHandle fileH, long channel, long record, double* secsPerTick);
        out_result[0] = ADI_GetRecordTickPeriod(fileH,channel,record,&secsPerTick);
        setDoubleOutput(plhs,1,secsPerTick);
        
    }
    else if (function_option == 5)
    {
        //  ADI_GetNumSamplesInRecord  <>  getNSamplesInRecord
        //  ========================================================
        //  [result_code,n_samples] = sdk_mex(5,file_handle,record_idx_0b,channel_idx_0b);
        
        fileH          = getFileHandle(prhs);
        
        long record   = getLongInput(prhs,2);
        long channel  = getLongInput(prhs,3);
        long nSamples = 0;
        
        //ADIResultCode ADI_GetNumSamplesInRecord(ADI_FileHandle fileH, long channel, long record, long* nSamples);
        out_result[0] = ADI_GetNumSamplesInRecord(fileH,channel,record,&nSamples);
        setLongOutput(plhs,1,nSamples);
    }
    else if (function_option == 6)
    {
        //  ADI_CreateCommentsAccessor  <>  getCommentAccessor
        //  ========================================================
        //  [result_code,comments_h] = sdk_mex(6,file_handle,record_idx_0b);
        
        ADI_CommentsHandle commentsH(0);
        fileH         = getFileHandle(prhs);
        long record   = getLongInput(prhs,2);
        
        //ADIResultCode ADI_CreateCommentsAccessor(ADI_FileHandle fileH, long record, ADI_CommentsHandle* commentsH);
        result = ADI_CreateCommentsAccessor(fileH,record,&commentsH);
        out_result[0] = result;
        
        int64_t *p_c;
        plhs[1]    = mxCreateNumericMatrix(1,1,mxINT64_CLASS,mxREAL);
        p_c = (int64_t *) mxGetData(plhs[1]);
        if (result == 0)
            p_c[0] = (int64_t)commentsH;
        else
            p_c[0] = 0;
    }
    else if (function_option == 7)
    {
        //  ADI_CloseCommentsAccessor   <>   closeCommentAccessor
        //  ========================================================
        //  result_code = sdk_mex(7,comments_h);
        
        ADI_CommentsHandle commentsH = getCommentsHandle(prhs);
        
        //ADIResultCode ADI_CloseCommentsAccessor(ADI_CommentsHandle *commentsH);
        out_result[0] = ADI_CloseCommentsAccessor(&commentsH);
    }
    else if (function_option == 8)
    {
        //  ADI_GetCommentInfo   <>   getCommentInfo
        //  ====================================================
        //  [result_code,comment_string,comment_length,tick_pos,channel,comment_num] = sdk_mex(8,comment_h)
        //
        //  Status: Done
        
        ADI_CommentsHandle commentsH = getCommentsHandle(prhs);
        
        wchar_t *messageOut = getStringOutputPointer(plhs,1);
        long tickPos    = 0;
        long channel    = 0;
        long commentNum = 0;
        long textLen    = 0;
        
        
        //ADIResultCode ADI_GetCommentInfo(ADI_CommentsHandle commentsH, long *tickPos, long *channel, long *commentNum, wchar_t* text,long maxChars, long *textLen);
        //          tickPos                 - receives the tick position of the comment in the record [outparam]
        //          commentNum              - receives the number of the comment [outparam]
        //          channel                 - receives the channel of the comment (-1 for all channel comments) [outparam]
        //          text                    - buffer to receive null terminated text for the comment (optional, may be NULL) [outparam]
        //          maxChars                - the size of the text buffer in wchar_t s. The text will be truncated to fit in this size
        //          textLen                 - receives the number of characters needed to hold the full comment text,
        //                                    even if parameter text is NULL (optional, may be NULL) [outparam]
        
        out_result[0] = ADI_GetCommentInfo(commentsH,&tickPos,&channel,&commentNum,messageOut,MAX_STRING_LENGTH,&textLen);
        
        setLongOutput(plhs,2,textLen);
        setLongOutput(plhs,3,tickPos);
        setLongOutput(plhs,4,channel);
        setLongOutput(plhs,5,commentNum);
        
    }
    else if (function_option == 9)
    {
        
        //  ADI_NextComment  <>  advanceComments
        //  ==================================================
        //  result_code = adi.sdk_mex(9,comments_h);
        //
        //  Returns kResultNoData if there are no more comments ...
        //
        //  Status: Done
        
        ADI_CommentsHandle commentsH = getCommentsHandle(prhs);
        
        //ADIResultCode ADI_NextComment(ADI_CommentsHandle commentsH);
        out_result[0] = ADI_NextComment(commentsH);
    }
    else if (function_option == 10)
    {
        //  ADI_GetSamples   <>   getChannelData
        //  ===========================================================
        //  [result,data,n_returned] = sdk_mex(10,file_h,channel_0b,record_0b,startPos,nLength,dataType)
        
        fileH = getFileHandle(prhs);
        
        long channel  = getLongInput(prhs,2);
        long record   = getLongInput(prhs,3);
        long startPos = getLongInput(prhs,4);
        long nLength  = getLongInput(prhs,5);
        
        ADICDataFlags dataType = static_cast<ADICDataFlags>(getLongInput(prhs,6));

        plhs[1]     = mxCreateNumericMatrix(1,(mwSize)nLength,mxSINGLE_CLASS,mxREAL);
        float *data = (float *)mxGetData(plhs[1]);
        
        long returned = 0;
        // Retrieves a block of sample data from the file into a buffer. Samples are in physical
        // prefixed units.
        //DLLEXPORT ADIResultCode ADI_GetSamples(ADI_FileHandle fileH, long channel, long record, long startPos,
        //  ADICDataFlags dataType, long nLength, float* data, long* returned);
        out_result[0] = ADI_GetSamples(fileH,channel,record,startPos,dataType,nLength,data,&returned);
        //out_result[0] = ADI_GetSamples(fileH,channel,record,startPos,kADICDataAtSampleRate,nLength,data,&returned);
        
        setLongOutput(plhs,2,returned);
        
        //out_result[0] = 4;
    }
    else if (function_option == 11)
    {
        //  ADI_GetUnitsName   <>  getUnits
        //  =======================================
        //  [result_code,str_data,str_length] = sdk_mex(11,file_h,record,channel);
        
        //Inputs
        fileH        = getFileHandle(prhs);
        long record  = getLongInput(prhs,2);
        long channel = getLongInput(prhs,3);
        
        //Outputs
        long textLen      = 0;
        wchar_t *unitsOut = getStringOutputPointer(plhs,1);
        
        
        // Retrieves the prefixed units of a channel, as a string.
        //
        //ADIResultCode ADI_GetUnitsName(ADI_FileHandle fileH, long channel, long record, wchar_t* units, long maxChars, long *textLen);
        out_result[0] = ADI_GetUnitsName(fileH, channel, record, unitsOut, MAX_STRING_LENGTH, &textLen);
        setLongOutput(plhs,2,textLen);
    }
    else if (function_option == 12)
    {
        //  ADI_GetChannelName   <>   getChannelName
        //  =============================================
        //  [result_code,str_data,str_length] = sdk_mex(12,file_h,channel);
        
        
        //Inputs
        fileH        = getFileHandle(prhs);
        long channel = getLongInput(prhs,2);
        
        //Outputs
        long textLen        = 0;
        wchar_t *nameOut    = getStringOutputPointer(plhs,1);
        
        // Retrieves the name of a channel, as a string.
        
        //ADIResultCode ADI_GetChannelName(ADI_FileHandle fileH, long channel, wchar_t* name, long maxChars, long *textLen);
        out_result[0] = ADI_GetChannelName(fileH, channel, nameOut, MAX_STRING_LENGTH, &textLen);
        setLongOutput(plhs,2,textLen);
    }
    else if (function_option == 13)
    {
        //  ADI_CloseFile   <>   closeFile
        //  ==============================================================
        //
        
        fileH          = getFileHandle(prhs);
        result         = ADI_CloseFile(&fileH);
        out_result[0]  = result;
    }
    else if (function_option == 14)
    {
        //  ADI_GetErrorMessage   <>   getErrorMessage
        //  ==============================================================
        //  err_msg = sdk_mex(14,error_code)
        
        long textLen        = 0;
        
        wchar_t *messageOut = getStringOutputPointer(plhs,1);
        
        ADIResultCode code  = (ADIResultCode)getLongInput(prhs,1);
        
        //ADIResultCode ADI_GetErrorMessage(ADIResultCode code, wchar_t* messageOut, long maxChars, long *textLen);
        out_result[0] = ADI_GetErrorMessage(code, messageOut, MAX_STRING_LENGTH, &textLen);
        setLongOutput(plhs,2,textLen);
        
    }
    else if (function_option == 15)
    {
        //   ADI_GetRecordSamplePeriod   <>   getSamplePeriod
        //   ==============================================================
        //   [result_code,dt_channel] = sdk_mex(15,file_h,record,channel)
        
        fileH        = getFileHandle(prhs);
        long record  = getLongInput(prhs,2);
        long channel = getLongInput(prhs,3);
        double secsPerSample = 0;
        
        out_result[0] = ADI_GetRecordSamplePeriod(fileH, channel, record, &secsPerSample);
        setDoubleOutput(plhs,1,secsPerSample);
    }
    else if (function_option == 16)
    {
        //   ADI_GetRecordTime   <>   getRecordStartTime
        //   ==============================================================
        //   [result_code,trigger_time,frac_secs,trigger_minus_rec_start] = sdk_mex(16,file_h,record)
        
        fileH        = getFileHandle(prhs);
        long record  = getLongInput(prhs,2);
        
        time_t triggerTime = 0;
        double fracSecs    = 0;
        long   triggerMinusStartTicks = 0;
        
        //Retrieves time information about the specified record.
        //The trigger time is the time origin of the record and may differ from the start time if
        //there is a pre or post trigger delay, as specified by the trigMinusRecStart parameter.
        // Params: fileH             - ADI_FileHandle for the open file
        //         record            - the record index (starting from 0)
        //         triggerTime       - time_t receives the date and time of the trigger
        //                             position for the new record. Measured as number of
        //                             seconds from 1 Jan 1970
        //         fracSecs          - receives the fractional seconds part of
        //                             the record trigger time ('triggerTime' parameter)
        //         trigMinusRecStart - trigger-time-minus-record-start-ticks. Receives the
        //                             difference between the time of trigger tick and the first
        //                             tick in the record. This +ve for pre-trigger delay and
        //                             -ve for post-trigger delay.
        // Return: a ADIResultCode for result of the operation
        
        //DLLEXPORT ADIResultCode ADI_GetRecordTime(ADI_FileHandle fileH, long record, time_t *triggerTime,
        //double *fracSecs, long *triggerMinusStartTicks);
        
        out_result[0] = ADI_GetRecordTime(fileH, record, &triggerTime, &fracSecs, &triggerMinusStartTicks);
        
        setDoubleOutput(plhs,1,(double)triggerTime);
        setDoubleOutput(plhs,2,fracSecs);
        setLongOutput(plhs,3,triggerMinusStartTicks);
        
    }
    else if (function_option == 17){
        //
        //   ADI_CreateFile   <>   createFile
        //   ==============================================================
        //   [result_code,file_h] = sdk_mex(17,file_path)
        //    
        //   Implemented via sdk.createFile
        
        wchar_t *w_file_path = (wchar_t *)mxGetData(prhs[1]);
        
        result        = ADI_CreateFile(w_file_path, &fileH);
        out_result[0] = result;
        
        setFileHandle(plhs,result,fileH);
    }
    else if (function_option == 18){
        //
        //   ADI_SetChannelName  <>  setChannelName
        //   ==============================================================
        //   [result_code,file_h] = sdk_mex(18,file_h,channel,name)
        //
        //   Implemented via sdk.setChannelName
        
        fileH = getFileHandle(prhs);
        long channel = getLongInput(prhs,2);
        wchar_t *channel_name = (wchar_t *)mxGetData(prhs[3]);
        
        out_result[0] = ADI_SetChannelName(fileH, channel, channel_name);
    }
    else if (function_option == 19){
        //
        //   ADI_CreateWriter  <>  createDataWriter
        //   ===========================================
        //   [result_code,writer_h] = sdk_mex(19,file_h)
        //
        //  Implemented via sdk.createDataWriter
        
        ADI_WriterHandle writerH(0);
        
        fileH = getFileHandle(prhs);
        
        result        = ADI_CreateWriter(fileH,&writerH);
        out_result[0] = result;
        
        setWriterHandle(plhs,result,writerH);
    }
    else if (function_option == 20){
        //
        //   ADI_SetChannelInfo  <>  setChannelInfo
        //   ===========================================
        //   [result_code] = sdk_mex(20,writer_h,channel,enabled,seconds_per_sample,units,limits)
        //
        //   implemented via adi.sdk.setChannelInfo
        
        ADI_WriterHandle writerH = getWriterHandle(prhs);
        
        long channel = getLongInput(prhs,2);
        int enabled  = getIntInput(prhs,3);
        double seconds_per_sample = getDoubleInput(prhs,4);
        wchar_t *units = (wchar_t *)mxGetData(prhs[5]);
        float *temp_limits = (float *)mxGetData(prhs[5]);
        ADIDataLimits limits;
        limits.mMaxLimit = temp_limits[1];
        limits.mMinLimit = temp_limits[0];
        
        out_result[0] = ADI_SetChannelInfo(writerH, channel, enabled, seconds_per_sample, units, &limits);
        
//       DLLEXPORT ADIResultCode ADI_SetChannelInfo(ADI_WriterHandle writerH, long channel, int enabled,
//       double secondsPerSample, const wchar_t* units, const ADIDataLimits *limits);
        
    }
    else if (function_option == 21){
        //
        //   ADI_StartRecord  <>  startRecord
        //   ===========================================
        //   result_code = sdk_mex(21, writerH, trigger_time, fractional_seconds, trigger_minus_rec_start)
        //
        //   implemented via adi.sdk.startRecord
        
        ADI_WriterHandle writerH = getWriterHandle(prhs);
        time_t trigger_time = (time_t)getDoubleInput(prhs,2);
        double fractional_seconds = getDoubleInput(prhs,3);
        long trigger_minus_rec_start = getLongInput(prhs,4);
        
        out_result[0] = ADI_StartRecord(writerH, trigger_time, fractional_seconds, trigger_minus_rec_start);
        
//            DLLEXPORT ADIResultCode ADI_StartRecord(ADI_WriterHandle writerH, time_t triggerTime,
//       double fracSecs, long triggerMinusStartTicks);
    }
    else if (function_option == 22){
        //
        //   ADI_AddChannelSamples  <>  addChannelSamples
        //   ===========================================
        //   [result_code,new_ticks_added] = sdk_mex(22, writerH, channel, data, n_samples)
        //
        //  adi.sdk.addChannelSamples
        
        ADI_WriterHandle writerH = getWriterHandle(prhs);
        long channel = getLongInput(prhs,2);
        float *data  = (float *)mxGetData(prhs[3]);
        long n_samples = (long)mxGetNumberOfElements(prhs[3]);
        long new_ticks_added = 0;
        
        out_result[0] = ADI_AddChannelSamples(writerH, channel, data, n_samples, &new_ticks_added);
        
        setLongOutput(plhs,1,new_ticks_added);
        
//       DLLEXPORT ADIResultCode ADI_AddChannelSamples(ADI_WriterHandle writerH, long channel,
//       float* data, long nSamples, long *newTicksAdded);
    }
    else if (function_option == 23){
        //
        //   ADI_FinishRecord  <>  finishRecord
        //   ===========================================
        //   [result_code] = sdk_mex(23, writerH)
        //
        //  Implemented via adi.sdk.finishRecord
        
        ADI_WriterHandle writerH = getWriterHandle(prhs);
        
        out_result[0] = ADI_FinishRecord(writerH);
        
//        DLLEXPORT ADIResultCode ADI_FinishRecord(ADI_WriterHandle writerH);
    }
    else if (function_option == 24){
        //
        //   ADI_CommitFile  <>  commitFile
        //   ===========================================
        //   [result_code] = sdk_mex(24, writerH, flags)
        //
        //  Implemented via adi.sdk.commitFile
        
        ADI_WriterHandle writerH = getWriterHandle(prhs);
        
        //TODO: What are the flags??????
        
        out_result[0] = ADI_CommitFile(writerH, 0);
        
//         DLLEXPORT ADIResultCode ADI_CommitFile(ADI_WriterHandle writerH, long flags);
    }
    else if (function_option == 25){
        //
        //   ADI_CloseWriter  <>  closeWriter
        //   ===========================================
        //   [result_code] = sdk_mex(25, writerH)
        //
        //  Implemented via adi.sdk.closeWriter
        
        ADI_WriterHandle writerH = getWriterHandle(prhs);
        
        out_result[0] = ADI_CloseWriter(&writerH);
        
//      DLLEXPORT ADIResultCode ADI_CloseWriter(ADI_WriterHandle *writerH);
    }
    else if (function_option == 26){
        //
        //   ADI_AddComment  <>  addComment
        //   ===========================================
        //   [result_code, comment_number] =
        //      sdk_mex(26, file_h, channel, record, tick_position, text)
        //
        //  Implemented via adi.sdk.addComment
        
        fileH = getFileHandle(prhs);
        long channel = getLongInput(prhs,2);
        long record  = getLongInput(prhs,3);
        long tick_position = getLongInput(prhs,4);
        wchar_t *text = (wchar_t *)mxGetData(prhs[5]);
        long comment_number = 0;
        //long comment_number = getLongInput(prhs,6);
        
        out_result[0] = ADI_AddComment(fileH, channel, record, tick_position, text, &comment_number);
        
        setLongOutput(plhs,1,comment_number);
        
//        DLLEXPORT ADIResultCode ADI_AddComment(ADI_FileHandle fileH, long channel, long record, long tickPos,
//       const wchar_t* text, long* commentNum);
    }
    else if (function_option == 27){
        //
        //   ADI_DeleteComment  <>  deleteComment
        //   ===========================================
        //   result_code = sdk_mex(27,file_h,comment_number)
        //
        //  Implemented via adi.sdk.deleteComment
        
        fileH = getFileHandle(prhs);
        long comment_number = getLongInput(prhs,2);
        
        out_result[0] = ADI_DeleteComment(fileH,comment_number);
        
//         DLLEXPORT ADIResultCode ADI_DeleteComment(ADI_FileHandle fileH, long commentNum);
    }
    else if (function_option == 100){
        mexUnlock();
        locked = 0;
    }
    else
    {
        mexErrMsgIdAndTxt("adinstruments:sdk_mex",
                "Invalid function option");
    }

    //ADI_GetRecordSamplePeriod
    //ADI_GetRecordTime
}
Ejemplo n.º 9
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  /* field data */
  LSMLIB_REAL *phi;
  LSMLIB_REAL *mask;
  LSMLIB_REAL **source_fields;
  LSMLIB_REAL *distance_function;
  LSMLIB_REAL **extension_fields;
  int num_ext_fields;
 
  /* grid data */
  const int *grid_dims = mxGetDimensions(PHI);
  double *dX = mxGetPr(DX);
  LSMLIB_REAL dX_matlab_order[2];

  /* numerical parameters */
  int spatial_discretization_order;

  /* auxilliary variables */
  int i;
  int error_code;
  mxArray* tmp_mxArray;

  /* Check for proper number of arguments */
  if (nrhs < 3) {
    mexErrMsgTxt(
      "Insufficient number of input arguments (3 required; 2 optional)");
  } else if (nrhs > 5) {
    mexErrMsgTxt("Too many input arguments (3 required; 2 optional)");
  } else if (nlhs > 2) {
    mexErrMsgTxt("Too many output arguments.");
  }

  /* Check that the inputs have the correct floating-point precision */
#ifdef LSMLIB_DOUBLE_PRECISION
    if (!mxIsDouble(PHI)) {
      mexErrMsgTxt("Incompatible precision: LSMLIB built for double-precision but phi is single-precision");
    }
#else
    if (!mxIsSingle(PHI)) {
      mexErrMsgTxt("Incompatible precision: LSMLIB built for single-precision but phi is double-precision");
    }
#endif

  /* Get mask */
  if ( (nrhs < 4) || (mxIsEmpty(MASK)) ) {
    mask = 0;  /* NULL mask ==> all points are in interior of domain */
  } else {

#ifdef LSMLIB_DOUBLE_PRECISION
    if (!mxIsDouble(MASK)) {
      mexErrMsgTxt("Incompatible precision: LSMLIB built for double-precision but mask is single-precision");
    }
#else
    if (!mxIsSingle(MASK)) {
      mexErrMsgTxt("Incompatible precision: LSMLIB built for single-precision but mask is double-precision");
    }
#endif

    mask = (LSMLIB_REAL*) mxGetPr(MASK);
  }

  /* Get spatial derivative order */
  if (nrhs < 5) {
    spatial_discretization_order = 2;
  } else {
    spatial_discretization_order = mxGetPr(SPATIAL_DERIVATIVE_ORDER)[0];
  }

  /* Assign pointers for phi and extension field data */
  phi = (LSMLIB_REAL*) mxGetPr(PHI);
  num_ext_fields = mxGetNumberOfElements(SOURCE_FIELDS);
  source_fields = (LSMLIB_REAL**) mxMalloc(num_ext_fields*sizeof(LSMLIB_REAL*));
  for (i = 0; i < num_ext_fields; i++) {
    tmp_mxArray = mxGetCell(SOURCE_FIELDS,i);

#ifdef LSMLIB_DOUBLE_PRECISION
    if (!mxIsDouble(tmp_mxArray)) {
      mexErrMsgTxt("Incompatible precision: LSMLIB built for double-precision but one of the source fields is single-precision");
    }
#else
    if (!mxIsSingle(tmp_mxArray)) {
      mexErrMsgTxt("Incompatible precision: LSMLIB built for single-precision but one of the source fields is double-precision");
    }
#endif

    source_fields[i] = (LSMLIB_REAL*) mxGetPr(tmp_mxArray);
  }


  /* Create distance function and extension field data */
#ifdef LSMLIB_DOUBLE_PRECISION
  DISTANCE_FUNCTION = mxCreateDoubleMatrix(grid_dims[0], grid_dims[1], mxREAL);
#else
  DISTANCE_FUNCTION = mxCreateNumericMatrix(grid_dims[0], grid_dims[1],
                                            mxSINGLE_CLASS, mxREAL);
#endif                                
  distance_function = (LSMLIB_REAL*) mxGetPr(DISTANCE_FUNCTION);
  num_ext_fields = mxGetNumberOfElements(SOURCE_FIELDS);
  EXTENSION_FIELDS = mxCreateCellArray(1, &num_ext_fields);
  extension_fields = 
    (LSMLIB_REAL**) mxMalloc(num_ext_fields*sizeof(LSMLIB_REAL*)); 
  for (i = 0; i < num_ext_fields; i++) {
#ifdef LSMLIB_DOUBLE_PRECISION
    tmp_mxArray = mxCreateDoubleMatrix(grid_dims[0], grid_dims[1], mxREAL);
#else
    tmp_mxArray= mxCreateNumericMatrix(grid_dims[0], grid_dims[1],
                                       mxSINGLE_CLASS, mxREAL);
#endif                                
    mxSetCell(EXTENSION_FIELDS, i, tmp_mxArray);
    extension_fields[i] = (LSMLIB_REAL*) mxGetPr(tmp_mxArray);
  }

  /* Change order of dX to be match MATLAB meshgrid() order for grids. */
  dX_matlab_order[0] = dX[1];
  dX_matlab_order[1] = dX[0];

  LSMLIB_REAL *extension_mask_dummy;	/* ipa.n */
  
  /* Carry out FMM calculation */
  error_code = computeExtensionFields2d(
                 distance_function,
                 extension_fields,
                 phi,
                 mask,
                 source_fields,
		          extension_mask_dummy,	/* ipa.n */
                 num_ext_fields,
                 spatial_discretization_order,
                 (int*) grid_dims,
                 dX_matlab_order);

  if (error_code) {
    mexErrMsgTxt("computeExtensionFields2d failed...");
  }

  /* Clean up memory */
  mxFree(source_fields); 
  mxFree(extension_fields);

  return;
}
Ejemplo n.º 10
0
// matlab entry point
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

  // input must have tree size, binary_feature_filename, binary_feature_flipped_filename, action name
  if (nrhs != 4)
    mexErrMsgTxt("Wrong number of inputs, treesize, binaryfeaturefilename, binaryfeatureflippedfilename, action_name needed"); 
  // Currently, we do not have any output
  if (nlhs != 0)
    mexErrMsgTxt("Wrong number of outputs");
  /* make sure the first input argument is scalar */
  if( !mxIsDouble(prhs[0]) || 
      mxIsComplex(prhs[0]) ||
      mxGetNumberOfElements(prhs[0])!=1 ) {
    mexErrMsgIdAndTxt("MyToolbox:arrayProduct:notScalar",
                      "Input tree size must be a scalar.");
  }
  /* input must be a string */
  if ( mxIsChar(prhs[1]) != 1 || mxIsChar(prhs[2]) != 1 || mxIsChar(prhs[3]) != 1 )
      mexErrMsgIdAndTxt( "MATLAB:revord:inputNotString",
                         "Input must be a string.");
  // read in input string
  char *DataFilename, *DataFilenameFlipped, *action;
  DataFilename = mxArrayToString(prhs[1]);
  DataFilenameFlipped = mxArrayToString(prhs[2]);
  action = mxArrayToString(prhs[3]);
  
  // set the seed for random numbers
  srand(time(NULL));

  mexPrintf("Entered dttrain.cpp!\n"); // debug!!!!
  // set parameters for learning
  PARAMETER param;
  SetParameters(param);
  param.treeNum = mxGetScalar(prhs[0]); // treat tree num specially
  mexPrintf("tree num is %d\n", param.treeNum); // debug!!!!

  mexPrintf("parameters set!\n"); // debug!!!!

  // get image feature descriptors from the file
  vector<SINGLEIM> imVec;
  int classNum;
  
  // use filename passed in here!!!
  GetData(imVec, DataFilename, param); 
  mexPrintf("read %s to image vector! total size of %d\n", DataFilename, imVec.size()); // debug!!!!
  
  //GetData(imVec, DataFilenameFlipped, param); 
  //mexPrintf("read %s to image vector, total size of %d\n", DataFilenameFlipped, imVec.size()); // debug!!!!
  
  AdjustLabels(imVec, classNum);
  mexPrintf("Finish getting data from the disk.\n");

  // pre-pooling for the background information, if it is necessary
//  if (param.bgPrePooling == 1)
//    BgPrePooling(imVec, param);
//  mexPrintf("Finish getting background feature.\n");

  // the pre-allocated memory for all the trees
  int treeMemSize = int(pow(2, param.maxTreeDepth + 1)) * param.treeNum;
  TREENODE *treeMemory = new TREENODE[treeMemSize];
  InitTreeMem(treeMemory, treeMemSize);
  int treeMemID = 0; 
  vector<TREENODE*> trees;  // the set of decision trees
  for (int i = 0; i < param.treeNum; i++)
  {
    trees.push_back(treeMemory + treeMemID);
    treeMemID++;
  }
  mexPrintf("Finish initializing all the trees.\n");

  // train decision trees
  clock_t t1, t2;
  vector<int> trainID, valID;  // indicate which samples are included in the current tree node
  char filename[1024];
  float *valDist = (float*)malloc(sizeof(float) * classNum);  // sample distribution in the val set
  for (int i = 0; i < param.treeNum; i++)
  {
    t1 = clock();
    GetFilename(filename, action);
    mexPrintf("\nThe current tree will be in this file: %s\n", filename);
    trainID.clear();
    valID.clear();
    InitializeTree(trees[i]);
    int nodeID = 0;
    memset(valDist, 0, sizeof(float) * classNum);
    TrainDecisionTree(trees[i], imVec, param, trainID, valID, 
        classNum, nodeID, treeMemory, treeMemID, valDist);
//    sprintf(filename, "trees/tree_%d%d%d.txt", i / 100, (i % 100) / 10, i % 10);
    OutputTree(trees[i], filename);
    t2 = clock();
    mexPrintf("\n    Time for trainning this tree: %f\n", (float)(t2 - t1) / CLOCKS_PER_SEC);
  }

  free(valDist);
  ReleaseData(imVec);
  for (int i = 0; i < param.treeNum; i++)
    ReleaseTreeMem(trees[i]);
  delete[] treeMemory;
  
 
}
void mexFunction (int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    /* check: only one input and one output argument */
    if (nrhs !=3)
        mexErrMsgTxt("Must have 3 input arguments");
    if (nlhs !=1)
        mexErrMsgTxt("Must have 1 output argument");
    
    /* Get inputs*/
    // Check data types
    if (!mxIsDouble(Distances_IN) || mxIsComplex(Distances_IN)){
        mexErrMsgTxt("Inputs must be double.");
    }
    if (!mxIsDouble(Constrains_IN) || mxIsComplex(Constrains_IN)){
        mexErrMsgTxt("Inputs must be double.");
    }
    
    // Check first input
    int nDims = mxGetNumberOfDimensions(Distances_IN);
    
    if (nDims!=2) {
        mexErrMsgTxt("First Input must be 2-D.");
    }
    
    double *D = (double*) mxGetPr(Distances_IN);

    
    if ( mxGetN(Distances_IN) != mxGetM(Distances_IN))
    {
        mexErrMsgTxt("First Input must be a squared matrix.");
    }
    
    unsigned int nDataPoints = (unsigned int) mxGetM(Distances_IN);
    
    
    // Check second input
    int nDims2 = mxGetNumberOfDimensions(Constrains_IN);
    if ( (nDims2>2)  ||
         (mxGetM(Constrains_IN)!=mxGetN(Constrains_IN)) || (mxGetN(Constrains_IN)!=nDataPoints) )
    {
        mexErrMsgTxt("Second Input must be squared 2-D matrix, with same size than first Input");
    }
    
    double * C = (double*) mxGetPr(Constrains_IN);
    //mexPrintf("nDataPoints = %d\n",nDataPoints);

	// Check third input
    int numElems= mxGetNumberOfElements(Threshold_IN);
    if (numElems>1)
    {
        mexErrMsgTxt("Third Input must be scalar.");
    }
	double Threshold = (double) mxGetScalar(Threshold_IN);
    
    
    clusterID_OUT = mxCreateNumericMatrix(nDataPoints, 1, mxDOUBLE_CLASS,mxREAL);
    double *clusterIds = (double *) mxGetPr(clusterID_OUT);
    
    // -------------------------- Main Algorithm --------------------- //
    
    //
    timer mytimer;
    mytimer.start("imposing Must Links");
    imposeMustLinks(D,C, nDataPoints);
    mytimer.check();
	mytimer.restart();
    
    mytimer.start("propagating Must Links");    
    propagateMustLinks(D,C,nDataPoints);
    mytimer.check();
	mytimer.restart();
    
    mytimer.start("imposing Cannot Links");    
    imposeCannotLinks(D,C,nDataPoints);
    mytimer.check();
	mytimer.restart();
    //propagateCannotLinks(D,C,nDataPoints);

    mytimer.start("complete-linkage clustering");    
    completeLink(D,clusterIds,nDataPoints,Threshold);
    mytimer.check();
    /*
    std::vector<int > centerPoints(nDataPoints,0);
    std::vector<int > memberships(nDataPoints,0);
    
    // intial centers and clusters
    for (unsigned int i=0;i<nDataPoints;i++)
    {
        memberships[i] = i;
        centerPoints[i] = i;
    }
    */
    
    /*
    // compute distance matrix
    double * distance = new double[nDataPoints*nDataPoints];
    
    double maxDist = 0;

    for (unsigned int i=0;i<nDataPoints;i++)
    {
        distance[i + i*nDataPoints] = 0;
        
        for(unsigned int j=i+1;j<nDataPoints;j++)
        {
            double sum = 0;

            for(unsigned int d=0; d<1;d++){
                //diff += ;
                sum += ((data[i + d*nDataPoints]- data[j + d*nDataPoints])
                        *(data[i + d*nDataPoints]- data[j + d*nDataPoints]))
                        /(3e-16+data[i + d*nDataPoints] + data[j + d*nDataPoints]);
            }
            distance[i + j*nDataPoints] = sum;
            distance[j + i*nDataPoints] = sum;
            if (sum>maxDist)
                maxDist = sum;
        }
    }
    */
    
    //mexPrintf("centers size = %d\n",centers.size());
    //mexPrintf("clusters size = %d\n",clusters.size());
/*
    int k=0;
    while (1) {
        
        
        double mindist = maxDist;
        unsigned int minidx[2];
        minidx[0] = 0;
        minidx[1] = 0;
        
        // get pair of closest centers
        for (unsigned int i=0; i<nDataPoints; i++)
        {
            if (centerPoints[i]==-1)
                continue;
            for (unsigned int j=i+1; j<nDataPoints; j++)
            {
                if (centerPoints[j]==-1)
                    continue;
                if (mindist>distance[centerPoints[i] + centerPoints[j]*nDataPoints])
                {
                    minidx[0] = i;
                    minidx[1] = j;
                    mindist = distance[centerPoints[i] + centerPoints[j]*nDataPoints];
                }
            }
        }
        //mexPrintf("trh = %g\tmindist = %g\n\n",threshold,mindist);

        // if min dist is larger than threshold, then stop
        if (mindist>threshold)
            break;
        
        // merge clusters by updating membership
        unsigned int new_clust = minidx[0];
        unsigned int old_clust = minidx[1];

        //mexPrintf("join clust %d and %d\n",new_clust,old_clust);

        for (unsigned int i=0;i<nDataPoints;i++)
        {
            //if ((memberships[i] == minidx[0]) || (memberships[i] == minidx[1]) )
            if (memberships[i] == old_clust)
            {
                memberships[i] = new_clust;
            }
        }
        
        // eliminate old center
        centerPoints[old_clust] = -1;
        
        // update cluster center
        unsigned int bestCenter = 0;
        double bestDist = maxDist*nDataPoints+1;
        
        for (unsigned int i=0;i<nDataPoints;i++)
        {

            if (memberships[i] != minidx[0])
                continue;
            //mexPrintf("comparing: %d\t",i);
            double thisDist = 0;
            for (unsigned int j=0; j<nDataPoints; j++)
            {
                if (memberships[j] != minidx[0])
                    continue;
                thisDist += distance[i + j*nDataPoints];
                //mexPrintf("%d\t",j);
            }
            if (thisDist<bestDist)
            {
                bestCenter = i;
                bestDist = thisDist;
            }
            //mexPrintf("\n");
        }
        
        centerPoints[new_clust] = bestCenter;
        //mexPrintf("new center is %d\n",bestCenter);
        k++;
        if (k>nDataPoints)
        {
            //mexPrintf("wtf?\n");
            return;
        }
    }
    
    delete [] distance;
*/    
    //initialize output
    /*
    centers_OUT = mxCreateNumericMatrix(nDataPoints, 1, mxDOUBLE_CLASS,mxREAL);
    double *centersOut = (double *) mxGetPr(centers_OUT);

    for (unsigned int idx = 0; idx< nDataPoints;idx++)
    {
        centersOut[idx] = centerPoints[idx]+1;
    }
    
    clusterID_OUT = mxCreateNumericMatrix(nDataPoints, 1, mxDOUBLE_CLASS,mxREAL);
    double *clusterIdOut = (double *) mxGetPr(clusterID_OUT);

    
    for (unsigned int idx = 0; idx< nDataPoints;idx++)
    {
        clusterIdOut[idx] = memberships[idx]+1;
    }
    */
}
Ejemplo n.º 12
0
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  if (!(nlhs==1))
  {
    mexErrMsgIdAndTxt("MyToolbox:arrayProduct:nlhs","One output required.");
  }
  if(nrhs<1)
  {
    mexErrMsgIdAndTxt("MyToolbox:arrayProduct:nrhs","At least one input required.");
  }
  if(nrhs>2)
  {
    mexErrMsgIdAndTxt("MyToolbox:arrayProduct:nrhs","A maximum of two inputs is required.");
  }
  if(nrhs==1 || nrhs==2)
  {
    if (!(mxIsDouble(prhs[0])))
    {
      mexErrMsgTxt("The input must be a double matrix.");
    }
    if (nrhs==2 && (!mxIsStruct(prhs[1])))
    {
      mexErrMsgTxt("The second and optional input must be a structure. Type \"help lsd\" to discover more.");
    }
    double scale=0.8;
    double sigma_coef=0.6;
    double quant=2.0;
    double ang_th=22.5;
    double log_eps=0.0;
    double density_th=0.7;
    int n_bins=1024;
    double width=1.5;

    double* ptr;
    if (nrhs==2)
    {
      int tmp=0;
      for( tmp=0; tmp<mxGetNumberOfFields(prhs[1]);tmp++)
      {
        if ( strcmp(mxGetFieldNameByNumber(prhs[1],tmp),"scale")==0)
        {
          if (!(mxIsDouble(mxGetField(prhs[1],0,mxGetFieldNameByNumber(prhs[1],0)))))
          {
            mexErrMsgTxt("A double argument was expected.");
          }
          if (mxGetNumberOfElements((mxGetField(prhs[1],0,mxGetFieldNameByNumber(prhs[1],0))))!=1)
          {
            mexErrMsgTxt("Only one value was expected.");
          }
          ptr=mxGetPr(mxGetField(prhs[1],0,mxGetFieldNameByNumber(prhs[1],tmp)));
          if(ptr[0]>=0)
          {
            scale=ptr[0];
            mexPrintf("ptr[0]=%e et scale=%e",ptr[0],scale);
          }
          else
          {
            mexErrMsgTxt("The scale value is not acceptable. For more information, type \"help lsd\"");
          }
        }

        if ( strcmp(mxGetFieldNameByNumber(prhs[1],tmp),"sigma_coef")==0)
        {
          if (!mxIsDouble(mxGetField(prhs[1],0,mxGetFieldNameByNumber(prhs[1],0))))
          {
            mexErrMsgTxt("A double argument was expected.");
          }
          if (!mxGetNumberOfElements((mxGetField(prhs[1],0,mxGetFieldNameByNumber(prhs[1],0)))))
          {
            mexErrMsgTxt("Only one value was expected.");
          }
          ptr=mxGetPr(mxGetField(prhs[1],0,mxGetFieldNameByNumber(prhs[1],tmp)));
          if(ptr[0]>=0)
          {
            sigma_coef=ptr[0];
          }
          else
          {
            mexErrMsgTxt("The sigma_coef value is not acceptable. For more information, type \"help lsd\"");
          }
        }

        if ( strcmp(mxGetFieldNameByNumber(prhs[1],tmp),"quant")==0)
        {
          if (!mxIsDouble(mxGetField(prhs[1],0,mxGetFieldNameByNumber(prhs[1],0))))
          {
            mexErrMsgTxt("A double argument was expected.");
          }
          if (!mxGetNumberOfElements((mxGetField(prhs[1],0,mxGetFieldNameByNumber(prhs[1],0)))))
          {
            mexErrMsgTxt("Only one value was expected.");
          }
          ptr=mxGetPr(mxGetField(prhs[1],0,mxGetFieldNameByNumber(prhs[1],tmp)));
          if(ptr[0]>=0)
          {
            quant=ptr[0];
          }
          else
          {
            mexErrMsgTxt("The quant value is not acceptable. For more information, type \"help lsd\"");
          }
        }

        if ( strcmp(mxGetFieldNameByNumber(prhs[1],tmp),"ang_th")==0)
        {
          if (!mxIsDouble(mxGetField(prhs[1],0,mxGetFieldNameByNumber(prhs[1],0))))
          {
            mexErrMsgTxt("A double argument was expected.");
          }
          if (!mxGetNumberOfElements((mxGetField(prhs[1],0,mxGetFieldNameByNumber(prhs[1],0)))))
          {
            mexErrMsgTxt("Only one value was expected.");
          }
          ptr=mxGetPr(mxGetField(prhs[1],0,mxGetFieldNameByNumber(prhs[1],tmp)));
          if(ptr[0]>=0 && ptr[0]<=180)
          {
            ang_th=ptr[0];
          }
          else
          {
            mexErrMsgTxt("The ang_th value is not acceptable. For more information, type \"help lsd\"");
          }
        }

        if ( strcmp(mxGetFieldNameByNumber(prhs[1],tmp),"log_eps")==0)
        {
          if (!mxIsDouble(mxGetField(prhs[1],0,mxGetFieldNameByNumber(prhs[1],0))))
          {
            mexErrMsgTxt("A double argument was expected.");
          }
          if (!mxGetNumberOfElements((mxGetField(prhs[1],0,mxGetFieldNameByNumber(prhs[1],0)))))
          {
            mexErrMsgTxt("Only one value was expected.");
          }
          ptr=mxGetPr(mxGetField(prhs[1],0,mxGetFieldNameByNumber(prhs[1],tmp)));
          log_eps=ptr[0];
        }

        if ( strcmp(mxGetFieldNameByNumber(prhs[1],tmp),"n_bins")==0)
        {
          if (!mxIsDouble(mxGetField(prhs[1],0,mxGetFieldNameByNumber(prhs[1],0))))
          {
            mexErrMsgTxt("A double argument was expected.");
          }
          if (!mxGetNumberOfElements((mxGetField(prhs[1],0,mxGetFieldNameByNumber(prhs[1],0)))))
          {
            mexErrMsgTxt("Only one value was expected.");
          }
          ptr=mxGetPr(mxGetField(prhs[1],0,mxGetFieldNameByNumber(prhs[1],tmp)));
          if(ptr[0]>=1)
          {
            n_bins=ptr[0];
          }
          else
          {
            mexErrMsgTxt("The n_bins value is not acceptable. For more information, type \"help lsd\"");
          }
        }

      }
    }
      double * image=mxGetPr(prhs[0]);
      int X=mxGetN(prhs[0]);
      int Y=mxGetM(prhs[0]);
      double * segs;
      int n;
      int dim = 7;
      int * region;
      int regX,regY;
      int i,j;

      /* execute LSD */
      segs = LineSegmentDetection( &n, image, X, Y,
                               scale,
                               sigma_coef,
                               quant,
                               ang_th,
                               log_eps,
                               density_th,
                               n_bins,
                               NULL,
                               &regX, &regY );
      /* The output which is created here will be an array */
      mwSize *dims;
      plhs[0]=mxCreateDoubleMatrix(n,dim,mxREAL);
      double* pointeur=(double*)mxGetPr(plhs[0]);
      int z=0;
      for(z=0;z<n;z++)
      {
        for(j=0;j<dim;j++)
        {
          pointeur[z+j*n]=segs[z*dim+j];
        }
      }
      z=0;
      j=0;
      for(z=0;z<n;z++)
      {
        for(j=0;j<dim;j++)
        {
        double c=pointeur[z+3*n];
        pointeur[z+3*n]=pointeur[z];
        pointeur[z]=c;
        c=pointeur[z+2*n];
        pointeur[z+2*n]=pointeur[z+n];
        pointeur[z+n]=c;
        }
      }
  } 
}
Ejemplo n.º 13
0
void SetupParams(int nrhs, const mxArray *prhs[]) {
	// PARAMETER DEFINITIONS GO HERE
	// First two paramers are feature matrix and initial cluster assignment (or emptry)
	int NumParams = nrhs-2;
	if (NumParams % 2 != 0)
		mexErrMsgTxt("Number of parameters should be even (i.e., param name, param value)");

	char buf[STRLEN];

	int ActualNumParam = NumParams/2;
	for (int paramiter=0;paramiter<ActualNumParam;paramiter++) {
			int strlen = (int) mxGetNumberOfElements(prhs[2+paramiter])+1;
			mxGetString(prhs[2+paramiter*2], buf, STRLEN);

		if (_strcmpi(buf, "MinClusters") == 0){
				MinClusters = (int) *(double*)mxGetPr(prhs[2+paramiter*2+1]);
				printf("Setting MinClusters to %d\n",MinClusters);
		} else if (_strcmpi(buf, "MaxClusters") == 0) {
			MaxClusters = (int) *(double*)mxGetPr(prhs[2+paramiter*2+1]);
			printf("Setting MaxClusters to %d\n",MaxClusters);
		} else if (_strcmpi(buf, "MaxPossibleClusters") == 0) {
			MaxPossibleClusters = (int) *(double*)mxGetPr(prhs[2+paramiter*2+1]);
			printf("Setting MaxPossibleClusters to %d\n",MaxPossibleClusters);
		} else if (_strcmpi(buf, "nStarts") == 0) {
			nStarts = (int) *(double*)mxGetPr(prhs[2+paramiter*2+1]);
			printf("Setting nStarts to %d\n",nStarts);
		} else if (_strcmpi(buf, "RandomSeed") == 0) {
			RandomSeed = (int) *(double*)mxGetPr(prhs[2+paramiter*2+1]);
			printf("Setting RandomSeed to %d\n",RandomSeed);
		} else if (_strcmpi(buf, "Verbose") == 0) {
			Verbose = (int) *(double*)mxGetPr(prhs[2+paramiter*2+1]);
			printf("Setting Verbose to %d\n",Verbose);
		} else if (_strcmpi(buf, "DistThresh") == 0) {
			DistThresh = (float) *(double*)mxGetPr(prhs[2+paramiter*2+1]);
			printf("Setting DistThresh to %f\n",DistThresh);
		} else if (_strcmpi(buf, "FullStepEvery") == 0) {
			FullStepEvery = (int) *(double*)mxGetPr(prhs[2+paramiter*2+1]);
			printf("Setting FullStepEvery to %d\n",FullStepEvery);
		} else if (_strcmpi(buf, "ChangedThresh") == 0) {
			ChangedThresh = (float) *(double*)mxGetPr(prhs[2+paramiter*2+1]);
			printf("Setting ChangedThresh to %f\n",ChangedThresh);
		} else if (_strcmpi(buf, "Screen") == 0) {
			Screen = (int) *(double*)mxGetPr(prhs[2+paramiter*2+1]);
			printf("Setting Screen to %d\n",Screen);
		} else if (_strcmpi(buf, "MaxIter") == 0) {
			MaxIter = (int) *(double*)mxGetPr(prhs[2+paramiter*2+1]);
			printf("Setting MaxIter to %d\n",MaxIter);
		} else if (_strcmpi(buf, "SplitEvery") == 0) {
			SplitEvery = (int) *(double*)mxGetPr(prhs[2+paramiter*2+1]);
			printf("Setting SplitEvery to %d\n",SplitEvery);
		} else if (_strcmpi(buf, "PenaltyMix") == 0) {
			PenaltyMix = (int) *(double*)mxGetPr(prhs[2+paramiter*2+1]);
			printf("Setting PenaltyMix to %d\n",PenaltyMix);
		} else if (_strcmpi(buf, "Subset") == 0) {
			Subset = (int) *(double*)mxGetPr(prhs[2+paramiter*2+1]);
			printf("Setting Subset to %d\n",Subset);
		} else
			mexErrMsgTxt("Unknown paramter");
	


	}
	/*
	INT_PARAM(MinClusters);
	INT_PARAM(MaxClusters);
	INT_PARAM(MaxPossibleClusters);
	INT_PARAM(nStarts);
	INT_PARAM(RandomSeed);
	INT_PARAM(Verbose);
	FLOAT_PARAM(DistThresh);
	INT_PARAM(FullStepEvery);
	FLOAT_PARAM(ChangedThresh);
	BOOLEAN_PARAM(Screen);
	INT_PARAM(MaxIter);
	INT_PARAM(SplitEvery);
	FLOAT_PARAM(PenaltyMix);
	INT_PARAM(Subset);
	*/
}
Ejemplo n.º 14
0
/* Function: mdlCheckParameters =============================================
 * Abstract:
 *    Validate our parameters to verify they are okay.
 */
static void mdlCheckParameters(SimStruct *S)
{
    int_T param;
    
    for( param=0; param<NPARAMS; param++ )
    {
        if ( (param != 11) && (param != 12) )
        if ( !IS_PARAM_DOUBLE(ssGetSFcnParam(S, param)) && (mxGetNumberOfElements(ssGetSFcnParam(S, param)) < 1) )
        {
            ssSetErrorStatus(S, "Parameter is not a double.");
            return;
        }
    }
    
    if( (mxGetNumberOfElements(paramQuaternionIndex) != 0) && (mxGetNumberOfElements(paramQuaternionIndex) != 2) )
    {
        ssSetErrorStatus(S, "The quaternion index must be defined by two numbers.");
        return;
    }
    
    if( mxGetNumberOfElements(paramQuaternionIndex) == 2 )
    {
        if( !IS_PARAM_DOUBLE(paramQuaternionIndex) )
        {
            ssSetErrorStatus(S, "The quaternion index must be defined by of type double.");
            return;
        }
        if( (intval(mxGetPr(paramQuaternionIndex)[0]) < 1) || (intval(mxGetPr(paramQuaternionIndex)[0]) > intval(mxGetPr(paramQuaternionIndex)[1])) )
        {
            ssSetErrorStatus(S, "The quaternion index interval is not defined properly.");
            return;
        }
        
        param = intval(mxGetPr(paramQuaternionIndex)[1]) - intval(mxGetPr(paramQuaternionIndex)[0]) + 1;
        if( param != (param/4)*4 )
        {
            ssSetErrorStatus(S, "The quaternion index interval must have a length multiple of 4.");
            return;
        }
    }
    
    if( (mxGetNumberOfElements(paramOmegaDotIndex) != 0) && (mxGetNumberOfElements(paramOmegaDotIndex) != 2) )
    {
        ssSetErrorStatus(S, "The omega_dot index must be defined by two numbers.");
        return;
    }
    
    if( mxGetNumberOfElements(paramOmegaDotIndex) == 2 )
    {
        if( !IS_PARAM_DOUBLE(paramOmegaDotIndex) )
        {
            ssSetErrorStatus(S, "The omega_dot index must be defined by of type double.");
            return;
        }
        if( (intval(mxGetPr(paramOmegaDotIndex)[0]) < 1) || (intval(mxGetPr(paramOmegaDotIndex)[0]) > intval(mxGetPr(paramOmegaDotIndex)[1])) )
        {
            ssSetErrorStatus(S, "The omega_dot index interval is not defined properly.");
            return;
        }
        
        param = intval(mxGetPr(paramOmegaDotIndex)[1]) - intval(mxGetPr(paramOmegaDotIndex)[0]) + 1;
        if( param != (param/3)*3 )
        {
            ssSetErrorStatus(S, "The omega_dot index interval must have a length multiple of 3.");
            return;
        }
    }

    if ( !((mxGetPr(paramSampleTime)[0] == -1) || (mxGetPr(paramSampleTime)[0] > 0)) )
    {
        ssSetErrorStatus(S, "The sample time must be inherited (-1) or discrete (a positive number).");
        return;
    }
}
void mexFunction(int nlhs, mxArray* plhs[],
                 const int nrhs, const mxArray* prhs[]) {
  
  if (nrhs != 3) {
    mexPrintf("Exactly 3 arguments required.");
  }
  
  // Grab the region mask (HxW).
  int H = mxGetM(prhs[ARG_IMG_REGIONS]);
  int W = mxGetN(prhs[ARG_IMG_REGIONS]);
  int N = H * W;
  
  if (mxGetClassID(prhs[ARG_IMG_REGIONS]) != mxINT32_CLASS) {
    mexErrMsgTxt("imgRegions must be of class 'int32'");
  }
  int* img_regions = (int*) mxGetData(prhs[ARG_IMG_REGIONS]);
  
  // Get the number of regions.
  int R = (int) mxGetScalar(prhs[ARG_NUM_REGIONS]);
  
  // Grab the eroded region mask.
  if (mxGetNumberOfDimensions(prhs[ARG_IMG_ERODED]) != 2
      || mxGetNumberOfElements(prhs[ARG_IMG_ERODED]) != N) {
    mexErrMsgTxt("imgEroded must be of size HxW");
  } else if (mxGetClassID(prhs[ARG_IMG_ERODED]) != mxLOGICAL_CLASS) {
    mexErrMsgTxt("imgEroded must be of class 'logical'");
  }
  bool* img_eroded = (bool*) mxGetData(prhs[ARG_IMG_ERODED]);
  
  // Create the outputs.
  mwSize ndims_out = 3;
  mwSize dims_out[] = {H, W, R};
  plhs[RET_INTERIORS] = mxCreateLogicalArray(ndims_out, &dims_out[0]);
  plhs[RET_EDGES] = mxCreateLogicalArray(ndims_out, &dims_out[0]);
  
  mwSize ndims_out_size = 2;
  mwSize dims_out_size[] = {R, 1};

  bool* img_interiors = (bool*) mxGetData(plhs[RET_INTERIORS]);
  bool* img_edges = (bool*) mxGetData(plhs[RET_EDGES]);
  
  // unsigned int num_pix_interiors[R];
  // unsigned int num_pix_edges[R];
  unsigned int * num_pix_interiors = new unsigned int [R];
  unsigned int * num_pix_edges = new unsigned int [R];

  for (int rr = 0; rr < R; ++rr) {
    num_pix_interiors[rr] = 0;
    num_pix_edges[rr] = 0;
  }

  for (int ii = 0; ii < N; ++ii, ++img_eroded) {
    // Grab the Region ID.
    int region_id = img_regions[ii] - 1;
    
    // Is it in the interior?
    if (*img_eroded) {
      img_interiors[region_id * N + ii] = true;
      ++num_pix_interiors[region_id];
    } else {
      img_edges[region_id * N + ii] = true;
      ++num_pix_edges[region_id];
    }
  }
  
  // Now that we know how many pixels make up the interiors and borders,
  // if we have any interiors or border regions that are empty, just use
  // the entire mask instead.
  for (int ii = 0; ii < N; ++ii) {
    // Grab the Region ID.
    int region_id = img_regions[ii] - 1;
    
    if (num_pix_interiors[region_id] == 0) {
      img_interiors[region_id * N + ii] = true;
    }
    
    if (num_pix_edges[region_id] == 0) {
      img_edges[region_id * N + ii] = true;
    }
  }
}
Ejemplo n.º 16
0
/* Entry point */
void mexFunction(
	int nlhs, 	mxArray *plhs[],
	int nrhs, const mxArray *prhs[])
{
	int*	channels;
	int		channelsNumber;
	float*	gains;
	int		gainsNumber;

	/* Load MCHA library */
	Mcha	mcha;
	if ( !mcha.noError() )
	{	
		mexErrMsgTxt( mcha.getErrorStr() );	
		return;
	}
    
	errorMsg = mxCreateString("");

	/* Check the number of input/output arguments */
	if ((nrhs != 2) || (nlhs>1)) 
	{
		errorMsg = mxCreateString("Wrong call to function.");
		return;
	}

	/* Check if the data is valid */
	if (!mxIsNumeric(channelsArray) || !mxIsNumeric(gainsArray)  )
	{
		errorMsg = mxCreateString("Wrong call to function: channels and gains arrays should be numeric vectors");
		return;
	}

	/* Get the size of the channels matrix */
	double* channelsData = mxGetPr(channelsArray);
	channelsNumber = (int) mxGetNumberOfElements(channelsArray);
    double* gainsData = mxGetPr(gainsArray);
	gainsNumber = (int) mxGetNumberOfElements(gainsArray);
	
    /* Check is the lengths are the same */
    if (channelsNumber != gainsNumber)
    {
 		errorMsg = mxCreateString("Wrong call to function: the lenghts of the channels and gains arrays should be the same");
		return;   
    }
    
	/* Create and fill channels and gains arrays */
	if (channelsNumber > 0)
	{
		channels = new int[channelsNumber];
		gains = new float[channelsNumber];
		for (int i= 0; i<channelsNumber; i++)
		{
			channels[i] = (int) channelsData[i];
			gains[i] = (float) gainsData[i];
		}
		
		/* Call the function */
		if ( !mcha.setGain (channels, channelsNumber, gains) )
		{
			errorMsg = mxCreateString( mcha.getLastError() );	
		}
	}

	/* Clean up */
	delete [] channels;
	delete [] gains;
}
Ejemplo n.º 17
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
    
    /* inputs:
     * image array
     * prmVect
     * mode
     * {options}
     */
    
    dataStruct_t data;
    
    /* check inputs */
    if (nrhs < 3) mexErrMsgTxt("Inputs should be: data, prmVect, mode.");
    if (!mxIsDouble(prhs[0])) mexErrMsgTxt("Data input must be double array.");
    size_t nx = mxGetN(prhs[0]);
    size_t ny = mxGetM(prhs[0]);
    if (nx != ny) mexErrMsgTxt("Input image must be a square.");
    if (!(nx % 2)) mexErrMsgTxt("The side of the input image must be odd.");
    int N = nx*ny;
    if (mxGetNumberOfElements(prhs[1])!=NPARAMS || !mxIsDouble(prhs[1])) mexErrMsgTxt("Incorrect parameter vector format.");
    if (!mxIsChar(prhs[2])) mexErrMsgTxt("Mode needs to be a string.");
    if (nrhs < 4) {
        data.maxIter = 500;
        data.eAbs = 1e-8;
        data.eRel = 1e-8;
    } else {
        if (!mxIsDouble(prhs[3]) || mxGetNumberOfElements(prhs[3])!=3) mexErrMsgTxt("Options must must be double array with 3 elements.");
        double *options = mxGetPr(prhs[3]);
        data.maxIter = options[0];
        data.eAbs = options[1];
        data.eRel = options[2];
    }
    
    
    /* read mode input */
    int np = (int)mxGetNumberOfElements(prhs[2]);
    char *mode;
    mode = (char*)malloc(sizeof(char)*(np+1));
    mxGetString(prhs[2], mode, np+1);
    
    int i;
    for (i=0; i<strlen(mode); ++i) {
        mode[i] = tolower(mode[i]);
    }
    
    np = 0; /* number of parameters to fit */
    for (i=0; i<NPARAMS; ++i) {
        if (strchr(mode, refMode[i])!=NULL) { np++; }
    }
    if (np==0) mexErrMsgTxt("Unknown mode.");
    
    /* allocate */
    data.nx = nx;
    data.np = np;
    data.pixels = mxGetPr(prhs[0]);
    data.gx = (double*)malloc(sizeof(double)*nx);
    data.gy = (double*)malloc(sizeof(double)*nx);
    data.estIdx = (int*)malloc(sizeof(int)*np);
    memcpy(data.prmVect, mxGetPr(prhs[1]), NPARAMS*sizeof(double));
    data.dfunc = (pfunc_t*) malloc(sizeof(pfunc_t) * np);
    
    /* read mask/pixels */
    data.nValid = N;
    for (i=0; i<N; ++i) {
        data.nValid -= (int)mxIsNaN(data.pixels[i]);
    }
    if (data.nValid < 5) mexErrMsgTxt("Input image must contain at least 5 data points.");

    data.idx = (int*)malloc(sizeof(int)*data.nValid);
    int *nanIdx = (int*)malloc(sizeof(int)*(N-data.nValid));
    int k = 0, l = 0;
    for (i=0; i<N; ++i) {
        if (!mxIsNaN(data.pixels[i])) {
            data.idx[k++] = i;
        } else {
            nanIdx[l++] = i;
        }
    }
    
    np = 0;
    if (strchr(mode, 'x')!=NULL) {data.estIdx[np] = 0; data.dfunc[np++] = df_dx;}
    if (strchr(mode, 'y')!=NULL) {data.estIdx[np] = 1; data.dfunc[np++] = df_dy;}
    if (strchr(mode, 'a')!=NULL) {data.estIdx[np] = 2; data.dfunc[np++] = df_dA;}
    if (strchr(mode, 's')!=NULL) {data.estIdx[np] = 3; data.dfunc[np++] = df_ds;}
    if (strchr(mode, 'c')!=NULL) {data.estIdx[np] = 4; data.dfunc[np++] = df_dc;}
    
    data.x_init = (double*)malloc(sizeof(double)*np);
    for (i=0; i<np; ++i) {
        data.x_init[i] = data.prmVect[data.estIdx[i]];
    }
    
    MLalgo(&data);
    
    /* parameters */
    if (nlhs > 0) {
        plhs[0] = mxCreateDoubleMatrix(1, NPARAMS, mxREAL);
        memcpy(mxGetPr(plhs[0]), data.prmVect, NPARAMS*sizeof(double));
    }
    
    /* standard dev. of parameters & covariance matrix */
    double RSS = 0.0;
    double* resValid = NULL;
    if (nlhs > 1) {    
        resValid = (double*)malloc(data.nValid*sizeof(double));
        for (i=0; i<data.nValid; ++i) {
            resValid[i] = gsl_vector_get(data.residuals, i);
            RSS += resValid[i]*resValid[i];
        }
        gsl_matrix *covar = gsl_matrix_alloc(np, np);
        gsl_multifit_covar(data.J, 0.0, covar);
        double iRSS = RSS/(data.nValid - data.np - 1);
        plhs[1] = mxCreateDoubleMatrix(1, data.np, mxREAL);
        double *prmStd = mxGetPr(plhs[1]);
        for (i=0; i<data.np; ++i) {
            prmStd[i] = sqrt(iRSS*gsl_matrix_get(covar, i, i));
        }
        if (nlhs > 2) {
            plhs[2] = mxCreateDoubleMatrix(np, np, mxREAL);
            /* cov. matrix is symmetric, no need to transpose */
            memcpy(mxGetPr(plhs[2]), covar->data, np*np*sizeof(double));
        }
        gsl_matrix_free(covar);
    }
    
    /* residuals */
    if (nlhs > 3) {
        const char *fieldnames[] = {"data", "hAD", "mean", "std", "RSS"};
        mwSize dims[2] = {1, 1};
        plhs[3] = mxCreateStructArray(2, dims, 5, fieldnames);
        mxArray *val = mxCreateDoubleMatrix(nx, nx, mxREAL);
        double* res = mxGetPr(val);
        
        double mean = 0.0, std = 0.0;
        for (i=0; i<data.nValid; ++i) {
            res[data.idx[i]] = resValid[i];
            mean += resValid[i];
        }
        std = sqrt((RSS-mean*mean/data.nValid)/(data.nValid-1));
        mean /= data.nValid;

        for (i=0; i<N-data.nValid; ++i) {
            res[nanIdx[i]] = mxGetNaN();
        }
        
        // A-D test, case 2: mean known
        unsigned char hAD = adtest(resValid, data.nValid, 2, 0.0, std, 0.05);
        mxSetFieldByNumber(plhs[3], 0, 0, val);
        mxSetFieldByNumber(plhs[3], 0, 1, mxCreateLogicalScalar(hAD));
        mxSetFieldByNumber(plhs[3], 0, 2, mxCreateDoubleScalar(mean));
        mxSetFieldByNumber(plhs[3], 0, 3, mxCreateDoubleScalar(std));
        mxSetFieldByNumber(plhs[3], 0, 4, mxCreateDoubleScalar(RSS));
    }
    
    /* Jacobian */
    if (nlhs > 4) {
        /* convert row-major double* data.J->data to column-major double* */
        plhs[4] = mxCreateDoubleMatrix(N, np, mxREAL);
        double *J = mxGetPr(plhs[4]);
        int k;
        for (k=0; k<np; ++k) {
            for (i=0; i<data.nValid; ++i) {
                J[data.idx[i]+k*N] = gsl_matrix_get(data.J, i, k);
            }
            for (i=0; i<N-data.nValid; ++i) {
                J[nanIdx[i]+k*N] = mxGetNaN();
            }
        }
    }
    
    free(resValid);
    gsl_matrix_free(data.J);
    gsl_vector_free(data.residuals);
    free(data.x_init);
    free(nanIdx);
    free(data.idx);
    free(data.dfunc);
    free(data.estIdx);
    free(data.gy);
    free(data.gx);
    free(mode);
}
Ejemplo n.º 18
0
int MatlabBufferGadget::process(GadgetContainerMessage<IsmrmrdReconData>* m1)
{
	// Initialize a string for matlab commands
	std::string cmd;

	auto recon_data = m1->getObjectPtr();
	mwSize nencoding_spaces = recon_data->rbit_.size();

	const char* fieldnames[2] = {"data","reference"};
	auto reconArray = mxCreateStructArray(1,&nencoding_spaces,2,fieldnames);
	//auto reconArray = mxCreateCellArray(1,&nencoding_spaces);

	for (int i = 0; i <  recon_data->rbit_.size(); i++){
		auto mxrecon = BufferToMatlabStruct(&recon_data->rbit_[i].data_);
		mxSetField(reconArray,i,"data",mxrecon);
		if (recon_data->rbit_[i].ref_){
			auto mxref = BufferToMatlabStruct(recon_data->rbit_[i].ref_.get_ptr());
			mxSetField(reconArray,i,"reference",mxref);
		}

	}
	engPutVariable(engine_, "recon_data", reconArray);

	cmd = "[imageQ,bufferQ] = matgadget.run_process(recon_data); matgadget.emptyQ(); whos()";
	send_matlab_command(cmd);

	// Get the size of the gadget's queue

	mxArray *imageQ = engGetVariable(engine_, "imageQ");
	if (imageQ == NULL) {
		GERROR("Failed to get the imageQ from matgadget\n");
		return GADGET_FAIL;
	}

	size_t qlen = mxGetNumberOfElements(imageQ);
	GDEBUG("Image Queue size: %d \n", qlen);

	const mwSize* dims = mxGetDimensions(imageQ);
	mwSize ndims = mxGetNumberOfDimensions(imageQ);



	//Read all Image bytes
	for (mwIndex idx = 0; idx < qlen; idx++) {
		mxArray *res_hdr  = mxGetField(imageQ, idx, "bytes");
		mxArray *res_data = mxGetField(imageQ, idx, "image");

		GadgetContainerMessage<ISMRMRD::ImageHeader>* m3 =
				new GadgetContainerMessage<ISMRMRD::ImageHeader>();
		ISMRMRD::ImageHeader *hdr_new = m3->getObjectPtr();
		memcpy(hdr_new, mxGetData(res_hdr), sizeof(ISMRMRD::ImageHeader));

		auto image= MatlabToHoNDArray<std::complex<float>>(res_data);
		auto m4 = new GadgetContainerMessage< hoNDArray< std::complex<float> > >(image);
		auto dims = *image->get_dimensions();

		delete image;
		m3->cont(m4);
		if (this->next()->putq(m3) < 0) {
			GDEBUG("Failed to put Image message on queue\n");
			return GADGET_FAIL;
		}

	}
	//Match engGetVariable with mxDestroy___s


	mxArray* bufferQ = engGetVariable(engine_,"bufferQ");

	qlen = mxGetNumberOfElements(bufferQ);
	GDEBUG("Buffer Queue size: %d \n", qlen);

	for (mwIndex idx = 0; idx <qlen; idx++){

		IsmrmrdReconData output_data;
		IsmrmrdReconBit bit;
		bit.data_ = MatlabStructToBuffer(mxGetField(bufferQ,idx,"data"));

		auto ref = mxGetField(bufferQ,idx,"reference");
		if (ref){
			GDEBUG("Adding reference");
			bit.ref_ = MatlabStructToBuffer(ref);
		}
		output_data.rbit_.push_back(bit);
		auto m3 = new GadgetContainerMessage<IsmrmrdReconData>(output_data.rbit_);
		if (this->next()->putq(m3) < 0){
			GDEBUG("Failed to put Buffer message on queue\n");
			return GADGET_FAIL;
		}

	}





	mxDestroyArray(bufferQ);
	mxDestroyArray(imageQ);
	//mxDestroyArray(reconArray); //We're not supposed to delete this?

	// We are finished with the incoming messages m1 and m2
	m1->release();

	return GADGET_OK;
}
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) {
    
    // INPUT:
    float *pfY, *pfBias, *pfWeight;
    
    // OUTPUT:
    float *pfYout;
    mwSize ndims_out, *dims_out;
    
    // Local:    
    const mxArray * pArg;
    // mwSize nrowsX,ncolsX, nrowsY1, ncolsY1, nrowsY2, ncolsY2;

        
    //long Nx, nshift, Nvecs, Ny;
    long h, w, kH, kW, dH, dW, nInputPlanes, nInputPlanes_y, h_out, w_out, nOutputPlanes;
    long nBias;
        
    
    /* --------------- Check inputs ---------------------*/
    if (nrhs != 5)
        mexErrMsgTxt("5 inputs required");
    if (nlhs > 1)  
        mexErrMsgTxt("only one output allowed");
    
    /// ------------------- Y ----------
	pArg = prhs[0];    
	if (!mxIsNumeric(pArg) || !mxIsSingle(pArg) || mxIsEmpty(pArg) || mxIsComplex(pArg) )
            mexErrMsgTxt("Input 1 (Y) must be a noncomplex matrix of singles.");
        
    pfY  = (float*) mxGetData(prhs[0]);
    h = mxGetM(pArg);
    w = mxGetSize(pArg, 2);
    nInputPlanes_y = (long) mxGetSize(pArg, 3);
    
    //mexPrintf("Size Y = %d x %d x %d\n", mxGetSize(pArg, 1), mxGetSize(pArg, 2), mxGetSize(pArg, 3));
    /// ------------------- Bias ----------
    pArg = prhs[1];
	if(!mxIsNumeric(pArg) || !mxIsSingle(pArg) || mxIsEmpty(pArg) || mxIsComplex(pArg) ) { 
            mexErrMsgTxt("Input 2 (Bias) must be a noncomplex single matrix.");
    }
    
    pfBias = (float*) mxGetData(pArg);
    nBias = (long) mxGetNumberOfElements(pArg);

            
    /// ------------------- Weight ----------
    pArg = prhs[2];
	if(!mxIsNumeric(pArg) || !mxIsSingle(pArg) || mxIsEmpty(pArg) || mxIsComplex(pArg) ) { 
            mexErrMsgTxt("Input 3 (Weight) must be a noncomplex single matrix.");
    }
    
    pfWeight = (float*) mxGetData(pArg);
    kH = mxGetM(pArg);
    kW = mxGetSize(pArg, 2); 
    nInputPlanes = mxGetSize(pArg, 3);
    nOutputPlanes = mxGetSize(pArg, 4);
    
    if (nInputPlanes_y  != nInputPlanes) {
        mexPrintf("number of input planes in weight mtx: %d. number of input planes in y : %d\n", nInputPlanes, nInputPlanes_y);
        mexErrMsgTxt("number of input planes in weight mtx must be the same as the number of input planes in y");
    }

    
    if (nBias != nOutputPlanes) {
        mexPrintf("number of elements in Bias : %d. size3 = %d\n", nBias, nOutputPlanes);
        mexErrMsgTxt("number of elements in Bias must be the same size of dimension 3 of weights");
    }
    //mexPrintf("Size Weight = %d x %d x %d\n", kH, kW, nOutputPlanes);
    //mexPrintf("Size Weight = %d x %d x %d\n", mxGetSize(pArg, 1), mxGetSize(pArg, 2), mxGetSize(pArg, 3));

    /// ------------------- dH ----------
    pArg = prhs[3];
	if(!mxIsNumeric(pArg) || !mxIsSingle(pArg) || mxIsEmpty(pArg) || mxIsComplex(pArg) || (mxGetNumberOfElements(pArg) > 1) ) { 
        mexErrMsgTxt("Input 4 (dH) must be a noncomplex single scalar.");
    }
    dH = (long) mxGetScalar(pArg);
    
    /// ------------------- dW ----------
    pArg = prhs[4];
	if(!mxIsNumeric(pArg) || !mxIsSingle(pArg) || mxIsEmpty(pArg) || mxIsComplex(pArg) || (mxGetNumberOfElements(pArg) > 1) ) { 
        mexErrMsgTxt("Input 5 (dW) must be a noncomplex single scalar.");
    }
    dW = (long) mxGetScalar(pArg);
        
    
    h_out = floor( (h - kH)/ dH) + 1;
    w_out = floor( (w - kW)/ dW) + 1;
    //mexPrintf("h, w, kH, kW, dH, dW = %d, %d, %d, %d, %d, %d\n", h, w, kH, kW, dH, dW);
    
//  y_out1 = zeros(h_out, w_out, nOutputPlanes);    
//    pfYout = (float***) mxCalloc(h_out * w_out * nOutputPlanes);
    
    /// ------------------- Yout (OUTPUT)----------        
    ndims_out = 3;
    dims_out = (mwSize*) mxCalloc(ndims_out, sizeof(mwSize));
    dims_out[0] = (mwSize) h_out;
    dims_out[1] = (mwSize) w_out;
    dims_out[2] = (mwSize) nOutputPlanes;
            
    //mexPrintf("Creating output ... \n");
    //mexPrintf("Size Yout (before) (a) = %d x %d x %d\n", h_out, w_out, nOutputPlanes);
    //mexPrintf("Size Yout (before) (b) = %d x %d x %d\n", dims_out[0], dims_out[1], dims_out[2]);
        
    
    plhs[0] = mxCreateNumericArray(ndims_out, dims_out,
         mxSINGLE_CLASS, mxREAL);
    pfYout = (float*) mxGetData(plhs[0]);

    //mexPrintf("Size Yout = %d x %d x %d\n", mxGetSize(plhs[0], 1), mxGetSize(plhs[0], 2), mxGetSize(plhs[0], 3));

            
    nn_spatialConvolution(pfY, pfBias, pfWeight, dH, dW, 
        h, w, kH, kW, nInputPlanes, nOutputPlanes, pfYout);


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

    int   SSEnable = 0;
    
    if (nrhs < 1) {
        mexErrMsgTxt("One input required.");
    } else if (nlhs != 1) {
        mexErrMsgTxt("One output argument required.");
    }
    
    // I am expecting an nxm array, where n is the # of clusters
    // and m is the number of features.
    mwSize nrows = mxGetM(prhs[0]);
    mwSize ncols = mxGetN(prhs[0]);
    mwSize elements = mxGetNumberOfElements(prhs[0]);
    mwSize number_of_dims=mxGetNumberOfDimensions(prhs[0]);
    
    if (!mxIsDouble(prhs[0])) {
        mexErrMsgTxt("Input must be a double array.");
    }
    
    if (nrhs > 1) {
        SSEnable = 1;
    }
    
    plhs[0] = mxCreateDoubleMatrix(1,3,mxREAL);
    
    double *pOut = mxGetPr(plhs[0]);
    
    pOut[0] = 12.2;
    pOut[1] = 1.12;
    pOut[2] = 12;
    
    double *pFa = mxGetPr(prhs[0]);
    
    double *pCol[10];
    for (int z=0;z<ncols;z++) {
        pCol[z] = pFa+z*nrows;
    }

    double dMin = 1e12;
    
    // Data is in column major order... so, 
    // x and y can point to each column, then
    // iterate on features by adding x/y + mcols
    double fsum;
    for (int x=0;x<nrows;x++) {
        for (int y=0;y<nrows;y++) {
            if (y != x) {
                fsum = 0;
                for (int z=0;z<ncols;z++) {
                    // Simple squared function.
                    double t = (pCol[z][x]-pCol[z][y]);
                    fsum += t*t;
                    //fsum += (pFa[x+z*nrows]-pFa[y+z*nrows])*(pFa[x+z*nrows]-pFa[y+z*nrows]);
                }
                //fsum = sqrt(fsum); // Don't bother with the sqrt.. we just want t relative value..
                if (fsum < dMin) {
                    dMin = fsum;
                    pOut[0] = double(x+1);
                    pOut[1] = double(y+1);
                    pOut[2] = dMin;
                    
                    // This is an optimization. If a lot of pixels are close to zero in distance,
                    // then it is not that important to find the "closest" of those.
                    if (dMin < 0.02) {
                        return;
                    }
                }
            }
        }
    }
}
Ejemplo n.º 21
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

	/* Check number of input arguments */
	if (nrhs > 9) {
		ERROR("Nine or fewer input arguments are required.");
    } else if (nrhs < 4) {
		ERROR("At least four input arguments are required.");
    }

	/* Check number of output arguments */
	if (nlhs > 1) {
		ERROR("Too many output arguments.");
    }

	DOUBLE *X = (DOUBLE*) mxGetData(prhs[0]);

	CHAR distLabelName;
	if (mxGetNumberOfElements(prhs[1]) != 0) {
		if (!mxIsChar(prhs[1])) {
			ERROR("Second argument must be of type CHAR.");
		}
		distLabelName = (CHAR)*(CHAR*) mxGetData(prhs[1]);
	} else {
		distLabelName = 'R';
	}

	DOUBLE *matlabConstraintMat = (DOUBLE*) mxGetData(prhs[2]);
	DOUBLE *tau = (DOUBLE*) mxGetData(prhs[3]);

	INT N = (INT) mxGetM(prhs[0]);
	INT numSamples = (INT) mxGetN(prhs[0]);
	INT numConstraints = (INT) mxGetM(prhs[2]);
	INT numRepeats = (INT) mxGetNumberOfElements(prhs[3]);

	DOUBLE tolerance;
	if ((nrhs >= 5) && (mxGetNumberOfElements(prhs[4]) != 0)) {
		tolerance = *(DOUBLE*) mxGetData(prhs[4]);
	} else {
		tolerance = 0.00001;
	}

	DOUBLE delta;
	if ((nrhs >= 6) && (mxGetNumberOfElements(prhs[5]) != 0)) {
		delta = *(DOUBLE*) mxGetData(prhs[5]);
	} else {
		delta = 0.00001;
	}

	INT numIters;
	if ((nrhs >= 7) && (mxGetNumberOfElements(prhs[6]) != 0)) {
		numIters = (INT)*(DOUBLE*) mxGetData(prhs[6]);
	} else {
		numIters = 100000;
	}

	DOUBLE tauMultiplier;
	if ((nrhs >= 8) && (mxGetNumberOfElements(prhs[7]) != 0)) {
		tauMultiplier = *(DOUBLE*) mxGetData(prhs[7]);
	} else {
		tauMultiplier = 10000;
	}

	DOUBLE tauRate;
	if ((nrhs >= 9) && (mxGetNumberOfElements(prhs[8]) != 0)) {
		tauRate = *(DOUBLE*) mxGetData(prhs[8]);
	} else {
		tauRate = 0.9;
	}

	INT *constraintMat;
	DOUBLE *betaVec;
	DIST_LABEL_TYPE distLabelType = convertDistLabelName(distLabelName);
	if (distLabelType == DIST_LABEL_TARGETS) {
		if (mxGetN(prhs[2]) != 3) {
			ERROR("Constraint matrix does not meet specified format: second dimension not equal to three.");
		}
		constraintMat = (INT *) MALLOC(numConstraints * 2 * sizeof(INT));
		betaVec = (DOUBLE *) MALLOC(numConstraints * 1 * sizeof(DOUBLE));
	} else if (distLabelType == DIST_LABEL_BOUNDS) {
		if (mxGetN(prhs[2]) != 4) {
			ERROR("Constraint matrix does not meet specified format: second dimension not equal to four.");
		}
		constraintMat = (INT *) MALLOC(numConstraints * 3 * sizeof(INT));
		betaVec = (DOUBLE *) MALLOC(numConstraints * 1 * sizeof(DOUBLE));
	} else if ((distLabelType == DIST_LABEL_RELATIONAL) \
				|| (distLabelType == DIST_LABEL_SQRHINGE) \
				|| (distLabelType == DIST_LABEL_HUBERHINGE)) {
		if (mxGetN(prhs[2]) != 5) {
			ERROR("Constraint matrix does not meet specified format: second dimension not equal to five.");
		}
		constraintMat = (INT *) MALLOC(numConstraints * 4 * sizeof(INT));
		betaVec = (DOUBLE *) MALLOC(numConstraints * 1 * sizeof(DOUBLE));
	}
	convertDistanceLabelMat(constraintMat, betaVec, distLabelType, \
					matlabConstraintMat, numConstraints, numSamples);

	plhs[0] = mxCreateNumericMatrix(N, N, MXPRECISION_CLASS, mxREAL);
	DOUBLE *A = (DOUBLE *) mxGetData(plhs[0]);

	if (numRepeats > 1) {
		nrml_fpc(A, X, distLabelType, constraintMat, betaVec, tau, delta, \
				numIters, tolerance, N, numConstraints, numRepeats);
	} else {
		nrml_fpc_continuation(A, X, distLabelType, constraintMat, betaVec, *tau, \
				delta, numIters, tolerance, tauMultiplier, tauRate, N, \
				numConstraints);
	}

	FREE(constraintMat);
	FREE(betaVec);
}
Ejemplo n.º 22
0
/*
 * The mex function runs a MST 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;
    
    /* matching */
    mwIndex *m;
    double *m_double;
    
    /* output */
    int verify = 0;
    
    /* 
     * The current calling pattern is
     * test_matching_mex(A,matching)
     */
    
    const mxArray* arg_matrix;
    const mxArray* arg_matching;
    int required_arguments = 2;
    
    if (nrhs != required_arguments) {
        mexErrMsgIdAndTxt("matlab_bgl:invalidMexArgument",
            "the function requires %i arguments, not %i\n", 
            required_arguments, nrhs);
    }
    
    arg_matrix = prhs[0];
    arg_matching = prhs[1];
    
    /* The first input must be a sparse matrix. */
    mrows = mxGetM(arg_matrix);
    ncols = mxGetN(arg_matrix);
    if (mrows != ncols || !mxIsSparse(arg_matrix)) {
        mexErrMsgIdAndTxt("matlab_bgl:invalidMexArgument",
            "the matrix must be sparse and square");
    }

    n = mrows;
    
    /* The second input must be of size n */
    if (mxGetNumberOfElements(arg_matching) != n) {
        mexErrMsgIdAndTxt("matlab_bgl:invalidMexArgument",
            "the matching must be size %i not %i", n,
            mxGetNumberOfElements(arg_matching));
    }
    m_double = mxGetPr(arg_matching);
    m = mxCalloc(n, sizeof(mwIndex));
    for (i=0; i < n; i++) {
        m[i] = (mwIndex)m_double[i] ;
        if (m[i] == 0) { m[i] = n; }
        else { --m[i]; }
    }
    
    /* Get the sparse matrix */
    
    /* recall that we've transposed the matrix */
    ja = mxGetIr(arg_matrix);
    ia = mxGetJc(arg_matrix);
    
    nz = ia[n];
    
    plhs[0] = mxCreateDoubleMatrix(1,1,mxREAL);

    #ifdef _DEBUG
    mexPrintf("test_maximum_cardinality_matching...");
    #endif 

    test_maximum_cardinality_matching(n, ja, ia, m, &verify);
    
    *mxGetPr(plhs[0]) = (double)verify;
    
    #ifdef _DEBUG
    mexPrintf("return\n");
    #endif 
    
}
Ejemplo n.º 23
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  /*declare variables*/
  const mwSize *dims;
  mwIndex indx;
  int i, numdims, indx1, indx2, indx3;
  int numelin;
  mxClassID classid;
  double *input1r, *input1i, *input2r, *input2i, *output1r, *output1i;
  double a[3][3],  b[3][3];
  double ai[3][3], bi[3][3];
  
  /*figure out the classid*/
  classid = mxGetClassID(prhs[0]);
     
  /*check inputs*/
  if (nrhs!=2)
    mexErrMsgTxt("Wrong number of input arguments");
  
  /*associate inputs*/
  input1r = mxGetData(prhs[0]);
  input1i = mxGetImagData(prhs[0]);
  
  input2r = mxGetData(prhs[1]);
  input2i = mxGetImagData(prhs[1]);
  
  /*figure out dimension info and number of elements*/
  dims    = mxGetDimensions(prhs[0]);
  numdims = mxGetNumberOfDimensions(prhs[0]);
  numelin = mxGetNumberOfElements(prhs[0]);
    
  /*associate output*/
  if (input1i == NULL && input2i == NULL)
  {
    plhs[0]  = mxCreateNumericArray(numdims, dims, classid, mxREAL);
    output1r = mxGetData(plhs[0]);
  }
  else
  {
    plhs[0]  = mxCreateNumericArray(numdims, dims, classid, mxCOMPLEX);
    output1r = mxGetData(plhs[0]);
    output1i = mxGetImagData(plhs[0]);
  }
  
  /* do the computation*/
  if (input1i == NULL && input2i == NULL)
  {  
    for (i=0; i<numelin/9; i++)
    {
      /* real-valued case*/
      for (indx2=0; indx2<3; ++indx2)
        for (indx1=0; indx1<3; ++indx1)
          {
           a[indx1][indx2] = input1r[i*9+indx1+indx2*3];
           b[indx1][indx2] = input2r[i*9+indx1+indx2*3];
          
           output1r[i*9+indx1+indx2*3] = 0;
          }
      
      for (indx3=0; indx3<3; ++indx3)
        for (indx2=0; indx2<3; ++indx2)
          for (indx1=0; indx1<3; ++indx1)
            {
             output1r[i*9+indx2+indx3*3] += a[indx2][indx1] * b[indx1][indx3];
            }

    }
    return;
  }
  else if (input1i == NULL && input2i != NULL) 
  {
    for (i=0; i<numelin/9; i++)
    {
      /* first input real-valued case, second input complex*/
      for (indx2=0; indx2<3; ++indx2)
        for (indx1=0; indx1<3; ++indx1)
          {
           a[indx1][indx2]  = input1r[i*9+indx1+indx2*3];
           b[indx1][indx2]  = input2r[i*9+indx1+indx2*3];
           bi[indx1][indx2] = input2i[i*9+indx1+indx2*3];
          
           output1r[i*9+indx1+indx2*3] = 0;
           output1i[i*9+indx1+indx2*3] = 0;
          }
      
      for (indx3=0; indx3<3; ++indx3)
        for (indx2=0; indx2<3; ++indx2)
          for (indx1=0; indx1<3; ++indx1)
            {
             output1r[i*9+indx2+indx3*3] += a[indx2][indx1] * b[indx1][indx3];
             output1i[i*9+indx2+indx3*3] += a[indx2][indx1] * bi[indx1][indx3];
            }

    }
    return;  
    
  }
  else if (input1i != NULL  && input2i == NULL)
  {
    for (i=0; i<numelin/9; i++)
    {
      /* first input complex-valued, second input real-valued*/
      for (indx2=0; indx2<3; ++indx2)
        for (indx1=0; indx1<3; ++indx1)
          {
           a[indx1][indx2]  = input1r[i*9+indx1+indx2*3];
           b[indx1][indx2]  = input2r[i*9+indx1+indx2*3];
           ai[indx1][indx2] = input1i[i*9+indx1+indx2*3];
          
           output1r[i*9+indx1+indx2*3] = 0;
           output1i[i*9+indx1+indx2*3] = 0;
          }
      
      for (indx3=0; indx3<3; ++indx3)
        for (indx2=0; indx2<3; ++indx2)
          for (indx1=0; indx1<3; ++indx1)
            {
             output1r[i*9+indx2+indx3*3] += a[indx2][indx1] * b[indx1][indx3];
             output1i[i*9+indx2+indx3*3] += ai[indx2][indx1] * b[indx1][indx3];
            }

    }
    return;  
  }  
  else
  {  
    for (i=0; i<numelin/9; i++)
    {
      /* both inputs complex-valued*/
      for (indx2=0; indx2<3; ++indx2)
        for (indx1=0; indx1<3; ++indx1)
          {
           a[indx1][indx2]  = input1r[i*9+indx1+indx2*3];
           b[indx1][indx2]  = input2r[i*9+indx1+indx2*3];
           ai[indx1][indx2] = input1i[i*9+indx1+indx2*3];
           bi[indx1][indx2] = input2i[i*9+indx1+indx2*3];
          
           output1r[i*9+indx1+indx2*3] = 0;
           output1i[i*9+indx1+indx2*3] = 0;
          }
      
      for (indx3=0; indx3<3; ++indx3)
        for (indx2=0; indx2<3; ++indx2)
          for (indx1=0; indx1<3; ++indx1)
            {
             output1r[i*9+indx2+indx3*3] += a[indx2][indx1] * b[indx1][indx3] - ai[indx2][indx1] * bi[indx1][indx3];
             output1i[i*9+indx2+indx3*3] += ai[indx2][indx1] * b[indx1][indx3] + a[indx2][indx1] * bi[indx1][indx3];
            }

    }
    return;
  }  
}
// btkAppendEvent(h, label, time, context)
// btkAppendEvent(h, label, time, context, subject)
// btkAppendEvent(h, label, time, context, subject, description)
// btkAppendEvent(h, label, time, context, subject, description, id)
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  if(nrhs < 4)
    mexErrMsgTxt("Minimum of four inputs required.");
  if (nlhs > 2)
    mexErrMsgTxt("Too many output arguments.");
  
  btk::Event::Pointer evt = btk::Event::New();
  btk::Acquisition::Pointer acq = btk_MOH_get_object<btk::Acquisition>(prhs[0]);
  btk::Acquisition::EventConstIterator it;
  
  // Label
  if (!mxIsChar(prhs[1]) || mxIsEmpty(prhs[1]))
    mexErrMsgTxt("The label must be a non-empty string.");
  else
  {
    size_t strlen_ = (mxGetM(prhs[1]) * mxGetN(prhs[1]) * sizeof(mxChar)) + 1;
    char* buffer = (char*)mxMalloc(strlen_);
    mxGetString(prhs[1], buffer, strlen_);
    evt->SetLabel(buffer);
    mxFree(buffer);
    it = acq->FindEvent(evt->GetLabel());
  }
  // Time
  if ((mxGetClassID(prhs[2]) != mxDOUBLE_CLASS) || mxIsEmpty(prhs[2]) || mxIsComplex(prhs[2]) || (mxGetNumberOfElements(prhs[2]) != 1))
    mexErrMsgTxt("The time must be set by a numerical (double) value.");
  else
  {
    double t = mxGetScalar(prhs[2]);
    evt->SetTime(t);
  }
  // Context
  if (!mxIsChar(prhs[3]))
    mexErrMsgTxt("The context must be a string.");
  else
  {
    size_t strlen_ = (mxGetM(prhs[3]) * mxGetN(prhs[3]) * sizeof(mxChar)) + 1;
    char* buffer = (char*)mxMalloc(strlen_);
    mxGetString(prhs[3], buffer, strlen_);
    evt->SetContext(buffer);
    mxFree(buffer);
  }
  // Subject (optional)
  if (nrhs >= 5) 
  {
    if (!mxIsChar(prhs[4]))
      mexErrMsgTxt("The subject must be a string.");
    else
    {
      size_t strlen_ = (mxGetM(prhs[4]) * mxGetN(prhs[4]) * sizeof(mxChar)) + 1;
      char* buffer = (char*)mxMalloc(strlen_);
      mxGetString(prhs[4], buffer, strlen_);
      evt->SetSubject(buffer);
      mxFree(buffer);
    }
  }
  else // Use the first name in the SUBJECTS:NAMES parameter.
  {
    btk::MetaData::ConstIterator subjects = acq->GetMetaData()->FindChild("SUBJECTS");
    if (subjects != acq->GetMetaData()->End())
    {
      btk::MetaData::ConstIterator names = (*subjects)->FindChild("NAMES");
      if (names != (*subjects)->End())
      {
        if ((*names)->GetInfo() != btk::MetaDataInfo::Null)
        {
          if ((*names)->GetInfo()->GetDimensionsProduct(1) != 0)
          {
            evt->SetSubject(btkTrimString((*names)->GetInfo()->ToString(0)));
          }
        }
      }
    } 
  }
  // Description (optional)
  if (nrhs >= 6)
  {
    if (!mxIsChar(prhs[5]))
      mexErrMsgTxt("The description must be a string.");
    else
    {
      size_t strlen_ = (mxGetM(prhs[5]) * mxGetN(prhs[5]) * sizeof(mxChar)) + 1;
      char* buffer = (char*)mxMalloc(strlen_);
      mxGetString(prhs[5], buffer, strlen_);
      evt->SetDescription(buffer);
      mxFree(buffer);
    }
  }
  else // Use the description of the first event with the same label.
  {
    if (it != acq->EndEvent())
      evt->SetDescription((*it)->GetDescription());
  }
  // ID (optional)
  // If the function was called from btkEmulateC3Dserver, the ID was set to a fake
  // value (-65536) used later to store correctly some events in the C3D header section
  if (nrhs >= 7)
  {
    if ((mxGetClassID(prhs[6]) != mxDOUBLE_CLASS) || mxIsEmpty(prhs[6]) || mxIsComplex(prhs[6]) || (mxGetNumberOfElements(prhs[6]) != 1))
      mexErrMsgTxt("The ID must be set by a single double representing an integer value.");
    else
    {
      int id = static_cast<int>(mxGetScalar(prhs[6]));
      if (id == -65536)
      {
        evt->SetDetectionFlags(0x10000);
        id = 0;
      }
      evt->SetId(id);
    }
  }
  else // Use the ID of the first event with the same label.
  {
    if (it != acq->EndEvent())
      evt->SetId((*it)->GetId());
  }
  
  acq->AppendEvent(evt);
  
  btkMXCreateEventsStructure(acq, nlhs, plhs);
};
Ejemplo n.º 25
0
/*
%COMP_IFILTERBANK_TD   Synthesis filterbank
%   Usage:  f=comp_ifilterbank_fft(c,g,a,Ls,offset,ext);
%
%   Input parameters:
%         c    : Cell array of length M, each element is N(m)*W matrix.
%         g    : Filterbank filters - length M cell-array, each element is vector of length filtLen(m)
%         a    : Upsampling factors - array of length M.
%         offset : Delay of the filters - scalar or array of length M.
%         Ls   : Output length.
%         ext  : Border exension technique.
%
%   Output parameters:
%         f  : Output Ls*W array.
%
*/
void LTFAT_NAME(ltfatMexFnc)( int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[] )
{
  // printf("Filename: %s, Function name %s, %d \n.",__FILE__,__func__,mxIsDouble(prhs[0]));
  const mxArray* mxc = prhs[0];
  const mxArray* mxg = prhs[1];
  double* a = mxGetPr(prhs[2]);
  double* Lsdouble = mxGetPr(prhs[3]);
  unsigned int Ls = (unsigned int) *Lsdouble;
  double* offset = mxGetPr(prhs[4]);
  char* ext = mxArrayToString(prhs[5]);

  // number of channels
  unsigned int W = mxGetN(mxGetCell(mxc,0));

  // filter number
  unsigned int M = mxGetNumberOfElements(mxg);

  // input data length
  unsigned int* Lc = mxMalloc(M*sizeof(unsigned int));
  for(unsigned int m=0;m<M;m++)
  {
     Lc[m] = (unsigned int) mxGetM(mxGetCell(mxc,m));
  }

  // filter lengths
  unsigned int* filtLen = mxMalloc(M*sizeof(unsigned int));
  for(unsigned int m=0;m<M;m++)
  {
     filtLen[m] = (unsigned int) mxGetNumberOfElements(mxGetCell(mxg,m));
  }

     // POINTER TO THE INPUT
     LTFAT_TYPE** cPtrs = (LTFAT_TYPE**) mxMalloc(M*sizeof(LTFAT_TYPE*));
     for(unsigned int m=0;m<M;++m)
     {
        cPtrs[m] = (LTFAT_TYPE*) mxGetData(mxGetCell(mxc,m));
     }

     // allocate output
     plhs[0] = ltfatCreateMatrix(Ls, W,LTFAT_MX_CLASSID,LTFAT_MX_COMPLEXITY);


      // POINTER TO OUTPUT
     LTFAT_TYPE* fPtr = (LTFAT_TYPE*) mxGetData(plhs[0]);
     // Set to zeros
     memset(fPtr,0,Ls*W*sizeof(LTFAT_TYPE));

     // POINTER TO THE FILTERS
     LTFAT_TYPE** gPtrs = (LTFAT_TYPE**) mxMalloc(M*sizeof(LTFAT_TYPE*));
     for(unsigned int m=0;m<M;m++)
     {
        gPtrs[m] = (LTFAT_TYPE*) mxGetData(mxGetCell(mxg, m));
     }

     // over all channels
   //  #pragma omp parallel for private(m)

        for(unsigned int m =0; m<M; m++)
        {
          for(unsigned int w =0; w<W; w++)
          {
           // Obtain pointer to w-th column in input
           LTFAT_TYPE *fPtrCol = fPtr + w*Ls;
           // Obtaing pointer to w-th column in m-th element of output cell-array
           LTFAT_TYPE *cPtrCol = cPtrs[m] + w*Lc[m];
           //(upconv_td)(const LTFAT_TYPE *in, int inLen, LTFAT_TYPE *out, const int outLen, const LTFAT_TYPE *filts, int fLen, int up, int skip, enum ltfatWavExtType ext)
           LTFAT_NAME(upconv_td)(cPtrCol,Lc[m],fPtrCol,Ls,gPtrs[m],filtLen[m],a[m],-offset[m],ltfatExtStringToEnum(ext));
          }
       }

}
Ejemplo n.º 26
0
/* driver */
void
mexFunction(int nout, mxArray *out[],
            int nin, const mxArray *in[])
{

  enum {IN_I=0, IN_ER} ;
  enum {OUT_MEMBERS} ;

  idx_t i ;
  int k, nel, ndims ;
  mwSize const * dims ;
  val_t const * I_pt ;
  int last = 0 ;
  int last_expanded = 0 ;
  val_t value = 0 ;

  double const * er_pt ;

  int*   subs_pt ;       /* N-dimensional subscript                 */
  int*   nsubs_pt ;      /* diff-subscript to point to neigh.       */
  idx_t* strides_pt ;    /* strides to move in image array          */
  val_t* visited_pt ;    /* flag                                    */
  idx_t* members_pt ;    /* region members                          */
  bool invert = VL_FALSE ;

  /** -----------------------------------------------------------------
   **                                               Check the arguments
   ** -------------------------------------------------------------- */
  if (nin != 2) {
    mexErrMsgTxt("Two arguments required.") ;
  } else if (nout > 4) {
    mexErrMsgTxt("Too many output arguments.");
  }

  if(mxGetClassID(in[IN_I]) != mxUINT8_CLASS) {
    mexErrMsgTxt("I must be of class UINT8.") ;
  }

  if(!vlmxIsPlainScalar(in[IN_ER])) {
    mexErrMsgTxt("ER must be a DOUBLE scalar.") ;
  }

  /* get dimensions */
  nel   = mxGetNumberOfElements(in[IN_I]) ;
  ndims = mxGetNumberOfDimensions(in[IN_I]) ;
  dims  = mxGetDimensions(in[IN_I]) ;
  I_pt  = mxGetData(in[IN_I]) ;

  /* allocate stuff */
  subs_pt    = mxMalloc( sizeof(int)      * ndims ) ;
  nsubs_pt   = mxMalloc( sizeof(int)      * ndims ) ;
  strides_pt = mxMalloc( sizeof(idx_t)    * ndims ) ;
  visited_pt = mxMalloc( sizeof(val_t)    * nel   ) ;
  members_pt = mxMalloc( sizeof(idx_t)    * nel   ) ;

  er_pt = mxGetPr(in[IN_ER]) ;

  /* compute strides to move into the N-dimensional image array */
  strides_pt [0] = 1 ;
  for(k = 1 ; k < ndims ; ++k) {
    strides_pt [k] = strides_pt [k-1] * dims [k-1] ;
  }

  /* load first pixel */
  memset(visited_pt, 0, sizeof(val_t) * nel) ;
  {
    idx_t idx = (idx_t) *er_pt ;
    if (idx < 0) {
      idx = -idx;
      invert = VL_TRUE ;
    }
    if( idx < 1 || idx > nel ) {
      char buff[80] ;
      snprintf(buff,80,"ER=%d out of range [1,%d]",idx,nel) ;
      mexErrMsgTxt(buff) ;
    }
    members_pt [last++] = idx - 1 ;
  }
  value = I_pt[ members_pt[0] ]  ;

  /* -----------------------------------------------------------------
   *                                                       Fill region
   * -------------------------------------------------------------- */
  while(last_expanded < last) {

    /* pop next node xi */
    idx_t index = members_pt[last_expanded++] ;

    /* convert index into a subscript sub; also initialize nsubs
       to (-1,-1,...,-1) */
    {
      idx_t temp = index ;
      for(k = ndims-1 ; k >=0 ; --k) {
        nsubs_pt [k] = 0 ;
        subs_pt  [k] = temp / strides_pt [k] ;
        temp         = temp % strides_pt [k] ;
      }
    }
    nsubs_pt[0] = -1;
    /* process neighbors of xi */
    int nk = 0;
    while(VL_TRUE) {
      int good = VL_TRUE ;
      idx_t nindex = 0 ;

      /* compute NSUBS+SUB, the correspoinding neighbor index NINDEX
         and check that the pixel is within image boundaries. */
      for(k = 0 ; k < ndims && good ; ++k) {
        int temp = nsubs_pt [k] + subs_pt [k] ;
        good &= 0 <= temp && temp < (signed) dims[k] ;
        nindex += temp * strides_pt [k] ;
      }

      /* process neighbor
         1 - the pixel is within image boundaries;
         2 - the pixel is indeed different from the current node
         (this happens when nsub=(0,0,...,0));
         3 - the pixel has value not greather than val
         is a pixel older than xi
         4 - the pixel has not been visited yet
      */
      if(good
         && nindex != index
         && ((!invert && I_pt [nindex] <= value) ||
             ( invert && I_pt [nindex] >= value))
         && ! visited_pt [nindex] ) {

        /* mark as visited */
        visited_pt [nindex] = 1 ;

        /* add to list */
        members_pt [last++] = nindex ;
      }

      /* move to next neighbor */
//       k = 0 ;
//       while(++ nsubs_pt [k] > 1) {
//         nsubs_pt [k++] = -1 ;
//         if(k == ndims) goto done_all_neighbors ;
//       }
      if (nsubs_pt[nk] == -1)
                nsubs_pt[nk] += 2;
            else{
                nsubs_pt[nk] = 0;
                nk++;
                if(nk == ndims) goto done_all_neighbors ;
                nsubs_pt[nk] = -1;
            }
        } 
    /* next neighbor */
  done_all_neighbors : ;
  } /* goto pop next member */

  /*
   * Save results
   */
  {
    mwSize dims[2] ;
    int unsigned * pt ;
    dims[0] = last ;
    out[OUT_MEMBERS] = mxCreateNumericArray(1,dims,mxUINT32_CLASS,mxREAL);
    pt = mxGetData(out[OUT_MEMBERS]) ;
    for (i = 0 ; i < last ; ++i) {
      *pt++ = members_pt[i] + 1 ;
    }
  }

  /* free stuff */
  mxFree( members_pt ) ;
  mxFree( visited_pt ) ;
  mxFree( strides_pt ) ;
  mxFree( nsubs_pt   ) ;
  mxFree( subs_pt    ) ;
}
Ejemplo n.º 27
0
void recurse_object(const mxArray* in, json& obj)
{
    // Go over all types
    if (mxIsCell(in)) {
        int N = mxGetNumberOfElements(in);
        for (int a = 0; a < N; ++a) {           
            obj.emplace_back();
            recurse_object(mxGetCell(in, a), obj.back());
        }
    } else if (mxIsStruct(in)) {
        int M = mxGetNumberOfFields(in);
        std::vector<const char*> fnames(M);
        for (int b = 0; b < M; ++b)
            fnames[b] = mxGetFieldNameByNumber(in, b);
        int N = mxGetNumberOfElements(in);
        for (int a = 0; a < N; ++a) {
            json obj_;
            for (int b = 0; b < M; ++b)
                recurse_object(mxGetFieldByNumber(in, a, b), obj_[fnames[b]]);
            if (N == 1)
                obj = std::move(obj_);
            else
                obj.push_back(std::move(obj_));
        }
    } else if (mxIsChar(in)) {
        obj = json::parse(mxArrayToString(in));
    } else if (mxIsLogical(in)) {
        int N = mxGetNumberOfElements(in);
        const mxLogical* data = (const mxLogical*)mxGetData(in);
        if (N == 1) {
            obj = data[0] != 0;
        } else {
            for (int a = 0; a < N; ++a)
                obj.push_back(data[a] != 0);
        }
    } else if (mxIsNumeric(in)) {
        int N = mxGetNumberOfElements(in);
        const void* data = (const void*)mxGetData(in);
        switch (mxGetClassID(in)) {
            case mxDOUBLE_CLASS:
                if (N == 1) { obj = ((const double*)data)[0]; } else {
                for (int a = 0; a < N; ++a) obj.push_back(((const double*)data)[a]); }
                break;
            case mxSINGLE_CLASS:
                if (N == 1) { obj = ((const float*)data)[0]; } else {
                for (int a = 0; a < N; ++a) obj.push_back(((const float*)data)[a]); }
                break;
            case mxUINT8_CLASS:
                if (N == 1) { obj = ((const uint8_t*)data)[0]; } else {
                for (int a = 0; a < N; ++a) obj.push_back(((const uint8_t*)data)[a]); }
                break;
            case mxINT8_CLASS:
                if (N == 1) { obj = ((const int8_t*)data)[0]; } else {
                for (int a = 0; a < N; ++a) obj.push_back(((const int8_t*)data)[a]); }
                break;
            case mxUINT16_CLASS:
                if (N == 1) { obj = ((const uint16_t*)data)[0]; } else {
                for (int a = 0; a < N; ++a) obj.push_back(((const uint16_t*)data)[a]); }
                break;
            case mxINT16_CLASS:
                if (N == 1) { obj = ((const int16_t*)data)[0]; } else {
                for (int a = 0; a < N; ++a) obj.push_back(((const int16_t*)data)[a]); }
                break;
            case mxUINT32_CLASS:
                if (N == 1) { obj = ((const uint32_t*)data)[0]; } else {
                for (int a = 0; a < N; ++a) obj.push_back(((const uint32_t*)data)[a]); }
                break;
            case mxINT32_CLASS:
                if (N == 1) { obj = ((const int32_t*)data)[0]; } else {
                for (int a = 0; a < N; ++a) obj.push_back(((const int32_t*)data)[a]); }
                break;
            case mxUINT64_CLASS:
                if (N == 1) { obj = ((const uint64_t*)data)[0]; } else {
                for (int a = 0; a < N; ++a) obj.push_back(((const uint64_t*)data)[a]); }
                break;
            case mxINT64_CLASS:
                if (N == 1) { obj = ((const int64_t*)data)[0]; } else {
                for (int a = 0; a < N; ++a) obj.push_back(((const int64_t*)data)[a]); }
                break;
            default:
                mexErrMsgTxt("Unsupported type.");
                break;
        }
    } else {
		mexErrMsgTxt("Unrecognized type.");
    }
}
Ejemplo n.º 28
0
void buffer_getdat(char *hostname, int port, mxArray *plhs[], const mxArray *prhs[])
{
  int server;
  int verbose = 0;
  size_t n;
  double *val;
  char msg[512];
  
  message_t *request  = NULL;
  message_t *response = NULL;
  header_t  *header   = NULL;
  data_t    *data     = NULL;
  datasel_t datasel;
  
  /* this is for the Matlab specific output */
  const char *field_names[NUMBER_OF_FIELDS] = {"nchans", "nsamples", "data_type", "bufsize", "buf"};
  mxArray *datp;
  
  /* allocate the elements that will be used in the communication */
  request      = malloc(sizeof(message_t));
  request->def = malloc(sizeof(messagedef_t));
  request->buf = NULL;
  request->def->version = VERSION;
  request->def->command = GET_DAT;
  request->def->bufsize = 0;
  
  if ((prhs[0]!=NULL) && (mxGetNumberOfElements(prhs[0])==2) && (mxIsDouble(prhs[0])) && (!mxIsComplex(prhs[0]))) {
    /* fprintf(stderr, "args OK\n"); */
    val = (double *)mxGetData(prhs[0]);
    datasel.begsample = (UINT32_T)(val[0]);
    datasel.endsample = (UINT32_T)(val[1]);
    if (verbose) print_datasel(&datasel);
    request->def->bufsize = append(&request->buf, request->def->bufsize, &datasel, sizeof(datasel_t));
  }
  
  /* open the TCP socket */
  if ((server = open_connection(hostname, port)) < 0) {
    sprintf(msg, "ERROR: failed to create socket (%d)\n", server);
		mexErrMsgTxt(msg);
  }
  
  if (verbose) print_request(request->def);
  clientrequest(server, request, &response);
  if (verbose) print_response(response->def);
  close_connection(server);
  
  if (response->def->command==GET_OK) {
    data      = malloc(sizeof(data_t));
    data->def = response->buf;
    data->buf = (char *)response->buf + sizeof(datadef_t);
    if (verbose) print_datadef(data->def);
    
    switch (data->def->data_type) {
      case DATATYPE_INT8:
        datp = mxCreateNumericMatrix(data->def->nchans, data->def->nsamples, mxINT8_CLASS, mxREAL);
        memcpy(mxGetPr(datp), data->buf, data->def->nchans*data->def->nsamples*WORDSIZE_INT8);
        break;
      case DATATYPE_INT16:
        datp = mxCreateNumericMatrix(data->def->nchans, data->def->nsamples, mxINT16_CLASS, mxREAL);
        memcpy(mxGetPr(datp), data->buf, data->def->nchans*data->def->nsamples*WORDSIZE_INT16);
        break;
      case DATATYPE_INT32:
        datp = mxCreateNumericMatrix(data->def->nchans, data->def->nsamples, mxINT32_CLASS, mxREAL);
        memcpy(mxGetPr(datp), data->buf, data->def->nchans*data->def->nsamples*WORDSIZE_INT32);
        break;
      case DATATYPE_INT64:
        datp = mxCreateNumericMatrix(data->def->nchans, data->def->nsamples, mxINT64_CLASS, mxREAL);
        memcpy(mxGetPr(datp), data->buf, data->def->nchans*data->def->nsamples*WORDSIZE_INT64);
        break;
      case DATATYPE_FLOAT32:
        datp = mxCreateNumericMatrix(data->def->nchans, data->def->nsamples, mxSINGLE_CLASS, mxREAL);
        memcpy(mxGetPr(datp), data->buf, data->def->nchans*data->def->nsamples*WORDSIZE_FLOAT32);
        break;
      case DATATYPE_FLOAT64:
        datp = mxCreateNumericMatrix(data->def->nchans, data->def->nsamples, mxDOUBLE_CLASS, mxREAL);
        memcpy(mxGetPr(datp), data->buf, data->def->nchans*data->def->nsamples*WORDSIZE_FLOAT64);
        break;
        default:
          mexErrMsgTxt("ERROR; unsupported data type\n");
    }
    
    plhs[0] = mxCreateStructMatrix(1, 1, NUMBER_OF_FIELDS, field_names);
    mxSetFieldByNumber(plhs[0], 0, 0, mxCreateDoubleScalar((double)data->def->nchans));
    mxSetFieldByNumber(plhs[0], 0, 1, mxCreateDoubleScalar((double)(data->def->nsamples)));
    mxSetFieldByNumber(plhs[0], 0, 2, mxCreateDoubleScalar((double)(data->def->data_type)));
    mxSetFieldByNumber(plhs[0], 0, 3, mxCreateDoubleScalar((double)(data->def->bufsize)));
    mxSetFieldByNumber(plhs[0], 0, 4, datp);
    FREE(data);
  }
  else {
    sprintf(msg, "ERROR: the buffer returned an error (%d)\n", response->def->command);
		mexErrMsgTxt(msg);
  }
  
  if (request) {
    FREE(request->def);
    FREE(request->buf);
    FREE(request);
  }
  if (response) {
    FREE(response->def);
    FREE(response->buf);
    FREE(response);
  }
  
  return;
}
Ejemplo n.º 29
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  /*declare variables*/
  const mwSize *dims;
  mwIndex indx;
  int i, numdims;
  int numelin;
  mxClassID classid;
  double *input1r, *input1i, *input2r, *input2i, *output1r, *output1i;
  double a,  b,  c,  d,  e,  f,  g,  h, absa, absb, absc, absd, offr, offi;
  double ai, bi, ci, di, ei, fi, gi, hi;
  
  /*figure out the classid*/
  classid = mxGetClassID(prhs[0]);
     
  /*check inputs*/
  if (nrhs!=2)
    mexErrMsgTxt("Wrong number of input arguments");
  
  /*associate inputs*/
  input1r = mxGetData(prhs[0]);
  input1i = mxGetImagData(prhs[0]);
  
  input2r = mxGetData(prhs[1]);
  input2i = mxGetImagData(prhs[1]);
  
  /*figure out dimension info and number of elements*/
  dims    = mxGetDimensions(prhs[0]);
  numdims = mxGetNumberOfDimensions(prhs[0]);
  numelin = mxGetNumberOfElements(prhs[0]);
    
  /*associate output*/
  if (input1i == NULL && input2i == NULL)
  {
    plhs[0]  = mxCreateNumericArray(numdims, dims, classid, mxREAL);
    output1r = mxGetData(plhs[0]);
  }
  else
  {
    plhs[0]  = mxCreateNumericArray(numdims, dims, classid, mxCOMPLEX);
    output1r = mxGetData(plhs[0]);
    output1i = mxGetImagData(plhs[0]);
  }
  
  /* do the computation*/
  if (input1i == NULL && input2i == NULL)
  {  
    for (i=0; i<numelin/4; i++)
    {
      a = input1r[i*4  ];
      b = input1r[i*4+1];
      c = input1r[i*4+2];
      d = input1r[i*4+3];
    
      e = input2r[i*4  ];
      f = input2r[i*4+1];
      g = input2r[i*4+2];
      h = input2r[i*4+3];
    
      output1r[i*4  ] = e*a*a + 2*f*a*c       + h*c*c;
      output1r[i*4+1] = e*a*b + f*a*d + f*b*c + h*c*d;
      output1r[i*4+2] = e*a*b + f*b*c + f*a*d + h*c*d;
      output1r[i*4+3] = e*b*b + 2*f*b*d       + h*d*d;
    }
    return;
  }
  else if (input1i == NULL)
  {
    for (i=0; i<numelin/4; i++)
    {
      /*matrix 1*/
      a  = input1r[i*4  ]; b  = input1r[i*4+1]; c  = input1r[i*4+2]; d  = input1r[i*4+3];
      
      /*matrix 2*/
      e  = input2r[i*4  ]; f  = input2r[i*4+1]; g  = input2r[i*4+2]; h  = input2r[i*4+3];
      ei = input2i[i*4  ]; fi = input2i[i*4+1]; gi = input2i[i*4+2]; hi = input2i[i*4+3];
      
      /*compute some quantities only once*/
      absa = a*a;
      absb = b*b;
      absc = c*c;
      absd = (d*d+di*di);
      offr = e*a*b + f*a*d + f*b*c + h*c*d;
      offi = -ei*a*b - fi*a*d + fi*b*c - hi*c*d;
      
      /*fill in the real part of the output matrix*/
      output1r[i*4  ] = e*absa + 2*f*a*c + h*absc;
      output1r[i*4+1] = offr;
      output1r[i*4+2] = offr;
      output1r[i*4+3] = e*absb + 2*f*b*d + h*absd;
    
      /*fill in the imaginary part of the output matrix*/
      output1i[i*4  ] = ei*absa + hi*absc;
      output1i[i*4+1] = -offi;
      output1i[i*4+2] =  offi;
      output1i[i*4+3] = ei*absb + hi*absd;
    
    }
  }
  /*else if (input2i == NULL)*/
    
  else
  {  
    for (i=0; i<numelin/4; i++)
    {
      /*matrix 1*/
      a  = input1r[i*4  ]; b  = input1r[i*4+1]; c  = input1r[i*4+2]; d  = input1r[i*4+3];
      ai = input1i[i*4  ]; bi = input1i[i*4+1]; ci = input1i[i*4+2]; di = input1i[i*4+3];
      
      /*matrix 2*/
      e  = input2r[i*4  ]; f  = input2r[i*4+1]; g  = input2r[i*4+2]; h  = input2r[i*4+3];
      ei = input2i[i*4  ]; fi = input2i[i*4+1]; gi = input2i[i*4+2]; hi = input2i[i*4+3];
    
      /*compute some quantities only once*/
      absa = (a*a+ai*ai);
      absb = (b*b+bi*bi);
      absc = (c*c+ci*ci);
      absd = (d*d+di*di);
      offr = (e*a*b+e*ai*bi-ei*a*bi+ei*ai*b) + (f*a*d+f*ai*di-fi*a*di+fi*ai*d) + (f*b*c+f*bi*ci-fi*b*ci+fi*bi*c) + (h*c*d+h*ci*di-hi*c*di+hi*ci*d);
      offi = (-ei*ai*bi-e*a*bi+e*ai*b-ei*a*b) + (-fi*ai*di-f*a*di+f*ai*d-fi*a*d) + (fi*bi*ci+f*b*ci-f*bi*c+fi*b*c) + (-hi*ci*di-h*c*di+h*ci*d-hi*c*d);
      
      /*fill in the real part of the output matrix*/
      output1r[i*4  ] = e*absa + 2*(f*a*c+f*ai*ci-fi*a*ci+fi*ai*c) + h*absc;
      output1r[i*4+1] = offr;
      output1r[i*4+2] = offr;
      output1r[i*4+3] = e*absb + 2*(f*b*d+f*bi*di-fi*b*di+fi*bi*d) + h*absd;
    
      /*fill in the imaginary part of the output matrix*/
      output1i[i*4  ] = ei*absa + hi*absc;
      output1i[i*4+1] = -offi;
      output1i[i*4+2] =  offi;
      output1i[i*4+3] = ei*absb + hi*absd;
    }
    return;
  }
}
Ejemplo n.º 30
0
static void mdlStart(SimStruct *S)
{
    PCONFIG_DATA config;
    int_T nports_in    = 1;
    int_T nports_out   = 2;
    int_T nq;
    
    /* Store new C object in the pointers vector */
    config = (PCONFIG_DATA) calloc(1, sizeof(CONFIG_DATA));
    ssGetPWork(S)[0] = config;
    
    /* Store the number of ports and their indices */
    config->idxin_xdot = 0;
    if( intval(mxGetScalar(paramInitialConditionSource)) > 1 )
    {
        config->idxin_x0 = nports_in++;
    }
    else
    {
        config->idxin_x0 = 0;
    }
    if( intval(mxGetScalar(paramSpecificationsSource)) == 3 )
    {
        config->idxin_params = nports_in++;
    }
    else
    {
        config->idxin_params = 0;
    }
    if( intval(mxGetScalar(paramExternalReset)) > 1 )
    {
        config->idxin_reset = nports_in++;
    }
    else
    {
        config->idxin_reset = 0;
    }
    
    config->idxout_x = 1;
    if( intval(mxGetScalar(paramOutputTime)) > 0 )
    {
        config->idxout_time = nports_out++;
    }
    else
    {
        config->idxout_time = 0;
    }
    
    /* Store the number of quaternions */
    if( mxGetNumberOfElements(paramQuaternionIndex) == 2 )
    {
        config->start_idx_q = intval(mxGetPr(paramQuaternionIndex)[0]) - 1;
        config->end_idx_q = intval(mxGetPr(paramQuaternionIndex)[1]) - 1;
        nq = 1 + config->end_idx_q - config->start_idx_q;
        nq = nq / 4;
        config->nq = nq;
        if( mxGetNumberOfElements(paramOmegaDotIndex) == 2 )
        {
            config->start_idx_omegadot = intval(mxGetPr(paramOmegaDotIndex)[0]) - 1;
            config->end_idx_omegadot = intval(mxGetPr(paramOmegaDotIndex)[1]) - 1;
            config->use_omegadot = 1;
        }
    }
    
    config->initial_time = ssGetTStart(S);
    config->nstates = ssGetInputPortWidth(S, 0);
}