void gaussfit_main (int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[], char* variant, char* model, const int m, const int background_int,
                        void(*fit_model)(double *p, double *x, int m, int n, void *data ), void(*fit_jacobian)(double *p, double *jac, int m, int n, void *data),
                        const int numEqualityConstraints, const int numBounds, const int numInputs, const int numDataEntries)

{

    //gets the dimensions of the input data (window size)
    int sizex = mxGetN(prhs[0]);
    int sizey = mxGetM(prhs[0]);


    //checks that input frame is square
    if( sizex != sizey)
    {
      mexErrMsgTxt("Input data is not a square matrix.");
    }
    //===================================


    //Read in data from MATLAB
    //===================================

    int n = sizex*sizey;    //number of pixels in data
    //here we create a dynamic array of size N to hold the data

    double* x;
    x = new double[n];


    //Read in data to x from MATLAB
    double *datapoint = mxGetPr(prhs[0]);

    for(int j = 0; j < sizey; j++)
    {
    for(int i = 0; i < sizex; i++)
    {

          x[(i * sizex) + j] = datapoint[i + (j*sizey)];
    }
    }

    //Because of the way MATLAB reads mxArrays (down each column)
    //important to account for this because of having fixed positions etc
    //or fixed elliptical width.

    //so read the MATLAB array consecutively for speed and then the first element (1,1) goes to
    //position (1,1) [element 0], then the second element (1,2) goes to position (2,1) [element 1*sizex] etc
    //so we fill x[] down the "columns" in the array
    //eg for a 3x3 matrix we read in elements[0 - 8] from MATLAB and input them in array element order
    //[0], [3], [6] (first column done), [1],[4],[7] (second), [2], [5], [8] - all done

       double* constraints_pointer;

       double *data;           //read in data from MATLAB input, if required
       if(numDataEntries == 0)
          {
             data = NULL;
          }
       else
           {
             data = new double[numDataEntries];

             for(int i = 0; i < numDataEntries; i++)
              {
                constraints_pointer = mxGetPr(prhs[i+1]); //+1 because array of intensity data is indexed at [0]
                data[i] = constraints_pointer[0];
              }

           }
    //data contains any constants we need to pass to the constraints functions
    //data contains any constants we need to pass to the constraints functions


    //Read in bounding condition data from MATLAB to pass to constraints function

    //Matlab input convention - data first, then all fixed parameters, then all bounded parameters in order -
    //lower bound first, then upper bound, with standard variable sequence A, sigma, b, Xo, Yo, then extra options. So bound indices for
    // prhs[] start at numEqualityConstraints + 1 ([0] is always the data)

    //===================================


    //Sets up initial guess and boundaries for solver
    //===================================
       double* p;
       p = new double[m];
       double back_p[1];

       void initialfit (char *variant, double p[], double data_array[], int n, double *data); //define initial fit function

       //if flag in input is set to true, use supplied guess, otherwise run initial fit routine
       bool* use_initial_fit_guess = mxGetLogicals(prhs[numInputs-3] );
       if (use_initial_fit_guess[0] == true) //then use the guess given in input
          {
              double* init_guess_pointer = mxGetPr(prhs[numEqualityConstraints + numBounds + 2]); //gets pointer to guess
              for(int i = 0; i < m; i++)
              {
		p[i] = init_guess_pointer[i];
	      } //transfer guess from input argument


          }

       else //run the initial fits program to generate the initial guess
          {initialfit(variant, p, x, n, data);}

    //sets the initial value for the background to be used by the background only solver
       back_p[0] = p[background_int - 1];

    //initialise boundary and penalty weighting arrays
    //which will be taken as the arguments for constraint calculating
    //and solver functions - constraints() and dlevmar_etc()

       double* lb;
       double* ub;
       lb = new double[m];
       ub = new double[m];
       //lower and upper boundary arrays

       double* weights;
       weights = new double[m]; //Not actually used but if we want to define weighting for penalty function
       //for use in the boundary conditions, this is here


       double info[LM_INFO_SZ];

     //Read in search stopping parameters from input array from Matlab

       double* options_pointer = mxGetPr(prhs[numEqualityConstraints+numBounds+3]);

       double searchstopParams[4] = {options_pointer[1], options_pointer[2], options_pointer[3]};
       int numIterations = options_pointer[4];

    //this array controls parameters epsilon 1,2 and 3 as described in http://www.ics.forth.gr/~lourakis/levmar/levmar.pdf
    //the purpose of them is to stop the solver before it has reached its maximum number of iterations if the solution has
    //already sufficiently converged
    //for data with reasonable signal to noise, sufficient convergence should only take ~10 iterations, so these conditions
    //are important

    //searchstopParams[0] is epsilon1, the gradient of chi squared, 10^-2 or -3 seems to work well
    //searchstopParams[1] is epsilon2, the gradient of the change in best fit parameters, relative to the values of those parameters
    //this is generally the condition that will determine the speed/accuracy/precision of the output and is worth playing around with
    //higher values generally slow down the routine, but increase the accuracy, especially for badly scaled conditions (eg. background
    //several orders of magnitude higher than peak amplitude) good values seem to be in the range 10^-3 (for well scaled conditions) to
    //10^-5
    //searchstopParams[2] is epsilon3, the absolute value of chi squared. For a well solved gaussian with low noise
    //this will still be a large number so this is essentially ignored





       double opts[LM_OPTS_SZ] = {LM_INIT_MU, searchstopParams[0],searchstopParams[1],searchstopParams[2], LM_DIFF_DELTA};
        //opts[0] is initial mu value - controls the damping in the LM algorithm - using default
        //opts[1] is stopping threshold on gradient of chi square
        //opts[2] is stopping threshold on relative change of parameter magnitudes
        //opts[3] is stopping threshold on value of chi squared
        //opts[4] is irrelevant if analytic jacobian used, so use default
    //===================================

       //define constraint setting function
       void constraints(char *variant, double *lb, double *ub, double *constraintMatrix, double *constraintRHS,
                    int numBounds, int numEqualityConstraints, double *weights, int m, int framesize, double *data);

        int ret;








    //We will always have box (inequality constraints) eg. amplitude must be positive
    //However, in some cases we will have linear (equality) constraints and will need to use dlevmar_blec_der
    //But, if we have no equality constraints we should use dlevmar_bc_der which uses box constraints only
    //So use an if-else condition on numEqualityConstraints to decide which to us

         if(numEqualityConstraints > 0) //then we need box and linear constraints

            {
               double *ConstraintMatrix = new double[numEqualityConstraints * m];
               double *ConstraintRHS    = new double[numEqualityConstraints];

        ///constraints function
         constraints(variant, lb, ub, ConstraintMatrix, ConstraintRHS, numBounds, numEqualityConstraints, weights, m, sizex, data);

                    ///main solver function if you need equality constraints
              ret = dlevmar_blec_der

                     ///basics
                     (fit_model,  //pointer to the function that calculates the model values at each datapoint given parameters p
                      fit_jacobian, //pointer to the function that calculates the analytic jacobian dxi/dpj at each datapoint
                      p, //current best guess parameters as an array of size m
                      x,  //data to be fitted to the model, array of size n
                      m,  //number of free parameters
                      n, //number of datapoints

                      ///constraints
                      lb, //array of m elements setting lower bounds on each parameter (p[i] >= lb[i])
                      ub, //array of m elements setting upper bounds on each parameter (p[i] <= ub[i])
                      ConstraintMatrix, //matrix of equality constraint values, numEqualityconstraints = number of rows
                                        //m is number of columns
                      ConstraintRHS, //RHS of constraint equation ConstraintMatrix * p = ConstraintRHS
                      numEqualityConstraints, //how many constraints you have
                      NULL, //penalty weighting array, set to NULL to use defaults, otherwise fill and use weights[m]

                     ///solver stop procedure and debugging
                      numIterations, //max no. of iterations the solver will run for, 50 to 100 seems about right
                      opts, //5 element array, described above
                      info, //from source code of levmar library:
                            /* info: information regarding the minimization. Used to find the lsq minimisation value at output
                      * info[0]= ||e||_2 at initial p.
                      * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||Dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.
                      * info[5]= # iterations,
                      * info[6]=reason for terminating: 1 - stopped by small gradient J^T e
                      *                                 2 - stopped by small Dp
                      *                                 3 - stopped by itmax
                      *                                 4 - singular matrix. Restart from current p with increased mu
                      *                                 5 - no further error reduction is possible. Restart with increased mu
                      *                                 6 - stopped by small ||e||_2
                      *                                 7 - stopped by invalid (i.e. NaN or Inf) "func" values. This is a user error
                      * info[7]= # function evaluations
                      * info[8]= # Jacobian evaluations
                      * info[9]= # linear systems solved, i.e. # attempts for reducing error
                      */

                      NULL,    // working memory at least LM_BLEC_DER_WORKSZ() reals large, allocated if NULL
                      NULL,   //Covariance matrix corresponding to LS solution; mxm. Set to NULL if not needed.
                      data); // pointer to additional data passed to function and jacobian calculator - fixed constants etc

                      delete [] ConstraintMatrix;
                      delete [] ConstraintRHS;
            }

           ///main solver function if you only need inequality constraints - see above for arg details
        else if(numEqualityConstraints == 0) //we only need box constraints

            {
                ///constraints function, inequality constraints only
                constraints(variant, lb, ub, NULL, NULL, numBounds, numEqualityConstraints, weights, m, sizex, data);
                ///main solver, see above for arg descriptions
                ret = dlevmar_bc_der

                       (fit_model, fit_jacobian, p, x, m, n,

                        lb, ub, NULL,

                        numIterations, opts, info, NULL, NULL, data); // with analytic Jacobian



            }

        else { mexErrMsgTxt("Number of equality constraints not set properly, should be an integer >= 0"); }





        ///OUTPUT
        //output brightness, final fit parameters and normalised least squares value to matlab

         double brightness, lsq_normalised;
         const double pi = 3.14159265358979323846; //easier to do this than require a library Pi function

         bool got_signal = true; //assume signal as default

         if ( model == "circular" )
         {brightness = 2.0*pi*p[0]*pow(p[1],2);}
         else if (model == "elliptical")
         {	
	   brightness = 2.0*pi*p[0]*p[1]*p[2];
	 }
         else
         {mexErrMsgTxt("Error, model not set.");}
	  
	  //if elliptical model transform the paramaters st. s_y is semimajor axis, and 0<theta<pi
	  if(model == "elliptical")
          {
	    double s_x,s_y,theta;
	    s_x = p[1];
	    s_y = p[2];
	    theta = p[6];

	    if (s_x > s_y) //ie if the fit has got the axes the wrong way around
	    { 
	      p[1] = s_y;//swap the two values
	      p[2] = s_x;
	      theta = theta + pi/2; //rotate the axes 90deg
	    }
	    
	    //transform theta st. its 0<theta<pi ie 
	    //ie, mod(theta,pi) = theta - floor(theta/pi) * pi;

	    theta = theta - floor(theta/pi) * pi;
	    p[6] = theta;
	  }




          lsq_normalised = info[1]/n;



          plhs[0] = mxCreateDoubleScalar(brightness); //send brightness as output to MATLAB

          plhs[1] = mxCreateDoubleMatrix(1, m, mxREAL);

          //need to do this next bit because can only pass an array back to Matlab as a matrix if array is
         //in dynamic memory via mxCalloc command
          void* final_fit_alloc = mxCalloc(m, sizeof(double));
          double* final_fit = (double *) final_fit_alloc;


          for (int i = 0; i < m; i++)
            {
                final_fit[i] = p[i];
            }

          mxFree(mxGetPr(plhs[1])); //mxSetPr will not deallocate memory allocated to plhs[1], so do it manually or get a leak
          mxSetPr(plhs[1],final_fit); //fit parameters are now ready to be passed to Matlab


          plhs[2] = mxCreateDoubleScalar(lsq_normalised);


          if (options_pointer[0] == true) //then display the output, otherwise just forward it to MATLAB
            {
                if (model == "circular")
                {
                mexPrintf("\nBrightness is %f.\n", brightness);
                mexPrintf("Fit parameters are: A = %f, sigma = %f, b = %f, Xo = %f, Yo = %f. \n", p[0],p[1],p[2],p[3],p[4]);
                mexPrintf("Normalised lsq is %f.\n", lsq_normalised);
                }

               if (model == "elliptical") //then display the output, otherwise just forward it to MATLAB
                {
                mexPrintf("\nBrightness is %f.\n", brightness);
                mexPrintf("Fit parameters are: A = %f, sigma = %f, %f, b = %f, Xo = %f, Yo = %f. Theta = %f. \n", p[0],p[1],p[2],p[3],p[4],p[5],p[6]);
                mexPrintf("Normalised lsq is %f.\n", lsq_normalised);
                }
   		//******************Uncomment for extra information*************************************** 
		mexPrintf("Levenberg-Marquardt returned in %g iter, reason %g, sumsq %g [%g]\n", info[5], info[6], info[1], info[0]);
   		   //   Debugging - info[0] returns lsq at start of fit,info[1] returns lsq at end of fit,
 		  //******************Uncomment for extra information*************************************** 
            }

            //freeup memory allocated for dynamic arrays
            delete [] x;
            delete [] p;
            delete [] data;
            delete [] lb;
            delete [] ub;
            delete [] weights;


            return;


}
Beispiel #2
0
mxArray *sf_c1_MON_sl_after_replacement3_get_autoinheritance_info(void)
{
  const char *autoinheritanceFields[] = { "checksum", "inputs", "parameters",
    "outputs", "locals", "postCodegenInfo" };

  mxArray *mxAutoinheritanceInfo = mxCreateStructMatrix(1, 1, sizeof
    (autoinheritanceFields)/sizeof(autoinheritanceFields[0]),
    autoinheritanceFields);

  {
    mxArray *mxChecksum = mxCreateString("zvoJrMZ2TLHvPtz6zVPShE");
    mxSetField(mxAutoinheritanceInfo,0,"checksum",mxChecksum);
  }

  {
    const char *dataFields[] = { "size", "type", "complexity" };

    mxArray *mxData = mxCreateStructMatrix(1,2,3,dataFields);

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,0,mxREAL);
      double *pr = mxGetPr(mxSize);
      mxSetField(mxData,0,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt", "isFixedPointType" };

      mxArray *mxType = mxCreateStructMatrix(1,1,sizeof(typeFields)/sizeof
        (typeFields[0]),typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(8));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxType,0,"isFixedPointType",mxCreateDoubleScalar(0));
      mxSetField(mxData,0,"type",mxType);
    }

    mxSetField(mxData,0,"complexity",mxCreateDoubleScalar(0));

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,0,mxREAL);
      double *pr = mxGetPr(mxSize);
      mxSetField(mxData,1,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt", "isFixedPointType" };

      mxArray *mxType = mxCreateStructMatrix(1,1,sizeof(typeFields)/sizeof
        (typeFields[0]),typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(8));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxType,0,"isFixedPointType",mxCreateDoubleScalar(0));
      mxSetField(mxData,1,"type",mxType);
    }

    mxSetField(mxData,1,"complexity",mxCreateDoubleScalar(0));
    mxSetField(mxAutoinheritanceInfo,0,"inputs",mxData);
  }

  {
    const char *dataFields[] = { "size", "type", "complexity" };

    mxArray *mxData = mxCreateStructMatrix(1,7,3,dataFields);

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,0,mxREAL);
      double *pr = mxGetPr(mxSize);
      mxSetField(mxData,0,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt", "isFixedPointType" };

      mxArray *mxType = mxCreateStructMatrix(1,1,sizeof(typeFields)/sizeof
        (typeFields[0]),typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(8));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxType,0,"isFixedPointType",mxCreateDoubleScalar(0));
      mxSetField(mxData,0,"type",mxType);
    }

    mxSetField(mxData,0,"complexity",mxCreateDoubleScalar(0));

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,0,mxREAL);
      double *pr = mxGetPr(mxSize);
      mxSetField(mxData,1,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt", "isFixedPointType" };

      mxArray *mxType = mxCreateStructMatrix(1,1,sizeof(typeFields)/sizeof
        (typeFields[0]),typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(8));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxType,0,"isFixedPointType",mxCreateDoubleScalar(0));
      mxSetField(mxData,1,"type",mxType);
    }

    mxSetField(mxData,1,"complexity",mxCreateDoubleScalar(0));

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,0,mxREAL);
      double *pr = mxGetPr(mxSize);
      mxSetField(mxData,2,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt", "isFixedPointType" };

      mxArray *mxType = mxCreateStructMatrix(1,1,sizeof(typeFields)/sizeof
        (typeFields[0]),typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(8));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxType,0,"isFixedPointType",mxCreateDoubleScalar(0));
      mxSetField(mxData,2,"type",mxType);
    }

    mxSetField(mxData,2,"complexity",mxCreateDoubleScalar(0));

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,0,mxREAL);
      double *pr = mxGetPr(mxSize);
      mxSetField(mxData,3,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt", "isFixedPointType" };

      mxArray *mxType = mxCreateStructMatrix(1,1,sizeof(typeFields)/sizeof
        (typeFields[0]),typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(3));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxType,0,"isFixedPointType",mxCreateDoubleScalar(0));
      mxSetField(mxData,3,"type",mxType);
    }

    mxSetField(mxData,3,"complexity",mxCreateDoubleScalar(0));

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,0,mxREAL);
      double *pr = mxGetPr(mxSize);
      mxSetField(mxData,4,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt", "isFixedPointType" };

      mxArray *mxType = mxCreateStructMatrix(1,1,sizeof(typeFields)/sizeof
        (typeFields[0]),typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(3));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxType,0,"isFixedPointType",mxCreateDoubleScalar(0));
      mxSetField(mxData,4,"type",mxType);
    }

    mxSetField(mxData,4,"complexity",mxCreateDoubleScalar(0));

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,0,mxREAL);
      double *pr = mxGetPr(mxSize);
      mxSetField(mxData,5,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt", "isFixedPointType" };

      mxArray *mxType = mxCreateStructMatrix(1,1,sizeof(typeFields)/sizeof
        (typeFields[0]),typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(3));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxType,0,"isFixedPointType",mxCreateDoubleScalar(0));
      mxSetField(mxData,5,"type",mxType);
    }

    mxSetField(mxData,5,"complexity",mxCreateDoubleScalar(0));

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,0,mxREAL);
      double *pr = mxGetPr(mxSize);
      mxSetField(mxData,6,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt", "isFixedPointType" };

      mxArray *mxType = mxCreateStructMatrix(1,1,sizeof(typeFields)/sizeof
        (typeFields[0]),typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(3));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxType,0,"isFixedPointType",mxCreateDoubleScalar(0));
      mxSetField(mxData,6,"type",mxType);
    }

    mxSetField(mxData,6,"complexity",mxCreateDoubleScalar(0));
    mxSetField(mxAutoinheritanceInfo,0,"parameters",mxData);
  }

  {
    const char *dataFields[] = { "size", "type", "complexity" };

    mxArray *mxData = mxCreateStructMatrix(1,1,3,dataFields);

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,0,mxREAL);
      double *pr = mxGetPr(mxSize);
      mxSetField(mxData,0,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt", "isFixedPointType" };

      mxArray *mxType = mxCreateStructMatrix(1,1,sizeof(typeFields)/sizeof
        (typeFields[0]),typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(8));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxType,0,"isFixedPointType",mxCreateDoubleScalar(0));
      mxSetField(mxData,0,"type",mxType);
    }

    mxSetField(mxData,0,"complexity",mxCreateDoubleScalar(0));
    mxSetField(mxAutoinheritanceInfo,0,"outputs",mxData);
  }

  {
    mxSetField(mxAutoinheritanceInfo,0,"locals",mxCreateDoubleMatrix(0,0,mxREAL));
  }

  {
    mxArray* mxPostCodegenInfo =
      sf_c1_MON_sl_after_replacement3_get_post_codegen_info();
    mxSetField(mxAutoinheritanceInfo,0,"postCodegenInfo",mxPostCodegenInfo);
  }

  return(mxAutoinheritanceInfo);
}
void mexFunction(int nlhs, mxArray *plhs[], 
                 int nrhs, const mxArray *prhs[])
{
	static bool firstTime = true;
	
	
	initSemaphore(&stepSEM);
	attachSEM(&stepSEM, STEP_SEM);
	
	initSharedMemory(&stepSHM, sizeof(episode_steps));
	attachSHM(&stepSHM, STEP_SHM, &stepSEM);
	
	readSHMemory(&stepSHM, &episodeStep, &stepSEM);
	
	int numStates = (int) *mxGetPr(prhs[0]);
	int numSteps = episodeStep.numTransmittedSteps;
	
	fprintf(stderr, "NumSteps: %d\n",numSteps);

	plhs[0]         = mxCreateDoubleMatrix(N_DOFS, numSteps, mxREAL);
	plhs[1]         = mxCreateDoubleMatrix(N_DOFS, numSteps, mxREAL);
	plhs[2]         = mxCreateDoubleMatrix(N_DOFS, numSteps,  mxREAL);
	
	plhs[3]         = mxCreateDoubleMatrix(N_DOFS, numSteps, mxREAL);
	plhs[4]         = mxCreateDoubleMatrix(N_DOFS, numSteps, mxREAL);
	plhs[5]         = mxCreateDoubleMatrix(N_DOFS, numSteps,  mxREAL);
	
	
	plhs[6]         = mxCreateDoubleMatrix(N_DOFS, numSteps,  mxREAL);
	plhs[7]         = mxCreateDoubleMatrix(7, numSteps, mxREAL);
	plhs[8]         = mxCreateDoubleMatrix(numStates, numSteps, mxREAL);
    
	plhs[9]         = mxCreateDoubleMatrix(1, numSteps, mxREAL);
	plhs[10]         = mxCreateDoubleMatrix(1, numSteps, mxREAL);
    plhs[10]        = mxCreateDoubleMatrix(1, numSteps, mxREAL);
	
	double *joints = mxGetPr(plhs[0]);
	double *jointsVel = mxGetPr(plhs[1]);
	double *jointsAcc = mxGetPr(plhs[2]);
	
	double *jointsDes = mxGetPr(plhs[3]);
	double *jointsVelDes = mxGetPr(plhs[4]);
	double *jointsAccDes = mxGetPr(plhs[5]);
	
	
	double *torque = mxGetPr(plhs[6]);
	
	double *cart = mxGetPr(plhs[7]);
	double *state = mxGetPr(plhs[8]);
	
    	double *commandIdx = mxGetPr(plhs[9]);
    	double *stepInTrajectory = mxGetPr(plhs[10]);

    
    double *doMotionIdx = mxGetPr(plhs[10]);
    
    
    
	memcpy(joints, episodeStep.joints, sizeof(double) * numSteps * N_DOFS);
	memcpy(jointsVel, episodeStep.jointsVel, sizeof(double) * numSteps * N_DOFS);
	memcpy(jointsAcc, episodeStep.jointsAcc, sizeof(double) * numSteps * N_DOFS);
	
	memcpy(jointsDes, episodeStep.jointsDes, sizeof(double) * numSteps * N_DOFS);
	memcpy(jointsVelDes, episodeStep.jointsVelDes, sizeof(double) * numSteps * N_DOFS);
	memcpy(jointsAccDes, episodeStep.jointsAccDes, sizeof(double) * numSteps * N_DOFS);
	
	
	memcpy(torque, episodeStep.torque, sizeof(double) * numSteps * N_DOFS);
	memcpy(cart, episodeStep.cart, sizeof(double) * numSteps * 7);
    

//     printf("doMotionIdx :");
//     for (int i = 0; i < numSteps; i ++)
//         printf(" %d",episodeStep.doMotionIdx[i]);
//     printf(" \n");

	for (int i = 0; i < numSteps; i ++)
	{
		for (int j = 0; j < numStates; j ++)
		{
			state[i * numStates + j] = episodeStep.state[i][j];
//			printf("%f ", episodeStep.state[i][j]);
		}
//		printf("\n");
	}
	

	for (int i = 0; i < numSteps; i ++)
    	{
        	commandIdx[i] = episodeStep.commandIdx[i];
            doMotionIdx[i] = episodeStep.doMotionIdx[i];
	}

	for (int i = 0; i < numSteps; i ++)
    	{
        	stepInTrajectory[i] = episodeStep.stepInTrajectory[i];
	}
    
	deleteSemaphore(&stepSEM);
	deleteSharedMemory(&stepSHM);
	
}
/*============================================================*/
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
  float *image, *tmp;
  int nX, nY;
  ccLabelStruct **label;
  int ixy;
  int numComp, t;
  double minTarget; /* 0.001 = boxes02-add.pgm */
  double *target;
  float debug = 0;

  /* for sparse array */
  mwIndex *rowPtr, *colInfo;
  double *data;
  int rows, cols;

  int sparseFlag = 0;

  RanTimeSeed;

  /*mexPrintf("nlhs:%d nrhs: %d\n",nlhs,nrhs);*/

  /* check for the required input arguments */
  if( nrhs < 2 )
    mexErrMsgTxt( "Input expected: <symmetric-positive-array-for-cca> <affty-threshold> <1-for-debug>" );

  /* get the symmetric matrix */
  /*mex2float( prhs[0], &target, &nX, &nY);*/
  nX = mxGetN(prhs[0]); /* cols */ 
  cols = nX;

  nY = mxGetM(prhs[0]); /* rows */
  rows = nY;
  
  mexPrintf("size: %d %d\n",nX,nY);

  if (mxIsSparse(prhs[0])) {
    sparseFlag = 1;
    mexPrintf("Affy matrix Sparse? : %d\n",sparseFlag);
  }

  if (sparseFlag) {
    mwIndex *jc;

    data    = mxGetPr(prhs[0]); /* pointer to real data */
    rowPtr  = mxGetIr(prhs[0]); /* pointer to row info */
    colInfo = mxGetJc(prhs[0]); /* pointer to col info */
    jc = mxGetJc(prhs[0]);

    /*
    for (ixy=0; ixy < mxGetN(prhs[0]); ixy++) {
      mexPrintf("rowBouMain %d (%d %d) \n",ixy,jc[ixy+1],colInfo[ixy+1]);    
    }

    analyze_sparse(prhs[0]);
    */
  } else {
    target = (double *)mxGetPr(prhs[0]);
  }


  /* get the threshold */
  minTarget = (double) mxGetScalar(prhs[1]);

  /* check for debug flag */
  if (nrhs == 3) {
    debug = mxGetScalar(prhs[2]);
  }

  if (debug)
    mexPrintf("Affty threshold: %2.3e\n",minTarget);

  /* set up empty labels */
  grabByteMemory((char **) &label,sizeof(ccLabelStruct *) * nX,"label");
  for (ixy=0;ixy < nX; ixy++)
    label[ixy] = NULL;

  if (debug) {
    mexPrintf("Computing connected components....");
  }

  if (sparseFlag) {
    connectSymmetricSparse(label,rowPtr,colInfo,data, minTarget, nX, nY);
  } else {
    connectSymmetricDbl(label, target, minTarget, nX, nY);
  }

  if (debug) {
    mexPrintf("done\n");
  }

  /* yalla edition */
  /*
  if (debug) {
    mexPrintf("Pruning small or weak connected components....");
  }
  pruneLabelComp(label, nX, 1, 1, 0);
  */

  if (debug) {
    mexPrintf("done\n");
  }

  /* component count */
  numComp = countLabelComp(label, nX, 1);
  if (debug) {
    mexPrintf("Found %d components.\n\n", numComp);
  }

  /*
  if (numComp > 0)
    printMarkovLabels(label,nX);
  */

  /* return the labels to matlab */
  if (nlhs > 0)
    labels2mex(label, &plhs[0], nX ); 
  if (nlhs > 1)
    {
      double *pout;
      plhs[1] = mxCreateDoubleMatrix(1,1,mxREAL);
      pout = mxGetPr(plhs[1]);
      *pout = (double) numComp;
    }
  /* free up memory. */
  /* target is a pointer to prhs[0]. so no need to free. */
  /*utilFree( (void **)&target );*/
  utilFree((void **)label);
  /*
  for (ixy = 0; ixy < nX; ixy++)
    free(label[ixy]);
  */

}
Beispiel #5
0
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    tt_buf* buffer;
    int timetagger;
    uint64_t datapoints;

    
    uint64_t total;	//The total amount of data in the buffer
    uint64_t i;		//Temporary loop variable
    uint64_t j=0;
    double *channels=NULL, *timebins;
    
    if (nrhs == 2 && mxIsDouble(prhs[0]) && mxIsDouble(prhs[1])) {
        timetagger = (int)mxGetScalar(prhs[0]);
        datapoints = (uint64_t)mxGetScalar(prhs[1]);
    } else {
        mexErrMsgTxt("Inputs:\n\tTime tag buffer to dump from.\n\tNumber of datapoints to dump.\nOutputs:\n\tArray of time stamps\n\tArray of corresponding channels");
        return;
    }
    
    if (datapoints==0) {
        mexErrMsgTxt("No datapoints were specified!");
        return;
    }
    
    buffer = tt_open(timetagger);
    if (buffer) {
        total = tt_datapoints(buffer);
        
        if (datapoints > total) {
            mexPrintf("Warning: Not enough datapoints! Only reading saved datapoints!\n");
            datapoints = total;
        }
        if (datapoints > tt_maxdata(buffer)) {
            mexPrintf("Warning: There are not enough datapoints saved. Only reading to end of buffer!\n");
            datapoints = tt_maxdata(buffer);    //Only read up to the entire buffer
        }

        
        if (nlhs>=2) {
            plhs[0] = mxCreateDoubleMatrix(1,(int)datapoints,mxREAL);
            channels = mxGetPr(plhs[0]);
            plhs[1] = mxCreateDoubleMatrix(1,(int)datapoints,mxREAL);
            timebins = mxGetPr(plhs[1]);
        } else {
            plhs[0] = mxCreateDoubleMatrix(1,(int)datapoints,mxREAL);
            timebins = mxGetPr(plhs[0]);
        }
        
        //Read the array of data
        if (isnan(tt_resolution(buffer))) {
            mexPrintf("Warning: Resolution unset. Returning raw data.\n");
            if (channels) {
                for (i=total-datapoints;i<total;i++) {
                    channels[j] = (double)tt_channel(buffer,i)+1.0;	//Matlab has this weird off by one thing...
                    timebins[j] = (double)tt_tag(buffer,i);
                    j++;
                }
            } else {
                for (i=total-datapoints;i<total;i++) {
                    timebins[j] = (double)tt_tag(buffer,i);
                    j++;
                }
            }
        } else {
            if (channels) {
                for (i=total-datapoints;i<total;i++) {
                    channels[j] = (double)tt_channel(buffer,i)+1.0;	//Matlab has this weird off by one thing...
                    timebins[j] = (double)tt_tag(buffer,i)*tt_resolution(buffer);
                    j++;
                }
            } else {
                for (i=total-datapoints;i<total;i++) {
                    timebins[j] = (double)tt_tag(buffer,i)*tt_resolution(buffer);
                    j++;
                }
            }
        }
        
        
        tt_close(buffer);
        
    } else {
        mexErrMsgTxt("Unable to connect to time tag buffer! Is all necessary software running?");
    }

}
unsigned int sf_BoatDynamic_process_check_sum_call( int nlhs, mxArray * plhs[],
  int nrhs, const mxArray * prhs[] )
{

#ifdef MATLAB_MEX_FILE

  char commandName[20];
  if (nrhs<1 || !mxIsChar(prhs[0]) )
    return 0;

  /* Possible call to get the checksum */
  mxGetString(prhs[0], commandName,sizeof(commandName)/sizeof(char));
  commandName[(sizeof(commandName)/sizeof(char)-1)] = '\0';
  if (strcmp(commandName,"sf_get_check_sum"))
    return 0;
  plhs[0] = mxCreateDoubleMatrix( 1,4,mxREAL);
  if (nrhs>1 && mxIsChar(prhs[1])) {
    mxGetString(prhs[1], commandName,sizeof(commandName)/sizeof(char));
    commandName[(sizeof(commandName)/sizeof(char)-1)] = '\0';
    if (!strcmp(commandName,"machine")) {
      ((real_T *)mxGetPr((plhs[0])))[0] = (real_T)(4266279937U);
      ((real_T *)mxGetPr((plhs[0])))[1] = (real_T)(2301601715U);
      ((real_T *)mxGetPr((plhs[0])))[2] = (real_T)(2035947662U);
      ((real_T *)mxGetPr((plhs[0])))[3] = (real_T)(2227678827U);
    } else if (!strcmp(commandName,"exportedFcn")) {
      ((real_T *)mxGetPr((plhs[0])))[0] = (real_T)(0U);
      ((real_T *)mxGetPr((plhs[0])))[1] = (real_T)(0U);
      ((real_T *)mxGetPr((plhs[0])))[2] = (real_T)(0U);
      ((real_T *)mxGetPr((plhs[0])))[3] = (real_T)(0U);
    } else if (!strcmp(commandName,"makefile")) {
      ((real_T *)mxGetPr((plhs[0])))[0] = (real_T)(1696694212U);
      ((real_T *)mxGetPr((plhs[0])))[1] = (real_T)(1767225808U);
      ((real_T *)mxGetPr((plhs[0])))[2] = (real_T)(3740181501U);
      ((real_T *)mxGetPr((plhs[0])))[3] = (real_T)(2256016198U);
    } else if (nrhs==3 && !strcmp(commandName,"chart")) {
      unsigned int chartFileNumber;
      chartFileNumber = (unsigned int)mxGetScalar(prhs[2]);
      switch (chartFileNumber) {
       case 1:
        {
          extern void sf_c1_BoatDynamic_get_check_sum(mxArray *plhs[]);
          sf_c1_BoatDynamic_get_check_sum(plhs);
          break;
        }

       case 2:
        {
          extern void sf_c2_BoatDynamic_get_check_sum(mxArray *plhs[]);
          sf_c2_BoatDynamic_get_check_sum(plhs);
          break;
        }

       default:
        ((real_T *)mxGetPr((plhs[0])))[0] = (real_T)(0.0);
        ((real_T *)mxGetPr((plhs[0])))[1] = (real_T)(0.0);
        ((real_T *)mxGetPr((plhs[0])))[2] = (real_T)(0.0);
        ((real_T *)mxGetPr((plhs[0])))[3] = (real_T)(0.0);
      }
    } else if (!strcmp(commandName,"target")) {
      ((real_T *)mxGetPr((plhs[0])))[0] = (real_T)(1666150109U);
      ((real_T *)mxGetPr((plhs[0])))[1] = (real_T)(3277580775U);
      ((real_T *)mxGetPr((plhs[0])))[2] = (real_T)(294114188U);
      ((real_T *)mxGetPr((plhs[0])))[3] = (real_T)(4213807667U);
    } else {
      return 0;
    }
  } else {
    ((real_T *)mxGetPr((plhs[0])))[0] = (real_T)(798114639U);
    ((real_T *)mxGetPr((plhs[0])))[1] = (real_T)(1876369125U);
    ((real_T *)mxGetPr((plhs[0])))[2] = (real_T)(3618836105U);
    ((real_T *)mxGetPr((plhs[0])))[3] = (real_T)(3292593476U);
  }

  return 1;

#else

  return 0;

#endif

}
Beispiel #7
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

  CUresult cudastatus = CUDA_SUCCESS;

  // no more than 2 arguments expected
  if (nrhs > 2)
    mexErrMsgTxt("Wrong number of arguments");

  if (init == 0) {
    // Initialize function
    //mexLock();

    // load GPUmat
    gm = gmGetGPUmat();

    // check gm
    gmCheckGPUmat(gm);

    // load module
    // NO MODULE REQUIRED

    // load float GPU function
    // NO FUNCTION REQUIRED

    init = 1;
  }

  // log
  gm->debug.log("> SIZE\n",0);
  gm->debug.logPush();

  if (gm->comp.getCompileMode() == 1) {
    mexWarnMsgTxt(WARNING_NUMERICS_COMPNOTIMPLEMENTED);
  }


  // mex parameters are:
  // IN: GPUtype variable

  GPUtype IN = gm->gputype.getGPUtype(prhs[0]);
  const int *in_size = gm->gputype.getSize(IN);
  int in_ndims = gm->gputype.getNdims(IN);
  // 2 cases
  // 1. s = size(A)
  // 2. s = size(A,dim)
  if (nrhs == 1) {
    // 2 cases:
    // 1. s = size(A)
    // 2. [a,b,c,...] = size(A)
    if (nlhs<=1) {
      // 1. s = size(A)
      // create output plhs[0]
      plhs[0] = mxCreateDoubleMatrix(1, in_ndims, mxREAL);
      // fill in plhs[0] with IN dimensions
      double *plhs_size = mxGetPr(plhs[0]);
      for (int i = 0; i < in_ndims; i++)
        plhs_size[i] = (double) in_size[i];
    } else {
      // 2. [a,b,c,...] = size(A)
      for (int i=0;i<nlhs;i++) {
        // create output
        // create output plhs[i]
        int r = 1;
        // if i is greater than IN dims return 1
        if (i>(in_ndims-1)) {
          // r = 1
        } else {
          r = in_size[i];
        }
        plhs[i] = mxCreateDoubleScalar(r);
      }
    }
  } else {
    // retrieve dim
    int dim  =  (int) mxGetScalar(prhs[1]);
    int r = 1;
    // if dim is greater than IN dims return 1
    if (dim>in_ndims) {
      // r = 1
    } else {
      r = in_size[dim-1];
    }
    // create output plhs[0]
    plhs[0] = mxCreateDoubleScalar(r);
  }



}
void
mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    int i, j, k;
    const int *Qtotdim;
    double *Qtot;
    mxArray *paramsarray;
    double *params;
    int paramsdim[3];
    mxArray *Qmatrix;
    double *Q;
    mxArray *qvector;
    double *q;
    double *p;
    mxArray *input[2];
    mxArray *output[1];
    int num_in, num_out;
    int N, M;
    
    /* Check the number of input and output arguments. */
    if (nrhs < 1)
	mexErrMsgTxt("Too few input arguments.");
    if (nrhs > 1)
	mexErrMsgTxt("Too many input arguments.");
    if (nlhs > 1)
	mexErrMsgTxt("Too many output arguments.");

    /* Check the formats of the input arguments. */
    for (i=0; i<nrhs; i++)
    {
	if (!mxIsNumeric(prhs[i]) || mxIsComplex(prhs[i])
	    || mxIsSparse(prhs[i]) || !mxIsDouble(prhs[i]))
	{
	    mexErrMsgTxt("All input arguments must be real and full numeric arrays, stored as doubles.");
	}
    }

    if (mxGetNumberOfDimensions(prhs[0]) != 4)
	mexErrMsgTxt("Qtot must be 4D.");
    Qtotdim = mxGetDimensions(prhs[0]);

    if (Qtotdim[2] != Qtotdim[3])
	mexErrMsgTxt("Qtot must be HEIGHT x WIDTH x n x n.");
    M = Qtotdim[2] - 1;

    Qtot = mxGetPr(prhs[0]);
    
    /* Create the output array. */
    paramsdim[0] = Qtotdim[0];
    paramsdim[1] = Qtotdim[1];
    paramsdim[2] = M;
    paramsarray = mxCreateNumericArray(3, paramsdim, mxDOUBLE_CLASS, mxREAL);
    params = mxGetPr(paramsarray);

    Qmatrix = mxCreateDoubleMatrix(M, M, mxREAL);
    Q = mxGetPr(Qmatrix);
    input[0] = Qmatrix;
    qvector = mxCreateDoubleMatrix(M, 1, mxREAL);
    q = mxGetPr(qvector);
    input[1] = qvector;
    num_in = 2;
    num_out = 1;
    
    N = Qtotdim[0] * Qtotdim[1];
    for (k=0; k<N; k++)
    {
	for (i=0; i<M; i++)
	{
	    for (j=0; j<M; j++)
	    {
		Q[i+j*M] = Qtot[k+(i+j*(M+1))*N];
	    }
	    q[i] = -Qtot[k+(i+M*(M+1))*N];
	}
	mexCallMATLAB(num_out, output, num_in, input, "\\");
	p = mxGetPr(output[0]);
	for (i=0; i<M; i++)
	    params[k+i*N] = p[i];
	mxDestroyArray(output[0]);
    }
    
    /* Output the computed result. */
    plhs[0] = paramsarray;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    int h = mxGetM(prhs[0]);
    int w = mxGetN(prhs[0])/3;
    int i, j, k;
    int nx[] = {0, 0, 1, -1, -1, 1, 1, -1};
    int ny[] = {1, -1, 0, 0, -1, 1, -1, 1};
    double cp[3], cq[3];
    double *chroma = mxGetPr(prhs[0]);
    double dist;
    plhs[0] = mxCreateDoubleMatrix(h*w, 1, mxREAL);
    plhs[1] = mxCreateDoubleMatrix(h, w, mxREAL);
    int dims[2] = {h*w, h*w};
    CvSparseMat* m_refConsMat = cvCreateSparseMat(2, dims, CV_32FC1);
    float sig_c= mxGetPr(prhs[1])[0];
    float sig_i= mxGetPr(prhs[2])[0];
    double *image = mxGetPr(prhs[3]);
    double ip[3], iq[3];
    double lp, lq;

    for(j=0; j<h; j++)
    {
        for(i=0; i<w; i++)
        {
            int p = i*h+j;
            cp[0] = chroma[i*h+j];
            cp[1] = chroma[h*w+i*h+j];
            cp[2] = chroma[2*h*w+i*h+j];
            ip[0] = image[i*h+j];
            ip[1] = image[h*w+i*h+j];
            ip[2] = image[2*h*w+i*h+j];
            lp = log(MAX(sqrt(ip[0]*ip[0] + ip[1]*ip[1] + ip[2]*ip[2]), 0.0001));
            for(k=0; k<8; k++)
            {
                int qi = i + nx[k];
                int qj = j + ny[k];
                int q = qi*h+qj;
                if(qi < 0 || qj < 0 || qi >= w || qj >= h)
                {
                    continue;
                }
                cq[0] = chroma[qi*h+qj];
                cq[1] = chroma[h*w+qi*h+qj];
                cq[2] = chroma[2*h*w+qi*h+qj];
                iq[0] = image[qi*h+qj];
                iq[1] = image[h*w+qi*h+qj];
                iq[2] = image[2*h*w+qi*h+qj];
                lq = log(MAX(sqrt(iq[0]*iq[0] + iq[1]*iq[1] + iq[2]*iq[2]), 0.0001));

                dist = 2.0 * (1.0 - (cp[0]*cq[0]+cp[1]*cq[1]+cp[2]*cq[2]));
                float weight = (1 + exp(-exp(lp) * exp(lp) / (sig_i*sig_i) - exp(lq)*exp(lq) / (sig_i*sig_i)));


                weight = weight * (exp(-dist*dist/(sig_c*sig_c)));
                if(k == 2)
                    mxGetPr(plhs[1])[p] = weight;

                if(_isnan(weight)) weight = 0;
                ((float*)cvPtr2D(m_refConsMat, p, p))[0] += weight;
                ((float*)cvPtr2D(m_refConsMat, q, q))[0] += weight;
                ((float*)cvPtr2D(m_refConsMat, p, q))[0] += -weight;
                ((float*)cvPtr2D(m_refConsMat, q, p))[0] += -weight;

                float dI = lp - lq;
                mxGetPr(plhs[0])[p] += weight * dI;
                mxGetPr(plhs[0])[q] -= weight * dI;
            }
        }
    }
    pushSparseMatrix(m_refConsMat, "WRC");

    cvReleaseSparseMat(&m_refConsMat);
}
Beispiel #10
0
const char *model_to_matlab_structure(mxArray *plhs[], int num_of_feature, struct svm_model *model)
{
	int i, j, n;
	double *ptr;
	mxArray *return_model, **rhs;
	int out_id = 0;

	rhs = (mxArray **)mxMalloc(sizeof(mxArray *)*NUM_OF_RETURN_FIELD);

	/* Parameters */
	rhs[out_id] = mxCreateDoubleMatrix(5, 1, mxREAL);
	ptr = mxGetPr(rhs[out_id]);
	ptr[0] = model->param.svm_type;
	ptr[1] = model->param.kernel_type;
	ptr[2] = model->param.degree;
	ptr[3] = model->param.gamma;
	ptr[4] = model->param.coef0;
	out_id++;

	/* nr_class*/
	rhs[out_id] = mxCreateDoubleMatrix(1, 1, mxREAL);
	ptr = mxGetPr(rhs[out_id]);
	ptr[0] = model->nr_class;
	out_id++;

	/* total SV */
	rhs[out_id] = mxCreateDoubleMatrix(1, 1, mxREAL);
	ptr = mxGetPr(rhs[out_id]);
	ptr[0] = model->l;
	out_id++;

	/* rho */
	n = model->nr_class*(model->nr_class-1)/2;
	rhs[out_id] = mxCreateDoubleMatrix(n, 1, mxREAL);
	ptr = mxGetPr(rhs[out_id]);
	for(i = 0; i < n; i++)
		ptr[i] = model->rho[i];
	out_id++;

	/* Label */
	if(model->label)
	{
		rhs[out_id] = mxCreateDoubleMatrix(model->nr_class, 1, mxREAL);
		ptr = mxGetPr(rhs[out_id]);
		for(i = 0; i < model->nr_class; i++)
			ptr[i] = model->label[i];
	}
	else
		rhs[out_id] = mxCreateDoubleMatrix(0, 0, mxREAL);
	out_id++;

	/* probA */
	if(model->probA != NULL)
	{
		rhs[out_id] = mxCreateDoubleMatrix(n, 1, mxREAL);
		ptr = mxGetPr(rhs[out_id]);
		for(i = 0; i < n; i++)
			ptr[i] = model->probA[i];
	}
	else
		rhs[out_id] = mxCreateDoubleMatrix(0, 0, mxREAL);
	out_id ++;

	/* probB */
	if(model->probB != NULL)
	{
		rhs[out_id] = mxCreateDoubleMatrix(n, 1, mxREAL);
		ptr = mxGetPr(rhs[out_id]);
		for(i = 0; i < n; i++)
			ptr[i] = model->probB[i];
	}
	else
		rhs[out_id] = mxCreateDoubleMatrix(0, 0, mxREAL);
	out_id++;

	/* nSV */
	if(model->nSV)
	{
		rhs[out_id] = mxCreateDoubleMatrix(model->nr_class, 1, mxREAL);
		ptr = mxGetPr(rhs[out_id]);
		for(i = 0; i < model->nr_class; i++)
			ptr[i] = model->nSV[i];
	}
	else
		rhs[out_id] = mxCreateDoubleMatrix(0, 0, mxREAL);
	out_id++;

	/* sv_coef */
	rhs[out_id] = mxCreateDoubleMatrix(model->l, model->nr_class-1, mxREAL);
	ptr = mxGetPr(rhs[out_id]);
	for(i = 0; i < model->nr_class-1; i++)
		for(j = 0; j < model->l; j++)
			ptr[(i*(model->l))+j] = model->sv_coef[i][j];
	out_id++;

	/* SVs */
	{
		int ir_index, nonzero_element;
		mwIndex *ir, *jc;
		mxArray *pprhs[1], *pplhs[1];	

		if(model->param.kernel_type == PRECOMPUTED)
		{
			nonzero_element = model->l;
			num_of_feature = 1;
		}
		else
		{
			nonzero_element = 0;
			for(i = 0; i < model->l; i++) {
				j = 0;
				while(model->SV[i][j].index != -1) 
				{
					nonzero_element++;
					j++;
				}
			}
		}

		/* SV in column, easier accessing */
		rhs[out_id] = mxCreateSparse(num_of_feature, model->l, nonzero_element, mxREAL);
		ir = mxGetIr(rhs[out_id]);
		jc = mxGetJc(rhs[out_id]);
		ptr = mxGetPr(rhs[out_id]);
		jc[0] = ir_index = 0;		
		for(i = 0;i < model->l; i++)
		{
			if(model->param.kernel_type == PRECOMPUTED)
			{
				/* make a (1 x model->l) matrix */
				ir[ir_index] = 0; 
				ptr[ir_index] = model->SV[i][0].value;
				ir_index++;
				jc[i+1] = jc[i] + 1;
			}
			else
			{
				int x_index = 0;
				while (model->SV[i][x_index].index != -1)
				{
					ir[ir_index] = model->SV[i][x_index].index - 1; 
					ptr[ir_index] = model->SV[i][x_index].value;
					ir_index++, x_index++;
				}
				jc[i+1] = jc[i] + x_index;
			}
		}
		/* transpose back to SV in row */
		pprhs[0] = rhs[out_id];
		if(mexCallMATLAB(1, pplhs, 1, pprhs, "transpose"))
			return "cannot transpose SV matrix";
		rhs[out_id] = pplhs[0];
		out_id++;
	}

	/* Create a struct matrix contains NUM_OF_RETURN_FIELD fields */
	return_model = mxCreateStructMatrix(1, 1, NUM_OF_RETURN_FIELD, field_names);

	/* Fill struct matrix with input arguments */
	for(i = 0; i < NUM_OF_RETURN_FIELD; i++)
		mxSetField(return_model,0,field_names[i],mxDuplicateArray(rhs[i]));
	/* return */
	plhs[0] = return_model;
	mxFree(rhs);

	return NULL;
}
mxArray
  *sf_c19_adcs_v15_integral_Power_no_charge_in_detumb_get_autoinheritance_info
  (void)
{
  const char *autoinheritanceFields[] = { "checksum", "inputs", "parameters",
    "outputs" };

  mxArray *mxAutoinheritanceInfo = mxCreateStructMatrix(1,1,4,
    autoinheritanceFields);

  {
    mxArray *mxChecksum = mxCreateDoubleMatrix(4,1,mxREAL);
    double *pr = mxGetPr(mxChecksum);
    pr[0] = (double)(3901237504U);
    pr[1] = (double)(937494241U);
    pr[2] = (double)(2394392834U);
    pr[3] = (double)(1299854650U);
    mxSetField(mxAutoinheritanceInfo,0,"checksum",mxChecksum);
  }

  {
    const char *dataFields[] = { "size", "type", "complexity" };

    mxArray *mxData = mxCreateStructMatrix(1,2,3,dataFields);

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,2,mxREAL);
      double *pr = mxGetPr(mxSize);
      pr[0] = (double)(1);
      pr[1] = (double)(1);
      mxSetField(mxData,0,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt" };

      mxArray *mxType = mxCreateStructMatrix(1,1,2,typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(10));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxData,0,"type",mxType);
    }

    mxSetField(mxData,0,"complexity",mxCreateDoubleScalar(0));

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,2,mxREAL);
      double *pr = mxGetPr(mxSize);
      pr[0] = (double)(1);
      pr[1] = (double)(1);
      mxSetField(mxData,1,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt" };

      mxArray *mxType = mxCreateStructMatrix(1,1,2,typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(10));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxData,1,"type",mxType);
    }

    mxSetField(mxData,1,"complexity",mxCreateDoubleScalar(0));
    mxSetField(mxAutoinheritanceInfo,0,"inputs",mxData);
  }

  {
    mxSetField(mxAutoinheritanceInfo,0,"parameters",mxCreateDoubleMatrix(0,0,
                mxREAL));
  }

  {
    const char *dataFields[] = { "size", "type", "complexity" };

    mxArray *mxData = mxCreateStructMatrix(1,4,3,dataFields);

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,2,mxREAL);
      double *pr = mxGetPr(mxSize);
      pr[0] = (double)(1);
      pr[1] = (double)(1);
      mxSetField(mxData,0,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt" };

      mxArray *mxType = mxCreateStructMatrix(1,1,2,typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(10));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxData,0,"type",mxType);
    }

    mxSetField(mxData,0,"complexity",mxCreateDoubleScalar(0));

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,2,mxREAL);
      double *pr = mxGetPr(mxSize);
      pr[0] = (double)(1);
      pr[1] = (double)(1);
      mxSetField(mxData,1,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt" };

      mxArray *mxType = mxCreateStructMatrix(1,1,2,typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(10));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxData,1,"type",mxType);
    }

    mxSetField(mxData,1,"complexity",mxCreateDoubleScalar(0));

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,2,mxREAL);
      double *pr = mxGetPr(mxSize);
      pr[0] = (double)(1);
      pr[1] = (double)(1);
      mxSetField(mxData,2,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt" };

      mxArray *mxType = mxCreateStructMatrix(1,1,2,typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(10));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxData,2,"type",mxType);
    }

    mxSetField(mxData,2,"complexity",mxCreateDoubleScalar(0));

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,2,mxREAL);
      double *pr = mxGetPr(mxSize);
      pr[0] = (double)(1);
      pr[1] = (double)(1);
      mxSetField(mxData,3,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt" };

      mxArray *mxType = mxCreateStructMatrix(1,1,2,typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(10));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxData,3,"type",mxType);
    }

    mxSetField(mxData,3,"complexity",mxCreateDoubleScalar(0));
    mxSetField(mxAutoinheritanceInfo,0,"outputs",mxData);
  }

  return(mxAutoinheritanceInfo);
}
mxArray *sf_c1_TTR_RobustMPC_RTmdl_get_autoinheritance_info(void)
{
  const char *autoinheritanceFields[] = { "checksum", "inputs", "parameters",
    "outputs", "locals" };

  mxArray *mxAutoinheritanceInfo = mxCreateStructMatrix(1,1,5,
    autoinheritanceFields);

  {
    mxArray *mxChecksum = mxCreateString("VVPNWBOOtqWmckFbbcB3AH");
    mxSetField(mxAutoinheritanceInfo,0,"checksum",mxChecksum);
  }

  {
    const char *dataFields[] = { "size", "type", "complexity" };

    mxArray *mxData = mxCreateStructMatrix(1,2,3,dataFields);

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,2,mxREAL);
      double *pr = mxGetPr(mxSize);
      pr[0] = (double)(1);
      pr[1] = (double)(1);
      mxSetField(mxData,0,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt" };

      mxArray *mxType = mxCreateStructMatrix(1,1,2,typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(10));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxData,0,"type",mxType);
    }

    mxSetField(mxData,0,"complexity",mxCreateDoubleScalar(0));

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,2,mxREAL);
      double *pr = mxGetPr(mxSize);
      pr[0] = (double)(2);
      pr[1] = (double)(1);
      mxSetField(mxData,1,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt" };

      mxArray *mxType = mxCreateStructMatrix(1,1,2,typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(10));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxData,1,"type",mxType);
    }

    mxSetField(mxData,1,"complexity",mxCreateDoubleScalar(0));
    mxSetField(mxAutoinheritanceInfo,0,"inputs",mxData);
  }

  {
    mxSetField(mxAutoinheritanceInfo,0,"parameters",mxCreateDoubleMatrix(0,0,
                mxREAL));
  }

  {
    const char *dataFields[] = { "size", "type", "complexity" };

    mxArray *mxData = mxCreateStructMatrix(1,1,3,dataFields);

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,2,mxREAL);
      double *pr = mxGetPr(mxSize);
      pr[0] = (double)(1);
      pr[1] = (double)(1);
      mxSetField(mxData,0,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt" };

      mxArray *mxType = mxCreateStructMatrix(1,1,2,typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(10));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxData,0,"type",mxType);
    }

    mxSetField(mxData,0,"complexity",mxCreateDoubleScalar(0));
    mxSetField(mxAutoinheritanceInfo,0,"outputs",mxData);
  }

  {
    mxSetField(mxAutoinheritanceInfo,0,"locals",mxCreateDoubleMatrix(0,0,mxREAL));
  }

  return(mxAutoinheritanceInfo);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
    size_t n;
    size_t prodDim;
    double *A, *B, *K;
    mxArray *retMat;
    const double sqrt2=sqrt(2);
    const double dblSqrt2=2*sqrt2;
    
    if(nrhs!=2) {
        mexErrMsgTxt("Incorrect number of inputs.");
        return;
    }

    if(nlhs>1) {
        mexErrMsgTxt("Wrong number of outputs.");
        return;
    }

    if((mxIsEmpty(prhs[0])&&!mxIsEmpty(prhs[1]))||(!mxIsEmpty(prhs[0])&&mxIsEmpty(prhs[1]))) {
        mexErrMsgTxt("Invalid use of empty matrices as inputs.");
        return;
    }
    
    //If two empty matrices are passed, then just return an empty matrix.
    if(mxIsEmpty(prhs[0])&&mxIsEmpty(prhs[1])) {
        plhs[0]=mxCreateDoubleMatrix(0,0,mxREAL);
        return;
    }
    
    checkRealDoubleArray(prhs[0]);
    checkRealDoubleArray(prhs[1]);
    
    if(mxGetNumberOfDimensions(prhs[0])>2||mxGetNumberOfDimensions(prhs[1])>2) {
        mexErrMsgTxt("This function does not work with hypermatrices.");
        return; 
    }
    
    n=mxGetN(prhs[0]);
    if(n!=mxGetM(prhs[0])||n!=mxGetN(prhs[1])||n!=mxGetM(prhs[1])) {
        mexErrMsgTxt("This function only works with pairs of squares matrices.");
        return;
    }
    
    prodDim=(n*(n+1))/2;
    
    //Allocate space for the return matrix
    retMat=mxCreateDoubleMatrix(prodDim,prodDim,mxREAL);
    A=(double*)mxGetData(prhs[0]);
    B=(double*)mxGetData(prhs[1]);
    K=(double*)mxGetData(retMat);
    
    {
       size_t i1,i2,j1,j2;
        
        for(i1=1;i1<=n;i1++) {
            size_t col=i1+((i1-1)*(2*n-i1))/2;
            for(j1=1;j1<=n;j1++) {
                size_t KIdx=prodDim*(col-1)+j1+((j1-1)*(2*n-j1))/2-1;
                const size_t j1i1=j1+n*(i1-1)-1;
                size_t j2i1=j1i1;
                
                K[KIdx]=(A[j1i1]*B[j1i1]+B[j1i1]*A[j1i1])/2;
                for(j2=(j1+1);j2<=n;j2++) {
                    KIdx++;
                    j2i1++;
                    
                    K[KIdx]=(A[j1i1]*B[j2i1]+B[j1i1]*A[j2i1])/sqrt2;
                }
            }

            for(i2=(i1+1);i2<=n;i2++) {
                col++;
                for(j1=1;j1<=n;j1++) {
                    size_t KIdx=prodDim*(col-1)+j1+((j1-1)*(2*n-j1))/2-1;
                    const size_t j1i1=j1+n*(i1-1)-1;
                    const size_t j1i2=j1+n*(i2-1)-1;
                    size_t j2i1=j1i1;
                    size_t j2i2=j1i2;

                    K[KIdx]=(A[j1i1]*B[j1i2]+A[j1i2]*B[j1i1]+B[j1i1]*A[j1i2]+B[j1i2]*A[j1i1])/dblSqrt2;     
                    for(j2=(j1+1);j2<=n;j2++) {
                        KIdx++;
                        j2i1++;
                        j2i2++;
                        
                        K[KIdx]=(A[j1i1]*B[j2i2]+A[j1i2]*B[j2i1]+B[j1i1]*A[j2i2]+B[j1i2]*A[j2i1])/2;
                    }
                }
            }
        }
    }
    
    plhs[0]=retMat;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs,
                 const mxArray *prhs[])
{
    char *CIN;
    int *DS, *WS, *SI, *ZIN;
    double *srww,*srwp,*srdp, *probs, *WC, *WWC, *ZZ;
    double ALPHA,BETA, GAMMA0, GAMMA1, DELTA;
    mwIndex *irww, *jcww, *irwp, *jcwp, *irdp, *jcdp;
    int *z,*d,*w, *s, *c, *sc, *order, *wp, *dp, *ztot, *wtot, *wc;
    int W,T,D,NN,SEED,OUTPUT, nzmax, nzmaxwp, nzmaxdp, ntokens;
    int i,j,cc,n,nt,wi,di;
    int startcond;

    mexPrintf( "GibbsSamplerLDACOL: entering...\n" );
    mexEvalString("drawnow;");

    /* Check for proper number of arguments. */
    if (nrhs < 13) {
        mexErrMsgTxt("At least 13 input arguments required");
    } else if (nlhs != 5) {
        mexErrMsgTxt("5 output arguments required");
    }

    //[ WP,DP,WC,C,Z ] = GibbsSamplerLDACOL( WS , DS , SI , WW , T , N , ALPHA , BETA , GAMMA0, GAMMA1 , DELTA , SEED , OUTPUT , CIN , ZIN );

    /* process the input arguments */
    if (mxIsInt32( prhs[ 0 ] ) != 1)
        mexErrMsgTxt("WS must be int32");
    WS = (int *)mxGetData(prhs[0]);

    if (mxIsInt32( prhs[ 1 ] ) != 1)
        mexErrMsgTxt("DS must be int32");
    DS = (int *)mxGetData(prhs[1]);

    if (mxIsInt32( prhs[ 2 ] ) != 1)
        mexErrMsgTxt("SI must be int32");
    SI = (int *)mxGetData(prhs[2]);

    //for (int yy=0;yy<20;yy++)
    //  printf("%2d: WS=%4u, DS=%4u, SI=%4u\n",yy,WS[yy],DS[yy],SI[yy]);

    if ((mxIsSparse( prhs[ 3 ] ) != 1) || (mxIsDouble( prhs[ 3 ] ) != 1))
        mexErrMsgTxt("WW collocation matrix must be a sparse double precision matrix");

    /* dealing with sparse array WW */
    srww = mxGetPr(prhs[3]);
    irww  = mxGetIr(prhs[3]);
    jcww  = mxGetJc(prhs[3]);
    nzmax= mxGetNzmax(prhs[3]);

    W    = mxGetM( prhs[3] );
    ntokens = mxGetM( prhs[ 0 ] ) * mxGetN( prhs[ 0 ] );

    D    = 0;
    for (i=0; i<ntokens; i++) {
        if (DS[ i ] > D) D = (int) DS[ i ];
    }

    T    = (int) mxGetScalar(prhs[4]);
    if (T<=0) mexErrMsgTxt("Number of topics must be greater than zero");

    NN    = (int) mxGetScalar(prhs[5]);
    if (NN<0) mexErrMsgTxt("Number of iterations must be greater than zero");

    ALPHA = (double) mxGetScalar(prhs[6]);
    if (ALPHA<=0) mexErrMsgTxt("ALPHA must be greater than zero");

    BETA = (double) mxGetScalar(prhs[7]);
    if (BETA<=0) mexErrMsgTxt("BETA must be greater than zero");

    GAMMA0 = (double) mxGetScalar(prhs[8]);
    if (GAMMA0<=0) mexErrMsgTxt("GAMMA0 must be greater than zero");

    GAMMA1 = (double) mxGetScalar(prhs[9]);
    if (GAMMA1<=0) mexErrMsgTxt("GAMMA1 must be greater than zero");

    DELTA = (double) mxGetScalar(prhs[10]);
    if (DELTA<=0) mexErrMsgTxt("DELTA must be greater than zero");

    SEED = (int) mxGetScalar(prhs[11]);
    // set the seed of the random number generator

    OUTPUT = (int) mxGetScalar(prhs[12]);

    // assume that we start a new chain
    startcond = 0;

    if (nrhs > 13) {
        startcond = 1;

        if (mxIsInt8( prhs[ 13 ] ) != 1)
            mexErrMsgTxt("C must be int8");
        CIN =  (char*)mxGetData(prhs[13]);

        if (mxIsInt32( prhs[ 14 ] ) != 1)
            mexErrMsgTxt("Z must be int32");
        ZIN =  (int*)mxGetData(prhs[14]);

        //for (int yy=0;yy<20;yy++)
        //   printf("%2d: Z=%4u, C=%4u\n",yy,ZIN[yy],CIN[yy]);

    }

    // seeding
    seedMT( 1 + SEED * 2 ); // seeding only works on uneven numbers

    /* allocate memory */
    if (OUTPUT==2) {
        mexPrintf( "GibbsSamplerLDACOL: Allocating z,d,w,s,c,sc\n" );
        mexEvalString("drawnow;");
    }
    z  = (int *) mxCalloc( ntokens , sizeof( int ));
    d  = (int *) mxCalloc( ntokens , sizeof( int ));
    w  = (int *) mxCalloc( ntokens , sizeof( int ));
    s  = (int *) mxCalloc( ntokens , sizeof( int ));
    c  = (int *) mxCalloc( ntokens , sizeof( int ));
    sc  = (int *) mxCalloc( ntokens , sizeof( int ));

    if (startcond==1) {
        if (OUTPUT==2) {
            mexPrintf( "GibbsSamplerLDACOL: Preparing c,z\n" );
        }
        for (i=0; i<ntokens; i++) c[ i ] = (int) CIN[ i ];
        for (i=0; i<ntokens; i++) z[ i ] = (int) ZIN[ i ] - (int) 1;
    }

    if (OUTPUT==2) {
        mexPrintf( "GibbsSamplerLDACOL: Preparing w,d,s\n" );
        mexEvalString("drawnow;");
    }
    for (i=0; i<ntokens; i++) w[ i ] = (int) (WS[ i ] - 1); // Matlab indexing not zero based
    for (i=0; i<ntokens; i++) d[ i ] = (int) (DS[ i ] - 1); // Matlab indexing not zero based
    for (i=0; i<ntokens; i++) s[ i ] = (int) SI[ i ];

    if (OUTPUT==2) {
        mexPrintf( "GibbsSamplerLDACOL: Allocating order,wp,dp,wc,ztot,wtot,probs\n" );
        mexEvalString("drawnow;");
    }
    order  = (int *) mxCalloc( ntokens , sizeof( int ));
    wp  = (int *) mxCalloc( T*W , sizeof( int ));
    dp  = (int *) mxCalloc( T*D , sizeof( int ));
    wc  = (int *) mxCalloc( W , sizeof( int ));
    ztot  = (int *) mxCalloc( T , sizeof( int ));
    wtot  = (int *) mxCalloc( W , sizeof( int ));
    probs  = (double *) mxCalloc( T , sizeof( double ));

    n = ntokens;

    /*for (i=0; i<10; i++) {
       mexPrintf( "i=%4d w[i]=%3d d[i]=%d z[i]=%d s[i]=%d\n" , i , w[i] , d[i] , z[i] , s[i] );
    }*/

    if (OUTPUT==2) {
        mexPrintf( "Running LDA COL Gibbs Sampler Version 1.0\n" );
        if (startcond==1) mexPrintf( "Starting with C and Z vector given as input\n" );
        mexPrintf( "Arguments:\n" );
        mexPrintf( "\tNumber of words      W = %d\n" , W );
        mexPrintf( "\tNumber of docs       D = %d\n" , D );
        mexPrintf( "\tNumber of topics     T = %d\n" , T );
        mexPrintf( "\tNumber of iterations N = %d\n" , NN );
        mexPrintf( "\tHyperparameter   ALPHA = %4.4f\n" , ALPHA );
        mexPrintf( "\tHyperparameter    BETA = %4.4f\n" , BETA );
        mexPrintf( "\tHyperparameter  GAMMA0 = %4.4f\n" , GAMMA0 );
        mexPrintf( "\tHyperparameter  GAMMA1 = %4.4f\n" , GAMMA1 );
        mexPrintf( "\tHyperparameter  DELTA  = %4.4f\n" , DELTA );
        mexPrintf( "\tSeed number            = %d\n" , SEED );
        mexPrintf( "\tNumber of tokens       = %d\n" , ntokens );
        mexPrintf( "Internal Memory Allocation\n" );
        mexPrintf( "\tw,d,z,s,c,sc,order indices combined = %d bytes\n" , 7 * sizeof( int) * ntokens );
        mexPrintf( "\twp (full) matrix (%dx%d) = %d bytes\n" , W,T,sizeof( int ) * W * T  );
        mexPrintf( "\tdp (full) matrix (%dx%d) = %d bytes\n" , D,T,sizeof( int ) * D * T  );
        mexPrintf( "Checking: sizeof(char)=%d sizeof(int)=%d sizeof(long)=%d sizeof(double)=%d\n" , sizeof(char),sizeof(int) , sizeof(long) , sizeof(double));
        mexEvalString("drawnow;");
    }

    /* run the model */
    GibbsSamplerLDACOL( ALPHA, BETA, GAMMA0,GAMMA1,DELTA, W, T, D, NN, OUTPUT, n, z, d, w, s, c, sc, wp, dp, ztot, wtot, order, probs , srww, irww, jcww, wc, startcond );

    /* ---------------------------------------------
     Erasing variables to free memory
     -----------------------------------------------*/
    if (OUTPUT==2) {
        mexPrintf( "Freeing internal memory from d,w,s,sc,ztot,wtot,probs\n");
        mexEvalString("drawnow;");
        mxFree(d);
        mxFree(w);
        mxFree(s);
        mxFree(sc);
        mxFree(ztot);
        mxFree(wtot);
        mxFree(probs);
    }

    /* ---------------------------------------------
     convert the full wp matrix into a sparse matrix
     -----------------------------------------------*/
    nzmaxwp = 0;
    for (i=0; i<W; i++) {
        for (j=0; j<T; j++)
            nzmaxwp += (int) ( *( wp + j + i*T )) > 0;
    }

    if (OUTPUT==2) {
        mexPrintf( "Constructing sparse output matrix wp\n" );
        mexPrintf( "Number of nonzero entries for WP = %d\n" , nzmaxwp );
        mexEvalString("drawnow;");
    }

    plhs[0] = mxCreateSparse( W,T,nzmaxwp,mxREAL);
    srwp  = mxGetPr(plhs[0]);
    irwp = mxGetIr(plhs[0]);
    jcwp = mxGetJc(plhs[0]);

    n = 0;
    for (j=0; j<T; j++) {
        *( jcwp + j ) = n;
        for (i=0; i<W; i++) {
            cc = (int) *( wp + i*T + j );
            if (cc >0) {
                *( srwp + n ) = cc;
                *( irwp + n ) = i;
                n++;
            }
        }
    }

    *( jcwp + T ) = n;

    if (OUTPUT==2) {
        mexPrintf( "Freeing internal memory from matrix 'wp'\n");
        mexEvalString("drawnow;");
        mxFree(wp);
    }

    /* ---------------------------------------------
     convert the full DP matrix into a sparse matrix
     -----------------------------------------------*/
    nzmaxdp = 0;
    for (i=0; i<D; i++) {
        for (j=0; j<T; j++)
            nzmaxdp += (int) ( *( dp + j + i*T )) > 0;
    }
    if (OUTPUT==2) {
        mexPrintf( "Constructing sparse output matrix dp\n" );
        mexPrintf( "Number of nonzero entries for DP = %d\n" , nzmaxdp );
        mexEvalString("drawnow;");
    }

    plhs[1] = mxCreateSparse( D,T,nzmaxdp,mxREAL);
    srdp  = mxGetPr(plhs[1]);
    irdp = mxGetIr(plhs[1]);
    jcdp = mxGetJc(plhs[1]);

    n = 0;
    for (j=0; j<T; j++) {
        *( jcdp + j ) = n;
        for (i=0; i<D; i++) {
            cc = (int) *( dp + i*T + j );
            if (cc >0) {
                *( srdp + n ) = cc;
                *( irdp + n ) = i;
                n++;
            }
        }
    }

    *( jcdp + T ) = n;

    if (OUTPUT==2) {
        mexPrintf( "Freeing internal memory from matrix 'dp'\n");
        mexEvalString("drawnow;");
        mxFree(dp);
    }

    /* ---------------------------------------------
       create the WC count matrix
     -----------------------------------------------*/
    plhs[ 2 ] = mxCreateDoubleMatrix( W , 1 , mxREAL );
    WC = mxGetPr( plhs[ 2 ] );
    for (i=0; i<W; i++) WC[ i ] = (double) wc[ i ];

    if (OUTPUT==2) {
        mexPrintf( "Freeing internal memory from matrix 'wc'\n");
        mexEvalString("drawnow;");
        mxFree(wc);
    }

    /* ---------------------------------------------
       create the C route vector
     -----------------------------------------------*/
    plhs[ 3 ] = mxCreateDoubleMatrix( ntokens , 1 , mxREAL );
    WWC = mxGetPr( plhs[ 3 ] );
    for (i=0; i<ntokens; i++) WWC[ i ] = (double) c[ i ];

    if (OUTPUT==2) {
        mexPrintf( "Freeing internal memory from matrix 'c'\n");
        mexEvalString("drawnow;");
        mxFree(c);
    }

    /* ---------------------------------------------
       create the topic assignment vector
     -----------------------------------------------*/
    plhs[ 4 ] = mxCreateDoubleMatrix( ntokens , 1 , mxREAL );
    ZZ = mxGetPr( plhs[ 4 ] );
    for (i=0; i<ntokens; i++) ZZ[ i ] = (double) z[ i ] + 1;

    if (OUTPUT==2) {
        mexPrintf( "Freeing internal memory from matrix 'z'\n");
        mexEvalString("drawnow;");
        mxFree(z);
    }

}
Beispiel #15
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    if(nrhs < 1) {
		mexPrintf("Error, at least 1 parameter required!\n");
		return;
	}

    int inp_m = mxGetM(prhs[0]);
    int inp_n = mxGetN(prhs[0]);

	int opcode = (int)(mxGetScalar(prhs[0]));

	switch(opcode) {
		case 0: // connect
			{
				if(nrhs != 2) {
					mexPrintf("Error, expecting 2 parameters\n");
					return;
				}
				int port = (int)(mxGetScalar(prhs[1]));
				mexPrintf("Connecting on port %d\n", port);
				int success = connect(port);
				plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL);
				double* output = mxGetPr(plhs[0]);
				output[0] = success;
			}
			return;
		case 1: // acc
			{
				if(nrhs != 1) {
					mexPrintf("Error, too many parameters\n");
					return;
				}
				int xyz[3];
				shake_acc(dev, xyz);
				plhs[0] = mxCreateDoubleMatrix(1, 4, mxREAL);
				double* output = mxGetPr(plhs[0]);
				for(int i=0;i<3;i++)
					output[i] = xyz[i];
				output[3] = shake_data_timestamp(dev, SHAKE_SENSOR_ACC);
			}
			return;
		case 2: // gyro
			{
				if(nrhs != 1) {
					mexPrintf("Error, too many parameters\n");
					return;
				}
				int xyz[3];
				shake_gyr(dev, xyz);
				plhs[0] = mxCreateDoubleMatrix(1, 4, mxREAL);
				double* output = mxGetPr(plhs[0]);
				for(int i=0;i<3;i++)
					output[i] = xyz[i];
				output[3] = shake_data_timestamp(dev, SHAKE_SENSOR_GYRO);
			}
			return;
		case 3: // mag
			{
				if(nrhs != 1) {
					mexPrintf("Error, too many parameters\n");
					return;
				}
				int xyz[3];
				shake_mag(dev, xyz);
				plhs[0] = mxCreateDoubleMatrix(1, 4, mxREAL);
				double* output = mxGetPr(plhs[0]);
				for(int i=0;i<3;i++)
					output[i] = xyz[i];
				output[3] = shake_data_timestamp(dev, SHAKE_SENSOR_MAG);
			}
			return;
		case 4: // write
			{
				if(nrhs != 3) {
					mexPrintf("Error, expecting 3 parameters\n");
					return;
				}
				int addr = (int)mxGetScalar(prhs[1]);
				int val = (int)mxGetScalar(prhs[2]);
				int success = write(addr, val);
				plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL);
				double* output = mxGetPr(plhs[0]);
				output[0] = success;
			}
			return;
		case 5: // close
			close();
			break;
		default:
			mexPrintf("Error, Unknown opcode\n");
			return;
	}
    return;
}
Beispiel #16
0
/* mexFunction is the gateway routine for the MEX-file. */ 
void mexFunction( int nlhs, mxArray *plhs[], 
		  int nrhs, const mxArray *prhs[] )
     
{ 
  double *xx,*yy,*zz; 
  double *x,*y,*z,*T,*Q,*S;
  int nrow,ncol; 
  
  
  
  /* Check for proper number of arguments */
  if (nrhs < 4) { 
    mexErrMsgTxt("At least 4 input arguments required."); 
  } 
  /* This Code doesn't work.  You must have 7 inputs */
  else if (nrhs < 5){
      T = mxGetPr(T_IN);
      Q[0] = 0.0;
      Q[1] = 0.0;
      Q[2] = 0.0;
      Q[3] = 1.0;
      *S = 1.0;
  }
  else if (nrhs < 6){
      T = mxGetPr(T_IN);
      Q = mxGetPr(Q_IN);
      *S = 1.0;
  }
  else if (nrhs < 7){
      T = mxGetPr(T_IN);
      Q = mxGetPr(Q_IN);
      S = mxGetPr(S_IN);
  }
  else if (nlhs > 3) {
    mexErrMsgTxt("Too many output arguments."); 
  } 
  
  
  /* Assign pointers to the input parameters */ 
  x = mxGetPr(X_IN); 
  y = mxGetPr(Y_IN);
  z = mxGetPr(Z_IN);
  
  
  nrow = mxGetM(X_IN);
  ncol = mxGetN(X_IN);
  
  /* Create a matrix for the return argument */ 
  X_OUT = mxCreateDoubleMatrix(nrow,ncol, mxREAL); 
  Y_OUT = mxCreateDoubleMatrix(nrow,ncol, mxREAL); 
  Z_OUT = mxCreateDoubleMatrix(nrow,ncol, mxREAL); 
  
   /* Assign pointers to the output parameters */ 
  xx = mxGetPr(X_OUT);
  yy = mxGetPr(Y_OUT);
  zz = mxGetPr(Z_OUT);
  
  /*N = plhs[0];
    D1= plhs[1];
    D2= plhs[2];*/
  
  
  /* Do the actual computations in a subroutine */
  xformq_surf(xx,yy,zz,x,y,z,T,Q,S,nrow,ncol); 
  return;
  
}
unsigned int sf_BoatDynamic_autoinheritance_info( int nlhs, mxArray * plhs[],
  int nrhs, const mxArray * prhs[] )
{

#ifdef MATLAB_MEX_FILE

  char commandName[32];
  char aiChksum[64];
  if (nrhs<3 || !mxIsChar(prhs[0]) )
    return 0;

  /* Possible call to get the autoinheritance_info */
  mxGetString(prhs[0], commandName,sizeof(commandName)/sizeof(char));
  commandName[(sizeof(commandName)/sizeof(char)-1)] = '\0';
  if (strcmp(commandName,"get_autoinheritance_info"))
    return 0;
  mxGetString(prhs[2], aiChksum,sizeof(aiChksum)/sizeof(char));
  aiChksum[(sizeof(aiChksum)/sizeof(char)-1)] = '\0';

  {
    unsigned int chartFileNumber;
    chartFileNumber = (unsigned int)mxGetScalar(prhs[1]);
    switch (chartFileNumber) {
     case 1:
      {
        if (strcmp(aiChksum, "m1LCUJKGDY65NnIe4NLJlF") == 0) {
          extern mxArray *sf_c1_BoatDynamic_get_autoinheritance_info(void);
          plhs[0] = sf_c1_BoatDynamic_get_autoinheritance_info();
          break;
        }

        plhs[0] = mxCreateDoubleMatrix(0,0,mxREAL);
        break;
      }

     case 2:
      {
        if (strcmp(aiChksum, "j70NnJ9afNTK58HSep1njE") == 0) {
          extern mxArray *sf_c2_BoatDynamic_get_autoinheritance_info(void);
          plhs[0] = sf_c2_BoatDynamic_get_autoinheritance_info();
          break;
        }

        plhs[0] = mxCreateDoubleMatrix(0,0,mxREAL);
        break;
      }

     default:
      plhs[0] = mxCreateDoubleMatrix(0,0,mxREAL);
    }
  }

  return 1;

#else

  return 0;

#endif

}
Beispiel #18
0
/* ************************************************************
   PROCEDURE mexFunction - Entry for Matlab
   ************************************************************ */
void mexFunction(const int nlhs, mxArray *plhs[],
  const int nrhs, const mxArray *prhs[])
{
  int inz, i, k, nk, nksqr, lenud, sdplen, gnnz;
  int *gjc, *iwork;
  const double *gjcPr;
  const double *g, *gk;
  double *y;
  coneK cK;
/* ------------------------------------------------------------
   Check for proper number of arguments 
   ------------------------------------------------------------ */
  mxAssert(nrhs >= NPARIN, "givensrot requires more input arguments.");
  mxAssert(nlhs <= NPAROUT, "givensrot generates less output arguments.");
/* ------------------------------------------------------------
   Disassemble cone K structure
   ------------------------------------------------------------ */
  conepars(K_IN, &cK);
/* ------------------------------------------------------------
   Get statistics of cone K structure
   ------------------------------------------------------------ */
  lenud = cK.rDim + cK.hDim;
  sdplen = cK.rLen + cK.hLen;
/* ------------------------------------------------------------
   Get inputs gjc,g,x
   ------------------------------------------------------------ */
  mxAssert(mxGetM(GJC_IN) * mxGetN(GJC_IN) == sdplen, "gjc size mismatch");
  gjcPr = mxGetPr(GJC_IN);
  g = (double *) mxGetPr(G_IN);
  gnnz = mxGetM(G_IN) * mxGetN(G_IN);
  mxAssert(mxGetM(X_IN) == lenud && mxGetN(X_IN) == 1, "x size mismatch");
/* ------------------------------------------------------------
   Allocate output y(lenud), and let y = x.
   ------------------------------------------------------------ */
  Y_OUT = mxCreateDoubleMatrix(lenud, 1, mxREAL);
  y = mxGetPr(Y_OUT);
  memcpy(y, mxGetPr(X_IN), lenud * sizeof(double));
/* ------------------------------------------------------------
   Allocate working array iwork(sum(K.s))
   ------------------------------------------------------------ */
  iwork = (int *) mxCalloc(MAX(1,sdplen), sizeof(int));
/* ------------------------------------------------------------
   Convert gjcPr from float to int, and store in gjc:=iwork.
   ------------------------------------------------------------ */
  gjc = iwork;
  for(i = 0; i < sdplen; i++)
    gjc[i] = gjcPr[i];                 /* don't subtract 1: already C-style */
/* ------------------------------------------------------------
   The actual job is done here: U_NEW = Q(g) * U_OLD
   ------------------------------------------------------------ */
  inz = 0;
  for(k = 0; k < cK.rsdpN; k++){                /* real symmetric */
    nk = cK.sdpNL[k];
    nksqr = SQR(nk);
    mxAssert(inz + 2 * gjc[nk-1] <= gnnz, "g size mismatch");
    gk = g+inz;
    matgivens(y, (twodouble *) gk, gjc, nk);
    y += nksqr;
    inz += 2 * gjc[nk-1];        /* each rotation consists of 2 doubles */
    gjc += nk;
  }
  for(; k < cK.sdpN; k++){                       /* complex Hermitian */
    nk = cK.sdpNL[k];
    nksqr = SQR(nk);
    mxAssert(inz + 3 * gjc[nk-1] <= gnnz, "g size mismatch");
    gk = g+inz;
    prpimatgivens(y,y+nksqr, (tridouble *) gk, gjc, nk);
    nksqr += nksqr;
    y += nksqr;
    inz += 3 * gjc[nk-1];            /* each rotation consists of 3 doubles */
    gjc += nk;
  }
/* ------------------------------------------------------------
   Release working arrays
   ------------------------------------------------------------ */
  mxFree(iwork);
}
mxArray *sf_c14_Model_justmodel_get_autoinheritance_info(void)
{
  const char *autoinheritanceFields[] = { "checksum", "inputs", "parameters",
    "outputs", "locals" };

  mxArray *mxAutoinheritanceInfo = mxCreateStructMatrix(1,1,5,
    autoinheritanceFields);

  {
    mxArray *mxChecksum = mxCreateString("H08gUXOQ4cGie6wRMFqGLH");
    mxSetField(mxAutoinheritanceInfo,0,"checksum",mxChecksum);
  }

  {
    const char *dataFields[] = { "size", "type", "complexity" };

    mxArray *mxData = mxCreateStructMatrix(1,1,3,dataFields);

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,2,mxREAL);
      double *pr = mxGetPr(mxSize);
      pr[0] = (double)(3);
      pr[1] = (double)(1);
      mxSetField(mxData,0,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt" };

      mxArray *mxType = mxCreateStructMatrix(1,1,2,typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(10));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxData,0,"type",mxType);
    }

    mxSetField(mxData,0,"complexity",mxCreateDoubleScalar(0));
    mxSetField(mxAutoinheritanceInfo,0,"inputs",mxData);
  }

  {
    mxSetField(mxAutoinheritanceInfo,0,"parameters",mxCreateDoubleMatrix(0,0,
                mxREAL));
  }

  {
    const char *dataFields[] = { "size", "type", "complexity" };

    mxArray *mxData = mxCreateStructMatrix(1,2,3,dataFields);

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,2,mxREAL);
      double *pr = mxGetPr(mxSize);
      pr[0] = (double)(3);
      pr[1] = (double)(3);
      mxSetField(mxData,0,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt" };

      mxArray *mxType = mxCreateStructMatrix(1,1,2,typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(10));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxData,0,"type",mxType);
    }

    mxSetField(mxData,0,"complexity",mxCreateDoubleScalar(0));

    {
      mxArray *mxSize = mxCreateDoubleMatrix(1,2,mxREAL);
      double *pr = mxGetPr(mxSize);
      pr[0] = (double)(3);
      pr[1] = (double)(3);
      mxSetField(mxData,1,"size",mxSize);
    }

    {
      const char *typeFields[] = { "base", "fixpt" };

      mxArray *mxType = mxCreateStructMatrix(1,1,2,typeFields);
      mxSetField(mxType,0,"base",mxCreateDoubleScalar(10));
      mxSetField(mxType,0,"fixpt",mxCreateDoubleMatrix(0,0,mxREAL));
      mxSetField(mxData,1,"type",mxType);
    }

    mxSetField(mxData,1,"complexity",mxCreateDoubleScalar(0));
    mxSetField(mxAutoinheritanceInfo,0,"outputs",mxData);
  }

  {
    mxSetField(mxAutoinheritanceInfo,0,"locals",mxCreateDoubleMatrix(0,0,mxREAL));
  }

  return(mxAutoinheritanceInfo);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {		  

  if(nrhs < 5 || nrhs > 5) {
    mexErrMsgTxt("expected either 5 input arguments.\n");
		return;
  }
  
  if(nlhs != 3) {
    mexErrMsgTxt("expected 3 output arguments.\n");
    return;
  }
  
  // read input arguments
  int arg = 0;
  
  // amount of training examples == number of slacks
	const mxArray *params = prhs[arg];
	const int num_exm = (int) *mxGetPr(mxGetField(params,0, "num_examples"));
//	printf("Number of examples (new): %i\n",num_exm);
  ++arg;
	
	// this is the current solution
	const int M3 = mxGetM(prhs[arg]);
  const int D = mxGetN(prhs[arg]);
/*
  assert(M3 == 1);
*/
	double *w = mxGetPr(prhs[arg]);
	++arg;
	// get the delta_y_ybar [1 x t]-array
	const int M1 = mxGetM(prhs[arg]);
  const int N1 = mxGetN(prhs[arg]);
  assert(M1 == 1);
	double *delta_y_ybar = mxGetPr(prhs[arg]);
	++arg;

	const int DIM = mxGetM(prhs[arg]);
  const int N = mxGetN(prhs[arg]);
	double *delta_psis = mxGetPr(prhs[arg]);
	++arg;

	// get the delta_psis_idxs [1 x n]-array
	const int M2 = mxGetM(prhs[arg]);
  const int N2 = mxGetN(prhs[arg]);
  assert(M2 == 1);
	double *delta_psis_idxs = mxGetPr(prhs[arg]);
	++arg;

	 

  // these are the output values (already initialized)
	plhs[0] = mxCreateDoubleMatrix(1, num_exm, mxREAL);
  double *losses = mxGetPr(plhs[0]);

 	plhs[1] = mxCreateDoubleMatrix(1, num_exm, mxREAL);
  double *idxs = mxGetPr(plhs[1]);

	  // copy the parameter structure without making changes
	// to the elements
	plhs[2] = mxDuplicateArray(params);



	// for all dpsis
	for (int n=0; n<N; n++) {
		double loss = delta_y_ybar[n];
		const int ind = delta_psis_idxs[n];

		// for all dimensions in dpsis
		for (int d=0; d<DIM; d++) {
			loss -= w[d]*delta_psis[DIM*n+d];
		}
		
		// hinge 
		if (loss<0.0) {
			loss = 0.0;
		}

		//printf("t=%i n=%i loss=%f idxs=%i\n",num_exm,n,loss,ind);

		// store loss 
		if (loss>=losses[ind-1]) {
			losses[ind-1] = loss;
			idxs[ind-1] = n+1;
		}

	}

}
Beispiel #21
0
void mexFunction( int nlhs, mxArray *plhs[], 
		  int nrhs, const mxArray*prhs[] )
     
{ 
  LSMLIB_REAL *phi_x, *phi_y;
  int ilo_grad_phi_gb, ihi_grad_phi_gb, jlo_grad_phi_gb, jhi_grad_phi_gb;
  LSMLIB_REAL *phi; 
  int ilo_phi_gb, ihi_phi_gb, jlo_phi_gb, jhi_phi_gb;
  LSMLIB_REAL *vel_x, *vel_y;
  int ilo_vel_gb, ihi_vel_gb, jlo_vel_gb, jhi_vel_gb;
  LSMLIB_REAL *D1;
  int ilo_D1_gb, ihi_D1_gb, jlo_D1_gb, jhi_D1_gb;
  LSMLIB_REAL *D2;
  int ilo_D2_gb, ihi_D2_gb, jlo_D2_gb, jhi_D2_gb;
  int ilo_fb, ihi_fb, jlo_fb, jhi_fb;
  double *dX;
  LSMLIB_REAL dX_meshgrid_order[2];
  int ghostcell_width;
  int num_data_array_dims;
  
  /* Check for proper number of arguments */
  if (nrhs != 5) { 
    mexErrMsgTxt("Five required input arguments."); 
  } else if (nlhs > 2) {
    mexErrMsgTxt("Too many output arguments."); 
  } 
    
  /* Parameter Checks */
  num_data_array_dims = mxGetNumberOfDimensions(PHI);
  if (num_data_array_dims != 2) {
    mexErrMsgTxt("phi should be a 2 dimensional array."); 
  }
  num_data_array_dims = mxGetNumberOfDimensions(VEL_X);
  if (num_data_array_dims != 2) {
    mexErrMsgTxt("vel_x should be a 2 dimensional array."); 
  }
  num_data_array_dims = mxGetNumberOfDimensions(VEL_Y);
  if (num_data_array_dims != 2) {
    mexErrMsgTxt("vel_y should be a 2 dimensional array."); 
  }

  /* 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");
    }
    if (!mxIsDouble(VEL_X)) {
      mexErrMsgTxt("Incompatible precision: LSMLIB built for double-precision but vel_x is single-precision");
    }
    if (!mxIsDouble(VEL_Y)) {
      mexErrMsgTxt("Incompatible precision: LSMLIB built for double-precision but vel_y is single-precision");
    }
#else
    if (!mxIsSingle(PHI)) {
      mexErrMsgTxt("Incompatible precision: LSMLIB built for single-precision but phi is double-precision");
    }
    if (!mxIsSingle(VEL_X)) {
      mexErrMsgTxt("Incompatible precision: LSMLIB built for single-precision but vel_x is double-precision");
    }
    if (!mxIsSingle(VEL_Y)) {
      mexErrMsgTxt("Incompatible precision: LSMLIB built for single-precision but vel_y is double-precision");
    }
#endif

  /* Get ghostcell_width */
  ghostcell_width = mxGetPr(GHOSTCELL_WIDTH)[0];

  /* Get dX */
  dX = mxGetPr(DX);

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

  /* Assign pointers for phi and velocities */
  phi = (LSMLIB_REAL*) mxGetPr(PHI);
  vel_x = (LSMLIB_REAL*) mxGetPr(VEL_X);
  vel_y = (LSMLIB_REAL*) mxGetPr(VEL_Y);
      
  /* Get size of phi data */
  ilo_phi_gb = 1;
  ihi_phi_gb = mxGetM(PHI);
  jlo_phi_gb = 1;
  jhi_phi_gb = mxGetN(PHI);

  /* Get size of velocity data (assume vel_x and vel_y have same size) */
  ilo_vel_gb = 1;
  ihi_vel_gb = mxGetM(VEL_X);
  jlo_vel_gb = 1;
  jhi_vel_gb = mxGetN(VEL_X);

  /* if necessary, shift ghostbox for velocity to be */
  /* centered with respect to the ghostbox for phi.  */
  if (ihi_vel_gb != ihi_phi_gb) {
    int shift = (ihi_phi_gb-ihi_vel_gb)/2;
    ilo_vel_gb += shift;
    ihi_vel_gb += shift;
  }
  if (jhi_vel_gb != jhi_phi_gb) {
    int shift = (jhi_phi_gb-jhi_vel_gb)/2;
    jlo_vel_gb += shift;
    jhi_vel_gb += shift;
  }

  /* Create matrices for upwind derivatives (i.e. phi_x and phi_y) */
  ilo_grad_phi_gb = ilo_phi_gb;
  ihi_grad_phi_gb = ihi_phi_gb;
  jlo_grad_phi_gb = ilo_phi_gb;
  jhi_grad_phi_gb = jhi_phi_gb;
#ifdef LSMLIB_DOUBLE_PRECISION
  PHI_X = mxCreateDoubleMatrix(ihi_grad_phi_gb-ilo_grad_phi_gb+1,
                               jhi_grad_phi_gb-jlo_grad_phi_gb+1,
                               mxREAL);
  PHI_Y = mxCreateDoubleMatrix(ihi_grad_phi_gb-ilo_grad_phi_gb+1,
                               jhi_grad_phi_gb-jlo_grad_phi_gb+1,
                               mxREAL);
#else
  PHI_X = mxCreateNumericMatrix(ihi_grad_phi_gb-ilo_grad_phi_gb+1,
                                jhi_grad_phi_gb-jlo_grad_phi_gb+1,
                                mxSINGLE_CLASS, mxREAL);
  PHI_Y = mxCreateNumericMatrix(ihi_grad_phi_gb-ilo_grad_phi_gb+1,
                                jhi_grad_phi_gb-jlo_grad_phi_gb+1,
                                mxSINGLE_CLASS, mxREAL);
#endif
  phi_x = (LSMLIB_REAL*) mxGetPr(PHI_X); 
  phi_y = (LSMLIB_REAL*) mxGetPr(PHI_Y); 


  /* Allocate scratch memory for undivided differences */
  ilo_D1_gb = ilo_phi_gb; 
  ihi_D1_gb = ihi_phi_gb;
  jlo_D1_gb = jlo_phi_gb; 
  jhi_D1_gb = jhi_phi_gb;
  D1 = (LSMLIB_REAL*) mxMalloc( sizeof(LSMLIB_REAL) 
                              * (ihi_D1_gb-ilo_D1_gb+1)
                              * (jhi_D1_gb-jlo_D1_gb+1) );

  ilo_D2_gb = ilo_phi_gb; 
  ihi_D2_gb = ihi_phi_gb;
  jlo_D2_gb = jlo_phi_gb; 
  jhi_D2_gb = jhi_phi_gb;
  D2 = (LSMLIB_REAL*) mxMalloc( sizeof(LSMLIB_REAL) 
                              * (ihi_D2_gb-ilo_D2_gb+1)
                              * (jhi_D2_gb-jlo_D2_gb+1) );

  if ( (!D1) || (!D2) ) {
    if (D1) mxFree(D1);
    if (D2) mxFree(D2);
    mexErrMsgTxt("Unable to allocate memory for scratch data...aborting....");
  }


  /* Do the actual computations in a Fortran 77 subroutine */
  ilo_fb = ilo_phi_gb+ghostcell_width;
  ihi_fb = ihi_phi_gb-ghostcell_width;
  jlo_fb = jlo_phi_gb+ghostcell_width;
  jhi_fb = jhi_phi_gb-ghostcell_width;

  /* 
   * NOTE: ordering of data arrays from meshgrid() is (y,x), so order
   * derivative and velocity data arrays need to be reversed.
   */
  LSM2D_UPWIND_HJ_ENO2(
    phi_y, phi_x, 
    &ilo_grad_phi_gb, &ihi_grad_phi_gb,
    &jlo_grad_phi_gb, &jhi_grad_phi_gb,
    phi, 
    &ilo_phi_gb, &ihi_phi_gb, &jlo_phi_gb, &jhi_phi_gb, 
    vel_y, vel_x, 
    &ilo_vel_gb, &ihi_vel_gb, &jlo_vel_gb, &jhi_vel_gb,
    D1,
    &ilo_D1_gb, &ihi_D1_gb, &jlo_D1_gb, &jhi_D1_gb, 
    D2,
    &ilo_D2_gb, &ihi_D2_gb, &jlo_D2_gb, &jhi_D2_gb, 
    &ilo_fb, &ihi_fb, &jlo_fb, &jhi_fb,
    &dX_meshgrid_order[0], &dX_meshgrid_order[1]);

  /* Deallocate scratch memory for undivided differences */
  mxFree(D1);
  mxFree(D2);

  return;
}
Beispiel #22
0
void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[])
{  
  
  if(nrhs<2) 
    mexErrMsgIdAndTxt( "MATLAB:xtimesy:invalidNumInputs",
            "Two group of input points required (optionally a threshold).");
  if(nlhs!=3) 
    mexErrMsgIdAndTxt( "MATLAB:xtimesy:invalidNumOutputs",
            "Three output required.");
  
  // retrive input parameters
  
  const mxArray *Mpoints1 = prhs[0];
  double *points1 = mxGetPr(Mpoints1);
  
  const mxArray *Mpoints2 = prhs[1];
  double *points2 = mxGetPr(Mpoints2);
  
  int numPoints1 = mxGetN(Mpoints1);
  int numPoints2 = mxGetN(Mpoints2);
  int dim1 = mxGetM(Mpoints1);
  int dim2 = mxGetM(Mpoints2);
  
  double maxDist = 5000*5000;
  
  double threshold = 0;
  if(nrhs==3) {
      threshold = *mxGetPr(prhs[2]);
      threshold = threshold*threshold;
  }
  
  if (dim1 != dim2)
      mexErrMsgIdAndTxt( "MATLAB:xtimesy:invalidNumInputs",
            "Points must have the same dimension.");
  
  // parallelize computation of distances
  
  double minDist[MAX_THREAD];
  int id1[MAX_THREAD],id2[MAX_THREAD];
  uint8_t numberOfThreads = 1;
  int stop = 0;
    
  #pragma omp parallel
  {
  int threadID = omp_get_thread_num();
  int numThreads = omp_get_num_threads();
  numberOfThreads = numThreads;
  int pointsPerThread = numPoints1/numThreads;
 
  int start = threadID*pointsPerThread;
  int finish = start + pointsPerThread - 1;
  minDist[threadID] = maxDist;
  
  double x1,y1,z1,dist,dx,dy,dz;
  
  for (int i=start; i<=finish; i++) {
    x1 = points1[i*3];
    y1 = points1[i*3+1];
    z1 = points1[i*3+2];
    for (int j=0; j<numPoints2; j++) {
        dx = (x1-points2[j*3]);
        dy = (y1-points2[j*3+1]);
        dz = (z1-points2[j*3+2]);
        dist = dx*dx+dy*dy+dz*dz;
        if (dist < minDist[threadID]) {
            minDist[threadID] = dist;
            id1[threadID] = i;
            id2[threadID] = j;
        }
    }
    if (stop)
        break;
    if (minDist[threadID] < threshold) {
        stop = 1;
        break;
    }
  }
  }
  
  // output parameters
  
  plhs[0] = mxCreateDoubleMatrix(1,1,mxREAL);
  double *min = mxGetPr(plhs[0]);
  min[0] = maxDist;
  plhs[1] = mxCreateDoubleMatrix(1,1,mxREAL);
  double *i = mxGetPr(plhs[1]);
  plhs[2] = mxCreateDoubleMatrix(1,1,mxREAL);
  double *j = mxGetPr(plhs[2]);
  
  for (int k = 0; k<numberOfThreads; k++) {
    if (minDist[k] < min[0]) {
        min[0] = minDist[k];
        i[0] = id1[k]+1;
        j[0] = id2[k]+1;
    }
  }
  min[0] = sqrt(min[0]);
}
/*============================================================*/
void mexFunction_float( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
  float *image, *tmp;
  int nX, nY;
  ccLabelStruct **label;
  int ixy;
  int numComp, t;
  float minTarget; /* 0.001 = boxes02-add.pgm */
  float *target;
  float debug = 0;

  RanTimeSeed;

  /*mexPrintf("nlhs:%d nrhs: %d\n",nlhs,nrhs);*/

  /* check for the required input arguments */
  if( nrhs < 2 )
    mexErrMsgTxt( "Input expected: <symmetric-positive-array-for-cca> <affty-threshold> <1-for-debug>" );

  /* get the symmetric matrix */
  mex2float( prhs[0], &target, &nX, &nY);
  /*mexPrintf("size: %d %d\n",nX,nY);*/

  /* get the threshold */
  minTarget = mxGetScalar(prhs[1]);
  if (nrhs == 3) {
    debug = mxGetScalar(prhs[2]);
  }

  if (debug)
    mexPrintf("Affty threshold: %2.3e\n",minTarget);

  /* set up empty labels */
  grabByteMemory((char **) &label,sizeof(ccLabelStruct *) * nX,"label");
  for (ixy=0;ixy < nX; ixy++)
    label[ixy] = NULL;

  if (debug) {
    mexPrintf("Computing connected components....");
  }
  connectSymmetric(label, target, minTarget, nX, nY);
  if (debug) {
    mexPrintf("done\n");
  }

  if (debug) {
    mexPrintf("Pruning small or weak connected components....");
  }
  pruneLabelComp(label, nX, 1, 1, 0);
  if (debug) {
    mexPrintf("done\n");
  }

  /* component count */
  numComp = countLabelComp(label, nX, 1);
  if (debug) {
    mexPrintf("Found %d components.\n\n", numComp);
  }

  /*
  if (numComp > 0)
    printMarkovLabels(label,nX);
  */

  /* return the labels to matlab */
  if (nlhs > 0)
    labels2mex(label, &plhs[0], nX ); 
  if (nlhs > 1)
    {
      double *pout;
      plhs[1] = mxCreateDoubleMatrix(1,1,mxREAL);
      pout = mxGetPr(plhs[1]);
      *pout = (double) numComp;
    }
  /* free up memory */
  utilFree( (void **)&target );
  utilFree((void **)label);
  /*
  for (ixy = 0; ixy < nX; ixy++)
    free(label[ixy]);
  */

}
Beispiel #24
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  int               i;
  mxClassID         category;
  int               total_num_of_elements, number_of_fields, index, field_index;
  const char        *field_name;

  const mxArray     *field_array_ptr;
  char              *buf, *sort1;
  int               number_of_dimensions;
  const int         *dims;
  int               buflen, d, page, total_number_of_pages, elements_per_page;
  double            *pr, *pr2, *pr3, *pr4;
  int               total_num_of_elements2, index2;

  int               mrows, mcols, mrows1, mcols1, toto;
  double            *sort2, sort3, *J1;
  int               sort3bis, *pr3bis;
  double            *sort4;
  double            *sort11, *sort22, *sort33;
  int               *sort55, *pr5, *ddl_n, *ddl_tt, *ddl_d;


  double            tol, k_lat;
  int               itermax, chat ;

  char              *mot1, *mot2, *mot3, *mot4, *mot5, *mot6, *mot7;
  method            meth_dfc_2D;
  double            *vec, *q, *z, *w, *info, *mu;
  int               n, *nr, *dim_tt, *dim_d;



  mot1 = "NLGS";
  mot2 = "Cfd_latin";
  mot3 = "Lemke";
  /*  mot4 = "CPG";
      mot5 = "Latin";
      mot6 = "QP";

  */
  mot7 = "NSQP";



  if (nlhs != 3)
  {
    mexErrMsgTxt("3 output required.");
  }


  if (nrhs != 5)
  {
    mexErrMsgTxt("5 input required.");
  }


  vec = mxGetPr(prhs[0]);
  q   = mxGetPr(prhs[1]);
  nr  = mxGetData(prhs[2]);
  mu  = mxGetPr(prhs[3]);



  meth_dfc_2D.dfc_2D.mu = *mu;



  n   = *nr;



  mrows1 = mxGetM(prhs[1]);
  mcols1 = mxGetN(prhs[1]);

  plhs[0] = mxCreateDoubleMatrix(mrows1, mcols1, mxREAL); /* z */
  plhs[1] = mxCreateDoubleMatrix(mrows1, mcols1, mxREAL); /* w */
  plhs[2] = mxCreateDoubleMatrix(mcols1, mcols1, mxREAL); /* info */


  z = mxGetPr(plhs[0]);
  w = mxGetPr(plhs[1]);
  info = mxGetPr(plhs[2]);

  category = mxGetClassID(prhs[4]);

  if (category != mxSTRUCT_CLASS)
  {
    mexPrintf("The thrid input must be a structure");
  }


  total_num_of_elements = mxGetNumberOfElements(prhs[4]);
  number_of_fields = mxGetNumberOfFields(prhs[4]);

  mexPrintf("\n\t\t");

  index = 0;
  field_index = 0;

  field_array_ptr = mxGetFieldByNumber(prhs[4],
                                       index,
                                       field_index);


  if (field_array_ptr == NULL)
    mexPrintf("\tEmpty Field\n");
  else
  {


    category = mxGetClassID(field_array_ptr);

    if (category != mxCHAR_CLASS)
    {
      mexPrintf("The first element of the structure must be a CHAR");
    }



    buflen = mxGetNumberOfElements(field_array_ptr) + 1;
    buf = mxCalloc(buflen, sizeof(char));

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

    printf("\n");

    meth_dfc_2D.dfc_2D.name = buf;

    /*                        2nd element of the structure                     */

    if ((strcmp(buf, mot2) == 0))
    {

      field_index = 1;

      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }




      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 2 element of the structure must be an integer\n");
      }






      pr3bis = (int *) mxGetData(field_array_ptr);





      total_num_of_elements2 = 1;


      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {
        if (field_index == 1)
        {

          itermax = pr3bis[index2];

          meth_dfc_2D.dfc_2D.itermax =  itermax;





        }
      }

      /*                      End of  2nd element of the structure                     */

      /*                        3 element of the structure                     */
      field_index = 2;

      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }

      if (category != mxDOUBLE_CLASS)
      {
        mexPrintf("The 3 element of the structure must be a DOUBLE");
      }

      if (field_index == 2) pr2 = mxGetPr(field_array_ptr);


      total_num_of_elements2 = 1;


      mrows = mxGetM(field_array_ptr);
      mcols = mxGetN(field_array_ptr);

      sort2 = (double*) malloc(1 * 1 * sizeof(double));



      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 2)
        {


          sort2[index2] = pr2[index2];

          tol = sort2[index2];
          meth_dfc_2D.dfc_2D.tol = tol;



        }
      }


      /*                       End 3  element of the structure                     */
      /*                       4  element of the structure                     */

      field_index = 3;

      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }

      if (category != mxDOUBLE_CLASS)
      {
        mexPrintf("The 4 element of the structure must be a DOUBLE");
      }


      if (field_index == 3)  pr3 = mxGetPr(field_array_ptr);


      total_num_of_elements2 = 1; /* mxGetNumberOfElements(field_array_ptr);*/


      mrows = mxGetM(field_array_ptr);
      mcols = mxGetN(field_array_ptr);


      sort22 = (double*) malloc(1 * 1 * sizeof(double));



      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 3)
        {
          sort22[index2] = pr3[index2];

          k_lat = sort22[index2];
          meth_dfc_2D.dfc_2D.k_latin = k_lat;





        }
      }


      /*                      End of 4  element of the structure                     */

      /*                       5  element of the structure                     */



      field_index = 4;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 5 element of the structure must be an integer");
      }




      pr5 = (int *) mxGetData(field_array_ptr);


      total_num_of_elements2 = 1; /*mxGetNumberOfElements(field_array_ptr);*/


      mrows = mxGetM(field_array_ptr);
      mcols = mxGetN(field_array_ptr);


      sort55 = (int*) malloc(1 * 1 * sizeof(int));


      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 4)
        {
          sort55[index2] = pr5[index2];

          chat = sort55[index2];
          meth_dfc_2D.dfc_2D.chat = chat;




        }
      }


      /*                       End of 5 element of the structure                     */

      /*                       6  element of the structure                     */

      field_index = 5;

      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }

      if (category != mxDOUBLE_CLASS)
      {
        mexPrintf("The 6 element of the structure must be a DOUBLE");
      }


      if (field_index == 5)  J1 = mxGetPr(field_array_ptr);


      total_num_of_elements2 = mrows1;


      mrows = mxGetM(field_array_ptr);
      mcols = mxGetN(field_array_ptr);


      meth_dfc_2D.dfc_2D.J1 = (double *) malloc(mrows1 * sizeof(double));

      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 5)
        {
          meth_dfc_2D.dfc_2D.J1[index2] = J1[index2];


        }
      }


      /*                      End of 6  element of the structure                     */

      /*                       7  element of the structure                     */



      field_index = 6;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 7 element of the structure must be an integer");
      }




      dim_tt = mxGetData(field_array_ptr);


      total_num_of_elements2 = 1; /*mxGetNumberOfElements(field_array_ptr);*/


      meth_dfc_2D.dfc_2D.dim_tt = *dim_tt;/* (int *) malloc(*dim_tt*sizeof(int));*/


      /*                       End of 7 element of the structure                     */
      /*                       8  element of the structure                     */



      field_index = 7;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 8 element of the structure must be an integer");
      }




      ddl_n = (int *) mxGetData(field_array_ptr);


      total_num_of_elements2 = *dim_tt; /*mxGetNumberOfElements(field_array_ptr);*/





      meth_dfc_2D.dfc_2D.ddl_n = (int *) malloc(*dim_tt * sizeof(int));

      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 7)
        {
          meth_dfc_2D.dfc_2D.ddl_n[index2] = ddl_n[index2] - 1;

        }
      }


      /*                       End of 8 element of the structure                     */

      /*                       9  element of the structure                     */



      field_index = 8;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 9 element of the structure must be an integer");
      }




      ddl_tt = (int *) mxGetData(field_array_ptr);


      total_num_of_elements2 = *dim_tt; /*mxGetNumberOfElements(field_array_ptr);*/



      meth_dfc_2D.dfc_2D.ddl_tt = (int *) malloc(*dim_tt * sizeof(int));

      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 8)
        {
          meth_dfc_2D.dfc_2D.ddl_tt[index2] = ddl_tt[index2] - 1;



        }
      }


      /*                       End of 9 element of the structure                     */

      /*                       10  element of the structure                     */



      field_index = 9;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 10 element of the structure must be an integer");
      }




      dim_d = mxGetData(field_array_ptr);


      total_num_of_elements2 = 1; /*mxGetNumberOfElements(field_array_ptr);*/




      meth_dfc_2D.dfc_2D.dim_d = *dim_d;/* (int *) malloc(*dim_tt*sizeof(int));*/



      /*                       End of 10 element of the structure                     */
      /*                       11  element of the structure                     */



      field_index = 10;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 11 element of the structure must be an integer");
      }




      ddl_d = (int *) mxGetData(field_array_ptr);


      total_num_of_elements2 = *dim_d;






      meth_dfc_2D.dfc_2D.ddl_d = (int *) malloc(*dim_d * sizeof(int));

      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 10)
        {
          meth_dfc_2D.dfc_2D.ddl_d[index2] = ddl_d[index2] - 1;






        }
      }

      free(sort2);
      free(sort22);
      free(sort55);
    }

    /*                       End of 11 element of the structure                     */




    if ((strcmp(buf, mot1) == 0))
    {

      field_index = 1;

      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }




      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 2 element of the structure must be an integer\n");
      }






      pr3bis = (int *) mxGetData(field_array_ptr);





      total_num_of_elements2 = 1;


      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {
        if (field_index == 1)
        {

          itermax = pr3bis[index2];

          meth_dfc_2D.dfc_2D.itermax =  itermax;





        }
      }


      field_index = 2;

      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }

      if (category != mxDOUBLE_CLASS)
      {
        mexPrintf("The 3 element of the structure must be a DOUBLE");
      }

      if (field_index == 2) pr2 = mxGetPr(field_array_ptr);


      total_num_of_elements2 = 1;


      mrows = mxGetM(field_array_ptr);
      mcols = mxGetN(field_array_ptr);

      sort2 = (double*) malloc(1 * 1 * sizeof(double));



      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 2)
        {


          sort2[index2] = pr2[index2];

          tol = sort2[index2];
          meth_dfc_2D.dfc_2D.tol = tol;



        }
      }


      field_index = 3;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 4 element of the structure must be an integer");
      }




      pr5 = (int *) mxGetData(field_array_ptr);


      total_num_of_elements2 = 1;


      mrows = mxGetM(field_array_ptr);
      mcols = mxGetN(field_array_ptr);


      sort55 = (int*) malloc(1 * 1 * sizeof(int));


      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 3)
        {
          sort55[index2] = pr5[index2];

          chat = sort55[index2];
          meth_dfc_2D.dfc_2D.chat = chat;




        }
      }


      field_index = 4;

      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }

      if (category != mxDOUBLE_CLASS)
      {
        mexPrintf("The 5 element of the structure must be a DOUBLE");
      }


      if (field_index == 4)  J1 = mxGetPr(field_array_ptr);


      total_num_of_elements2 = mrows1;


      mrows = mxGetM(field_array_ptr);
      mcols = mxGetN(field_array_ptr);


      meth_dfc_2D.dfc_2D.J1 = (double *) malloc(mrows1 * sizeof(double));

      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 4)
        {
          meth_dfc_2D.dfc_2D.J1[index2] = J1[index2];


        }
      }



      field_index = 5;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 6 element of the structure must be an integer");
      }




      dim_tt = mxGetData(field_array_ptr);


      total_num_of_elements2 = 1;


      meth_dfc_2D.dfc_2D.dim_tt = *dim_tt;



      field_index = 6;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 7 element of the structure must be an integer");
      }




      ddl_n = (int *) mxGetData(field_array_ptr);


      total_num_of_elements2 = *dim_tt;





      meth_dfc_2D.dfc_2D.ddl_n = (int *) malloc(*dim_tt * sizeof(int));

      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 6)
        {
          meth_dfc_2D.dfc_2D.ddl_n[index2] = ddl_n[index2] - 1;

        }
      }



      field_index = 7;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 8 element of the structure must be an integer");
      }




      ddl_tt = (int *) mxGetData(field_array_ptr);


      total_num_of_elements2 = *dim_tt;



      meth_dfc_2D.dfc_2D.ddl_tt = (int *) malloc(*dim_tt * sizeof(int));

      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 7)
        {
          meth_dfc_2D.dfc_2D.ddl_tt[index2] = ddl_tt[index2] - 1;



        }
      }


      field_index = 8;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 9 element of the structure must be an integer");
      }




      dim_d = mxGetData(field_array_ptr);


      total_num_of_elements2 = 1;




      meth_dfc_2D.dfc_2D.dim_d = *dim_d;


      field_index = 9;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 10 element of the structure must be an integer");
      }




      ddl_d = (int *) mxGetData(field_array_ptr);


      total_num_of_elements2 = *dim_d;






      meth_dfc_2D.dfc_2D.ddl_d = (int *) malloc(*dim_d * sizeof(int));

      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 9)
        {
          meth_dfc_2D.dfc_2D.ddl_d[index2] = ddl_d[index2] - 1;
        }
      }

      free(sort2);
      free(sort55);
    }



    if ((strcmp(buf, mot3) == 0))
    {

      field_index = 1;

      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }




      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 2 element of the structure must be an integer\n");
      }






      pr3bis = (int *) mxGetData(field_array_ptr);





      total_num_of_elements2 = 1;


      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {
        if (field_index == 1)
        {

          itermax = pr3bis[index2];

          meth_dfc_2D.dfc_2D.itermax =  itermax;
        }
      }


      field_index = 2;

      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 3 element of the structure must be an integer");
      }




      pr5 = (int *) mxGetData(field_array_ptr);


      total_num_of_elements2 = 1;


      mrows = mxGetM(field_array_ptr);
      mcols = mxGetN(field_array_ptr);


      sort55 = (int*) malloc(1 * 1 * sizeof(int));


      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 2)
        {
          sort55[index2] = pr5[index2];

          chat = sort55[index2];
          meth_dfc_2D.dfc_2D.chat = chat;




        }
      }


      field_index = 3;

      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }

      if (category != mxDOUBLE_CLASS)
      {
        mexPrintf("The 4 element of the structure must be a DOUBLE");
      }


      if (field_index == 3)  J1 = mxGetPr(field_array_ptr);


      total_num_of_elements2 = mrows1;


      mrows = mxGetM(field_array_ptr);
      mcols = mxGetN(field_array_ptr);


      meth_dfc_2D.dfc_2D.J1 = (double *) malloc(mrows1 * sizeof(double));

      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 3)
        {
          meth_dfc_2D.dfc_2D.J1[index2] = J1[index2];


        }
      }



      field_index = 4;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 5 element of the structure must be an integer");
      }




      dim_tt = mxGetData(field_array_ptr);


      total_num_of_elements2 = 1;


      meth_dfc_2D.dfc_2D.dim_tt = *dim_tt;



      field_index = 5;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 6 element of the structure must be an integer");
      }




      ddl_n = (int *) mxGetData(field_array_ptr);


      total_num_of_elements2 = *dim_tt;





      meth_dfc_2D.dfc_2D.ddl_n = (int *) malloc(*dim_tt * sizeof(int));

      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 5)
        {
          meth_dfc_2D.dfc_2D.ddl_n[index2] = ddl_n[index2] - 1;

        }
      }



      field_index = 6;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 7 element of the structure must be an integer");
      }




      ddl_tt = (int *) mxGetData(field_array_ptr);


      total_num_of_elements2 = *dim_tt;



      meth_dfc_2D.dfc_2D.ddl_tt = (int *) malloc(*dim_tt * sizeof(int));

      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 6)
        {
          meth_dfc_2D.dfc_2D.ddl_tt[index2] = ddl_tt[index2] - 1;



        }
      }


      field_index = 7;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 8 element of the structure must be an integer");
      }




      dim_d = mxGetData(field_array_ptr);


      total_num_of_elements2 = 1;




      meth_dfc_2D.dfc_2D.dim_d = *dim_d;


      field_index = 8;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 9 element of the structure must be an integer");
      }




      ddl_d = (int *) mxGetData(field_array_ptr);


      total_num_of_elements2 = *dim_d;






      meth_dfc_2D.dfc_2D.ddl_d = (int *) malloc(*dim_d * sizeof(int));

      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 8)
        {
          meth_dfc_2D.dfc_2D.ddl_d[index2] = ddl_d[index2] - 1;
        }
      }

      free(sort55);
    }


    if ((strcmp(buf, mot7) == 0))
    {

      field_index = 1;

      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }

      if (category != mxDOUBLE_CLASS)
      {
        mexPrintf("The 2 element of the structure must be a DOUBLE");
      }

      if (field_index == 2) pr2 = mxGetPr(field_array_ptr);


      total_num_of_elements2 = 1;


      mrows = mxGetM(field_array_ptr);
      mcols = mxGetN(field_array_ptr);

      sort2 = (double*) malloc(1 * 1 * sizeof(double));



      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 2)
        {


          sort2[index2] = pr2[index2];

          tol = sort2[index2];
          meth_dfc_2D.dfc_2D.tol = tol;



        }
      }


      field_index = 2;

      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 3 element of the structure must be an integer");
      }




      pr5 = (int *) mxGetData(field_array_ptr);


      total_num_of_elements2 = 1;


      mrows = mxGetM(field_array_ptr);
      mcols = mxGetN(field_array_ptr);


      sort55 = (int*) malloc(1 * 1 * sizeof(int));


      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 2)
        {
          sort55[index2] = pr5[index2];

          chat = sort55[index2];
          meth_dfc_2D.dfc_2D.chat = chat;




        }
      }


      field_index = 3;

      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }

      if (category != mxDOUBLE_CLASS)
      {
        mexPrintf("The 4 element of the structure must be a DOUBLE");
      }


      if (field_index == 3)  J1 = mxGetPr(field_array_ptr);


      total_num_of_elements2 = mrows1;


      mrows = mxGetM(field_array_ptr);
      mcols = mxGetN(field_array_ptr);


      meth_dfc_2D.dfc_2D.J1 = (double *) malloc(mrows1 * sizeof(double));

      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 3)
        {
          meth_dfc_2D.dfc_2D.J1[index2] = J1[index2];


        }
      }



      field_index = 4;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 5 element of the structure must be an integer");
      }




      dim_tt = mxGetData(field_array_ptr);


      total_num_of_elements2 = 1;


      meth_dfc_2D.dfc_2D.dim_tt = *dim_tt;



      field_index = 5;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 6 element of the structure must be an integer");
      }




      ddl_n = (int *) mxGetData(field_array_ptr);


      total_num_of_elements2 = *dim_tt;





      meth_dfc_2D.dfc_2D.ddl_n = (int *) malloc(*dim_tt * sizeof(int));

      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 5)
        {
          meth_dfc_2D.dfc_2D.ddl_n[index2] = ddl_n[index2] - 1;

        }
      }



      field_index = 6;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 7 element of the structure must be an integer");
      }




      ddl_tt = (int *) mxGetData(field_array_ptr);


      total_num_of_elements2 = *dim_tt;



      meth_dfc_2D.dfc_2D.ddl_tt = (int *) malloc(*dim_tt * sizeof(int));

      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 6)
        {
          meth_dfc_2D.dfc_2D.ddl_tt[index2] = ddl_tt[index2] - 1;



        }
      }


      field_index = 7;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 8 element of the structure must be an integer");
      }




      dim_d = mxGetData(field_array_ptr);


      total_num_of_elements2 = 1;




      meth_dfc_2D.dfc_2D.dim_d = *dim_d;


      field_index = 8;
      field_array_ptr = mxGetFieldByNumber(prhs[4],
                                           index,
                                           field_index);


      if (field_array_ptr == NULL)
        mexPrintf("\tEmpty Field\n");
      else
      {
        category = mxGetClassID(field_array_ptr);
      }


      if (category != mxINT32_CLASS)
      {
        mexPrintf("The 9 element of the structure must be an integer");
      }




      ddl_d = (int *) mxGetData(field_array_ptr);


      total_num_of_elements2 = *dim_d;






      meth_dfc_2D.dfc_2D.ddl_d = (int *) malloc(*dim_d * sizeof(int));

      for (index2 = 0; index2 < total_num_of_elements2; index2++)
      {

        if (field_index == 8)
        {
          meth_dfc_2D.dfc_2D.ddl_d[index2] = ddl_d[index2] - 1;
        }
      }

      free(sort55);
    }



    printf("we enter in dfc_2D_solver using the %s method \n\n", buf);

    if (strcmp(buf, mot1) == 0)
    {
      toto = dfc_2D_solver(vec, q, &mrows1, &meth_dfc_2D, z, w);
    }
    else if (strcmp(buf, mot2) == 0)
      toto = dfc_2D_solver(vec, q, &mrows1, &meth_dfc_2D, z, w);
    else if (strcmp(buf, mot3) == 0)
      toto = dfc_2D_solver(vec, q, &mrows1, &meth_dfc_2D, z, w);
    else if (strcmp(buf, mot7) == 0)
      toto = dfc_2D_solver(vec, q, &mrows1, &meth_dfc_2D, z, w);
    /*   else if (strcmp(buf, mot5) == 0)
     toto = dfc_2D_solver(vec, q, &mrows1, &meth_dfc_2D, z, w);*/
    else printf("Warning : Unknown solving method : %s\n", buf);

    *info = toto;





    free(meth_dfc_2D.dfc_2D.J1);
    free(meth_dfc_2D.dfc_2D.ddl_n);
    free(meth_dfc_2D.dfc_2D.ddl_tt);
    free(meth_dfc_2D.dfc_2D.ddl_d);


  }
}
Beispiel #25
0
/* ************************************************************
   PROCEDURE mexFunction - Entry for Matlab
   ************************************************************ */
void mexFunction(const int nlhs, mxArray *plhs[],
  const int nrhs, const mxArray *prhs[])
{
  const mxArray *UD_FIELD;
  int lenfull, lenud, sdplen, transp, fwsiz, i,k;
  double *fwork, *y,*permPr;
  const double *x,*ud;
  int *perm, *iwork;
  coneK cK;
  char use_pivot;

/* ------------------------------------------------------------
   Check for proper number of arguments 
   ------------------------------------------------------------ */
  if(nrhs < NPARIN){
    transp = 0;
    mxAssert(nrhs >= NPARINMIN, "psdscale requires more input arguments.");
  }
  else
    transp = mxGetScalar(TRANSP_IN);
  mxAssert(nlhs <= NPAROUT, "psdscale generates less output arguments.");
/* ------------------------------------------------------------
   Disassemble cone K structure
   ------------------------------------------------------------ */
  conepars(K_IN, &cK);
/* ------------------------------------------------------------
   Get statistics of cone K structure
   ------------------------------------------------------------ */
  lenud = cK.rDim + cK.hDim;
  lenfull = cK.lpN +  cK.qDim + lenud;
  sdplen = cK.rLen + cK.hLen;
/* ------------------------------------------------------------
   Get scale data: ud.{u,perm}.
   ------------------------------------------------------------ */
  if(!mxIsStruct(UD_IN)){
    mxAssert(mxGetM(UD_IN) * mxGetN(UD_IN) == lenud, "ud size mismatch."); /* ud is vector */
    ud = mxGetPr(UD_IN);
    use_pivot = 0;
  }
  else{                         /* ud is structure */
    UD_FIELD = mxGetField(UD_IN,0,"u");
      mxAssert( UD_FIELD!= NULL,  "Field ud.u missing.");    /* ud.u */
    mxAssert(mxGetM(UD_FIELD) * mxGetN(UD_FIELD) == lenud, "ud.u size mismatch.");
    ud = mxGetPr(UD_FIELD);
    UD_FIELD = mxGetField(UD_IN,0,"perm");                       /* ud.perm */
    if((use_pivot = (UD_FIELD != NULL))){
      if(mxGetM(UD_FIELD) * mxGetN(UD_FIELD) == sdplen)
        permPr = mxGetPr(UD_FIELD);
      else {
        mxAssert(mxGetM(UD_FIELD) * mxGetN(UD_FIELD) == 0, "ud.perm size mismatch");
        use_pivot = 0;
      }
    }
  }
/* ------------------------------------------------------------
   Get input x
   ------------------------------------------------------------ */
  mxAssert(!mxIsSparse(X_IN), "Sparse x not supported by this version of psdscale.");
  x = mxGetPr(X_IN);
/* ------------------------------------------------------------
   Validate x-input, and let x point to PSD part.
   ------------------------------------------------------------ */
  if(mxGetM(X_IN) * mxGetN(X_IN) == lenfull)
    x += cK.lpN + cK.qDim;
  else mxAssert(mxGetM(X_IN) * mxGetN(X_IN) == lenud, "size x mismatch.");
/* ------------------------------------------------------------
   Allocate output Y(lenud)
   ------------------------------------------------------------ */
  Y_OUT =  mxCreateDoubleMatrix(lenud, 1, mxREAL);
  y = mxGetPr(Y_OUT);
/* ------------------------------------------------------------
   Allocate fwork 2 * [ max(cK.rMaxn^2, 2*cK.hMaxn^2) ]
   iwork = int(sdplen)
   ------------------------------------------------------------ */
  fwsiz = MAX(SQR(cK.rMaxn),2*SQR(cK.hMaxn));
  fwork = (double *) mxCalloc( MAX(1,2 * fwsiz), sizeof(double));
  iwork = (int *) mxCalloc( MAX(1, sdplen), sizeof(int) );
/* ------------------------------------------------------------
   Convert Fortran to C-style in perm:
   ------------------------------------------------------------ */
  if(use_pivot){
    perm = iwork;
    for(k = 0; k < sdplen; k++){
      i = permPr[k];
      perm[k] = --i;
    }
  }
  else
    perm = (int *) NULL;
/* ------------------------------------------------------------
   The actual job is done here:.
   ------------------------------------------------------------ */
  psdscaleK(y, ud, perm, x, cK, transp, fwork);
/* ------------------------------------------------------------
   Release working arrays.
   ------------------------------------------------------------ */
  mxFree(fwork);
  mxFree(iwork);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{                                                                            
	try {                                                 
		char* conffile = mxArrayToString(prhs[0]);
		char* robotname = mxArrayToString(prhs[1]);
        
		//mexPrintf("conffile = %s", conffile);
		Robot robj(conffile, robotname);

		// Create link structure
		mxArray *links = mxCreateStructMatrix(robj.get_dof(), 1, NUM_LINK_FIELDS, link_field_names);
		double *ptr = NULL;
		Link *rlinks = robj.links;
		std::vector<double> immobileID;
		for (int i=1; i <= robj.get_dof(); ++i) 
		{
			// Return the joint type.
			mxSetField(links, i-1, "joint_type", mxCreateDoubleScalar( rlinks[i].get_joint_type() ));
            
			// Return theta.
			mxSetField(links, i-1, "theta", mxCreateDoubleScalar( rlinks[i].get_theta() ));
            
			//!< Return d.
			mxSetField(links, i-1, "d", mxCreateDoubleScalar( rlinks[i].get_d() ));
            
			// Return a.
			mxSetField(links, i-1, "a", mxCreateDoubleScalar( rlinks[i].get_a() ));
            
			// Return alpha.
			mxSetField(links, i-1, "alpha", mxCreateDoubleScalar( rlinks[i].get_alpha() ));
            
			//!< Return q
			mxSetField(links, i-1, "q", mxCreateDoubleScalar( rlinks[i].get_q() ));
            
			// Return theta_min.
			mxSetField(links, i-1, "theta_min", mxCreateDoubleScalar( rlinks[i].get_theta_min() ));
            
			// Return theta_max.
			mxSetField(links, i-1, "theta_max", mxCreateDoubleScalar( rlinks[i].get_theta_max() ));
            
			// Return joint_offset.
			mxSetField(links, i-1, "joint_offset", mxCreateDoubleScalar( rlinks[i].get_joint_offset() ));
            
			// Return r.
			mxArray *mr = mxArrayFromNMArray( rlinks[i].get_r() );
			//mxArray *mr = mxCreateDoubleMatrix(1,3,mxREAL); ptr = mxGetPr(mr);
			//ColumnVector r = rlinks[i].get_r();
			//for (int n=0; n < 3; ++n) ptr[n] = r(n+1);
			mxSetField(links, i-1, "r", mr);
            
			// Return p.
			mxArray *mp = mxArrayFromNMArray( rlinks[i].get_p() );
			//mxArray *mp = mxCreateDoubleMatrix(1,3,mxREAL); ptr = mxGetPr(mp);
			//ColumnVector p = rlinks[i].get_p();
			//for (int n=0; n < 3; ++n) ptr[n] = p(n+1);
			mxSetField(links, i-1, "p", mp);
            
			// Return m.
			mxSetField(links, i-1, "m", mxCreateDoubleScalar( rlinks[i].get_m() ));
            
			// Return Im.
			mxSetField(links, i-1, "Im", mxCreateDoubleScalar( rlinks[i].get_Im() ));
            
			// Return Gr.
			mxSetField(links, i-1, "Gr", mxCreateDoubleScalar( rlinks[i].get_Gr() ));
            
			// Return B.
			mxSetField(links, i-1, "B", mxCreateDoubleScalar( rlinks[i].get_B() ));
            
			// Return Cf.
			mxSetField(links, i-1, "Cf", mxCreateDoubleScalar( rlinks[i].get_Cf() ));
            
			// Return I.
			mxArray *mI = mxArrayFromNMArray( rlinks[i].get_I() );
			//mxArray *mI = mxCreateDoubleMatrix(3,3,mxREAL);
			//Matrix I = rlinks[i].get_I();
			//NMMatrixToMxArray(mxGetPr(mI), I, 3, 3);
			mxSetField(links, i-1, "I", mI);
            
			// Return immobile.
			mxSetField(links, i-1, "immobile", mxCreateLogicalScalar( rlinks[i].get_immobile() ));
			if ( rlinks[i].get_immobile() )
			{
				immobileID.push_back((double)i);
			}
		}
        
		// Create robot structure
		LHS_ARG_1 = mxCreateStructMatrix(1, 1, NUM_ROBOT_FIELDS, robot_field_names);
        
		// Set name
		mxSetField(LHS_ARG_1, 0, "name", mxCreateString(robotname) );
		
		// Set gravity
		mxSetField(LHS_ARG_1, 0, "gravity", mxArrayFromNMArray( robj.gravity ) );
		
		// Return DH type
		mxSetField(LHS_ARG_1, 0, "DH", mxCreateLogicalScalar( robj.get_DH() ));
        
		// Return DOF
		mxSetField(LHS_ARG_1, 0, "dof", mxCreateDoubleScalar( (double)robj.get_dof() ));
        
		// Return available DOF
		mxSetField(LHS_ARG_1, 0, "available_dof", mxCreateDoubleScalar( robj.get_available_dof() ));
        
		// Return IDs of immobile joints
		int nImmobileJnts = (int)immobileID.size();
		if ( nImmobileJnts > 0 )
		{
			mxArray *tempArray = mxCreateDoubleMatrix(1,nImmobileJnts,mxREAL);
			memcpy( mxGetPr(tempArray), &immobileID[0], nImmobileJnts*sizeof(double) );
			mxSetField(LHS_ARG_1, 0, "immobile_joints", tempArray);
		} else {
			mxSetField(LHS_ARG_1, 0, "immobile_joints", mxCreateDoubleMatrix(0,0, mxREAL));
		}
		
		// Return links
		mxSetField(LHS_ARG_1, 0, "links", links);
		
		// Free allocated stuff
		mxFree( conffile );
		mxFree( robotname );
	
	} catch(Exception) {
		std::ostringstream msg;
		msg << mexFunctionName() << ":: " << Exception::what();
		mexErrMsgTxt(msg.str().c_str());
	} catch (const std::runtime_error& e) {
		std::ostringstream msg;
		msg << mexFunctionName() << ":: " << e.what();
		mexErrMsgTxt(msg.str().c_str());
	} catch (...) {
		std::ostringstream msg;
		msg << mexFunctionName() << ":: Unknown failure during operation";
		mexErrMsgTxt(msg.str().c_str());
	}                                                                           
}
Beispiel #27
0
/*
 * function [ res ] = graphKernelDelta( edges1, edges2, nodes1, nodes2, lambda, epsilon );
 */
void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
{
  // check number of output params
  if( nlhs == 0 ) {
    printf( "%s\n", copyright );
    mexErrMsgTxt( syntax );
    return;
  }
  if( !( nlhs == 1 ) ) {
    int i;
    for( i = 1; i < nlhs; ++i ) {
      plhs[i] = mxCreateDoubleMatrix( 1, 1, mxREAL );
      double* const matPtr = mxGetPr( plhs[i] );
      *matPtr = mxGetNaN();
    }
  }

  // prepare for unsuccessful return
  plhs[0] = mxCreateDoubleMatrix( 1, 1, mxREAL );
  double* const resPtr = mxGetPr( plhs[0] );
  *resPtr = mxGetNaN();

  // check number of input params
  if( !( nrhs == 6 ) ) {
    printf( "found %d parameters\n", nrhs );
    mexErrMsgTxt( syntax );
    return;
  }

  // setup constants
  #define edges1_ ( prhs[ 0 ] )
  #define edges2_ ( prhs[ 1 ] )
  #define nodes1_ ( prhs[ 2 ] )
  #define nodes2_ ( prhs[ 3 ] )
  #define lambda_ ( prhs[ 4 ] )
  #define epsilon_ ( prhs[ 5 ] )
  const double* const edges1 = (double *) mxGetPr( edges1_ );
  const double* const edges2 = (double *) mxGetPr( edges2_ );
  const double* const nodes1 = (double *) mxGetPr( nodes1_ );
  const double* const nodes2 = (double *) mxGetPr( nodes2_ );
  const double lambda = *( (double *) mxGetPr( lambda_ ) );
  const double epsilon = *( (double *) mxGetPr( epsilon_ ) );
  const int n1 = (int)( mxGetM( nodes1_ ) );
  const int n2 = (int)( mxGetM( nodes2_ ) );

  // check input
  if( !( mxGetN( nodes1_ ) == 1 && mxGetN( nodes2_ ) == 1 ) ) {
    printf( "second dimension of nodes must be 1\n" );
    mexErrMsgTxt( syntax );
    return;
  }
  if( !( mxGetM( edges1_ ) == n1 && mxGetN( edges1_ ) == n1 ) ) {
    printf( "edges1 must be square\n" );
    mexErrMsgTxt( syntax );
    return;
  }
  if( !( mxGetM( edges2_ ) == n2 && mxGetN( edges2_ ) == n2 ) ) {
    printf( "edges2 must be square\n" );
    mexErrMsgTxt( syntax );
    return;
  }
  if( !( 0 <= lambda && lambda <= 1 ) ) {
    printf( "lambda must be between 0 and 1\n" );
    mexErrMsgTxt( syntax );
    return;
  }

  // setup variables
  double* probStart1s = new double[ n1 ];
  double* probStart2s = new double[ n2 ];
  double* probQuit1s = new double[ n1 ];
  double* probQuit2s = new double[ n2 ];
  double* probTrans1s = new double[ n1*n1 ];
  double* probTrans2s = new double[ n2*n2 ];

  // initialize probability distributions
  const double probStart1 = 1.0 / n1;
  const double probStart2 = 1.0 / n2;
  const double lambdaNeg = 1 - lambda;
  register int i;
  register int j;
  register int l;
  // - start and quit probabilities, graph 1
  for( i = 0; i < n1; ++i ) {
    probStart1s[ i ] = probStart1;
    probQuit1s[ i ] = lambda;
  }
  // - start and quit probabilities, graph 2
  for( i = 0; i < n2; ++i ) {
    probStart2s[ i ] = probStart2;
    probQuit2s[ i ] = lambda;
  }
  // - transition probabilities, graph 1
  l = 0;
  for( j = 0; j < n1; ++j ) {
    register int outDegree = 0;
    for( i = 0; i < n1; ++i ) {
      outDegree += ( edges1[l] != 0 );
      ++l;
    }
    l -= n1;
    register const double probTrans = lambdaNeg / outDegree;
    for( i = 0; i < n1; ++i ) {
      probTrans1s[l] = ( edges1[l] != 0 ) ? probTrans : 0;
      ++l;
    }
  }
  // - transition probabilities, graph 2
  l = 0;
  for( j = 0; j < n2; ++j ) {
    register int outDegree = 0;
    for( i = 0; i < n2; ++i ) {
      outDegree += ( edges2[l] != 0 );
      ++l;
    }
    l -= n2;
    register const double probTrans = lambdaNeg / outDegree;
    for( i = 0; i < n2; ++i ) {
      probTrans2s[l] = ( edges2[l] != 0 ) ? probTrans : 0;
      ++l;
    }
  }

  // compute
  const double result = graphKernel( n1, n2, edges1, edges2, nodes1, nodes2, probStart1s, probStart2s, probQuit1s, probQuit2s, probTrans1s, probTrans2s, epsilon );

  // return the final result
  *resPtr = result;
  delete[] probStart1s;
  delete[] probStart2s;
  delete[] probQuit1s;
  delete[] probQuit2s;
  delete[] probTrans1s;
  delete[] probTrans2s;
}
Beispiel #28
0
void mexFunction( int nlhs,         mxArray *plhs[],
                  int nrhs, const   mxArray *prhs[])
{/*mexFunction*/
    char *fname, dummyCharBuffer[1000], title[100], title_fname[100];
    double *t_index, *eegv_index;
    short *shortBuffer;
    double file_size, *double_pointer, Delta, samplingrate;
    unsigned long numOfRecords=0, tstamp, totalNumOfSamples=0;
    mxArray *wait_prhs[3], *wait_plhs[1];
    int r, c, i, numOfSamples, j;
    FILE *infile;

    /*get file name*/
        r=mxGetM(FNAME);
        c=mxGetN(FNAME);

        fname=mxCalloc(r*c, sizeof(char));
        fname=mxArrayToString(FNAME);

    /*get samplingrate*/
        samplingrate=mxGetScalar(SRATE);

	Delta=1e4/samplingrate;

    /*Open the input file read only*/
        if ((infile=fopen(fname, "rb"))==NULL)
	{
		printf("eegread: Could not open input file\n");
		return;
	}

	/*Determine the file size*/
	if (fseek(infile, 0, SEEK_END))
	{
	    printf("eegread: Could not determine file size\n");
	    return;
	}
	file_size=(double)ftell(infile);
	rewind(infile);

	/*Create the waitbar*/
	    /*Set bar length to 0*/
		wait_prhs[0]=mxCreateDoubleMatrix(1,1,mxREAL);
		double_pointer=mxGetPr(wait_prhs[0]);
		*double_pointer=0;

		/*Set title*/
		        j=0;
		        for(i=0; i<strlen(fname); i++)
		        {
		            title_fname[j++]=fname[i];
		            if (fname[i]=='\\')
		            {
		                title_fname[j++]='\\';
		            }
		        }
		        title_fname[j]='\0';
		        
                snprintf(title, 100, "Counting Data Records in %s: ", title_fname);
                wait_prhs[1]=mxCreateString(title);

		/*Launch waitbar*/
		if(mexCallMATLAB(1, wait_plhs, 2, wait_prhs, "waitbar"))
		{
		    printf("Could not create waitbar\n");
		    return;
		}

		/*Move the title array to third entry*/
		mxDestroyArray(wait_prhs[1]);
		wait_prhs[2]=mxCreateString(title);

		/*Use the second argument to hold the handle to waitbar figure*/
		wait_prhs[1]=wait_plhs[0];

	/*Scan the input file to determine the number of records*/
	    /*Skip the header (5 lines)*/
	    for (i=0; i<5; i++)
	    {
	        if (NULL==fgets(dummyCharBuffer, 1000, infile))
	        {
	            printf("eegread: Error skipping header\n");
	            return;
	        }
	    }

        /*Count the records*/
	    if(1!=freadLE((unsigned char *)&tstamp, sizeof(unsigned long), 1, infile) && !feof(infile))
	    {
	        printf("Failure to read tstamp\n");
	        return;
	    }
	    else if (feof(infile))
	    {
	        printf("No records found in this file\n");
	        return;
	    }
            if(1!=freadLE((unsigned char *)&numOfSamples, sizeof(int), 1, infile))
            {
                printf("Failure to read the number of samples\n");
                return;
            }
	    while(!feof(infile))
		{/*while(!feof(infile))*/

		    if (fseek(infile, (double) (numOfSamples*sizeof(short)), SEEK_CUR))
		    {
		        printf("Failure to skip data block\n");
		        return;
		    }

                    ++numOfRecords;

		    totalNumOfSamples+=(unsigned long)numOfSamples;

                    /*Update bar length*/
                    *double_pointer=(double)ftell(infile)/file_size/2;
		    if(mexCallMATLAB(0, NULL, 3, wait_prhs, "waitbar"))
		    {
			printf("Could not update waitbar\n");
			return;
		    }

 		    if(1!=freadLE((unsigned char *)&tstamp, sizeof(unsigned long), 1, infile) && !feof(infile))
		    {
		        printf("Failure to read tstamp\n");
		        return;
		    }
                    else if(!feof(infile))
                    {
                        if(fseek(infile, sizeof(int), SEEK_CUR))
                        {
                            printf("Failure to skip nsamples\n");
                            return;
                        }
                    }
		}/*while(!feof(infile))*/

	/*printf("There are %lu records and %lu sample points in file\n", numOfRecords, totalNumOfSamples);*/

	/*Create the output arrays*/
	T=mxCreateDoubleMatrix(totalNumOfSamples, 1, mxREAL);
        EEGV=mxCreateDoubleMatrix(totalNumOfSamples, 1, mxREAL);

        /*Create the array to hold short data for conversion to double*/
        shortBuffer=mxCalloc(numOfSamples, sizeof(short));

        /*get pointers to output data*/
        t_index=mxGetPr(T);
        eegv_index=mxGetPr(EEGV);

       /*Prepare the wait bar for new task*/
	*double_pointer=0; /*This still points to wait_prhs[0]*/
	/*Set new title to the third entry*/
        snprintf(title, 100, "Creating Output Vectors for %s", title_fname);
        mxDestroyArray(wait_prhs[2]);
	wait_prhs[2]=mxCreateString(title);

	/*Launch waitbar*/
	if(mexCallMATLAB(0, NULL, 3, wait_prhs, "waitbar"))
	{
	    printf("Could not create waitbar\n");
	    return;
	}

    /*Fill in the output vectors*/
    	rewind(infile);
        /*Skip the header (5 lines)*/
        for (i=0; i<5; i++)
        {
            if (NULL==fgets(dummyCharBuffer, 1000, infile))
            {
                printf("eegread: Error skipping header\n");
                return;
            }
        }

        /*Read and write data*/
        if(1!=freadLE((unsigned char *)&tstamp, sizeof(unsigned long), 1, infile) && !feof(infile))
        {
            printf("Failure to read tstamp\n");
            return;
        }
        else if (feof(infile))
        {
            printf("No records found in this file\n");
            return;
        }
        if(fseek(infile, sizeof(int), SEEK_CUR))
        {
                printf("Failure to skip nsamples\n");
                return;
        }
        numOfRecords=0;
        while(!feof(infile))
	{/*while(!feof(infile))*/

            if (numOfSamples!=freadLE((unsigned char *)shortBuffer, sizeof(short), numOfSamples, infile))
            {
		        printf("Failure to read eeg data\n");
		        return;
            }

            /*Store eeg data in the output matrix by converting into double*/
            for (i=0; i<numOfSamples; i++)
            {
            	*(eegv_index++)=(double)shortBuffer[i];
	        *(t_index++)=((double)tstamp+i*Delta)/1e4;
            }

           /*Update bar length*/
            *double_pointer=(double)ftell(infile)/file_size/2+0.5;
            if(mexCallMATLAB(0, NULL, 3, wait_prhs, "waitbar"))
	    {
	        printf("Could not update waitbar\n");
	        return;
	    }

            if(1!=freadLE((unsigned char *)&tstamp, sizeof(unsigned long), 1, infile) && !feof(infile))
            {
                printf("Failure to read tstamp\n");
                return;
            }
            else if(!feof(infile))
            {
                if(fseek(infile, sizeof(int), SEEK_CUR))
                {
                        printf("Failure to skip nsamples\n");
                        return;
                }
            }
	 }/*while(!feof(infile))*/


	/*Delete the waitbar value array*/
	mxDestroyArray(wait_prhs[0]);

	/*Delete the title array*/
	mxDestroyArray(wait_prhs[2]);

	/*We are done with the waitbar. Delete it*/
	if(mexCallMATLAB(0, NULL, 1, wait_plhs, "delete"))
	{
	    printf("Could not delete waitbar\n");
	    return;
	}

    mxFree(fname);
    return;

}/*mexFunction*/
Beispiel #29
0
/* The gateway function */
void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[])
{
  int px, py, ax, ay, ordmin, ordmax;
  size_t Ntx, Nty, nrows, ncols;
  double x0, x1, y0, y1, dhx, dhy;
  double *Tphi, *Tpsi, *toto, *Z;

  /* check for proper number of arguments */
  if(nrhs!=6) {
    mexErrMsgIdAndTxt("MyToolbox:arrayProduct:nrhs","Seven inputs required.");
  }
  if(nlhs!=1) {
    mexErrMsgIdAndTxt("MyToolbox:arrayProduct:nlhs","One output required.");
  }
    
  Tphi = mxGetPr(prhs[0]); /* Table of phi */  
  toto = mxGetPr(prhs[1]); 
  x0 = toto[0]; x1 = toto[1]; /* support range of phi */
  Ntx = mxGetNumberOfElements(prhs[0]); /* Number of table values */
  dhx = (x1-x0)/(double)Ntx; /* Sampling step */

  Tpsi = mxGetPr(prhs[2]); /* Table of psi */  
  toto = mxGetPr(prhs[3]); 
  y0 = toto[0]; y1 = toto[1]; /* support range of psi */
  Nty = mxGetNumberOfElements(prhs[2]); /* Number of table values */
  dhy = (y1-y0)/(double)Nty; /* Sampling step */

  toto = mxGetPr(prhs[4]); ordmin = (int)(*toto);
  toto = mxGetPr(prhs[5]); ordmax = (int)(*toto);


  /* mexPrintf("x0,x1,y0,y1:%f,%f,%f,%f\n",x0,x1,y0,y1); */
  /* mexPrintf("Sampling step:%f\n",dh); */
  /* mexPrintf("ordmin, ordmax, Nt:%d,%d,%d\n",ordmin, ordmax, Nt); */

  nrows=ordmax-ordmin+1;
  ncols=ordmax-ordmin+1;

  plhs[0] = mxCreateDoubleMatrix(nrows, ncols, mxREAL);

  double *A = mxGetPr(plhs[0]);
  size_t idx;

  /* Loop for all position indexes in the range of rx X ry */
#pragma omp parallel for shared(A,Tphi,Tpsi,x0,y0) private(ax, ay, px,py,idx)
  for(ay=ordmin; ay<=ordmax; ay++){ /* column iteration */
    for(ax=ordmin; ax<=ordmax; ax++){ /* row iteration */

      idx = (ay-ordmin) * nrows + (ax-ordmin);
      A[idx] = 0;

      /* Loop inside the support of the wavelet for the inner product*/
      for(px=0; px<Ntx; px++){
	for(py=0; py<Nty; py++){
	  A[idx] += Tphi[px]*Tpsi[py] * pow(px*dhx+x0, ax)*pow(py*dhy+y0, ay);
	}
      }
      A[idx] *= (dhx*dhy);
    }
  }
}
Beispiel #30
0
/* ************************************************************
   PROCEDURE mexFunction - Entry for Matlab
   ************************************************************ */
void mexFunction(int nlhs, mxArray *plhs[],
  int nrhs, const mxArray *prhs[])
{
  const mxArray *L_FIELD;
  mxArray *myplhs[NPAROUT];
  mwIndex    m, i, j, iwsiz, nsuper, tmpsiz, fwsiz, nskip, nadd, m1;
  double *fwork, *d, *skipPr, *orgd;
  const double *permPr,*xsuperPr,*Ppr,*absd;
  mwIndex    *perm, *snode, *xsuper, *iwork, *xlindx, *skip, *skipJc;
  const mwIndex *LINir, *Pjc, *Pir;
  double canceltol, maxu, abstol;
  jcir   L;
  char useAbsd, useDelay;
/* ------------------------------------------------------------
   Check for proper number of arguments
   blkchol(L,P, pars,absd) with nparinmin=2.
   ------------------------------------------------------------ */
  mxAssert(nrhs >= NPARINMIN, "blkchol requires more input arguments");
  mxAssert(nlhs <= NPAROUT, "blkchol produces less output arguments");
/* ------------------------------------------------------------
   Get input matrix P to be factored.
   ------------------------------------------------------------ */
  m = mxGetM(P_IN);
  mxAssert( m == mxGetN(P_IN), "P must be square");
  mxAssert(mxIsSparse(P_IN), "P must be sparse");
  Pjc    = mxGetJc(P_IN);
  Pir    = mxGetIr(P_IN);
  Ppr    = mxGetPr(P_IN);
/* ------------------------------------------------------------
   Disassemble block Cholesky structure L
   ------------------------------------------------------------ */
  mxAssert(mxIsStruct(L_IN), "Parameter `L' should be a structure.");
  L_FIELD = mxGetField(L_IN,(mwIndex)0,"perm");       /* L.perm */
  mxAssert( L_FIELD != NULL, "Missing field L.perm.");
  mxAssert(m == mxGetM(L_FIELD) * mxGetN(L_FIELD), "perm size mismatch");
  permPr = mxGetPr(L_FIELD);
  L_FIELD = mxGetField(L_IN,(mwIndex)0,"L");         /* L.L */
  mxAssert( L_FIELD != NULL, "Missing field L.L.");
  mxAssert( m == mxGetM(L_FIELD) && m == mxGetN(L_FIELD), "Size L.L mismatch.");
  mxAssert(mxIsSparse(L_FIELD), "L.L should be sparse.");
  L.jc = mxGetJc(L_FIELD);
  LINir = mxGetIr(L_FIELD);
  L_FIELD = mxGetField(L_IN,(mwIndex)0,"xsuper");       /* L.xsuper */
  mxAssert( L_FIELD != NULL, "Missing field L.xsuper.");
  nsuper = mxGetM(L_FIELD) * mxGetN(L_FIELD) - 1;
  mxAssert( nsuper <= m, "Size L.xsuper mismatch.");
  xsuperPr = mxGetPr(L_FIELD);
  L_FIELD = mxGetField(L_IN,(mwIndex)0,"tmpsiz");         /* L.tmpsiz */
  mxAssert( L_FIELD != NULL, "Missing field L.tmpsiz.");
  tmpsiz   = (mwIndex) mxGetScalar(L_FIELD);
/* ------------------------------------------------------------
   Disassemble pars structure: canceltol, maxu
   ------------------------------------------------------------ */
  canceltol = 1E-12;           /* supply with defaults */
  maxu = 5E2;
  abstol = 1E-20;
  useAbsd = 0;
  useDelay = 0;
  if(nrhs >= NPARINMIN + 1){       /* 3rd argument = pars */
    mxAssert(mxIsStruct(PARS_IN), "Parameter `pars' should be a structure.");
    if( (L_FIELD = mxGetField(PARS_IN,(mwIndex)0,"canceltol")) != NULL)
      canceltol  = mxGetScalar(L_FIELD);  /* pars.canceltol */
    if( (L_FIELD = mxGetField(PARS_IN,(mwIndex)0,"maxu")) != NULL)
      maxu = mxGetScalar(L_FIELD);  /* pars.maxu */
    if( (L_FIELD = mxGetField(PARS_IN,(mwIndex)0,"abstol")) != NULL){
      abstol = mxGetScalar(L_FIELD);  /* pars.abstol */
      abstol = MAX(abstol, 0.0);
    }
    if( (L_FIELD = mxGetField(PARS_IN,(mwIndex)0,"delay")) != NULL)
      useDelay = (char) mxGetScalar(L_FIELD);  /* pars.delay */
/* ------------------------------------------------------------
   Get optional vector absd
   ------------------------------------------------------------ */
    if(nrhs >= NPARIN){
      useAbsd = 1;
      absd = mxGetPr(ABSD_IN);
      mxAssert(m == mxGetM(ABSD_IN) * mxGetN(ABSD_IN), "absd size mismatch");
    }
  }
/* ------------------------------------------------------------
   Create sparse output matrix L(m x m).
   ------------------------------------------------------------ */
  L_OUT = mxCreateSparse(m,m, L.jc[m],mxREAL);
  L.ir  = mxGetIr(L_OUT);
  L.pr  = mxGetPr(L_OUT);
  memcpy(mxGetJc(L_OUT), L.jc, (m+1) * sizeof(mwIndex));
  memcpy(L.ir, LINir, L.jc[m] * sizeof(mwIndex));
/* ------------------------------------------------------------
   Create ouput vector d(m).
   ------------------------------------------------------------ */
  D_OUT = mxCreateDoubleMatrix(m,(mwSize)1,mxREAL);
  d     = mxGetPr(D_OUT);
/* ------------------------------------------------------------
   Compute required sizes of working arrays:
   iwsiz = 2*(m + nsuper).
   fwsiz = tmpsiz.
   ------------------------------------------------------------ */
  iwsiz = MAX(2*(m+nsuper), 1);
  fwsiz = MAX(tmpsiz, 1);
/* ------------------------------------------------------------
   Allocate working arrays:
   integer: perm(m), snode(m), xsuper(nsuper+1),
      iwork(iwsiz), xlindx(m+1), skip(m),
   double: orgd(m), fwork(fwsiz).
   ------------------------------------------------------------ */
  m1 = MAX(m,1);                  /* avoid alloc to 0 */
  perm      = (mwIndex *) mxCalloc(m1,sizeof(mwIndex)); 
  snode     = (mwIndex *) mxCalloc(m1,sizeof(mwIndex)); 
  xsuper    = (mwIndex *) mxCalloc(nsuper+1,sizeof(mwIndex));
  iwork     = (mwIndex *) mxCalloc(iwsiz,sizeof(mwIndex));
  xlindx    = (mwIndex *) mxCalloc(m+1,sizeof(mwIndex));
  skip      = (mwIndex *) mxCalloc(m1, sizeof(mwIndex));
  orgd    = (double *) mxCalloc(m1,sizeof(double)); 
  fwork   = (double *) mxCalloc(fwsiz,sizeof(double)); 
/* ------------------------------------------------------------
   Convert PERM, XSUPER to integer and C-Style
   ------------------------------------------------------------ */
  for(i = 0; i < m; i++){
    j = (mwIndex) permPr[i];
    mxAssert(j>0,"");
    perm[i] = --j;
  }
  for(i = 0; i <= nsuper; i++){
    j =  (mwIndex) xsuperPr[i];
    mxAssert(j>0,"");
    xsuper[i] = --j;
  }
/* ------------------------------------------------------------
   Let L = tril(P(PERM,PERM)), uses orgd(m) as temp working storage.
   ------------------------------------------------------------ */
  permuteP(L.jc,L.ir,L.pr, Pjc,Pir,Ppr, perm, orgd, m);
/* ------------------------------------------------------------
   If no orgd has been supplied, take orgd = diag(L on input)
   Otherwise, let orgd = absd(perm).
   ------------------------------------------------------------ */
  if(useAbsd)
    for(j = 0; j < m; j++)
      orgd[j] = absd[perm[j]];
  else
    for(j = 0; j < m; j++)
      orgd[j] = L.pr[L.jc[j]];
/* ------------------------------------------------------------
   Create "snode" and "xlindx"; change L.ir to the compact subscript
   array (with xlindx), and do BLOCK SPARSE CHOLESKY.
   ------------------------------------------------------------ */
  nskip = spchol(m, nsuper, xsuper, snode, xlindx,
                 L.ir, orgd, L.jc, L.pr, d, perm, abstol,
                 canceltol, maxu, skip, &nadd, iwsiz, iwork, fwsiz, fwork);
  mxAssert(nskip >= 0, "Insufficient workspace in pblkchol");
/* ------------------------------------------------------------
   Copy original row-indices from LINir to L.ir.
   ------------------------------------------------------------ */
  memcpy(L.ir, LINir, L.jc[m] * sizeof(mwIndex));
/* ------------------------------------------------------------
   Create output matrices skip = sparse([],[],[],m,1,nskip),
   diagadd = sparse([],[],[],m,1,nadd),
   ------------------------------------------------------------ */
  SKIP_OUT = mxCreateSparse(m,(mwSize)1, MAX(1,nskip),mxREAL);
  memcpy(mxGetIr(SKIP_OUT), skip, nskip * sizeof(mwIndex));
  skipJc = mxGetJc(SKIP_OUT);
  skipJc[0] = 0; skipJc[1] = nskip;
  skipPr   = mxGetPr(SKIP_OUT);
/* ------------------------------------------------------------
   useDelay = 1 then L(:,i) is i-th column before ith pivot; useful
     for pivot-delaying strategy. (Fwslv(L, L(:,i)) still required.)
   ------------------------------------------------------------ */
  if(useDelay == 1)
    for(j = 0; j < nskip; j++)
      skipPr[j] = 1.0;
  else
    for(j = 0; j < nskip; j++){
      i = skip[j];
      skipPr[j] = L.pr[L.jc[i]];             /* Set skipped l(:,i)=ei. */
      L.pr[L.jc[i]] = 1.0;
      fzeros(L.pr+L.jc[i]+1,L.jc[i+1]-L.jc[i]-1);
    }
  DIAGADD_OUT = mxCreateSparse(m,(mwSize)1, MAX(1,nadd),mxREAL);
  memcpy(mxGetIr(DIAGADD_OUT), iwork, nadd * sizeof(mwIndex));
  skipJc = mxGetJc(DIAGADD_OUT);
  skipJc[0] = 0; skipJc[1] = nadd;
  skipPr   = mxGetPr(DIAGADD_OUT);
  for(j = 0; j < nadd; j++)
    skipPr[j] = orgd[iwork[j]];
/* ------------------------------------------------------------
   Release working arrays.
   ------------------------------------------------------------ */
  mxFree(fwork);
  mxFree(orgd);
  mxFree(skip);
  mxFree(xlindx);
  mxFree(iwork);
  mxFree(xsuper);
  mxFree(snode);
  mxFree(perm);
/* ------------------------------------------------------------
   Copy requested output parameters (at least 1), release others.
   ------------------------------------------------------------ */
  i = MAX(nlhs, 1);
  memcpy(plhs,myplhs, i * sizeof(mxArray *));
  for(; i < NPAROUT; i++)
    mxDestroyArray(myplhs[i]);
}