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;
}
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]);
*/
}
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
}
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);
}
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);
}
}
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;
}
/* 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
}
/* ************************************************************
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;
}
}
}
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;
}
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]);
*/
}
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);
}
}
/* ************************************************************
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());
}
}
/*
* 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;
}
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*/
/* 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);
}
}
}
/* ************************************************************
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]);
}