void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
mxArray* tmp;
/* ================ */
/* Check the INPUTS */
/* ================ */
#if DO_CHECK > 0
if( nrhs != 4 )
mexErrMsgIdAndTxt("InvalidInput:nrhs", "Require 4 inputs.");
if ( !mxIsStruct(prhs[0]) )
mexErrMsgIdAndTxt("InvalidInput:DICTIONARY", "'DICTIONARY' must be a struct");
#endif
mxArray* IC = mxGetField( prhs[0], 0, "IC" );
mxArray* EC = mxGetField( prhs[0], 0, "EC" );
mxArray* ISO = mxGetField( prhs[0], 0, "ISO" );
#if DO_CHECK > 0
if ( !mxIsStruct(IC) || !mxIsStruct(EC) || !mxIsStruct(ISO) )
mexErrMsgIdAndTxt("InvalidInput:DICTIONARY", "'DICTIONARY' format is wrong");
#endif
// Parse "DICTIONARY.nV"
tmp = mxGetField( prhs[0], 0, "nV" );
#if DO_CHECK > 0
if( !mxIsDouble(tmp) || mxIsComplex(tmp) || mxGetNumberOfElements(tmp)!=1 )
mexErrMsgIdAndTxt("InvalidInput:DICTIONARY.nV","'DICTIONARY.nV' must be a real scalar");
#endif
nV = mxGetScalar( tmp );
// Parse "DICTIONARY.EC.nE"
tmp = mxGetField( EC, 0, "nE" );
#if DO_CHECK > 0
if( !mxIsDouble(tmp) || mxIsComplex(tmp) || mxGetNumberOfElements(tmp)!=1 )
mexErrMsgIdAndTxt("InvalidInput:DICTIONARY.EC.nE","'DICTIONARY.EC.nE' must be a real scalar");
#endif
nE = mxGetScalar( tmp );
// Parse "DICTIONARY.IC.n"
tmp = mxGetField( IC, 0, "n" );
#if DO_CHECK > 0
if( !mxIsDouble(tmp) || mxIsComplex(tmp) || mxGetNumberOfElements(tmp)!=1 )
mexErrMsgIdAndTxt("InvalidInput:DICTIONARY.IC.n","'DICTIONARY.IC.n' must be a real scalar");
#endif
n = mxGetScalar( tmp );
// Parse "DICTIONARY.IC.fiber"
tmp = mxGetField( IC, 0, "fiber" );
#if DO_CHECK > 0
if ( mxGetNumberOfDimensions(tmp) != 2 || mxGetN(tmp) != 1 || !mxIsClass(tmp,"uint32") )
mexErrMsgIdAndTxt("InvalidInput:DICTIONARY.IC.fiber","'DICTIONARY.IC.fiber' must be a n*1 vector (uint32)");
#endif
ICf = (UINT32_T*) mxGetData( tmp );
// Parse "DICTIONARY.IC.len"
tmp = mxGetField( IC, 0, "len" );
#if DO_CHECK > 0
if ( mxGetNumberOfDimensions(tmp) != 2 || mxGetN(tmp) != 1 || !mxIsClass(tmp,"single") )
mexErrMsgIdAndTxt("InvalidInput:DICTIONARY.IC.len","'DICTIONARY.IC.len' must be a n*1 vector (single)");
#endif
ICl = (float*) mxGetData(tmp);
// Parse "DICTIONARY.IC.v", "DICTIONARY.IC.o"
tmp = mxGetField( IC, 0, "v" );
#if DO_CHECK > 0
if ( mxGetNumberOfDimensions(tmp) != 2 || mxGetN(tmp) != 1 || !mxIsClass(tmp,"uint32") )
mexErrMsgIdAndTxt("InvalidInput:DICTIONARY.IC.v","'DICTIONARY.IC.v' must be a n*1 vector (uint32)");
#endif
ICv = (UINT32_T*) mxGetData(tmp);
tmp = mxGetField( IC, 0, "o" );
#if DO_CHECK > 0
if ( mxGetNumberOfDimensions(tmp) != 2 || mxGetN(tmp) != 1 || !mxIsClass(tmp,"uint16") )
mexErrMsgIdAndTxt("InvalidInput:DICTIONARY.IC.o","'DICTIONARY.IC.o' must be a n*1 vector (uint16)");
#endif
ICo = (UINT16_T*) mxGetData(tmp);
// Parse "DICTIONARY.EC.v","DICTIONARY.EC.o"
tmp = mxGetField( EC, 0, "v" );
#if DO_CHECK > 0
if ( mxGetNumberOfDimensions(tmp) != 2 || mxGetN(tmp) != 1 || !mxIsClass(tmp,"uint32") )
mexErrMsgIdAndTxt("InvalidInput:DICTIONARY.EC.v","'DICTIONARY.EC.v' must be a n*1 vector (uint32)");
#endif
ECv = (UINT32_T*) mxGetData(tmp);
tmp = mxGetField( EC, 0, "o" );
#if DO_CHECK > 0
if ( mxGetNumberOfDimensions(tmp) != 2 || mxGetN(tmp) != 1 || !mxIsClass(tmp,"uint16") )
mexErrMsgIdAndTxt("InvalidInput:DICTIONARY.EC.o","'DICTIONARY.EC.o' must be a n*1 vector (uint16)");
#endif
ECo = (UINT16_T*) mxGetData(tmp);
// Parse "DICTIONARY.ISO.v"
tmp = mxGetField( ISO, 0, "v" );
#if DO_CHECK > 0
if ( mxGetNumberOfDimensions(tmp) != 2 || mxGetN(tmp) != 1 || !mxIsClass(tmp,"uint32") )
mexErrMsgIdAndTxt("InvalidInput:DICTIONARY.ISO.v","'DICTIONARY.ISO.v' must be a n*1 vector (uint32)");
#endif
ISOv = (UINT32_T*) mxGetData(tmp);
// Parse "KERNELS.nS"
tmp = mxGetField( prhs[1], 0, "nS" );
#if DO_CHECK > 0
if( !mxIsDouble(tmp) || mxIsComplex(tmp) || mxGetNumberOfElements(tmp)!=1 )
mexErrMsgIdAndTxt("InvalidInput:KERNELS.nS","'KERNELS.nS' must be a real scalar");
#endif
nS = mxGetScalar( tmp );
// Parse "KERNELS.wmr", "KERNELS.wmh" and "KERNELS.iso"
mxArray* wmr = mxGetField( prhs[1], 0, "wmr" );
#if nIC>=1
#if DO_CHECK > 0
if ( !mxIsCell(wmr) )
mexErrMsgIdAndTxt("InvalidInput:KERNELS.wmr","'KERNELS.wmr' must be a cell array");
if( !mxIsClass(mxGetCell(wmr,0),"single") )
mexErrMsgIdAndTxt("InvalidInput:KERNELS.wmr","'KERNELS.wmr' must contain single");
#endif
wmrSFP0 = (float*) mxGetData( mxGetCell(wmr,0) );
#endif
#if nIC>=2
#if DO_CHECK > 0
if( !mxIsClass(mxGetCell(wmr,1),"single") )
mexErrMsgIdAndTxt("InvalidInput:KERNELS.wmr","'KERNELS.wmr' must contain single");
#endif
wmrSFP1 = (float*) mxGetData( mxGetCell(wmr,1) );
#endif
#if nIC>=3
#if DO_CHECK > 0
if( !mxIsClass(mxGetCell(wmr,2),"single") )
mexErrMsgIdAndTxt("InvalidInput:KERNELS.wmr","'KERNELS.wmr' must contain single");
#endif
wmrSFP2 = (float*) mxGetData( mxGetCell(wmr,2) );
#endif
#if nIC>=4
#if DO_CHECK > 0
if( !mxIsClass(mxGetCell(wmr,3),"single") )
mexErrMsgIdAndTxt("InvalidInput:KERNELS.wmr","'KERNELS.wmr' must contain single");
#endif
wmrSFP3 = (float*) mxGetData( mxGetCell(wmr,3) );
#endif
#if nEC>=1
mxArray* wmh = mxGetField( prhs[1], 0, "wmh" );
#if DO_CHECK > 0
if ( !mxIsCell(wmh) )
mexErrMsgIdAndTxt("InvalidInput:KERNELS.wmh","'KERNELS.wmh' must be a cell array");
if( !mxIsClass(mxGetCell(wmh,0),"single") )
mexErrMsgIdAndTxt("InvalidInput:KERNELS.wmh","'KERNELS.wmh' must contain single");
#endif
wmhSFP0 = (float*) mxGetData( mxGetCell(wmh,0) );
#if nEC>=2
#if DO_CHECK > 0
if( !mxIsClass(mxGetCell(wmh,1),"single") )
mexErrMsgIdAndTxt("InvalidInput:KERNELS.wmh","'KERNELS.wmh' must contain single");
#endif
wmhSFP1 = (float*) mxGetData( mxGetCell(wmh,1) );
#endif
#if nEC>=3
#if DO_CHECK > 0
if( !mxIsClass(mxGetCell(wmh,2),"single") )
mexErrMsgIdAndTxt("InvalidInput:KERNELS.wmh","'KERNELS.wmh' must contain single");
#endif
wmhSFP2 = (float*) mxGetData( mxGetCell(wmh,2) );
#endif
#if nEC>=4
#if DO_CHECK > 0
if( !mxIsClass(mxGetCell(wmh,3),"single") )
mexErrMsgIdAndTxt("InvalidInput:KERNELS.wmh","'KERNELS.wmh' must contain single");
#endif
wmhSFP3 = (float*) mxGetData( mxGetCell(wmh,3) );
#endif
#endif
#if nISO>=1
mxArray* iso = mxGetField( prhs[1], 0, "iso" );
#if DO_CHECK > 0
if ( !mxIsCell(iso) )
mexErrMsgIdAndTxt("InvalidInput:KERNELS.iso","'KERNELS.iso' must be a cell array");
if( !mxIsClass(mxGetCell(iso,0),"single") )
mexErrMsgIdAndTxt("InvalidInput:KERNELS.iso","'KERNELS.iso' must contain single");
#endif
isoSFP0 = (float*) mxGetData( mxGetCell(iso,0) );
#if nISO>=2
#if DO_CHECK > 0
if( !mxIsClass(mxGetCell(iso,1),"single") )
mexErrMsgIdAndTxt("InvalidInput:KERNELS.iso","'KERNELS.iso' must contain single");
#endif
isoSFP1 = (float*) mxGetData( mxGetCell(iso,1) );
#endif
#if nISO>=3
#if DO_CHECK > 0
if( !mxIsClass(mxGetCell(iso,2),"single") )
mexErrMsgIdAndTxt("InvalidInput:KERNELS.iso","'KERNELS.iso' must contain single");
#endif
isoSFP2 = (float*) mxGetData( mxGetCell(iso,2) );
#endif
#if nISO>=4
#if DO_CHECK > 0
if( !mxIsClass(mxGetCell(iso,3),"single") )
mexErrMsgIdAndTxt("InvalidInput:KERNELS.iso","'KERNELS.iso' must contain single");
#endif
isoSFP3 = (float*) mxGetData( mxGetCell(iso,3) );
#endif
#endif
// Parse "x"
#if DO_CHECK > 0
if ( mxGetNumberOfDimensions(prhs[2]) != 2 || mxGetN(prhs[2]) != 1 || !mxIsClass(prhs[2],"double") )
mexErrMsgIdAndTxt("InvalidInput:x","'x' must be a n*1 vector (double)");
#endif
x = (double*) mxGetData( prhs[2] );
if ( nIC > 0 )
nF = ( mxGetM(prhs[2]) - nE*nEC - nV*nISO) / nIC;
else
nF = 0;
// parse "THREADS"
#if DO_CHECK > 0
if ( !mxIsStruct(prhs[3]) )
mexErrMsgIdAndTxt("InvalidInput:THREADS", "'THREADS' must be a struct");
#endif
tmp = mxGetField( prhs[3], 0, "IC" );
#if DO_CHECK > 0
if ( mxGetNumberOfDimensions(tmp) != 2 || mxGetN(tmp) != 1 || !mxIsClass(tmp,"uint32") )
mexErrMsgIdAndTxt("InvalidInput:THREADS.IC","'THREADS.IC' must be a n*1 vector (uint32)");
#endif
ICthreads = (UINT32_T*) mxGetData( tmp );
tmp = mxGetField( prhs[3], 0, "EC" );
#if DO_CHECK > 0
if ( mxGetNumberOfDimensions(tmp) != 2 || mxGetN(tmp) != 1 || !mxIsClass(tmp,"uint32") )
mexErrMsgIdAndTxt("InvalidInput:THREADS.EC","'THREADS.EC' must be a n*1 vector (uint32)");
#endif
ECthreads = (UINT32_T*) mxGetData( tmp );
tmp = mxGetField( prhs[3], 0, "ISO" );
#if DO_CHECK > 0
if ( mxGetNumberOfDimensions(tmp) != 2 || mxGetN(tmp) != 1 || !mxIsClass(tmp,"uint32") )
mexErrMsgIdAndTxt("InvalidInput:THREADS.ISO","'THREADS.ISO' must be a n*1 vector (uint32)");
#endif
ISOthreads = (UINT32_T*) mxGetData( tmp );
/* =============== */
/* Set the OUTPUTS */
/* =============== */
#if DO_CHECK > 0
if( nlhs != 1 )
mexErrMsgIdAndTxt("InvalidOutput:nlhs", "Required 1 output.");
#endif
const int outDims[2] = { nS*nV, 1 };
plhs[0] = mxCreateNumericArray(2, outDims, mxDOUBLE_CLASS, mxREAL);
Y = (double*)mxGetData( plhs[0] );
/* ================================================== */
/* Run SEPARATE THREADS to perform the multiplication */
/* ================================================== */
pthread_t threads[nTHREADS];
for(int t=0; t<nTHREADS ; t++)
pthread_create( &threads[t], NULL, computeProductBlock, (void *) (long int)t );
for(int t=0; t<nTHREADS ; t++)
pthread_join( threads[t], NULL );
return;
}
/* entrance point for mex file */
void mexFunction(
int nlhs, // Number of left hand side (output) arguments
mxArray *plhs[], // Array of left hand side arguments
int nrhs, // Number of right hand side (input) arguments
const mxArray *prhs[] // Array of right hand side arguments
)
{
/* no inputs - one output : give data regarding the class */
if ( nlhs == 1 && nrhs == 0 ) {
plhs[0] = mxCreateNumericMatrix(1, 1, mxCOORD_CLASS, mxREAL);
ANNcoord * tmp = (ANNcoord*)mxGetData(plhs[0]);
*tmp = 0;
return;
}
/* regular modes */
if ( nrhs < 2 )
mexErrMsgIdAndTxt("annmex:inputs","too few input arguments");
/* first argument - mode of operation */
ann_mex_mode_t mode;
int tmpi(0);
int * iptr = NULL;
GetScalar(prhs[0], tmpi);
mode = (ann_mex_mode_t)tmpi;
/* special treatment for opening */
if ( mode == OPEN ) {
if ( nlhs != 1 )
mexErrMsgIdAndTxt("annmex:open","wrong number of outputs - must be one");
ann_mex_t * annp = new ann_mex_t(prhs[1]);
/* convert the pointer and return it as pointer_type */
plhs[0] = mxCreateNumericMatrix(1, 1, mxPOINTER_CLASS, mxREAL);
pointer_t * ptr = (pointer_t*)mxGetData(plhs[0]);
*ptr = (pointer_t)annp;
return;
}
/* for all pther modes - second argument is the ann_mex_t pointer */
/* extract pointer */
pointer_t * ptr = (pointer_t*)mxGetData(prhs[1]);
ann_mex_t * annp = (ann_mex_t*)(*ptr);
if ( ! annp->IsGood() )
mexErrMsgIdAndTxt("annmex:pointer","internal data structure is faulty - possible wrong handle");
if ( mode == CLOSE ) {
delete annp;
return;
}
/* for the search operations - extract the parameters */
if ( nrhs < 3 )
mexErrMsgIdAndTxt("annmex:inputs","must have at least 3 inputs");
int k = 1;
if ( nrhs >= 4 )
GetScalar(prhs[3], k);
double eps = 0.0;
if ( nrhs >= 5 )
GetScalar(prhs[4], eps);
if ( mode == FRSEARCH ) {
if ( nrhs != 6 )
mexErrMsgIdAndTxt("annmex:FRSEARCH_inputs","must have 6 inputs");
}
switch (mode) {
case KSEARCH:
if ( nlhs != 2 )
mexErrMsgIdAndTxt("annmex:outputs","must have two output arguments");
annp->annkSearch(prhs[2], k, &plhs[0], &plhs[1], eps);
return;
case PRISEARCH:
if ( nlhs != 2 )
mexErrMsgIdAndTxt("annmex:outputs","must have two output arguments");
annp->annkPriSearch(prhs[2], k, &plhs[0], &plhs[1], eps);
return;
case FRSEARCH:
if ( nlhs != 3 )
mexErrMsgIdAndTxt("annmex:outputs","must have three output arguments");
annp->annkFRSearch(prhs[2], prhs[5], k, &plhs[0], &plhs[1], &plhs[2], eps);
return;
default:
mexErrMsgIdAndTxt("annmex:mode","unrecognized mode");
}
}
void GetArrSq(const mxArray* x, T* arr)
{
int ii, n = mxGetNumberOfElements(x);
void *p = mxGetData(x);
char* cp;
unsigned char* ucp;
short* sp;
unsigned short* usp;
int* ip;
unsigned int* uip;
int64_T *i64p;
uint64_T *ui64p;
double* dp;
float* fp;
switch (mxGetClassID(x)) {
case mxCHAR_CLASS:
case mxINT8_CLASS:
cp = (char*)p;
for ( ii = 0 ; ii < n ; ii++)
arr[ii] = cp[ii]*cp[ii];
return;
case mxDOUBLE_CLASS:
dp = (double*)p;
for ( ii = 0 ; ii < n ; ii++)
arr[ii] = dp[ii]*dp[ii];
return;
case mxSINGLE_CLASS:
fp = (float*)p;
for ( ii = 0 ; ii < n ; ii++)
arr[ii] = fp[ii]*fp[ii];
return;
case mxUINT8_CLASS:
ucp = (unsigned char*)p;
for ( ii = 0 ; ii < n ; ii++)
arr[ii] = ucp[ii]*ucp[ii];
return;
case mxINT16_CLASS:
sp = (short*)p;
for ( ii = 0 ; ii < n ; ii++)
arr[ii] = sp[ii]*sp[ii];
return;
case mxUINT16_CLASS:
usp = (unsigned short*)p;
for ( ii = 0 ; ii < n ; ii++)
arr[ii] = usp[ii]*usp[ii];
return;
case mxINT32_CLASS:
ip = (int*)p;
for ( ii = 0 ; ii < n ; ii++)
arr[ii] = ip[ii]*ip[ii];
return;
case mxUINT32_CLASS:
uip = (unsigned int*)p;
for ( ii = 0 ; ii < n ; ii++)
arr[ii] = uip[ii]*uip[ii];
return;
case mxINT64_CLASS:
i64p = (int64_T*)p;
for ( ii = 0 ; ii < n ; ii++)
arr[ii] = i64p[ii]*i64p[ii];
return;
case mxUINT64_CLASS:
ui64p = (uint64_T*)p;
for ( ii = 0 ; ii < n ; ii++)
arr[ii] = ui64p[ii]*ui64p[ii];
return;
default:
mexErrMsgIdAndTxt("annmex:GetArrSq","unsupported data type");
}
}
// ------------------------------- mexFunction ---------------------------------------//
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[]) {
/* Check for proper number of arguments */
if(nrhs!=5) {
Usage();
mexErrMsgIdAndTxt("MyToolbox:arrayProduct:nrhs",
"Five inputs required.");
}
if(nlhs!=4) {
Usage();
mexErrMsgIdAndTxt("MyToolbox:arrayProduct:nlhs",
"Four output required.");
}
// Input argument and dimensions
const mwSize DataR = mxGetM(prhs[0]),
DataC = mxGetN(prhs[0]),
MeanIR = mxGetM(prhs[1]),
MeanIC = mxGetN(prhs[1]);
const double *Data = mxGetPr(prhs[0]),
*MeanI = mxGetPr(prhs[1]);
const double MaxIter = mxGetScalar(prhs[2]),
Thresh = mxGetScalar(prhs[3]);
const int Verbose = (int) mxGetScalar(prhs[4]);
// Set the number of means elements, dimension and number of data points
int NM = MeanIC,
ND = DataC,
Dim = DataR;
// Check appropriate sizes and modes
if( Dim != MeanIR ) {
Usage();
mexErrMsgIdAndTxt("KMeans:Means","Means matrix dimension");
}
// Data elements
double *Prior = NULL,
*Mean = NULL,
*Cov = NULL,
*Assign;
// ** KM parameters
double *SqrTemp = new double[Dim];
double *DistTemp = new double[NM];
double alpha = 1,
beta = 1,
Scale = 0;
double Term, TermP = 0;
int AssignTemp;
int *AssignNum; AssignNum = new int[NM];
mwSize CSize[] = {Dim,Dim,NM};
int CNumSize = 3;
int i,k,j;
int inc = 1;
int index = 0;
Assign = new double[ND];
Prior = new double[NM];
Mean = new double[Dim*NM];
Cov = new double[Dim*Dim*NM];
memcpy(Mean, MeanI, Dim*NM*sizeof(double));
for(int E = 0; E < MaxIter; E++){
// ** Assign features to each center
#pragma omp parallel for shared(Data, Mean, Dim, inc, Assign) private(i,j, SqrTemp, DistTemp, AssignTemp)
for(i = 0; i < ND; i++){
for(j = 0; j < NM; j++){
// Calculate elements of Data-Mean
vdSub( Dim, &Data[i*Dim], &Mean[j*Dim], SqrTemp );
// Square elements
vdSqr( Dim, SqrTemp, SqrTemp );
// Sum distances
DistTemp[j] = dasum( &Dim, SqrTemp, &inc);
}
// Find minimum distance index
AssignTemp = idamin(&NM, DistTemp, &inc);
Assign[i] = double(AssignTemp-1);
}
// ** Calculate new center values
// Zero out centers
memset(Mean, 0, Dim*NM*sizeof(double));
memset(AssignNum, 0, NM*sizeof(int));
for(i = 0; i < ND; i++){
index = Assign[i];
daxpy(&Dim, &alpha, &Data[i*Dim], &inc, &Mean[index*Dim], &inc);
AssignNum[index] = AssignNum[index] + 1;
}
for(i = 0; i < NM; i++){
// If no point was assigned to an average, assign a different point
if(AssignNum[i] == 0){
memcpy(&Mean[i*Dim], &Data[(rand()%ND)*Dim], Dim*sizeof(double));
Scale = 1;
dscal(&Dim, &Scale, &Mean[i*Dim], &inc);
}
else{
Scale = 1 / double(AssignNum[i]);
dscal(&Dim, &Scale, &Mean[i*Dim], &inc);
}
}
// Termination conditions
k = Dim*NM;
Term = dasum(&k, Mean, &inc);
if(Verbose == 1){
printf("Iteration %d - Shift %3.9f\n",E, fabs(Term-TermP));
mexEvalString("pause(.000001);");
}
if(fabs(Term-TermP) <= Thresh)
break;
TermP = Term;
}
// ** Create and assign output variables
double *Out;
plhs[0] = mxCreateDoubleMatrix(1, ND ,mxREAL );
plhs[1] = mxCreateDoubleMatrix(Dim, NM, mxREAL );
plhs[2] = mxCreateDoubleMatrix(1, NM, mxREAL );
plhs[3] = mxCreateNumericArray( CNumSize, CSize,
mxDOUBLE_CLASS, mxREAL);
// Priors
k = 0;
for(i = 0; i < NM; i++ )
k = k + AssignNum[i];
for(i = 0; i < NM; i++){
Prior[i] = double(AssignNum[i])/k;
}
// Covariances
memset(Cov, 0, Dim*Dim*NM*sizeof(double));
char trans = 'N';
k = 1;
for(i = 0; i < ND; i++){
index = Assign[i];
Scale = 1/(Prior[index]*ND);
// Calculate elements of Data-Mean
vdSub( Dim, &Data[i*Dim], &Mean[index*Dim], SqrTemp );
// covariance
dgemm(&trans, &trans, &Dim, &Dim, &k, &Scale, SqrTemp, &Dim, SqrTemp,
&k, &beta, &Cov[index*Dim*Dim], &Dim);
}
Out = mxGetPr(plhs[0]);
memcpy(Out, Assign, ND*sizeof(double));
Out = mxGetPr(plhs[1]);
memcpy(Out, Mean, Dim*NM*sizeof(double));
Out = mxGetPr(plhs[2]);
memcpy(Out, Prior, NM*sizeof(double));
Out = mxGetPr(plhs[3]);
memcpy(Out, Cov, Dim*Dim*NM*sizeof(double));
//** Delete dynamic variables
delete [] Prior;
delete [] Mean;
delete [] Cov;
delete [] Assign;
delete [] AssignNum;
}
// The entry point for mex
void
mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[])
{
/* Check for proper number of input and output arguments */
if (nrhs != 2) {
mexErrMsgIdAndTxt( "Mscope:getLastFrame16:invalidNumInputs",
"Two input argument required.");
}
if (nlhs > 3) {
mexErrMsgIdAndTxt( "Mscope:getLastFrame16:maxlhs",
"Too many output arguments.");
}
int xPix, yPix;
if (!isScalarArray(prhs[0])) {
mexErrMsgIdAndTxt("Mscope:setTimings:invalidArg","2-3 floating point scalars required");
}
xPix = (unsigned long) mxGetScalar(prhs[0]);
/* validate exposure time etc... */
if (!isScalarArray(prhs[1])) {
mexErrMsgIdAndTxt("Mscope:setTimings:invalidArg","2-3 floating point scalars required");
}
yPix = (unsigned long) mxGetScalar(prhs[1]);
/* construct the array we will return the image in */
unsigned long numPix = xPix*yPix; // total number of pixels
WORD * imageArray = new WORD[numPix];
/* call the Andor SDK functions */
long first, last;
GetNumberNewImages(&first,&last);
INT32_T last32 = (INT32_T) last;
// long validFirst, validLast; // Comment this out if not getting EXACT frame
// IMPORTANT ONE FOR ACTUAL IMAGE RETRIEVAL
UINT32_T returnInt = (UINT32_T) GetMostRecentImage16(imageArray,numPix);
// UINT32_T returnInt = (UINT32_T) GetImages16(last,last,imageArray,numPix,&validFirst,&validLast);
/* Copy the memory into MATLAB form */
mwSignedIndex returnIntDims[2] = {1,1}; // how many elements does the output code need
plhs[0] = mxCreateNumericArray(1, returnIntDims, mxUINT32_CLASS, mxREAL);
double * codePtr = mxGetPr(plhs[0]);
memcpy(codePtr, &returnInt, sizeof(returnInt));
mwSignedIndex imageDims[2] = {xPix,yPix}; // elements in image
plhs[1] = mxCreateNumericArray(2, imageDims, mxUINT16_CLASS, mxREAL);
double * imgPtr = mxGetPr(plhs[1]);
memcpy(imgPtr, imageArray, numPix*sizeof(imageArray[0]));
plhs[2] = mxCreateNumericArray(2, returnIntDims, mxINT32_CLASS, mxREAL);
double * latestImNoPtr = mxGetPr(plhs[2]);
memcpy(latestImNoPtr, &last32, sizeof(last32));
/* clear up the array we used */
delete imageArray;
return;
}
/*!
* mexFunction is the main function of the mex simulation file this function carries out all numerical integration and writes results into the sol struct.
*
* @param[in] nlhs number of output arguments of the matlab call @type int
* @param[out] plhs pointer to the array of output arguments @type mxArray
* @param[in] nrhs number of input arguments of the matlab call @type int
* @param[in] prhs pointer to the array of input arguments @type mxArray
* @return void
*/
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
void *ami_mem = nullptr; /* pointer to cvodes memory block */
UserData *udata = nullptr; /* user data */
ReturnData *rdata = nullptr; /* return data */
ExpData *edata = nullptr; /* experimental data */
TempData *tdata = nullptr; /* temporary data */
int status = 0; /* general status flag */
double *pstatus; /* return status flag */
int iroot = 0;
booleantype setupBdone = false;
pstatus = (double *) mxMalloc(sizeof(double));
udata = setupUserData(prhs);
if (udata == NULL) {
/* goto freturn will fail here as freeXXXXData routines will fail*/
*pstatus = -98;
return;
}
/* options */
if (!prhs[3]) {
mexErrMsgIdAndTxt("AMICI:mex:options","No options provided!");
}
tdata = new TempData();
if (tdata == NULL) goto freturn;
if (nx>0) {
ami_mem = setupAMI(&status, udata, tdata);
if (ami_mem == NULL){
status = -96;
goto freturn;
}
}
rdata = setupReturnData(plhs, udata, pstatus);
if (rdata == NULL) {
status = -96;
goto freturn;
}
if (nx>0) {
edata = setupExpData(prhs, udata);
if (edata == NULL) {
status = -97;
goto freturn;
}
}
if(nx>0) {
status = workForwardProblem(udata, tdata, rdata, edata, &status, ami_mem, &iroot);
if(status<0) goto freturn;
status = workBackwardProblem(udata, tdata, rdata, edata, &status, ami_mem, &iroot, &setupBdone);
}
freturn:
freeTempDataAmiMem(udata, tdata, ami_mem, setupBdone, *pstatus);
freeUserData(udata);
delete edata;
*pstatus = (double) status;
}
/** the gateway function */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
/* [t, b] = ddm_rand_full(mu, sig2, b_lo, b_up,
delta_t, n[, inv_leak[, seed]]) */
/* Check argument number */
if (nlhs != 2) {
mexErrMsgIdAndTxt("ddm_rand_full:WrongOutputs",
"Wrong number of output arguments");
}
if (nrhs < 6) {
mexErrMsgIdAndTxt("ddm_rand_full:WrongInputs",
"Too few input arguments");
}
/* Process first 8 arguments */
if (!MEX_ARGIN_IS_REAL_VECTOR(0))
mexErrMsgIdAndTxt("ddm_rand_full:WrongInput",
"First input argument expected to be a vector");
if (!MEX_ARGIN_IS_REAL_VECTOR(1))
mexErrMsgIdAndTxt("ddm_rand_full:WrongInput",
"Second input argument expected to be a vector");
if (!MEX_ARGIN_IS_REAL_VECTOR(2))
mexErrMsgIdAndTxt("ddm_rand_full:WrongInput",
"Third input argument expected to be a vector");
if (!MEX_ARGIN_IS_REAL_VECTOR(3))
mexErrMsgIdAndTxt("ddm_rand_full:WrongInput",
"Fourth input argument expected to be a vector");
if (!MEX_ARGIN_IS_REAL_DOUBLE(4))
mexErrMsgIdAndTxt("ddm_rand_full:WrongInput",
"Fifth input argument expected to be a double");
if (!MEX_ARGIN_IS_REAL_DOUBLE(5))
mexErrMsgIdAndTxt("ddm_rand_full:WrongInput",
"Sixth input argument expected to be a double");
if (nrhs >= 7 && !MEX_ARGIN_IS_REAL_DOUBLE(6))
mexErrMsgIdAndTxt("ddm_rand_sym:WrongInput",
"Seventh input argument expected to be a double");
if (nrhs >= 8 && !MEX_ARGIN_IS_REAL_DOUBLE(7))
mexErrMsgIdAndTxt("ddm_rand_sym:WrongInput",
"Eighth input argument expected to be a double");
int mu_size = std::max(mxGetN(prhs[0]), mxGetM(prhs[0]));
int sig2_size = std::max(mxGetN(prhs[1]), mxGetM(prhs[1]));
int b_lo_size = std::max(mxGetN(prhs[2]), mxGetM(prhs[2]));
int b_up_size = std::max(mxGetN(prhs[3]), mxGetM(prhs[3]));
ExtArray mu(ExtArray::shared_noowner(mxGetPr(prhs[0])), mu_size);
ExtArray sig2(ExtArray::shared_noowner(mxGetPr(prhs[1])), sig2_size);
ExtArray b_lo(ExtArray::shared_noowner(mxGetPr(prhs[2])), b_lo_size);
ExtArray b_up(ExtArray::shared_noowner(mxGetPr(prhs[3])), b_up_size);
ExtArray b_lo_deriv = ExtArray::const_array(0.0);
ExtArray b_up_deriv = ExtArray::const_array(0.0);
double delta_t = mxGetScalar(prhs[4]);
int n = (int) mxGetScalar(prhs[5]);
if (delta_t <= 0.0)
mexErrMsgIdAndTxt("ddm_rand_full:WrongInput",
"delta_t needs to be larger than 0.0");
if (n <= 0)
mexErrMsgIdAndTxt("ddm_rand_full:WrongInput",
"n needs to be larger than 1");
bool has_leak = false;
double inv_leak = 0.0;
if (nrhs >= 7) {
inv_leak = mxGetScalar(prhs[6]);
has_leak = (inv_leak != 0.0);
if (inv_leak < 0.0)
mexErrMsgIdAndTxt("ddm_rand_full:WrongInput",
"inv_leak needs to be non-negative");
}
int rngseed = 0;
if (nrhs >= 8)
rngseed = (int) mxGetScalar(prhs[7]);
if (nrhs > 8)
mexErrMsgIdAndTxt("ddm_rand_full:WrongInputs",
"Too many input arguments");
/* reserve space for output */
plhs[0] = mxCreateDoubleMatrix(1, n, mxREAL);
plhs[1] = mxCreateLogicalMatrix(1, n);
double* t = mxGetPr(plhs[0]);
mxLogical* b = mxGetLogicals(plhs[1]);
/* perform sampling */
DMBase* dm = nullptr;
if (has_leak)
dm = DMBase::create(mu, sig2, b_lo, b_up, b_lo_deriv, b_up_deriv,
delta_t, inv_leak);
else
dm = DMBase::create(mu, sig2, b_lo, b_up, b_lo_deriv, b_up_deriv,
delta_t);
DMBase::rngeng_t rngeng;
if (rngseed == 0) rngeng.seed(std::random_device()());
else rngeng.seed(rngseed);
for (int i = 0; i < n; ++i) {
DMSample s = dm->rand(rngeng);
t[i] = s.t();
b[i] = s.upper_bound() ? 1 : 0;
}
delete dm;
}
double MxArray::toDouble() const {
if (numel() != 1)
mexErrMsgIdAndTxt("mexopencv:error", "MxArray is not a scalar");
return at<double>(0);
}
bool MxArray::toBool() const {
if (numel() != 1)
mexErrMsgIdAndTxt("mexopencv:error", "MxArray is not a scalar");
return at<bool>(0);
}
MxArray::MxArray(const char** fields, int nfields, int m, int n) :
p_(mxCreateStructMatrix(m, n, nfields, fields))
{
if (!p_)
mexErrMsgIdAndTxt("mexopencv:error", "Allocation error");
}
int MxArray::toInt() const {
if (numel() != 1)
mexErrMsgIdAndTxt("mexopencv:error", "MxArray is not a scalar");
return at<int>(0);
}
MxArray::MxArray(const std::string& s) : p_(mxCreateString(s.c_str()))
{
if (!p_)
mexErrMsgIdAndTxt("mexopencv:error", "Allocation error");
}
MxArray::MxArray(const bool b) : p_(mxCreateLogicalScalar(b))
{
if (!p_)
mexErrMsgIdAndTxt("mexopencv:error", "Allocation error");
}
MxArray::MxArray(const double d) : p_(mxCreateDoubleScalar(d))
{
if (!p_)
mexErrMsgIdAndTxt("mexopencv:error", "Allocation error");
}
mxArray* myGetField(const mxArray* pobj, const int idx, const char* propname)
{
mxArray* pm = mxGetField(pobj,idx,propname);
if (!pm) mexErrMsgIdAndTxt("DRC:ControlUtil:BadInput","ControlUtil is trying to load object field '%s', but failed.", propname);
return pm;
}
MxArray MxArray::at(mwIndex index) const
{
if (!isCell())
mexErrMsgIdAndTxt("mexopencv:error", "MxArray is not cell");
return MxArray(mxGetCell(p_, index));
}
/*
* [type, num_constraint, constraint_val, dconstraint_val, constraint_name, lower_bound, upper_bound]
* = testSingleTimeKinCnstmex(kinCnst_ptr, q, t)
* @param kinCnst_ptr A pointer to a SingleTimeKinematicConstraint
* object
* @param q A nqx1 double vector
* @param t A double scalar, the time to evaluate constraint
* value, bounds and name. This is optional.
* @retval type The type of the constraint
* @retval num_constraint The number of constraint active at time t
* @retval constraint_val The value of the constraint at time t
* @retval dconstraint_val The gradient of the constraint w.r.t q at time t
* @retval constraint_name The name of the constraint at time t
* @retval lower_bound The lower bound of the constraint at time t
* @retval upper_bound The upper bound of the constraint at time t
* */
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
if ((nrhs != 3 && nrhs != 2) || nlhs != 7) {
mexErrMsgIdAndTxt("Drake:testSingleTimeKinCnstmex:BadInputs",
"Usage [type, "
"num_cnst, cnst_val, dcnst_val, cnst_name, lb, ub] = "
"testSingleTimeKinKinCnstmex(kinCnst, q, t)");
}
SingleTimeKinematicConstraint* cnst =
(SingleTimeKinematicConstraint*)getDrakeMexPointer(prhs[0]);
double* t_ptr;
if (nrhs == 2) {
t_ptr = nullptr;
} else {
size_t num_t = mxGetNumberOfElements(prhs[2]);
if (num_t == 0) {
t_ptr = nullptr;
}
if (num_t == 1) {
t_ptr = mxGetPrSafe(prhs[2]);
}
if (num_t > 1) {
mexErrMsgIdAndTxt("Drake:testSingleTimeKinCnstmex:BadInputs",
"t must be either empty or a single number");
}
}
int type = cnst->getType();
int num_cnst = cnst->getNumConstraint(t_ptr);
// mexPrintf("num_cnst = %d\n", num_cnst);
int nq = cnst->getRobotPointer()->number_of_positions();
Eigen::Map<Eigen::VectorXd> q(mxGetPrSafe(prhs[1]), nq);
KinematicsCache<double> cache = cnst->getRobotPointer()->doKinematics(q);
Eigen::VectorXd c(num_cnst);
Eigen::MatrixXd dc(num_cnst, nq);
cnst->eval(t_ptr, cache, c, dc);
// mexPrintf("get c, dc\n");
Eigen::VectorXd lb(num_cnst);
Eigen::VectorXd ub(num_cnst);
cnst->bounds(t_ptr, lb, ub);
// mexPrintf("get lb, ub\n");
std::vector<std::string> cnst_names;
cnst->name(t_ptr, cnst_names);
// mexPrintf("get name\n");
int retvec_size;
if (num_cnst == 0) {
retvec_size = 0;
} else {
retvec_size = 1;
}
plhs[0] = mxCreateDoubleScalar((double)type);
plhs[1] = mxCreateDoubleScalar((double)num_cnst);
plhs[2] = mxCreateDoubleMatrix(num_cnst, retvec_size, mxREAL);
memcpy(mxGetPrSafe(plhs[2]), c.data(), sizeof(double) * num_cnst);
plhs[3] = mxCreateDoubleMatrix(num_cnst, nq, mxREAL);
memcpy(mxGetPrSafe(plhs[3]), dc.data(), sizeof(double) * num_cnst * nq);
int name_ndim = 1;
mwSize name_dims[] = {(mwSize)num_cnst};
plhs[4] = mxCreateCellArray(name_ndim, name_dims);
mxArray* name_ptr;
for (int i = 0; i < num_cnst; i++) {
name_ptr = mxCreateString(cnst_names[i].c_str());
mxSetCell(plhs[4], i, name_ptr);
}
plhs[5] = mxCreateDoubleMatrix(num_cnst, retvec_size, mxREAL);
plhs[6] = mxCreateDoubleMatrix(num_cnst, retvec_size, mxREAL);
memcpy(mxGetPrSafe(plhs[5]), lb.data(), sizeof(double) * num_cnst);
memcpy(mxGetPrSafe(plhs[6]), ub.data(), sizeof(double) * num_cnst);
}
MxArray::MxArray(const int i)
: p_(mxCreateDoubleScalar(static_cast<double>(i)))
{
if (!p_)
mexErrMsgIdAndTxt("mexopencv:error", "Allocation error");
}
/* The gateway function */
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
double *yt;
double *rhot;
int L;
int R;
int dim;
int CSP;
int CSD;
double *scales2;
double *scaleweight2;
double energyweight;
double *dy;
/* check for proper number of arguments */
if(nrhs!=8) {
mexErrMsgIdAndTxt("gradC:nrhs","8 inputs required.");
}
if(nlhs!=1) {
mexErrMsgIdAndTxt("gradC:nlhs","One output required.");
}
/* get the values of the non-array inputs */
L = (double)(mxGetScalar(prhs[2]));
R = (double)(mxGetScalar(prhs[3]));
dim = (double)(mxGetScalar(prhs[4]));
energyweight = (double)(mxGetScalar(prhs[7]));
CSP = dim*(1+R);
CSD = dim*R;
/* get the array inputs */
yt = mxGetPr(prhs[0]);
int n= mxGetM(prhs[0]);
if(n != CSP*L) {
mexErrMsgIdAndTxt("gradC:nlhs","yt dimension mismatch (got %d, expected %d).",n,CSP*L);
}
rhot = mxGetPr(prhs[1]);
if(CSP*L != mxGetM(prhs[1])) {
mexErrMsgIdAndTxt("gradC:nlhs","rhot dimension mismatch (got %d, expected %d).",mxGetM(prhs[1]),CSP*L);
}
scales2 = mxGetPr(prhs[5]);
if(R != mxGetN(prhs[5])) {
mexErrMsgIdAndTxt("gradC:nlhs","scales2 dimension mismatch (got %d, expected %d).",mxGetN(prhs[5]),R);
}
scaleweight2 = mxGetPr(prhs[6]);
if(R != mxGetN(prhs[6])) {
mexErrMsgIdAndTxt("gradC:nlhs","scaleweight2 dimension mismatch (got %d, expected %d).",mxGetN(prhs[6]),R);
}
/* create the output matrix */
plhs[0] = mxCreateDoubleMatrix(n,1,mxREAL);
/* get a pointer to the real data in the output matrix */
dy = mxGetPr(plhs[0]);
/* debug */
/*mexPrintf("%f %f %f %d %d %d %d %f %f\n",t,yt[0],rhot[0],L,R,CSP,CSD,scales2[0],scaleweight2[0]);*/
/* call the computational routine */
gradOrder1ScaleC(dy,yt,rhot,L,R,dim,CSP,CSD,scales2,scaleweight2,energyweight);
}
/**
* Input
* [0] n: The number of vertices [double]
* [1] m: The number of links [double]
* [2] sv: The vector of source vertices of edges [int32 one-based]
* [3] tv: The vector of target vertices of edges [int32 one-based]
* [4] nc: The capacities of neighboring-links
* [5] rc: The reverse capacities of neighboring-links
* [6] tc_src: The capacities of links from source
* [7] tc_snk: The capacities of links from sink
*
* Note the type of nc can be double|single|int32|uint32, tc should be
* of the same type as nc
*
* Output
* [0] L: The labeling of the nodes [int32 1 x n]
* - 1: assigned to source
* - -1: assigned to sink
* - 0: can be assigned to either source or sink
*
* [1] maxflow: The value of maxflow
*/
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
// take input
const mxArray *mxN = prhs[0];
const mxArray *mxM = prhs[1];
const mxArray *mxSv = prhs[2];
const mxArray *mxTv = prhs[3];
const mxArray *mxNc = prhs[4];
const mxArray *mxRc = prhs[5];
const mxArray *mxTcSrc = prhs[6];
const mxArray *mxTcSnk = prhs[7];
int n = (int)mxGetScalar(mxN);
int m = (int)mxGetScalar(mxM);
const int *sv = (const int*)mxGetData(mxSv);
const int *tv = (const int*)mxGetData(mxTv);
mxClassID cid = decide_capacity_type(mxNc, mxRc, mxTcSrc, mxTcSnk);
switch (cid)
{
case mxDOUBLE_CLASS:
{
const double *nb_cap = (const double*)mxGetData(mxNc);
const double *rv_cap = (const double*)mxGetData(mxRc);
const double *tsrc_cap = (const double*)mxGetData(mxTcSrc);
const double *tsnk_cap = (const double*)mxGetData(mxTcSnk);
do_cut(cid, n, m, sv, tv, nb_cap, rv_cap, tsrc_cap, tsnk_cap, plhs);
}
break;
case mxSINGLE_CLASS:
{
const float *nb_cap = (const float*)mxGetData(mxNc);
const float *rv_cap = (const float*)mxGetData(mxRc);
const float *tsrc_cap = (const float*)mxGetData(mxTcSrc);
const float *tsnk_cap = (const float*)mxGetData(mxTcSnk);
do_cut(cid, n, m, sv, tv, nb_cap, rv_cap, tsrc_cap, tsnk_cap, plhs);
}
break;
case mxINT32_CLASS:
{
const int *nb_cap = (const int*)mxGetData(mxNc);
const int *rv_cap = (const int*)mxGetData(mxRc);
const int *tsrc_cap = (const int*)mxGetData(mxTcSrc);
const int *tsnk_cap = (const int*)mxGetData(mxTcSnk);
do_cut(cid, n, m, sv, tv, nb_cap, rv_cap, tsrc_cap, tsnk_cap, plhs);
}
break;
case mxUINT32_CLASS:
{
const unsigned int *nb_cap = (const unsigned int*)mxGetData(mxNc);
const unsigned int *rv_cap = (const unsigned int*)mxGetData(mxRc);
const unsigned int *tsrc_cap = (const unsigned int*)mxGetData(mxTcSrc);
const unsigned int *tsnk_cap = (const unsigned int*)mxGetData(mxTcSnk);
do_cut(cid, n, m, sv, tv, nb_cap, rv_cap, tsrc_cap, tsnk_cap, plhs);
}
break;
default:
mexErrMsgIdAndTxt("kolmogorov_mincut:invalidarg",
"The capacity type is unsupported.");
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
double *C, *B;
double *ref_values, *ref_dx, *ref_dy;
double* weights;
double* betaplane;
mwSignedIndex quadPoints;
int i;
/* check input. */
if (nrhs != 6) {
mexErrMsgIdAndTxt("ARGYRISPACK:apMatrixBetaplaneMex",
"Requires six arguments.");
}
if ((21 != mxGetN(prhs[0])) || (21 != mxGetM(prhs[0]))) {
mexErrMsgIdAndTxt("ARGYRISPACK:apMatrixBetaplaneMex",
"The C matrix must be the first argument.");
}
if ((2 != mxGetN(prhs[1])) || (2 != mxGetM(prhs[1]))) {
mexErrMsgIdAndTxt("ARGYRISPACK:apMatrixBetaplaneMex",
"The B matrix must be the second argument.");
}
for (i = 2; i < 5; i++) {
if (mxGetN(prhs[i]) != mxGetN(prhs[2])) {
mexErrMsgIdAndTxt("ARGYRISPACK:apMatrixBetaplaneMex",
"Mismatch in number of quadrature "
"points.");
}
if (mxGetM(prhs[i]) != 21) {
mexErrMsgIdAndTxt("ARGYRISPACK:apMatrixBetaplaneMex",
"There should be 21 basis functions "
"corresponding to 21 rows.");
}
}
if (mxGetN(prhs[2]) != mxGetN(prhs[5]) &&
mxGetN(prhs[2]) != mxGetM(prhs[5])) {
mexErrMsgIdAndTxt("ARGYRISPACK:apMatrixBetaplaneMex",
"Mismatch in number of weights "
"and number of quadrature points.");
}
if (mxGetM(prhs[5]) != 1 && mxGetN(prhs[5]) != 1) {
mexErrMsgIdAndTxt("ARGYRISPACK:apMatrixBetaplaneMex",
"The weights must be in a one-dimensional array.");
}
C = mxGetPr(prhs[0]);
B = mxGetPr(prhs[1]);
ref_values = mxGetPr(prhs[2]);
ref_dx = mxGetPr(prhs[3]);
ref_dy = mxGetPr(prhs[4]);
weights = mxGetPr(prhs[5]);
quadPoints = mxGetN(prhs[2]);
/* check output. */
if (nlhs != 1) {
mexErrMsgIdAndTxt("ARGYRISPACK:apMatrixBetaplaneMex",
"Requires one output.");
}
plhs[0] = mxCreateDoubleMatrix(21, 21, mxREAL);
betaplane = mxGetPr(plhs[0]);
ap_matrix_betaplane(C, B, ref_values, ref_dx, ref_dy, weights,
quadPoints, betaplane);
}
void
mexFunction(
int nout,
mxArray* pout[],
int nin,
const mxArray* pin[])
{
/****************************************************************
* Check Inputs
*/
if ( nin != nI )
mexErrMsgIdAndTxt("MultiLabelSubModular_mex:n_inputs", "Must have %d inputs", nI);
if ( nout != nO )
mexErrMsgIdAndTxt("MultiLabelSubModular_mex:n_outputs", "Must have %d outputs", nO);
// check unary term
if ( !mxIsDouble(pin[iD]) || mxIsComplex(pin[iD]) ||
mxIsSparse(pin[iD]) || mxGetNumberOfDimensions(pin[iD])!=2 )
mexErrMsgIdAndTxt("MultiLabelSubModular_mex:D_mat", "D must be 2D double real matrix");
mwSize L = mxGetM(pin[iD]);
mwSize N = mxGetN(pin[iD]);
double* pD = mxGetPr(pin[iD]);
// check W
if ( !mxIsDouble(pin[iW]) || !mxIsComplex(pin[iW]) || ! mxIsSparse(pin[iW]) ||
mxGetNumberOfDimensions(pin[iW])!=2 || mxGetM(pin[iW]) != N || mxGetN(pin[iW]) != N )
mexErrMsgIdAndTxt("MultiLabelSubModular_mex:W_mat",
"W must be sparse %dx%d matrix imaginary matrix", N, N);
// check V
if ( !mxIsDouble(pin[iV]) || mxIsComplex(pin[iV]) || mxIsSparse(pin[iV]) ||
mxGetNumberOfDimensions(pin[iV]) < 2 || mxGetNumberOfDimensions(pin[iV]) > 3)
mexErrMsgIdAndTxt("MultiLabelSubModular_mex:V_arr","V array must be real 3D double array");
const mwSize* dim_v = mxGetDimensions(pin[iV]);
// how many V matrices are there?
mwIndex NumV(1);
if ( mxGetNumberOfDimensions(pin[iV]) == 3 ) {
NumV = dim_v[2];
}
if ( dim_v[0] != L || dim_v[1] != L )
mexErrMsgIdAndTxt("MultiLabelSubModular_mex:V_arr_size", "V must be %dx%dxK array", L, L);
double* pV = mxGetPr(pin[iV]);
/****************************************************************
* construct the graph
*/
double* pWij= mxGetPr(pin[iW]);
double* pVi = mxGetPi(pin[iW]);
mwIndex* pir = mxGetIr(pin[iW]);
mwIndex* pjc = mxGetJc(pin[iW]);
mwSize E = pjc[N]; // number of non zeros is the last element in Jc
const double INF(1e100); // or other very large number
// allocate space for graph
typedef Graph<double, double, double> dGraph;
dGraph* gp =
new dGraph( N*(L-1),/* number of nodes excluding s/t */
N*(L-2) + E*(L-1)*(L-1),/* number of edges */
my_err_function /* error function handle */ );
gp->add_node(N*(L-1)); // add all nodes
/****************************************************************
* Unary term + all associated edges
*/
mwSize edgeCounter(0);
for ( mwIndex ii(0) ; ii < N ; ii++ ) {
for ( mwIndex li(0) ; li < L-1 ; li++ ) {
double qrk(0);
// all neighbors of ii - a column in W
for ( mwIndex jj = pjc[ii] ; // starting row index
jj < pjc[ii+1] ; // stopping row index
jj++) {
double wij = pWij[jj];
mxAssert( wij > 0 , "weight wij must be positive");
mwIndex vi = static_cast<mwIndex>(pVi[jj]);
mxAssert( vi > 0 && vi <= NumV, "index into v out of range" );
vi--; // convert to 0-based index
qrk += wij * ( pV[L*L*vi + li] // grr'(k,1)
+ pV[L*L*vi + li + L*(L-1)] // grr'(k,|K|)
- pV[L*L*vi + li + 1] // grr'(k+1,1)
- pV[L*L*vi + li + 1 + L*(L-1)] ); // grr'(k+1,|K|)
}
qrk = qrk / 2.0;
qrk += ( pD[ii*L + li] - pD[ii*L + li + 1 ] );
if ( qrk > 0 ) {
gp -> add_tweights( ii*(L-1) + li, qrk, 0);
} else {
gp -> add_tweights( ii*(L-1) + li, 0, -qrk);
}
// adding between states edges
if ( li < L - 2 ) {
gp -> add_edge( ii*(L-1) + li, ii*(L-1) + li + 1, 0, INF );
edgeCounter++;
}
}
}
mxAssert( edgeCounter == N*(L-2) , "wrong number of constraint edges");
edgeCounter = 0;
/****************************************************************
* pair-wise terms
*/
mwIndex lo, hi;
for ( mwIndex ii(0) ; ii < N ; ii++ ) {
for ( mwIndex ri = pjc[ii] ; // starting row index
ri < pjc[ii+1] ; // stopping row index
ri++) {
mwIndex jj = pir[ri];
double wij = pWij[ri];
mxAssert( wij > 0 , "weight wij must be positive");
mwIndex vi = static_cast<mwIndex>(pVi[ri]);
mxAssert( vi > 0 && vi <= NumV, "index into v out of range" );
vi--; // convert to 0-based index
for ( mwIndex li(0) ; li < L-1 ; li++ ) {
for ( mwIndex lj(0); lj < L-1 ; lj++ ) {
double arr(0);
// Assume that the Vm matrices are functions of the lower-
// numbered node in the first dimension (rows) and higher-
// numbered node in the second dimension. Then we must
// transpose indices when ii > ri.
//
// For more details, see MultiLabelSubModularBasic.m.
if (ii <= ri) {
lo = li;
hi = lj;
} else {
lo = lj;
hi = li;
}
arr = wij * ( pV[L*L*vi + lo + L*hi] // grr'(k,k')
+ pV[L*L*vi + lo + 1 + L*(hi+1)] // grr'(k+1,k'+1)
- pV[L*L*vi + lo + 1 + L*hi] // grr'(k+1,k')
- pV[L*L*vi + lo + L*(hi+1)] );// grr'(k,k'+1)
arr = -arr/2;
mxAssert( arr > 0 , "non submodular term?");
gp -> add_edge( ii*(L-1) + li, jj*(L-1) + lj, arr, arr);
edgeCounter++;
}
}
}
}
mxAssert( edgeCounter == E*(L-1)*(L-1), "wrong number of pair-wise edges");
/****************************************************************
* optimize!
*/
double e = gp->maxflow();
/****************************************************************
* read results
*/
// pout[oE] = mxCreateDoubleScalar(e); // output the flow
pout[oX] = mxCreateDoubleMatrix(1, N, mxREAL);
double* pX = mxGetPr(pout[oX]);
for ( mwIndex ii(0) ; ii < N ; ii++ ) {
pX[ii] = static_cast<double>(L);
for ( mwIndex li(0) ; li < L-1 ; li++ ) {
if ( gp->what_segment( ii*(L-1) + li ) == dGraph::SINK ) {
pX[ii] = static_cast<double>(li+1);
break;
}
}
}
/*
pout[oX] = mxCreateDoubleMatrix(N, L-1, mxREAL);
double* pX = mxGetPr(pout[oX]);
for ( mwIndex li(0) ; li < L-1 ; li++ ) {
for ( mwIndex ii(0) ; ii < N ; ii++ ) {
pX[ii + N*li] = static_cast<double>(
gp->what_segment( ii*(L-1) + li ) == dGraph::SINK
);
}
}
*/
delete gp; // clear memory
}
void mexFunction( int nargout, mxArray *varargout[],
int nargin, const mxArray *varargin[])
{
bool xIsDouble;
if(nargin!=1)
mexErrMsgIdAndTxt("Horizon:FastVar:input","1 input required.");
if(nargout!=1)
mexErrMsgIdAndTxt("Horizon:FastVar:output","One output required.");
xIsDouble = mxIsDouble(varargin[0]);
if (!xIsDouble && !mxIsSingle(varargin[0]))
mexErrMsgIdAndTxt("Horizon:FastVar:input","Input x must be a single or double array.");
if (xIsDouble)
{
if(mxIsComplex(varargin[0]))
{
mwSize numElements = mxGetNumberOfElements(varargin[0]);
double meanR = 0;
double meanI = 0;
double var = 0;
double *xInR = mxGetPr(varargin[0]);
double *xInI = mxGetPi(varargin[0]);
double *xr = xInR;
double *xi = xInI;
double n = numElements;
while (n-- > 0)
{
meanR += *xr++;
meanI += *xi++;
}
meanR = meanR / (double) numElements;
meanI = meanI / (double) numElements;
xr = xInR;
xi = xInI;
n = numElements;
while (n-- > 0)
{
double r = fabs(*xr++ - meanR);
double i = fabs(*xi++ - meanI);
var += r*r + i*i;
}
if (numElements > 1)
var = var / (double) (numElements-1);
else
var = var / (double) numElements;
varargout[0] = mxCreateDoubleMatrix(1, 1, mxREAL);
*mxGetPr(varargout[0]) = var;
}
else
{
mwSize numElements = mxGetNumberOfElements(varargin[0]);
double mean = 0;
double var = 0;
double *xIn = mxGetPr(varargin[0]);
double *x = xIn;
double n = numElements;
while (n-- > 0)
mean += *x++;
mean = mean / (double) numElements;
x = xIn;
n = numElements;
while (n-- > 0)
{
double tempX = fabs(*x++ - mean);
var += tempX*tempX;
}
if (numElements > 1)
var = var / (double) (numElements-1);
else
var = var / (double) numElements;
varargout[0] = mxCreateDoubleMatrix(1, 1, mxREAL);
*mxGetPr(varargout[0]) = var;
}
}
else
{
if(mxIsComplex(varargin[0]))
{
mwSize numElements = mxGetNumberOfElements(varargin[0]);
float meanR = 0;
float meanI = 0;
float var = 0;
float *xInR = (float*) mxGetData(varargin[0]);
float *xInI = (float*) mxGetImagData(varargin[0]);
float *xr = xInR;
float *xi = xInI;
float n = numElements;
while (n-- > 0)
{
meanR += *xr++;
meanI += *xi++;
}
meanR = meanR / (float) numElements;
meanI = meanI / (float) numElements;
xr = xInR;
xi = xInI;
n = numElements;
while (n-- > 0)
{
float r = fabs(*xr++ - meanR);
float i = fabs(*xi++ - meanI);
var += r*r + i*i;
}
if (numElements > 1)
var = var / (float) (numElements-1);
else
var = var / (float) numElements;
varargout[0] = mxCreateDoubleMatrix(1, 1, mxREAL);
*(float*) mxGetData(varargout[0]) = var;
}
else
{
mwSize numElements = mxGetNumberOfElements(varargin[0]);
float mean = 0;
float var = 0;
float *xIn = (float*) mxGetData(varargin[0]);
float *x = xIn;
float n = numElements;
while (n-- > 0)
mean += *x++;
mean = mean / (float) numElements;
x = xIn;
n = numElements;
while (n-- > 0)
{
float tempX = fabs(*x++ - mean);
var += tempX*tempX;
}
if (numElements > 1)
var = var / (float) (numElements-1);
else
var = var / (float) numElements;
varargout[0] = mxCreateNumericMatrix(1, 1, mxSINGLE_CLASS, mxREAL);
*(float*) mxGetData(varargout[0]) = var;
}
}
}
/* The gateway function */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
int errnum = 0;
char *fname = NULL;
const mxArray *arg;
size_t buflen, nrow, ncol;
ssize_t i, j;
mxClassID category;
sltrace_result_t sr;
double zs=DBL_MAX, ze=-DBL_MAX;
double *om;
int mode = MODE_START_END;
/* check for proper number of arguments */
if(nrhs<1) {
errnum = 1; goto out;
} else {
arg = prhs[0];
category = mxGetClassID(arg);
if(category != mxCHAR_CLASS) {
errnum = 2; goto out;
}
buflen = mxGetNumberOfElements(arg) + 1;
fname = mxCalloc(buflen, sizeof(char));
if(mxGetString(arg, fname, buflen) != 0) {
errnum = 3; goto out;
}
mexPrintf("Analyzing file %s\n", fname);
}
if(nrhs>=4) {
arg = prhs[1];
if(!mxIsDouble(arg) || mxIsComplex(arg) || mxGetNumberOfElements(arg)!=1 ) {
errnum = 4; goto out;
}
mode = (int)mxGetScalar(arg);
arg = prhs[2];
if(!mxIsDouble(arg) || mxIsComplex(arg) || mxGetNumberOfElements(arg)!=1 ) {
errnum = 4; goto out;
}
zs = mxGetScalar(arg);
arg = prhs[3];
if(!mxIsDouble(arg) || mxIsComplex(arg) || mxGetNumberOfElements(arg)!=1 ) {
errnum = 4; goto out;
}
ze = mxGetScalar(arg);
}
/* return value */
if(nlhs>0) {
if(nlhs!=1) {
errnum = 5; goto out;
}
bzero(&sr, sizeof(sr));
if(sltrace(fname, mode, zs, ze, 0.0, &sr)) {
nrow = sr.n;
ncol = 7;
plhs[0] = mxCreateDoubleMatrix((mwSize)nrow, (mwSize)ncol, mxREAL);
om = mxGetPr(plhs[0]);
j = 0; for(i=0; i<nrow; i++) om[i + j * nrow] = sr.xs[i];
j = 1; for(i=0; i<nrow; i++) om[i + j * nrow] = sr.ys[i];
j = 2; for(i=0; i<nrow; i++) om[i + j * nrow] = sr.zs[i];
j = 3; for(i=0; i<nrow; i++) om[i + j * nrow] = sr.xe[i];
j = 4; for(i=0; i<nrow; i++) om[i + j * nrow] = sr.ye[i];
j = 5; for(i=0; i<nrow; i++) om[i + j * nrow] = sr.ze[i];
j = 6; for(i=0; i<nrow; i++) om[i + j * nrow] = sr.sl[i];
/* free memory */
if(sr.xs) free(sr.xs);
if(sr.ys) free(sr.ys);
if(sr.zs) free(sr.zs);
if(sr.xe) free(sr.xe);
if(sr.ye) free(sr.ye);
if(sr.ze) free(sr.ze);
if(sr.sl) free(sr.sl);
}
}
out:
if(fname) mxFree(fname);
switch(errnum) {
case 1:
/* this will return from the function back to MATLAB as well */
mexErrMsgIdAndTxt("TopmetalElectrodeSim:sltrace:nrhs",
"At least the streamline data filename is required");
break;
case 2:
mexErrMsgIdAndTxt("TopmetalElectrodeSim:sltrace:fname",
"First argument is not a filename string");
break;
case 3:
mexErrMsgIdAndTxt("MATLAB:explore:invalidStringArray",
"Could not convert string data");
break;
case 4:
mexErrMsgIdAndTxt("TopmetalElectrodeSim:sltrace:mode",
"mode, zs, ze ... parameters must be scalar");
break;
case 5:
mexErrMsgIdAndTxt("TopmetalElectrodeSim:sltrace:nlhs",
"Can only return one variable");
break;
default:
break;
}
}
/**
* @brief [tree,size] = create_ngrame_tree(texts, N) creates ngram tree from review texts
*
* @param texts Cell array of cell arrays, each filled with unigrams
* @param N Type of grams to generate (2, 3, etc...)
* @return tree Tree structure generated
* @param size Size of the vocabulary
*/
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
// check inputs
if(nrhs != 1 && nrhs != 2)
{
mexErrMsgIdAndTxt( "create_ngram_tree:invalidNumInputs",
"One or two inputs required.");
}
else if (nlhs != 1 && nlhs != 2)
{
mexErrMsgIdAndTxt( "create_ngram_tree:invalidNumOutputs",
"One or two outputs required.");
}
else if(!mxIsCell(prhs[0]))
{
mexErrMsgIdAndTxt( "create_ngram_tree:inputNotStruct",
"Input must be a cell array.");
}
else if ((nrhs==2) && !IS_SCALAR(prhs[1]))
{
mexErrMsgIdAndTxt( "create_ngram_tree:inputNotScalar",
"Second input must be a scalar");
}
TreeNode * tree = 0;
size_t num_observations = mxGetNumberOfElements(prhs[0]);
if (num_observations == 0) {
mexErrMsgIdAndTxt("create_ngram_tree:invalidInput",
"There must be more than 0 observations!");
}
double integral = 2.0; // default is 2 (bigrams)
if (nrhs == 2) {
const double gram_count_input = mxGetScalar(prhs[1]);
if (std::modf(gram_count_input, &integral) != 0.0) {
mexErrMsgIdAndTxt("create_ngram_tree:invalidInput",
"Bigram count must be an integer!");
}
}
const int gram_count = static_cast<int>(integral);
//mexPrintf("Running ngram tree on %i observations\n", num_observations);
//mexPrintf("Building tree...\n");
//mexEvalString("drawnow;"); // force flush of IO
// iterate over observations
for (size_t i=0; i < num_observations; i++)
{
mxArray * cell = mxGetCell(prhs[0], i);
if (!mxIsCell(cell)) {
if (tree) {
delete tree;
}
mexErrMsgIdAndTxt("create_ngram_tree:invalidInput",
"Input must be a cell array.");
}
size_t num_unigrams = mxGetNumberOfElements(cell);
// iterate over unigrams for this observation
std::vector<std::string> unigrams_cleaned;
unigrams_cleaned.reserve(num_unigrams);
for (size_t n=0; n < num_unigrams; n++)
{
mxArray * gram_cell = mxGetCell(cell, n);
if (!mxIsChar(gram_cell)) {
if (tree) {
delete tree;
}
mexErrMsgIdAndTxt( "create_ngram_tree:invalidInput",
"Input cell arrays must contain strings.");
}
char * cstr = mxArrayToString(gram_cell);
// convert ngram to cpp string
std::string unigram_string = std::string(cstr);
mxFree(cstr);
// make lowercase
std::transform(unigram_string.begin(), unigram_string.end(), unigram_string.begin(), ::tolower);
// remove everything except alphanumerics + spaces and tabs
unigram_string.erase(std::remove_if(unigram_string.begin(), unigram_string.end(), clean_predicate), unigram_string.end());
// trim starting and ending whitespace
unigram_string = trim_whitespace(unigram_string);
if (unigram_string.empty()) {
continue;
}
//mexPrintf("extracted unigram: %s\n", unigram_string.c_str());
unigrams_cleaned.push_back(unigram_string);
}
// build bigrams
std::vector <std::string> bigrams;
build_ngrams(bigrams, unigrams_cleaned, gram_count);
// append to tree
for (std::vector <std::string> :: iterator it = bigrams.begin(); it != bigrams.end(); it++) {
if (!tree) {
tree = new TreeNode(*it);
}
tree->append_increment(*it, static_cast<int>(i));
}
}
int col=0;
if (tree) {
tree->assign_columns(col); // lazy - traverse tree to assign column values
}
//mexPrintf("Done building tree, %lu instances.\n", tree->count_observations());
// debug
//std::string left = tree->leftmost_token();
//std::string right = tree->rightmost_token();
//mexPrintf("Leftmost term: %s, rightmost term: %s\n", left.c_str(), right.c_str());
// pass back tree
if (tree)
{
plhs[0] = tree->create_mex_struct();
if (nlhs == 2) {
plhs[1] = mxCreateDoubleScalar((double)tree->count_nodes());
}
// cleanup
delete tree;
}
else
{
// no tree was created, not enough unigrams - pass back logical false
plhs[0] = mxCreateLogicalScalar(false);
if (nlhs == 2) {
plhs[1] = mxCreateDoubleScalar(0);
}
}
}
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
/* Declaration of input and output arguments. */
double *x, *u, **p, *dx, *y, *t;
int i, np, nu, nx;
const mxArray *auxvar = NULL; /* Cell array of additional data. */
if (nrhs < 3) {
mexErrMsgIdAndTxt("IDNLGREY:ODE_FILE:InvalidSyntax",
"At least 3 inputs expected (t, u, x).");
}
/* Determine if auxiliary variables were passed as last input. */
if ((nrhs > 3) && (mxIsCell(prhs[nrhs-1]))) {
/* Auxiliary variables were passed as input. */
auxvar = prhs[nrhs-1];
np = nrhs - 4; /* Number of parameters (could be 0). */
} else {
/* Auxiliary variables were not passed. */
np = nrhs - 3; /* Number of parameters. */
}
/* Determine number of inputs and states. */
nx = mxGetNumberOfElements(prhs[1]); /* Number of states. */
nu = mxGetNumberOfElements(prhs[2]); /* Number of inputs. */
/* Obtain double data pointers from mxArrays. */
t = mxGetPr(prhs[0]); /* Current time value (scalar). */
x = mxGetPr(prhs[1]); /* States at time t. */
u = mxGetPr(prhs[2]); /* Inputs at time t. */
p = mxCalloc(np, sizeof(double*));
for (i = 0; i < np; i++) {
p[i] = mxGetPr(prhs[3+i]); /* Parameter arrays. */
}
/* Create matrix for the return arguments. */
plhs[0] = mxCreateDoubleMatrix(nx, 1, mxREAL);
plhs[1] = mxCreateDoubleMatrix(NY, 1, mxREAL);
dx = mxGetPr(plhs[0]); /* State derivative values. */
y = mxGetPr(plhs[1]); /* Output values. */
/*
Call the state and output update functions.
Note: You may also pass other inputs that you might need,
such as number of states (nx) and number of parameters (np).
You may also omit unused inputs (such as auxvar).
For example, you may want to use orders nx and nu, but not time (t)
or auxiliary data (auxvar). You may write these functions as:
compute_dx(dx, nx, nu, x, u, p);
compute_y(y, nx, nu, x, u, p);
*/
/* Call function for state derivative update. */
compute_dx(dx, t[0], x, u, p, auxvar);
/* Call function for output update. */
compute_y(y, t[0], x, u, p, auxvar);
/* Clean up. */
mxFree(p);
}
void
ann_mex_t::annkFRSearch( // approx fixed-radius kNN search
const mxArray* mxQ, // query points
const mxArray* mxRad, // radius of query ball (a scalar or array of size N)
int k, // number of near neighbors to return
mxArray** mxIdx, // nearest neighbor array (modified)
mxArray** mxDst, // dist to near neighbors (modified)
mxArray** mxInr, // number of points within r of each point
double eps) // error bound
{
index_t j(0);
if ( ! IsGood() )
mexErrMsgIdAndTxt("annmex:annkFRSearch","Class integrity check failed");
// check input point(s)
// dimension of points
if (m_idim != mxGetM(mxQ))
mexErrMsgIdAndTxt("annmex:annkFRSearch","points dimension does not match");
// number of query point(s)
int nqp = mxGetN(mxQ);
if ( k <= 0 )
mexErrMsgIdAndTxt("annmex:annkFRSearch","k must be positive");
if ( k > m_inpoints ) {
// generate warning
mexWarnMsgIdAndTxt("annmex:annkFRSearch","searching for more points than in tree - returning only %d",m_inpoints);
k = m_inpoints;
}
int rad_array_inc = 1;
if ( mxGetNumberOfElements(mxRad) == 1 )
rad_array_inc = 0;
else if ( mxGetNumberOfElements(mxRad) != nqp )
mexErrMsgIdAndTxt("annmex:annkFRSearch","R must be either a scalar or array of size N");
ANNdist* prad = new ANNdist[mxGetNumberOfElements(mxRad)];
GetArrSq<ANNdist>(mxRad, prad);
// allocate space for outputs
*mxIdx = mxCreateNumericMatrix(k, nqp, mxIDX_CLASS, mxREAL);
*mxDst = mxCreateNumericMatrix(k, nqp, mxDIST_CLASS, mxREAL);
*mxInr = mxCreateNumericMatrix(1, nqp, mxINT32_CLASS, mxREAL);
if ( *mxIdx == NULL || *mxDst == NULL || mxInr == NULL)
mexErrMsgIdAndTxt("annmex:annkPriSearch","cannot allocate memory for outputs");
ANNidx * pidx = (ANNidx*)mxGetData(*mxIdx);
ANNdist* pdist= (ANNdist*)mxGetData(*mxDst);
int * pinr = (int*)mxGetData(*mxInr);
ANNpoint pp = (ANNpoint)mxGetData(mxQ);
for ( j = 0 ; j < nqp ; j++ ) {
*pinr = m_pTree->annkFRSearch(pp, prad[rad_array_inc*j], k, pidx, pdist, eps);
pp += m_idim;
pidx += k;
pdist += k;
pinr ++;
}
delete [] prad;
}
mxArray* myGetProperty(const mxArray* pobj, const char* propname)
{
mxArray* pm = mxGetProperty(pobj,0,propname);
if (!pm) mexErrMsgIdAndTxt("DRC:ControlUtil:BadInput","ControlUtil is trying to load object property '%s', but failed.", propname);
return pm;
}
mxArray* getCameraProperty(const EdsCameraRef handle, \
const CAMERA_PROPERTY property) {
switch(property) {
case CAMERA_AEMODE: {
EdsUInt32 AEMode;
EdsError errorCode = EdsGetPropertyData(handle,
kEdsPropID_AEMode, 0,
sizeof(AEMode),
&AEMode);
if (errorCode != EDS_ERR_OK) {
handleErrorCode(errorCode);
}
mxArray *mxarr = mxCreateDoubleScalar(AEModeEdsToDouble(AEMode));
return mxarr;
}
case CAMERA_DRIVEMODE: {
EdsUInt32 driveMode;
EdsError errorCode = EdsGetPropertyData(handle,
kEdsPropID_DriveMode, 0,
sizeof(driveMode),
&driveMode);
if (errorCode != EDS_ERR_OK) {
handleErrorCode(errorCode);
}
mxArray *mxarr = mxCreateDoubleScalar(driveModeEdsToDouble(driveMode));
return mxarr;
}
case CAMERA_IMAGEQUALITY: {
EdsUInt32 imageQuality;
EdsError errorCode = EdsGetPropertyData(handle,
kEdsPropID_ImageQuality, 0,
sizeof(imageQuality),
&imageQuality);
if (errorCode != EDS_ERR_OK) {
handleErrorCode(errorCode);
}
mxArray *mxarr = mxCreateDoubleScalar(imageQualityEdsToDouble(imageQuality));
return mxarr;
}
case CAMERA_ISOSPEED: {
EdsUInt32 isoSpeed;
EdsError errorCode = EdsGetPropertyData(handle,
kEdsPropID_ISOSpeed, 0,
sizeof(isoSpeed),
&isoSpeed);
if (errorCode != EDS_ERR_OK) {
handleErrorCode(errorCode);
}
mxArray *mxarr = mxCreateDoubleScalar(isoSpeedEdsToDouble(isoSpeed));
return mxarr;
}
case CAMERA_APERTUREVALUE: {
EdsUInt32 apertureValue;
EdsError errorCode = EdsGetPropertyData(handle,
kEdsPropID_Av, 0,
sizeof(apertureValue),
&apertureValue);
if (errorCode != EDS_ERR_OK) {
handleErrorCode(errorCode);
}
mxArray *mxarr = mxCreateDoubleScalar(apertureValueEdsToDouble(apertureValue));
return mxarr;
}
case CAMERA_SHUTTERSPEED: {
EdsUInt32 shutterSpeed;
EdsError errorCode = EdsGetPropertyData(handle,
kEdsPropID_Tv, 0,
sizeof(shutterSpeed),
&shutterSpeed);
if (errorCode != EDS_ERR_OK) {
handleErrorCode(errorCode);
}
mxArray *mxarr = mxCreateDoubleScalar(shutterSpeedEdsToDouble(shutterSpeed));
return mxarr;
}
case CAMERA_EVFOUTPUTDEVICE: {
EdsUInt32 evfOutputDevice;
EdsError errorCode = EdsGetPropertyData(handle,
kEdsPropID_Evf_OutputDevice, 0,
sizeof(evfOutputDevice),
&evfOutputDevice);
if (errorCode != EDS_ERR_OK) {
handleErrorCode(errorCode);
}
mxArray *mxarr = mxCreateDoubleScalar(evfOutputDeviceEdsToDouble(evfOutputDevice));
return mxarr;
}
case CAMERA_EVFMODE: {
EdsUInt32 evfMode;
EdsError errorCode = EdsGetPropertyData(handle,
kEdsPropID_Evf_Mode, 0,
sizeof(evfMode),
&evfMode);
if (errorCode != EDS_ERR_OK) {
handleErrorCode(errorCode);
}
mxArray *mxarr = mxCreateDoubleScalar(evfModeEdsToDouble(evfMode));
return mxarr;
}
case CAMERA_SAVETO: {
EdsUInt32 saveTo;
EdsError errorCode = EdsGetPropertyData(handle,
kEdsPropID_SaveTo, 0,
sizeof(saveTo),
&saveTo);
if (errorCode != EDS_ERR_OK) {
handleErrorCode(errorCode);
}
mxArray *mxarr = mxCreateDoubleScalar(saveToEdsToDouble(saveTo));
return mxarr;
}
case CAMERA_AVAILABLESHOTS: {
EdsUInt32 availableShots;
EdsError errorCode = EdsGetPropertyData(handle,
kEdsPropID_AvailableShots, 0,
sizeof(availableShots),
&availableShots);
if (errorCode != EDS_ERR_OK) {
handleErrorCode(errorCode);
}
mxArray *mxarr = mxCreateDoubleScalar(availableShotsEdsToDouble(availableShots));
return mxarr;
}
default: {
char propertyName[canon_MAX_STRING_LENGTH];
cameraPropertyToString(property, propertyName);
mexErrMsgIdAndTxt(ERROR_ID, "Unknown or unsupported camera property: %s.", propertyName);
return NULL;
}
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
// Input: Board, center, position, velocity
const mxArray *lookahead, *board_input, *center, *mxx, *mxv;
int x, y, vx, vy, lookahead_int;
// output: new velocity, dTheta
mxArray *newv, *dTheta;
int i,j;
int v1x, v1y;
if (nrhs != 5)
mexErrMsgIdAndTxt("optimalDirection", "Input arguments: board, center, initial positioni, initial velocity and lookahead.");
if (nlhs != 2)
mexErrMsgIdAndTxt("optimalDirection", "Two output arguments: acceleration and change of theta");
lookahead = prhs[0];
board_input = prhs[1];
center = prhs[2];
mxx = prhs[3];
mxv = prhs[4];
boardN = mxGetN(board_input); // Number of columns
boardM = mxGetM(board_input); // Number of rows
board = mxGetData(board_input);
double (*center_p)[2] = mxGetData(center);
center_x = (int) (*center_p)[0];
center_y = (int) (*center_p)[1];
//mexPrintf("center: %i, %i\n", center_x,center_y);
//printf("%i\n",collision(center_x,center_y));
double (*mxx_p)[2] = mxGetData(mxx);
double (*mxv_p)[2] = mxGetData(mxv);
x = (int) (*mxx_p)[0];
y = (int) (*mxx_p)[1];
vx = (int) (*mxv_p)[0];
vy = (int) (*mxv_p)[1];
//mexPrintf("x,y: %i, %i\n", x,y);
//printf("%i\n",collision(x,y));
double *lookahead_double = mxGetData(lookahead);
lookahead_int = (int) *lookahead_double;
initial_lookahead = lookahead_int;
//mexPrintf("%i\n", collision(42,31));
optimalDirection(lookahead_int, x, y, vx, vy, &v1x, &v1y);
// Give matlab the output:
newv = mxCreateDoubleMatrix(2,1,0);
dTheta = mxCreateDoubleMatrix(1,1,0);
double (*newv_p)[2] = mxGetData(newv);
(*newv_p)[0] = v1x;
(*newv_p)[1] = v1y;
double *dTheta_p = mxGetData(dTheta);
*dTheta_p = angledist(x,y,x+v1x,y+v1y);
if (collision(x+v1x,y+v1y)) {
*dTheta_p -= M_PI_4;
}
//mexPrintf("vnew = %i, %i\n", v1x, v1y);
plhs[0] = newv;
plhs[1] = dTheta;
}
