void mexFunction( int nlhs, mxArray *plhs[] , int nrhs, const mxArray *prhs[] )
{
bool * front;
double * M;
unsigned int row, col;
const mwSize *dims;
if(nrhs == 0 || nlhs > 1)
{
printf("\nsynopsis: front = paretofront(X)");
plhs[0] = mxCreateDoubleMatrix(0 , 0 , mxREAL);
return;
}
M = mxGetPr(prhs[0]);
dims = mxGetDimensions(prhs[0]);
row = dims[0];
col = dims[1];
/* ----- output ----- */
plhs[0] = mxCreateLogicalMatrix (row , 1);
front = (bool *) mxGetPr(plhs[0]);
/* main call */
paretofront(front, M, row, col);
}
void MxArray::fromVector(const std::vector<bool>& v)
{
p_ = mxCreateLogicalMatrix(1, v.size());
if (!p_)
mexErrMsgIdAndTxt("mexopencv:error", "Allocation error");
std::copy(v.begin(), v.end(), mxGetLogicals(p_));
}
void save_output(mxArray **dst, void *src, int len) {
double *out_real, *out_imag;
char *out_bit;
int i;
float *f_src = src;
_Complex float *c_src = src;
char *b_src = src;
if (output_sample_sz == sizeof(_Complex float)) {
*dst = mxCreateDoubleMatrix(1, len, mxCOMPLEX);
out_real = mxGetPr(*dst);
out_imag = mxGetPi(*dst);
for (i=0;i<len;i++) {
out_real[i] = (double) __real__ c_src[i];
out_imag[i] = (double) __imag__ c_src[i];
}
} else if (output_sample_sz == sizeof(float)) {
*dst = mxCreateDoubleMatrix(1, len, mxREAL);
out_real = mxGetPr(*dst);
for (i=0;i<len;i++) {
out_real[i] = (double) f_src[i];
}
} else if (output_sample_sz == sizeof(char)) {
*dst = mxCreateLogicalMatrix(1, len);
out_bit = (char*) mxGetData(*dst);
for (i=0;i<len;i++) {
out_bit[i] = b_src[i];
}
}
}
mxArray *mxCreateLogicalScalar(mxLogical value)
{
mxArray *ptr = mxCreateLogicalMatrix(1, 1);
((types::Bool *)ptr)->set(0, value);
return ptr;
}
/*
PsychSetStructArrayBooleanElement()
Note: The variable "index" is zero-indexed.
*/
void PsychSetStructArrayBooleanElement( char *fieldName,
int index,
boolean state,
PsychGenericScriptType *pStruct)
{
int fieldNumber, numElements;
boolean isStruct;
mxArray *mxFieldValue;
char errmsg[256];
//check for bogus arguments
numElements=mxGetM(pStruct) *mxGetN(pStruct);
if(index>=numElements)
PsychErrorExitMsg(PsychError_internal, "Attempt to set a structure field at an out-of-bounds index");
fieldNumber=mxGetFieldNumber(pStruct, fieldName);
if(fieldNumber==-1) {
sprintf(errmsg, "Attempt to set a non-existent structure name field: %s", fieldName);
PsychErrorExitMsg(PsychError_internal, errmsg);
}
isStruct= mxIsStruct(pStruct);
if(!isStruct)
PsychErrorExitMsg(PsychError_internal, "Attempt to set a field within a non-existent structure.");
//do stuff
mxFieldValue=mxCreateLogicalMatrix(1, 1);
mxGetLogicals(mxFieldValue)[0]= state;
mxSetField(pStruct, index, fieldName, mxFieldValue);
if (PSYCH_LANGUAGE == PSYCH_OCTAVE) mxDestroyArray(mxFieldValue);
}
mxArray *emlrtMexFcnProperties(void)
{
const char *mexProperties[] = {
"Version",
"ResolvedFunctions",
"EntryPoints"};
const char *epProperties[] = {
"Name",
"NumberOfInputs",
"NumberOfOutputs",
"ConstantInputs"};
mxArray *xResult = mxCreateStructMatrix(1,1,3,mexProperties);
mxArray *xEntryPoints = mxCreateStructMatrix(1,1,4,epProperties);
mxArray *xInputs = NULL;
xInputs = mxCreateLogicalMatrix(1, 6);
mxSetFieldByNumber(xEntryPoints, 0, 0, mxCreateString("naivePerfusionSSEP2X4"));
mxSetFieldByNumber(xEntryPoints, 0, 1, mxCreateDoubleScalar(6));
mxSetFieldByNumber(xEntryPoints, 0, 2, mxCreateDoubleScalar(1));
mxSetFieldByNumber(xEntryPoints, 0, 3, xInputs);
mxSetFieldByNumber(xResult, 0, 0, mxCreateString("8.1.0.604 (R2013a)"));
mxSetFieldByNumber(xResult, 0, 1, (mxArray*)emlrtMexFcnResolvedFunctionsInfo());
mxSetFieldByNumber(xResult, 0, 2, xEntryPoints);
return xResult;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
region_container* p = NULL;
if( nrhs < 3 ) mexErrMsgTxt("One vector and two integer arguments required.");
if( nlhs != 1 ) mexErrMsgTxt("Exactly one output argument required.");
if (!MEX_TEST_VECTOR(0) || !MEX_TEST_DOUBLE(0)) mexErrMsgTxt("First argument must be a vector of type double");
region_clear_flags(REGION_LEGACY_RASTERIZATION);
if (nrhs > 3) {
char* codestr = get_string(prhs[3]);
if (strcmpi(codestr, "legacy") == 0)
region_set_flags(REGION_LEGACY_RASTERIZATION);
free(codestr);
}
int width = getSingleInteger(prhs[1]);
int height = getSingleInteger(prhs[2]);
p = get_polygon(prhs[0]);
float* tmp = p->data.polygon.x; p->data.polygon.x = p->data.polygon.y; p->data.polygon.y = tmp;
plhs[0] = mxCreateLogicalMatrix(height, width);
char *result = (char*) mxGetData(plhs[0]);
region_mask(p, result, width, height);
if (p) region_release(&p);
}
mxArray *emlrtMexFcnProperties()
{
const char *mexProperties[] = {
"Version",
"ResolvedFunctions",
"EntryPoints",
"CoverageInfo",
NULL };
const char *epProperties[] = {
"Name",
"NumberOfInputs",
"NumberOfOutputs",
"ConstantInputs" };
mxArray *xResult = mxCreateStructMatrix(1,1,4,mexProperties);
mxArray *xEntryPoints = mxCreateStructMatrix(1,1,4,epProperties);
mxArray *xInputs = NULL;
xInputs = mxCreateLogicalMatrix(1, 2);
mxSetFieldByNumber(xEntryPoints, 0, 0, mxCreateString("dummy"));
mxSetFieldByNumber(xEntryPoints, 0, 1, mxCreateDoubleScalar(2));
mxSetFieldByNumber(xEntryPoints, 0, 2, mxCreateDoubleScalar(1));
mxSetFieldByNumber(xEntryPoints, 0, 3, xInputs);
mxSetFieldByNumber(xResult, 0, 0, mxCreateString("8.5.0.197613 (R2015a)"));
mxSetFieldByNumber(xResult, 0, 1, (mxArray*)emlrtMexFcnResolvedFunctionsInfo());
mxSetFieldByNumber(xResult, 0, 2, xEntryPoints);
return xResult;
}
/* main matab function */
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) {
/* b = fast_inseg( seg, t ) */
/* current t in current seg? start while block until out of seg, then next seg
* current t before seg start? binsearch for next t in seg, start while block, next seg
* current t after seg end? go to next seg
*/
double *seg, *t;
mxLogical *pout;
int s, current_t, idx;
int nseg, nt;
if (nrhs!=2)
mexErrMsgIdAndTxt("fast_inseg:invalidArguments", "Expecting two arguments");
if ( mxGetNumberOfDimensions(prhs[0])!=2 || mxGetN(prhs[0])!=2 )
mexErrMsgIdAndTxt("fast_inseg:invalidArguments", "Invalid segments");
seg = mxGetPr( prhs[0] );
t = mxGetPr( prhs[1] );
nseg = mxGetM( prhs[0] );
nt = mxGetNumberOfElements(prhs[1]);
plhs[0] = mxCreateLogicalMatrix( nt, 1 );
if (nseg==0 || nt==0)
return;
pout = mxGetLogicals( plhs[0] );
current_t = 0;
for (s=0; s<nseg; s++) {
if ( t[current_t]<seg[s] ) {
/* binsearch for next t in seg */
idx = (int) ceil( bsearchi( &(t[current_t]), nt-current_t, seg[s] ) );
if (idx==-1)
break;
current_t += idx;
} else if ( t[current_t]>seg[s+nseg] ) {
continue;
}
while ( t[current_t]<=seg[s+nseg] && current_t<nt ) {
pout[current_t] = 1;
current_t++;
}
if (current_t>=nt)
break;
}
}
void CMatlabInterface::set_bool(bool scalar)
{
mxArray* o=mxCreateLogicalMatrix(1, 1);
if (!o)
SG_ERROR("Couldn't create Logical.\n");
bool* data=(bool*) mxGetData(o);
data[0]=scalar;
set_arg_increment(o);
}
/* ********************************************************** */
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
int mrows, ncols, i, j, jcol;
double *inputD64;
unsigned char *inputU8;
mxLogical *output;
/* ************* ACCESS MATLAB VARIABLES ************* */
mrows = mxGetM(prhs[0]); ncols = mxGetN(prhs[0]);
/* Create an array to hold the output. */
plhs[0] = mxCreateLogicalMatrix(mrows, ncols);
output = mxGetLogicals(plhs[0]);
/* ********************* COMPUTE ********************* */
switch (mxGetClassID(prhs[0])) {
case (mxDOUBLE_CLASS):
inputD64 = (double *) mxGetPr(prhs[0]);
for (j = 0; j < ncols; j++) {
jcol = j*mrows;
output[0+jcol] = (inputD64[0+jcol] != 0);
for (i = 1; i < mrows; i++) {
output[i+jcol] = inputD64[i+jcol] && !inputD64[(i-1)+jcol];
}
}
break;
case(mxUINT8_CLASS):
case(mxLOGICAL_CLASS):
inputU8 = (unsigned char *) mxGetData(prhs[0]);
for (j = 0; j < ncols; j++) {
jcol = j*mrows;
output[0+jcol] = (inputU8[0+jcol] != 0);
for (i = 1; i < mrows; i++) {
output[i+jcol] = inputU8[i+jcol] && !inputU8[(i-1)+jcol];
}
}
break;
default:
mexErrMsgTxt("Invalid Data Type.\n");
}
/* ********************* CLEAN UP ******************** */
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
mxLogical *image, *outImage;
mwSize w, h;
if (nrhs != 1){
mexErrMsgIdAndTxt( "MATLAB:borderFill:wrongInputCount",
"One input (image) expected.");
}
/* The input must be a logical.*/
if(!mxIsLogical(prhs[0])){
mexErrMsgIdAndTxt( "MATLAB:borderFill:inputNotLogical",
"Input must be a logical.");
}
h = mxGetM(prhs[0]);
w = mxGetN(prhs[0]);
image = mxGetLogicals(prhs[0]);
/* Create matrix for the return argument. */
plhs[0] = mxCreateLogicalMatrix(h, w);
outImage = mxGetLogicals(plhs[0]);
/* first and last column */
for (int y = 0; y < h; y++){
if (!outImage[y]){
fill(image, h, w, y, 0, image[y], outImage);
}
if (!outImage[h * (w - 1) + y]){
fill(image, h, w, y, w - 1, image[h * (w - 1) + y], outImage);
}
}
/* first and last row */
for (int x = 1; x < w - 1; x++){
if (!outImage[x * h]){
fill(image, h, w, 0, x, image[x * h], outImage);
}
if (!outImage[x * h + h - 1]){
fill(image, h, w, h - 1, x, image[x * h + h - 1], outImage);
}
}
}
void mexFunction(int nlhs_m, mxArray *plhs_m[], int nrhs_m, const mxArray *prhs_m[])
{
if (nlhs_m > 1)
{
printf("Error, only one ouput parameter allowed!\n");
return;
}
if (nrhs_m != 1)
{
printf("Error, only one input parameter allowed!\n");
return;
}
plhs_m[0] = mxCreateLogicalMatrix(1,1);
*(mxGetLogicals(plhs_m[0])) = mxIsComplex(prhs_m[0]);
return;
}
void mexFunction(
int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
double *phi;
mxLogical *contour;
int iWidth, iHeight;
/* The input must be a noncomplex scalar double.*/
iHeight = mxGetM(prhs[0]);
iWidth = mxGetN(prhs[0]);
/* Create matrix for the return argument. */
plhs[0] = mxCreateLogicalMatrix(iHeight,iWidth);
/* Assign pointers to each input and output. */
phi = mxGetPr(prhs[0]);
contour = mxGetLogicals(plhs[0]);
for (int i = 0; i < iWidth; i++) {
for (int j = 0; j < iHeight; j++) {
if (phi[i*iHeight+j] >= 0) {
bool bIsNeighbourBelowZero = false;
for (int k = -1; k <= 1; k++) {
for (int l = -1; l <= 1; l++) {
if (i+k >= 0 && i+k < iWidth && j+l >= 0 && j+l < iHeight && phi[(i+k)*iHeight+j+l] < 0) {
if (-phi[(i+k)*iHeight+j+l] < phi[i*iHeight+j]) {
contour[(i+k)*iHeight+j+l] = true;
}
else {
contour[i*iHeight+j] = true;
}
}
}
}
}
}
}
}
void
mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
mxArray *par; /* pointer to array structure */
double *pda; /* polynom data */
mxLogical *pout; /* pointer to output data */
int ma;
int k, Na;
/* check argument pa */
if ( !mxIsCell(prhs[0]) ) {
mexErrMsgTxt("poly_iscwmex : argument must be a cell array.");
}
Na = mxGetNumberOfElements(prhs[0]);
if (!Na) {
mexErrMsgTxt("poly_iscwmex : no input polygons pa.");
}
/* create output array */
plhs[0] = mxCreateLogicalMatrix(1, Na);
pout = (mxLogical *)mxGetData(plhs[0]);
/*
* copy and prepare data
*/
/* pa */
for (k=0; k<Na; k++) {
/* get the next polygon from the cell array */
par = mxGetCell(prhs[0], k); /* ptr to mxArray */
pda = mxGetData(par); /* ptr to a data */
ma = mxGetM(par); /* rows = vertex number */
/* calculate orientation */
pout[k] = (mxLogical)clock_wise(pda, ma);
}
}
mxArray *emlrtMexFcnProperties(void)
{
const char *mexProperties[] = {
"Version",
"EntryPoints"};
const char *epProperties[] = {
"Name",
"NumberOfInputs",
"NumberOfOutputs",
"ConstantInputs"};
mxArray *xResult = mxCreateStructMatrix(1,1,2,mexProperties);
mxArray *xEntryPoints = mxCreateStructMatrix(1,1,4,epProperties);
mxArray *xInputs = NULL;
xInputs = mxCreateLogicalMatrix(1, 6);
mxSetFieldByNumber(xEntryPoints, 0, 0, mxCreateString("drr_SK_Simplified"));
mxSetFieldByNumber(xEntryPoints, 0, 1, mxCreateDoubleScalar(6));
mxSetFieldByNumber(xEntryPoints, 0, 2, mxCreateDoubleScalar(1));
mxSetFieldByNumber(xEntryPoints, 0, 3, xInputs);
mxSetFieldByNumber(xResult, 0, 0, mxCreateString("7.14.0.739 (R2012a)"));
mxSetFieldByNumber(xResult, 0, 1, xEntryPoints);
return xResult;
}
/* Gateway of insidepoly_dblengine */
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[]) {
TYPE *x, *y;
TYPE dPx, dPy;
TYPE *xx, *yy, *xlast;
TYPE *Px1, *Py1, *Px2, *Py2;
TYPE x1, y1, x2, y2;
TYPE a, b, c, tol, onthreshold;
TYPE l2;
TYPE cmin, cmax;
TYPE xmin, xmax, ymin, ymax;
int08 *in, *on, *ii, *oo;
mwSize n, nvertices, npnt;
double *dfirst, *dlast;
mwSignedIndex first, last;
int presorting;
nvertices = mxGetM(PX1);
npnt = mxGetM(X);
x = (TYPE*)mxGetData(X);
y = (TYPE*)mxGetData(Y);
Px1 = (TYPE*)mxGetData(PX1);
Py1 = (TYPE*)mxGetData(PY1);
Px2 = (TYPE*)mxGetData(PX2);
Py2 = (TYPE*)mxGetData(PY2);
IN = mxCreateLogicalMatrix(npnt, 1);
/* in and on is already prefilled with 0 */
in = (int08*)mxGetData(IN);
ON = mxCreateLogicalMatrix(npnt, 1);
on = (int08*)mxGetData(ON);
tol = *(TYPE*)mxGetData(TOL);
/* FIRST and LAST are provided */
presorting = nrhs>=9;
if (presorting)
{
dfirst = mxGetPr(FIRST);
dlast = mxGetPr(LAST);
}
/* Loop on segments */
for (n=0; n<nvertices; n++) {
x1 = *Px1++;
y1 = *Py1++;
x2 = *Px2++;
y2 = *Py2++;
if (presorting) {
/* Remove one to convert Matlab to C zero-based indexing */
first = (mwSignedIndex)(dfirst[n])-1;
last = (mwSignedIndex)(dlast[n])-1;
}
else {
/* Scan all points */
first = 0;
last = npnt-1;
}
if (first>last) continue; /* Nothing to do */
/* Some quantities depend only on the edge */
dPx = x2-x1;
dPy = y2-y1;
a = x1*y2 - x2*y1;
l2 = dPx*dPx + dPy*dPy;
onthreshold = (TYPE)sqrt(l2)*tol;
b = x1*dPy - y1*dPx;
cmin = b-onthreshold;
cmax = b+onthreshold;
/* Initlialize pointers to the begining of the arrays before looping */
xx = x+first;
yy = y+first;
ii = in+first;
oo = on+first;
/* The last pointer scanned by xx */
xlast = x+last;
/* Branching */
if (dPx>0) { /* x2>x1 */
/* Loop on data points */
while (xx<=xlast) {
if (*xx>=x1 && *xx<x2) {
c = *xx*dPy - *yy*dPx;
*ii ^= (c >= a);
*oo |= (c>cmin && c<cmax);
}
xx++;
yy++;
ii++;
oo++;
} /* while */
} else if (dPx<0) { /* x2<x1 */
/* Loop on data points */
while (xx<=xlast) {
if (*xx>=x2 && *xx<x1) {
c = *xx*dPy - *yy*dPx;
*ii ^= (c <= a);
*oo |= (c>cmin && c<cmax);
}
xx++;
yy++;
ii++;
oo++;
} /* while */
}
else { /* (dPx == 0.0) x1==x2 */
xmin = x1-tol;
xmax = x1+tol;
/* Process special cases for on-boundary test */
if (dPy>0.0) /* y2 > y1 */ {
ymin = y1;
ymax = y2;
}
else { /* y2 <= y1 */
ymin = y2;
ymax = y1;
}
/* Loop on data points */
while (xx<=xlast) {
*oo |= (*yy>=ymin && *yy<=ymax && *xx>xmin && *xx<xmax);
xx++;
yy++;
oo++;
} /* while */
} /* if dPx */
} /* for-loop segments*/
return;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray * prhs[]) {
/* compute the infimum in CSFM, provided that the W
* (which is 2 x nPoint x nFrame) is centered
*/
double *wOri, *q, *res;
int i, ii, j, k, l, m, n, p, iFrame, jFrame, nFrame, nPoint;
mxArray *pIsDone;
const mwSize *dim_array;
double *errArray, *quaternionArray ;
char *isDone;
mxArray *pWPairwiseProd, *pTmp2By2, *pRTransposeR, *pDRTransposeR,
*pQuaternionTmp, *pGradient, *pWPrimeWPrimeTranspose;
double *wPairwiseProd, *tmp2By2, *RTransposeR, *dRTransposeR,
*quaternionTmp, *gradient, *wPrimeWPrimeTranspose;
/* Retrieve the useful data */
wOri= mxGetPr(prhs[0]);
dim_array = mxGetDimensions(prhs[0]);
nPoint = dim_array[1];
nFrame = dim_array[2];
/* Create the output data */
plhs[0] = mxCreateDoubleMatrix(nFrame, nFrame, mxREAL);
errArray = mxGetPr(plhs[0]);
plhs[1] = mxCreateDoubleMatrix(4, nFrame * nFrame, mxREAL);
quaternionArray = mxGetPr(plhs[1]);
pIsDone = mxCreateLogicalMatrix(nFrame, nFrame);
isDone = (char*) mxGetPr(pIsDone);
for (iFrame = 0, k = 0; iFrame < nFrame; ++iFrame)
for (jFrame = 0; jFrame < nFrame; ++jFrame, ++k) {
errArray[k] = 0;
if (iFrame == jFrame) {
/* set the rotation to identity */
isDone[k] = 1;
quaternionArray[4 * k + 0] = 1;
for (i = 1; i < 4; ++i)
quaternionArray[4 * k + i] = 0;
} else
isDone[k] = 0;
}
/* create the pairwise W multiplications between frames */
pWPairwiseProd = mxCreateDoubleMatrix(2*nFrame, 2*nFrame, mxREAL);
wPairwiseProd = mxGetPr(pWPairwiseProd);
pTmp2By2 = mxCreateDoubleMatrix(2, 2, mxREAL);
tmp2By2 = mxGetPr(pTmp2By2);
for (i = 0; i < nFrame; ++i )
for (j = 0; j <= i; ++j ) {
matrixMultiply(wOri + i * 2 * nPoint, wOri + j * 2 * nPoint, tmp2By2, 2, nPoint, 2);
for( k = 0; k < 2; ++k )
for( l = 0; l < 2; ++l )
wPairwiseProd[(2*i+k)*2*nFrame + 2*j + l] = tmp2By2[l+2*k];
}
/* symmetrize the matrix */
for (i = 0; i < 2*nFrame; ++i )
for (j = i+1; j < 2*nFrame; ++j )
wPairwiseProd[i*2*nFrame+j] = wPairwiseProd[j*2*nFrame+i];
/* create some cache some temporary matrices */
pRTransposeR = mxCreateDoubleMatrix(4, 4, mxREAL);
RTransposeR = mxGetPr(pRTransposeR);
pDRTransposeR = mxCreateDoubleMatrix(4, 4*4, mxREAL);
dRTransposeR = mxGetPr(pDRTransposeR);
pQuaternionTmp = mxCreateDoubleMatrix(4, 1, mxREAL);
quaternionTmp = mxGetPr(pQuaternionTmp);
pGradient = mxCreateDoubleMatrix(4, 1, mxREAL);
gradient = mxGetPr(pGradient);
pWPrimeWPrimeTranspose = mxCreateDoubleMatrix(4, 4, mxREAL);
wPrimeWPrimeTranspose = mxGetPr(pWPrimeWPrimeTranspose);
/* do everything else */
for (iFrame = 0; iFrame < nFrame; ++iFrame) {
for (jFrame = iFrame+1; jFrame < nFrame; ++jFrame)
gradientDescent(wPrimeWPrimeTranspose, iFrame, jFrame, wPairwiseProd, errArray, quaternionArray, isDone, nFrame, nPoint, RTransposeR, dRTransposeR, quaternionTmp, gradient);
++iFrame;
for (jFrame = nFrame - 1; jFrame > iFrame; --jFrame)
gradientDescent(wPrimeWPrimeTranspose, iFrame, jFrame, wPairwiseProd, errArray, quaternionArray, isDone, nFrame, nPoint, RTransposeR, dRTransposeR, quaternionTmp, gradient);
}
/* do everything else in a different order */
for (jFrame = nFrame-1; jFrame > 0; --jFrame) {
for (iFrame = jFrame+1; iFrame < nFrame; ++iFrame)
gradientDescent(wPrimeWPrimeTranspose, iFrame, jFrame, wPairwiseProd, errArray, quaternionArray, isDone, nFrame, nPoint, RTransposeR, dRTransposeR, quaternionTmp, gradient);
--jFrame;
for (iFrame = nFrame - 1; iFrame > jFrame; --iFrame)
gradientDescent(wPrimeWPrimeTranspose, iFrame, jFrame, wPairwiseProd, errArray, quaternionArray, isDone, nFrame, nPoint, RTransposeR, dRTransposeR, quaternionTmp, gradient);
}
/* Free memory */
mxDestroyArray(pIsDone);
mxDestroyArray(pWPrimeWPrimeTranspose);
mxDestroyArray(pTmp2By2);
mxDestroyArray(pWPairwiseProd);
mxDestroyArray(pRTransposeR);
mxDestroyArray(pDRTransposeR);
mxDestroyArray(pQuaternionTmp);
mxDestroyArray(pGradient);
}
/** 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;
}
/* Gateway function with Matlab */
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[]) {
char *cmd;
/* If no input argument, print out the usage */
if (nrhs == 0) {
mexErrMsgTxt("Usage: messenger('init|listen|respond', extra1, extra2, ...)");
}
/* Get the input command */
if(!(cmd = mxArrayToString(prhs[0]))) {
mexErrMsgTxt("Cannot read the command");
}
/* Initialize a new server session */
if (strcmp(cmd, "init") == 0) {
char *socket_addr;
mxLogical *p;
/* Check if the input format is valid */
if (nrhs != 2) {
mexErrMsgTxt("Missing argument: socket address");
}
if (!(socket_addr = mxArrayToString(prhs[1]))) {
mexErrMsgTxt("Cannot read socket address");
}
plhs[0] = mxCreateLogicalMatrix(1, 1);
p = mxGetLogicals(plhs[0]);
if (!initialized) {
if (!initialize(socket_addr)) {
p[0] = 1;
mexPrintf("Socket created at: %s\n", socket_addr);
} else {
p[0] = 0;
mexErrMsgTxt("Socket creation failed.");
}
} else {
mexErrMsgTxt("One socket has already been initialized.");
}
return;
/* Listen over an existing socket */
} else if (strcmp(cmd, "listen") == 0) {
char *recv_buffer = mxCalloc(BUFLEN, sizeof(char));
int byte_recvd = listen_zmq(recv_buffer, BUFLEN);
while (byte_recvd == -1 && errno == EAGAIN) {
mexCallMATLAB(0, NULL, 0, NULL, "drawnow");
byte_recvd = listen_zmq(recv_buffer, BUFLEN);
}
/* Check if the received data is complete and correct */
if ((byte_recvd > -1) && (byte_recvd <= BUFLEN)) {
plhs[0] = mxCreateString(recv_buffer);
} else if (byte_recvd > BUFLEN){
mexErrMsgTxt("Receiver buffer overflow. Message truncated");
} else {
sprintf(recv_buffer, "Failed to receive a message due to ZMQ error %s", strerror(errno));
mexErrMsgTxt(recv_buffer);
}
return;
/* Send a message out */
} else if (strcmp(cmd, "respond") == 0) {
size_t msglen;
char *msg_out;
mxLogical *p;
/* Check if the input format is valid */
if (nrhs != 2) {
mexErrMsgTxt("Please provide the message to send");
}
msglen = mxGetNumberOfElements(prhs[1]);
msg_out = mxArrayToString(prhs[1]);
plhs[0] = mxCreateLogicalMatrix(1, 1);
p = mxGetLogicals(plhs[0]);
if (msglen == respond(msg_out, msglen)) {
p[0] = 1;
} else {
p[0] = 0;
mexErrMsgTxt("Failed to send message due to ZMQ error");
}
return;
/* Close the socket and context */
} else if (strcmp(cmd, "exit") == 0) {
cleanup();
return;
} else {
mexErrMsgTxt("Unidentified command");
}
}
/* gateway Function */
void mexFunction(int nlhs, mxArray * plhs[]
,int nrhs, const mxArray * prhs[])
{
/* check for proper number of arguments */
/* validate the input values */
/* variable declaration */
/* input variable declaration */
double dX;
double mRows;
double nCols;
double consideringCellsNo;
/* 주의: mxArray 자료형 변수는 mexGetVariablePtr 함수를 이용하여
* 호출함수의 작업공간에 있는 변수들의 포인터만 불러옴 */
const mxArray * mxArray4;
const mxArray * mxArray9;
const mxArray * mxArray10;
const mxArray * mxArray11;
const mxArray * mxArray12;
const mxArray * mxArray13;
const mxArray * mxArray14;
const mxArray * mxArray15;
const mxArray * mxArray16;
const mxArray * mxArray17;
const mxArray * mxArray18;
const mxArray * mxArray19;
const mxArray * mxArray20;
double * mexSortedIndicies;
double * mexSDSNbrIndicies;
double * flood;
double * floodedRegionCellsNo;
double * floodedRegionStorageVolume;
double * bankfullWidth;
double * transportCapacity;
double * bedrockIncision;
double * chanBedSed;
double * sedimentThick;
mxLogical * hillslope;
double * transportCapacityForShallow;
double * bedrockElev;
/* output variable declaration */
double * dSedimentThick;
double * dBedrockElev;
double * dChanBedSed;
double * inputFlux;
double * outputFlux;
double * inputFloodedRegion;
mxLogical * isFilled;
/* create a pointer to the real data in the input matrix */
dX = mxGetScalar(prhs[0]);
mRows = mxGetScalar(prhs[1]);
nCols = mxGetScalar(prhs[2]);
consideringCellsNo = mxGetScalar(prhs[3]);
mxArray4 = mexGetVariablePtr("caller","mexSortedIndicies");
mxArray9 = mexGetVariablePtr("caller","mexSDSNbrIndicies");
mxArray10 = mexGetVariablePtr("caller","flood");
mxArray11 = mexGetVariablePtr("caller","floodedRegionCellsNo");
mxArray12 = mexGetVariablePtr("caller","floodedRegionStorageVolume");
mxArray13 = mexGetVariablePtr("caller","bankfullWidth");
mxArray14 = mexGetVariablePtr("caller","transportCapacity");
mxArray15 = mexGetVariablePtr("caller","bedrockIncision");
mxArray16 = mexGetVariablePtr("caller","chanBedSed");
mxArray17 = mexGetVariablePtr("caller","sedimentThick");
mxArray18 = mexGetVariablePtr("caller","hillslope");
mxArray19 = mexGetVariablePtr("caller","transportCapacityForShallow");
mxArray20 = mexGetVariablePtr("caller","bedrockElev");
mexSortedIndicies = mxGetPr(mxArray4);
mexSDSNbrIndicies = mxGetPr(mxArray9);
flood = mxGetPr(mxArray10);
floodedRegionCellsNo = mxGetPr(mxArray11);
floodedRegionStorageVolume = mxGetPr(mxArray12);
bankfullWidth = mxGetPr(mxArray13);
transportCapacity = mxGetPr(mxArray14);
bedrockIncision = mxGetPr(mxArray15);
chanBedSed = mxGetPr(mxArray16);
sedimentThick = mxGetPr(mxArray17);
hillslope = mxGetLogicals(mxArray18);
transportCapacityForShallow = mxGetPr(mxArray19);
bedrockElev = mxGetPr(mxArray20);
/* prepare output data */
/* create the output matrix */
plhs[0] = mxCreateDoubleMatrix(mRows,nCols,mxREAL);
plhs[1] = mxCreateDoubleMatrix(mRows,nCols,mxREAL);
plhs[2] = mxCreateDoubleMatrix(mRows,nCols,mxREAL);
plhs[3] = mxCreateDoubleMatrix(mRows,nCols,mxREAL);
plhs[4] = mxCreateDoubleMatrix(mRows,nCols,mxREAL);
plhs[5] = mxCreateDoubleMatrix(mRows,nCols,mxREAL);
plhs[6] = mxCreateLogicalMatrix(mRows,nCols);
/* get a pointer to the real data in the output matrix */
dSedimentThick = mxGetPr(plhs[0]);
dBedrockElev = mxGetPr(plhs[1]);
dChanBedSed = mxGetPr(plhs[2]);
inputFlux = mxGetPr(plhs[3]);
outputFlux = mxGetPr(plhs[4]);
inputFloodedRegion = mxGetPr(plhs[5]);
isFilled = mxGetLogicals(plhs[6]);
/* 서브 루틴 수행 */
EstimateDElevByFluvialProcessBySDS(
dSedimentThick,
dBedrockElev,
dChanBedSed,
inputFlux,
outputFlux,
inputFloodedRegion,
isFilled,
dX,
consideringCellsNo,
mexSortedIndicies,
mexSDSNbrIndicies,
flood,
floodedRegionCellsNo,
floodedRegionStorageVolume,
bankfullWidth,
transportCapacity,
bedrockIncision,
chanBedSed,
sedimentThick,
hillslope,
transportCapacityForShallow,
bedrockElev);
}
// [X,isValid] = rayTrace(D,C,vertices,faces);
//
// Inputs:
// D - 3xM matrix of direction vectors (single)
// C - 3x1 vector for camera center (single)
// vertices - 3xN matrix of vertices (single)
// faces - 3xK matrix of triangle face indices (int32)
//
// Outputs:
// X - 3xM matrix of 3D points that intersects the mesh (single)
// isValid - 1xM vector that indicates whether 3D point is valid intersection (logic)
void mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[]) {
if(nrhs != 4) {
mexErrMsgTxt("Error: 4 args. needed.");
return;
}
// Get inputs:
float *D = (float*)mxGetData(prhs[0]);
float *C = (float*)mxGetData(prhs[1]);
float *vertices = (float*)mxGetData(prhs[2]);
int *faces = (int*)mxGetData(prhs[3]);
int Nrays = mxGetN(prhs[0]);
int Nvertices = mxGetN(prhs[2]);
int Nfaces = mxGetN(prhs[3]);
fprintf(stdout,"Nfaces: %d\n",Nfaces);
fflush(stdout);
// Allocate outputs:
plhs[0] = mxCreateNumericMatrix(3,Nrays,mxSINGLE_CLASS,mxREAL);
float *X = (float*)mxGetData(plhs[0]);
plhs[1] = mxCreateLogicalMatrix(1,Nrays);
bool *isValid = (bool*)mxGetData(plhs[1]);
plhs[2] = mxCreateNumericMatrix(1,Nrays,mxINT32_CLASS,mxREAL);
int *faceNdx = (int*)mxGetData(plhs[2]);
// Allocate temporary memory:
float *lambda_num = (float*)mxMalloc(Nfaces*sizeof(float));
int *Uaxis = (int*)mxMalloc(Nfaces*sizeof(int));
int *Vaxis = (int*)mxMalloc(Nfaces*sizeof(int));
int *Waxis = (int*)mxMalloc(Nfaces*sizeof(int));
float *Cu = (float*)mxMalloc(Nfaces*sizeof(float));
float *Cv = (float*)mxMalloc(Nfaces*sizeof(float));
float *bnu = (float*)mxMalloc(Nfaces*sizeof(float));
float *bnv = (float*)mxMalloc(Nfaces*sizeof(float));
float *cnu = (float*)mxMalloc(Nfaces*sizeof(float));
float *cnv = (float*)mxMalloc(Nfaces*sizeof(float));
float *vu = (float*)mxMalloc(Nfaces*sizeof(float));
float *vv = (float*)mxMalloc(Nfaces*sizeof(float));
float *nu = (float*)mxMalloc(Nfaces*sizeof(float));
float *nv = (float*)mxMalloc(Nfaces*sizeof(float));
// Pre-compute quantities on mesh:
PreCompute(vertices,faces,Nfaces,C,Uaxis,Vaxis,Waxis,lambda_num,bnu,bnv,cnu,cnv,Cu,Cv,nu,nv,vu,vv);
// Compute KD-tree:
fprintf(stdout,"Creating KDtree...\n");
fflush(stdout);
int maxDepth = 20;//3;
float bound_min[3] = {INF,INF,INF};
float bound_max[3] = {-INF,-INF,-INF};
int *leaves = (int*)mxMalloc(Nfaces*sizeof(int));
for(int i = 0; i < Nfaces; i++) leaves[i] = i;
for(int i = 0; i < Nvertices; i++) {
for(int j = 0; j < 3; j++) {
if(vertices[j+i*3]<bound_min[j]) bound_min[j] = vertices[j+i*3];
if(vertices[j+i*3]>bound_max[j]) bound_max[j] = vertices[j+i*3];
}
}
// for(int i = 0; i < 3; i++) fprintf(stdout,"Bounds %d: %f %f\n",i,bound_min[i],bound_max[i]);
KDtree *root = CreateKDtree(vertices,faces,bound_min,bound_max,0,leaves,Nfaces,0,maxDepth);
// mxFree(leaves);
fprintf(stdout,"Finished creating KDtree...\n");
fflush(stdout);
int Nleaves = Nfaces;
leaves = (int*)mxMalloc(Nleaves*sizeof(int));
for(int i = 0; i < Nleaves; i++) leaves[i] = i;
// Get 3D points:
float maxLambda;
float min_lambda,max_lambda;
// float *Di;
float Di[3];
int j;
float ll;
int jmax;
for(int i = 0; i < Nrays; i++) {
// Di = D+i*3;
// This was originally written for LH coordinates; change for RH coordinates
Di[0] = -D[i*3];
Di[1] = -D[1+i*3];
Di[2] = -D[2+i*3];
// Get bounds on ray:
// min_lambda = -INF;
max_lambda = -INF;
for(j = 0; j < 3; j++) {
ll = (bound_min[j]-C[j])/Di[j];
if(ll<=0) {
if((C[j]>=bound_min[j])&&(ll>max_lambda)) max_lambda = ll;
// if((C[j]<bound_min[j])&&(ll>min_lambda)) min_lambda = ll;
}
ll = (bound_max[j]-C[j])/Di[j];
if(ll<=0) {
// if((C[j]>=bound_max[j])&&(ll>min_lambda)) min_lambda = ll;
if((C[j]<bound_max[j])&&(ll>max_lambda)) max_lambda = ll;
}
}
min_lambda = 0.0f;
// fprintf(stdout,"Min/max lambda: (%f,%f)\n",min_lambda,max_lambda);
// fflush(stdout);
// Intersect triangles:
maxLambda = IntersectTriangle_KDtree(Di,Uaxis,Vaxis,Waxis,lambda_num,bnu,bnv,cnu,cnv,Cu,Cv,nu,nv,vu,vv,root,min_lambda,max_lambda,C,&jmax);
// maxLambda = IntersectTriangle(Di,Uaxis,Vaxis,Waxis,lambda_num,bnu,bnv,cnu,cnv,Cu,Cv,nu,nv,vu,vv,leaves,Nleaves);
if(maxLambda != -INF) {
isValid[i] = 1;
faceNdx[i] = jmax+1;
for(j = 0; j < 3; j++) X[j+i*3] = -D[j+i*3]*maxLambda+C[j];
}
}
mxFree(leaves);
// Free memory:
mxFree(lambda_num);
mxFree(Uaxis);
mxFree(Vaxis);
mxFree(Waxis);
mxFree(Cu);
mxFree(Cv);
mxFree(bnu);
mxFree(bnv);
mxFree(cnu);
mxFree(cnv);
mxFree(vu);
mxFree(vv);
mxFree(nu);
mxFree(nv);
// Deallocate KDtree:
FreeKDtree(root);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
if ((nrhs == 0) || !mxIsChar(prhs[0])) {
mexErrMsgTxt("BotParamClient: first argument must be command string");
}
if ((nrhs < 2) || !mxIsChar(prhs[1])) {
mexErrMsgTxt("BotParamClient: second argument must be key");
}
std::string command = ::getString(prhs[0]);
std::transform(command.begin(), command.end(), command.begin(), ::tolower);
std::string key = ::getString(prhs[1]);
if ((nrhs > 3) || ((nrhs == 3) && !::isSetCommand(command))) {
mexErrMsgTxt("BotParamClient: too many input arguments");
}
if (isSetCommand(command) && (nrhs != 3)) {
mexErrMsgTxt("BotParamClient: need value argument");
}
BotParam* param = BotParamClient::instance().getUnderlyingBotParam();
if (param == NULL) {
mexErrMsgTxt("BotParamClient: no param client; is server running?");
}
bool hasKey = (0 != bot_param_has_key(param, key.c_str()));
if (!hasKey && !isSetCommand(command) && (command != "haskey")) {
mexErrMsgTxt("BotParamClient: invalid key");
}
if (command == "haskey") {
plhs[0] = mxCreateLogicalMatrix(1,1);
mxLogical* out = mxGetLogicals(plhs[0]);
out[0] = hasKey;
}
else if (command == "subkeys") {
char** subkeysRaw = bot_param_get_subkeys(param, key.c_str());
std::vector<std::string> subkeys;
for (char** subkeyPtr = subkeysRaw; *subkeyPtr != NULL; ++subkeyPtr) {
subkeys.push_back(std::string(*subkeyPtr));
}
bot_param_str_array_free(subkeysRaw);
plhs[0] = mxCreateCellMatrix(1,subkeys.size());
for (size_t i = 0; i < subkeys.size(); ++i) {
mxSetCell(plhs[0], i, mxCreateString(subkeys[i].c_str()));
}
}
else if (command == "getnum") {
int len = bot_param_get_array_len(param, key.c_str());
std::vector<double> vals;
if (len <= 0) {
double val;
if (bot_param_get_double(param, key.c_str(), &val) != 0) {
mexErrMsgTxt("BotParamClient: cannot find numeric");
}
vals.push_back(val);
}
else {
vals.resize(len);
if (bot_param_get_double_array(param, key.c_str(), vals.data(), len) != len) {
mexErrMsgTxt("BotParamClient: non-numeric value(s)");
}
}
plhs[0] = mxCreateDoubleMatrix(1,vals.size(),mxREAL);
double* ptr = mxGetPr(plhs[0]);
for (size_t i = 0; i < vals.size(); ++i) {
ptr[i] = vals[i];
}
}
else if (command == "getbool") {
int len = bot_param_get_array_len(param, key.c_str());
std::vector<bool> vals;
if (len <= 0) {
int val;
if (bot_param_get_boolean(param, key.c_str(), &val) != 0) {
mexErrMsgTxt("BotParamClient: cannot find bool");
}
vals.push_back(val!=0);
}
else {
int valsRaw[len];
if (bot_param_get_boolean_array(param, key.c_str(), valsRaw, len) != len) {
mexErrMsgTxt("BotParamClient: non-boolean value(s)");
}
for (int i = 0; i < len; ++i) {
vals.push_back(valsRaw[i]!=0);
}
}
plhs[0] = mxCreateLogicalMatrix(1,vals.size());
mxLogical* out = mxGetLogicals(plhs[0]);
for (size_t i = 0; i < vals.size(); ++i) {
out[i] = vals[i];
}
}
else if (command == "getstr") {
int len = bot_param_get_array_len(param, key.c_str());
std::vector<std::string> vals;
if (len <= 0) {
char* val = NULL;
if (bot_param_get_str(param, key.c_str(), &val) != 0) {
mexErrMsgTxt("BotParamClient: cannot find string");
}
vals.push_back(std::string(val));
free(val);
}
else {
char** valsRaw = bot_param_get_str_array_alloc(param, key.c_str());
for (char** valsPtr = valsRaw; *valsPtr != NULL; ++valsPtr) {
vals.push_back(std::string(*valsPtr));
}
bot_param_str_array_free(valsRaw);
}
plhs[0] = mxCreateCellMatrix(1,vals.size());
for (size_t i = 0; i < vals.size(); ++i) {
mxSetCell(plhs[0], i, mxCreateString(vals[i].c_str()));
}
}
else if (command == "setstr") {
bot_param::set_t msg = constructSetMessage(param, key);
std::string value;
if (mxIsCell(prhs[2])) {
int len = mxGetNumberOfElements(prhs[2]);
for (int i = 0; i < len; ++i) {
mxArray* cellVal = mxGetCell(prhs[2],i);
if (!mxIsChar(cellVal)) {
mexErrMsgTxt("BotParamClient: invalid string value argument");
}
std::string curVal = ::getString(cellVal);
value += (curVal + std::string(","));
}
if (value.size() > 0) {
value = value.substr(0,value.size()-1);
}
msg.entries[0].is_array = true;
}
else {
if (!mxIsChar(prhs[2])) {
mexErrMsgTxt("BotParamClient: invalid string value argument");
}
value = ::getString(prhs[2]);
}
msg.entries[0].value = value;
BotParamClient::instance().getLcm()->publish("PARAM_SET", &msg);
}
else if (command == "setnum") {
if (!mxIsDouble(prhs[2]) || mxIsComplex(prhs[2])) {
mexErrMsgTxt("BotParamClient: third argument must be real value array");
}
bot_param::set_t msg = constructSetMessage(param, key);
std::string value;
int len = mxGetNumberOfElements(prhs[2]);
double* valArray = mxGetPr(prhs[2]);
for (int i = 0; i < len; ++i) {
std::ostringstream oss;
oss << valArray[i] << ",";
value += oss.str();
}
if (value.size() > 0) {
value = value.substr(0,value.size()-1);
}
msg.entries[0].value = value;
msg.entries[0].is_array = (len > 1);
BotParamClient::instance().getLcm()->publish("PARAM_SET", &msg);
}
else if (command == "print") {
bot_param_write(param, stderr);
}
else {
mexErrMsgTxt("BotParamClient: command must be haskey, subkeys, getnum, getbool, getstr, or setstr");
}
}
void PPDEVWrite(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
int numAddresses = 1, numVals = 8, i, j, result, addrStrLen, port;
uint8_T *data;
mxArray *tmp;
uint64_T val;
mxLogical *out;
uint8_T bitNum, regOffset, value = 0;
unsigned char mask[NUM_REGISTERS] = { 0 }, vals[NUM_REGISTERS] = { 0 }, pos;
bool writing = true;
/* Check number of input arguments */
if (nrhs != 2) {
mexErrMsgTxt("Exactly 2 arguments required.");
}
/* The device number and value must be noncomplex scalar double */
for (i = 0; i < nrhs; i++) {
/* if (!mxIsDouble(prhs[i]) || mxIsComplex(prhs[i]) || ! mxIsScalar(prhs[i])) { */
if (!mxIsDouble(prhs[i]) || mxIsComplex(prhs[i]) || !(mxGetN(prhs[i])==1 && mxGetM(prhs[i])==1)) {
mexErrMsgTxt("Arguments must be noncomplex scalar double.");
}
}
/* Assign pointers to each input */
port = *mxGetPr(prhs[0]);
if (port < 1 || port > MAX_DEVS) {
mexErrMsgTxt("Port number out of range.");
}
port--;
val = *mxGetPr(prhs[1]);
if (val < 0 || val > 255) {
mexErrMsgTxt("Value out of range.");
}
/* Convert value to data/bit matrix */
tmp = mxCreateNumericMatrix(8, 3, mxUINT8_CLASS, mxREAL);
data = mxGetData(tmp);
for (i = 0; i < numVals; i++) {
data[i] = i;
data[i + numVals] = 0;
data[i + 2 * numVals] = getBit(val, i);
}
/* Assign pointers to each output */
if (DEBUG) printf("%d lhs\n",nlhs);
switch (nlhs) {
case 1:
*plhs = mxCreateLogicalMatrix(numVals,numAddresses);
if (*plhs == NULL) {
mexErrMsgTxt("couldn't allocate output");
}
out = mxGetLogicals(*plhs);
break;
case 0:
if (!writing) {
mexErrMsgTxt("exactly 1 output argument required when reading");
}
out = NULL;
break;
default:
mexErrMsgTxt("at most 1 output argument allowed");
break;
}
if (DEBUG) printf("\n\ndata:\n");
/* Convert data matrix for use with doPort */
for (i = 0; i < numVals; i++) {
bitNum = data[i ];
regOffset = data[i+ numVals];
if (writing) {
value = data[i+2*numVals];
}
if (DEBUG) {
printf("\t%d, %d", bitNum, regOffset);
if (writing) printf(" %d", value);
printf("\n");
}
if (bitNum>7 || regOffset>2 || value>1) {
mexErrMsgTxt("bitNum must be 0-7, regOffset must be 0-2, value must be 0-1.");
}
pos = 1<<bitNum;
mask[regOffset] |= pos;
if (value) vals[regOffset] |= pos;
}
if (DEBUG) {
for (j = 0; j < NUM_REGISTERS; j++) {
printf("mask:");
printBits(mask[j]);
if (writing) {
printf(" val:");
printBits(vals[j]);
}
printf("\n");
}
}
i = 0;
if (ppdev_table[port] != NULL) {
doPort(ppdev_table[port]->addr, ppdev_table[port]->parportfd, mask, vals, out, i, data, numVals, writing);
} else {
mexErrMsgTxt("Port not open.");
}
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
mxLogical *image;
mxLogical *path;
bool *track;
double *dist, *idx;
mwSize w, h, size, sizeIdx, i, x, y;
long conn;
double inf = mxGetInf();
double nan = mxGetNaN();
if (nrhs < 2){
mexErrMsgIdAndTxt( "Image:distgeodesic:wrongInputCount",
"At least two input (image, startPoints) expected.");
}
if (nrhs > 3){
mexErrMsgIdAndTxt( "Image:distgeodesic:wrongInputCount",
"Too may input parameter.");
}
if (!mxIsDouble(prhs[1])){
mexErrMsgIdAndTxt( "Image:distgeodesic:wrongInputType",
"Index parameter has to be doubles.");
}
if (nrhs < 3 || mxGetM(prhs[2]) * mxGetN(prhs[2]) == 0){
conn = 6;
}
else {
if (mxIsDouble(prhs[2])){
if (mxGetM(prhs[2]) * mxGetN(prhs[2]) == 1){
conn = (long) mxGetPr(prhs[2])[0];
}
else {
mexErrMsgIdAndTxt( "Image:distgeodesic:wrongInputType",
"Connection parameter has to be a scalar.");
}
}
else {
mexErrMsgIdAndTxt( "Image:distgeodesic:wrongInputType",
"Connection parameter has to be a double.");
}
}
h = mxGetM(prhs[0]);
w = mxGetN(prhs[0]);
size = h*w;
/* The input must be a logical.*/
if (!mxIsLogical(prhs[0])){
image = mxGetLogicals(mxCreateLogicalMatrix(h, w));
double *data = mxGetPr(prhs[0]);
for (int i = 0; i < size; i += 1){
image[i] = (mxLogical) data[i];
}
}
else {
image = mxGetLogicals(prhs[0]);
}
/* Create matrix for the return argument. */
plhs[0] = mxCreateDoubleMatrix(h, w, mxREAL);
dist = mxGetPr(plhs[0]);
track = new bool[size];
for (i = 0; i < size; i += 1){
track[i] = false;
dist[i] = image[i]? inf: nan;
}
idx = mxGetPr(prhs[1]);
sizeIdx = mxGetM(prhs[1]) * mxGetN(prhs[1]);
bool any = false;
for (i = 0; i < sizeIdx; i += 1){
mwSize startIdx = (long) idx[i];
if (idx[i] != startIdx){
mexErrMsgIdAndTxt( "Image:distgeodesic:idxNotIntegral",
"Startindex has to be an integral number.");
}
if (startIdx <= 0 || startIdx > size){
mexErrMsgIdAndTxt( "Image:distgeodesic:idxOutOfRange",
"Startindex is out of range.");
}
track[startIdx - 1] = true;
dist[startIdx - 1] = 0;
any = true;
}
bool *trackCopy = new bool[size];
while (any){
any = false;
for (i = 0; i < size; i += 1){
trackCopy[i] = track[i];
}
for (i = 0; i < size; i += 1){ // cityblock
if (trackCopy[i]){
track[i] = false;
y = i % h;
x = (i - y) / h;
setDist(x - 1, y, i, 1);
setDist(x + 1, y, i, 1);
setDist(x, y - 1, i, 1);
setDist(x, y + 1, i, 1);
}
}
if (conn == 6){ // chessboard
for (i = 0; i < size; i += 1){
if (trackCopy[i]){
y = i % h;
x = (i - y) / h;
setDist(x - 1, y - 1, i, 1);
setDist(x - 1, y + 1, i, 1);
setDist(x + 1, y - 1, i, 1);
setDist(x + 1, y + 1, i, 1);
}
}
}
if (conn == 8){ // quasi-euclidean
for (i = 0; i < size; i += 1){
if (trackCopy[i]){
y = i % h;
x = (i - y) / h;
setDist(x - 1, y - 1, i, M_SQRT2);
setDist(x - 1, y + 1, i, M_SQRT2);
setDist(x + 1, y - 1, i, M_SQRT2);
setDist(x + 1, y + 1, i, M_SQRT2);
}
}
}
}
delete[] trackCopy;
trackCopy = nullptr;
/* free memory */
delete[] track;
track = nullptr;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
bool isError;
thread_policy_flavor_t flavorConstant;
int flavorPolicySize, flavorPolicySizeBytes, kernError;
task_t threadID;
thread_policy_t threadPolicy;
mach_msg_type_number_t policySizeFilled;
boolean_t isDefault, getDefault;
char commandString[COMMAND_STRING_LENGTH];
// for return structure
// ...outer
const char *outerNames[] = {"threadID", "flavor", "policy", "policySize", "policyFillSize", "getDefault", "isDefault"};
int numOuterDims=2, numOuterFields=7;
int outerDims[2]={1,1};
// ...inner
const char *standardNames[] = {"no_data"};
int numInnerFieldsStandard= 1;
/*
const char *extendedNames[] = {"timeshare"};
int numInnerFieldsExtended= 1;
*/
const char *timeConstraintNames[]= {"period", "computation", "constraint", "preemptible"};
int numInnerFieldsTimeConstraint= 4;
const char *precedenceNames[]= {"imporantance"};
int numInnerFieldsPrecedence= 1;
int numInnerDims=2;
int innerDims[2]={1,1};
// ...both
mxArray *tempFieldValue, *innerStruct, *outerStruct;
//get the policy flavor constant specified by the user and the getDefault argument
if(nrhs<2 || nrhs > 2)
mexErrMsgTxt("MachGetPriorityMex requires two input arguments. See help MachGetPriorityMex.");
if(!mxIsChar(prhs[0]))
mexErrMsgTxt("First input argument is not a string. See help MachGetPriorityMex.");
mxGetString(prhs[0], commandString, COMMAND_STRING_LENGTH);
isError=GetFlavorConstantFromFlavorString(commandString, mxGetM(prhs[0]) * mxGetN(prhs[0]), &flavorConstant); //case sensitive.
if(isError)
mexErrMsgTxt("Unrecognized command. See help MachGetPriorityMex.");
if(!(mxIsDouble(prhs[1]) || mxIsLogical(prhs[1])) || mxGetN(prhs[1]) * mxGetM(prhs[1]) != 1)
mexErrMsgTxt("Second argument must be 1x1 logical or double value. See help MachGetPriorityMex.");
if(mxIsLogical(prhs[1]))
getDefault= (boolean_t)mxGetLogicals(prhs[1])[0];
if(mxIsDouble(prhs[1]))
getDefault= (boolean_t)mxGetPr(prhs[1])[0];
//read the priority settings
switch(flavorConstant){
case THREAD_STANDARD_POLICY:
flavorPolicySizeBytes=sizeof(thread_standard_policy_data_t);
flavorPolicySize=THREAD_STANDARD_POLICY_COUNT;
break;
case THREAD_TIME_CONSTRAINT_POLICY:
flavorPolicySizeBytes=sizeof(thread_time_constraint_policy_data_t);
flavorPolicySize=THREAD_TIME_CONSTRAINT_POLICY_COUNT;
break;
case THREAD_PRECEDENCE_POLICY:
flavorPolicySizeBytes=sizeof(thread_precedence_policy_data_t);
flavorPolicySize=THREAD_PRECEDENCE_POLICY_COUNT;
break;
}
threadPolicy=(thread_policy_t)malloc(flavorPolicySizeBytes);
threadID= mach_thread_self();
policySizeFilled=flavorPolicySize;
isDefault=getDefault;
kernError=thread_policy_get(threadID, flavorConstant, threadPolicy, &policySizeFilled, &isDefault);
//create and populate the return structure
outerStruct= mxCreateStructArray(numOuterDims, outerDims, numOuterFields, outerNames);
tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
mxGetPr(tempFieldValue)[0]=(double)threadID;
mxSetField(outerStruct, 0, "threadID", tempFieldValue);
tempFieldValue= mxCreateString(commandString);
mxSetField(outerStruct, 0, "flavor", tempFieldValue);
switch(flavorConstant){
case THREAD_STANDARD_POLICY:
innerStruct= mxCreateStructArray(numInnerDims, innerDims, numInnerFieldsStandard, standardNames);
tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
mxGetPr(tempFieldValue)[0]= (double)((thread_standard_policy_t)threadPolicy)->no_data;
mxSetField(innerStruct, 0, "no_data", tempFieldValue);
break;
/* THREAD_EXTENDED_POLICY is equal to THREAD_STANDARD_POLICY. Also, THREAD_EXTENDED_POLICY is undocumented. So we ignore it.
case THREAD_EXTENDED_POLICY:
innerStruct= mxCreateStructArray(numInnerDims, innerDims, numInnerFieldsExtended, extendedNames);
tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
mxGetPr(tempFieldValue)[0]= (double)((thread_extended_policy_t)threadPolicy)->timeshare;
mxSetField(innerStruct, 1, "timeshare", tempFieldValue);
break;
*/
case THREAD_TIME_CONSTRAINT_POLICY:
innerStruct= mxCreateStructArray(numInnerDims, innerDims, numInnerFieldsTimeConstraint, timeConstraintNames);
tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
mxGetPr(tempFieldValue)[0]= (double)((thread_time_constraint_policy_t)threadPolicy)->period;
mxSetField(innerStruct, 0, "period", tempFieldValue);
tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
mxGetPr(tempFieldValue)[0]= (double)((thread_time_constraint_policy_t)threadPolicy)->computation;
mxSetField(innerStruct, 0, "computation", tempFieldValue);
tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
mxGetPr(tempFieldValue)[0]= (double)((thread_time_constraint_policy_t)threadPolicy)->constraint;
mxSetField(innerStruct, 0, "constraint", tempFieldValue);
tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
mxGetPr(tempFieldValue)[0]= (double)((thread_time_constraint_policy_t)threadPolicy)->preemptible;
mxSetField(innerStruct, 0, "preemptible", tempFieldValue);
break;
case THREAD_PRECEDENCE_POLICY:
innerStruct= mxCreateStructArray(numInnerDims, innerDims, numInnerFieldsPrecedence, precedenceNames);
tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
mxGetPr(tempFieldValue)[0]= (double)((thread_precedence_policy_t)threadPolicy)->importance;
mxSetField(innerStruct, 0, "imporantance", tempFieldValue);
break;
}
mxSetField(outerStruct,0, "policy", innerStruct);
tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
mxGetPr(tempFieldValue)[0]=flavorPolicySize;
mxSetField(outerStruct, 0, "policySize", tempFieldValue);
tempFieldValue= mxCreateDoubleMatrix(1,1,mxREAL);
mxGetPr(tempFieldValue)[0]=policySizeFilled;
mxSetField(outerStruct, 0, "policyFillSize", tempFieldValue);
tempFieldValue= mxCreateLogicalMatrix(1, 1);
mxGetLogicals(tempFieldValue)[0]=(bool)getDefault;
mxSetField(outerStruct, 0, "getDefault", tempFieldValue);
tempFieldValue= mxCreateLogicalMatrix(1, 1);
mxGetLogicals(tempFieldValue)[0]=(bool)isDefault;
mxSetField(outerStruct, 0, "isDefault", tempFieldValue);
plhs[0]=outerStruct;
free((void*)threadPolicy);
}
// Main function
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
// Macros for output
#define ITER plhs[0]
#define TIN_OUT plhs[1]
#define HOLD plhs[2]
#define DHOLD plhs[3]
#define D2HOLD plhs[4]
// Macros for input
#define ITERMAX prhs[0]
#define TIN_IN prhs[1]
#define WBREAK prhs[2]
#define WORDER prhs[3]
#define WCOEFS prhs[4]
#define UIN prhs[5]
#define N_IN prhs[6]
#define SLAMBDA prhs[7]
#define TQIN prhs[8]
#define SCRRNG prhs[9]
#define CRIT prhs[10]
mxArray *WMAT = call_eval_fd(TIN_IN, WFDOBJ, 0);
mxArray *DWMAT = call_eval_fd(TIN_IN, WFDOBJ, 1);
mxArray *D2WMAT = call_eval_fd(TIN_IN, WFDOBJ, 2);
double *wmat = mxGetPr(WMAT);
double *dwmat = mxGetPr(DWMAT);
double *d2wmat = mxGetPr(D2WMAT);
double emat, pmat, rmat;
double *uin = mxGetPr(UIN);
long M = mxGetM(WMAT);
long N = mxGetN(WMAT);
long Nsubjects = (long)mxGetPr(N_IN)[0];
double *tin = mxGetPr(TIN_IN);
double *tqin = mxGetPr(TQIN);
double slambda = mxGetPr(SLAMBDA)[0];
HOLD = mxCreateDoubleMatrix(M, 1, mxREAL);
double *hold = mxGetPr(HOLD);
DHOLD = mxCreateDoubleMatrix(M, 1, mxREAL);
double *dhold = mxGetPr(DHOLD);
D2HOLD = mxCreateDoubleMatrix(M, 1, mxREAL);
double *d2hold = mxGetPr(D2HOLD);
double crit = mxGetPr(CRIT)[0];
long *activenew = (long *)calloc(M, sizeof(long));
long nactivenew = 0;
// Compute initial probability values for interior scores
long m, n;
for (m = 0; m < M; m++) {
// Initial function, first and second derivative values wrt theta
hold[m] = 0.0;
dhold[m] = 0.0;
d2hold[m] = 0.0;
for (n = 0; n < N; n++) {
long index = m + M*n;
emat = exp(wmat[index]);
pmat = emat/(1 + emat);
rmat = uin[index] - pmat;
hold[m] += uin[index] * wmat[index] - log(1 + emat);
dhold[m] += rmat * dwmat[index];
d2hold[m] += ( (-d2wmat[index] * rmat) +
(pow(dwmat[index], 2) * pmat * (1 - pmat)) );
}
hold[m] = -hold[m]/Nsubjects;
dhold[m] = -dhold[m]/Nsubjects;
d2hold[m] = d2hold[m]/Nsubjects;
if (slambda > 0) {
hold[m] += slambda * pow(tin[m] - tqin[m], 2) / Nsubjects;
dhold[m] += (2 * slambda * (tin[m] - tqin[m])) / Nsubjects;
d2hold[m] += 2 * slambda / Nsubjects;
}
// Find cases where gradient size exceeds criterion and further optimization is required
if (fabs(dhold[m]) > crit && d2hold[m] > 0) {
activenew[nactivenew++] = m + 1;
}
}
mxArray *ACTIVE = mxCreateDoubleMatrix(nactivenew, 1, mxREAL);
double *active = mxGetPr(ACTIVE);
mxArray *TINA = mxCreateDoubleMatrix(nactivenew, 1, mxREAL);
double *tina = mxGetPr(TINA);
mxArray *UA = mxCreateDoubleMatrix(nactivenew, N, mxREAL);
double *ua = mxGetPr(UA);
for (m = 0; m < nactivenew; m++) {
active[m] = activenew[m];
// Select active theta's and data
tina[m] = tin[activenew[m] - 1];
for (n = 0; n < N; n++) {
long newindex = m + nactivenew*n;
long index = activenew[m] - 1 + Nsubjects*n;
ua[newindex] = uin[index];
}
}
free(activenew);
// Update scores in active set by a Newton-Raphson step
ITER = mxCreateDoubleMatrix(1, 1, mxREAL);
double *iter = mxGetPr(ITER);
iter[0] = 0;
long itermax = (long)mxGetPr(ITERMAX)[0];
nactivenew = 1;
TIN_OUT = TIN_IN;
while (nactivenew > 0 && iter[0] < itermax) {
iter[0]++;
WMAT = call_eval_fd(TINA, WFDOBJ, NULL);
DWMAT = call_eval_fd(TINA, WFDOBJ, 1);
D2WMAT = call_eval_fd(TINA, WFDOBJ, 2);
wmat = mxGetPr(WMAT);
dwmat = mxGetPr(DWMAT);
d2wmat = mxGetPr(D2WMAT);
M = mxGetM(WMAT);
N = mxGetN(WMAT);
ua = mxGetPr(UA);
double *step = (double *)malloc(M * sizeof(double));
double stepnew;
tina = mxGetPr(TINA);
tqin = mxGetPr(TQIN);
active = mxGetPr(ACTIVE);
double scrmin = mxGetPr(SCRRNG)[0];
double scrmax = mxGetPr(SCRRNG)[1];
tin = mxGetPr(TIN_IN);
mxArray *TINANEW = mxCreateDoubleMatrix(M, 1, mxREAL);
double *tinanew = mxGetPr(TINANEW);
double tinanewfail;
double *hanew = (double *)malloc(M * sizeof(double));
// Initial set of thetas failing to meet criterion
mxArray *FAILINDEX = mxCreateLogicalMatrix(M, 1);
mxLogical *fail = mxGetLogicals(FAILINDEX);
long m, n;
for (m = 0; m < M; m++) {
long activeindex = (long)active[m] - 1;
dhold[activeindex] = 0.0;
d2hold[activeindex] = 0.0;
// Compute probabilites and other quantities for active cases
for (n = 0; n < N; n++) {
long index = m + M*n;
emat = exp(wmat[index]);
pmat = emat/(1 + emat);
rmat = ua[index] - pmat;
dhold[activeindex] += rmat * dwmat[index];
d2hold[activeindex] += ( (-d2wmat[index] * rmat) +
(pow(dwmat[index], 2) * pmat * (1 - pmat)) );
}
dhold[activeindex] = -dhold[activeindex]/Nsubjects;
d2hold[activeindex] = d2hold[activeindex]/Nsubjects;
// Add regularization terms if needed
if (slambda > 0) {
dhold[activeindex] += (2 * slambda * (tina[m] - tqin[activeindex])) / Nsubjects;
d2hold[activeindex] += 2 * slambda / Nsubjects;
}
// Find a step size that reduces all fuction values in the active set
// Computer the negative of initial step for each theta
step[m] = dhold[activeindex] / d2hold[activeindex];
if (step[m] < -2) step[m] = -2;
if (step[m] > 2) step[m] = 2;
// Ensure that initial step does not go to boundaries
if (tina[m] - step[m] <= scrmin) step[m] = tina[m] - scrmin * 1.01;
if (tina[m] - step[m] >= scrmax) step[m] = -(scrmax * 0.99 - tina[m]);
tinanew[m] = tin[activeindex];
hanew[m] = hold[activeindex];
fail[m] = true;
}
// Half fac as required to achieve the reduction of all function values
double fac = 1;
long loopfail = 1;
long stepiter = 0;
while (loopfail > 0 && stepiter < 10){
stepiter++;
loopfail = 0;
long m;
for (m = 0; m < M; m++) {
// Compute current step size
if (fail[m]) {
stepnew = fac * step[m];
tinanewfail = tinanew[m];
if (tinanewfail - stepnew <= scrmin) stepnew = tinanewfail - scrmin * 1.01;
if (tinanewfail - stepnew >= scrmax) stepnew = -(scrmax * 0.99 - tinanewfail);
// Update ability values
tinanew[m] -= stepnew;
}
}
// Compute function values
WMAT = call_eval_fd(TINANEW, WFDOBJ, NULL);
wmat = mxGetPr(WMAT);
N = mxGetN(WMAT);
// Halve fac in preparation for next step size iteration
fac /= 2;
long n;
for (m = 0; m < M; m++){
long activeindex = (long)active[m] - 1;
if (fail[m]){
hanew[m] = 0.0;
for (n = 0; n < N; n++){
long index = m + M*n;
emat = exp(wmat[index]);
// Current vector of function values
hanew[m] -= (ua[index] * wmat[index] - log(1 + emat));
}
hanew[m] /= Nsubjects;
if (slambda > 0) {
hanew[m] += slambda * pow(tinanew[m] - tqin[activeindex], 2) / Nsubjects;
}
}
fail[m] = hold[activeindex] - hanew[m] < -crit;
if (fail[m]) loopfail++;
}
}
WMAT = call_eval_fd(TINANEW, WFDOBJ, NULL);
DWMAT = call_eval_fd(TINANEW, WFDOBJ, 1);
D2WMAT = call_eval_fd(TINANEW, WFDOBJ, 2);
wmat = mxGetPr(WMAT);
dwmat = mxGetPr(DWMAT);
d2wmat = mxGetPr(D2WMAT);
long *activenew = (long *)calloc(M, sizeof(long));
nactivenew = 0;
// Either all function values reduced or 10 halvings completed
// Save current function values
for (m = 0; m < M; m++) {
long activeindex = (long)active[m] - 1;
tin[activeindex] = tinanew[m];
dhold[activeindex] = 0.0;
d2hold[activeindex] = 0.0;
for (n = 0; n < N; n++) {
long index = m + M*n;
emat = exp(wmat[index]);
pmat = emat/(1 + emat);
rmat = ua[index] - pmat;
dhold[activeindex] += rmat * dwmat[index];
d2hold[activeindex] += ( (-d2wmat[index] * rmat) +
(pow(dwmat[index], 2) * pmat * (1 - pmat)) );
}
dhold[activeindex] = -dhold[activeindex]/Nsubjects;
d2hold[activeindex] = d2hold[activeindex]/Nsubjects;
if (slambda > 0) {
dhold[activeindex] += (2 * slambda * (tinanew[m] - tqin[activeindex])) / Nsubjects;
d2hold[activeindex] += 2 * slambda / Nsubjects;
}
hold[activeindex] = hanew[m];
if (fabs(dhold[activeindex]) > crit && d2hold[activeindex] > 0) {
activenew[nactivenew++] = (long)active[m];
}
}
ACTIVE = mxCreateDoubleMatrix(nactivenew, 1, mxREAL);
double *activeout = mxGetPr(ACTIVE);
TINA = mxCreateDoubleMatrix(nactivenew, 1, mxREAL);
double *tina_out = mxGetPr(TINA);
UA = mxCreateDoubleMatrix(nactivenew, N, mxREAL);
double *ua_out = mxGetPr(UA);
double *uin = mxGetPr(UIN);
for (m = 0; m < nactivenew; m++) {
activeout[m] = activenew[m];
tina_out[m] = tin[activenew[m] - 1];
for (n = 0; n < N; n++) {
long newindex = m + nactivenew*n;
long index = activenew[m] - 1 + Nsubjects*n;
ua_out[newindex] = uin[index];
}
}
free(hanew);
free(step);
free(activenew);
}
return;
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
unsigned int maxHoleSize;
mxLogical *image, *fillImage, *outImage;
mwSize w, h;
if (nrhs == 0){
mexErrMsgIdAndTxt( "MATLAB:fill:wrongInputCount",
"At least one input (image) expected.");
}
if (nrhs == 1 || mxGetM(prhs[1]) * mxGetN(prhs[1]) == 0){
maxHoleSize = 5;
}
else {
maxHoleSize = (unsigned int) mxGetScalar(prhs[1]);
}
if (nrhs > 2){
mexErrMsgIdAndTxt( "MATLAB:fill:wrongInputCount",
"Too many input parameter.");
}
/* The input must be a logical.*/
if(!mxIsLogical(prhs[0])){
mexErrMsgIdAndTxt( "MATLAB:fill:inputNotLogical",
"Input must be a logical.");
}
h = mxGetM(prhs[0]);
w = mxGetN(prhs[0]);
image = mxGetLogicals(mxDuplicateArray(prhs[0]));
/* Create matrix for the return argument. */
plhs[0] = mxDuplicateArray(prhs[0]);
outImage = mxGetLogicals(plhs[0]);
fillImage = mxGetLogicals(mxCreateLogicalMatrix(h, w));
for (unsigned int x = 0; x < w; x += 1){
unsigned int x_ = x * h;
for (unsigned int y = 0; y < h; y += 1){
unsigned int idx = x_ + y;
if (!image[idx]){
unsigned int
minX = w,
maxX = 0,
minY = h,
maxY = 0,
count = 0;
floodFill(
image, fillImage,
w, h, x, y,
minX, maxX, minY, maxY,
count
);
if (count <= maxHoleSize){
copyFilling(
fillImage, outImage,
w, h,
minX, maxX, minY, maxY
);
}
copyFilling(
fillImage, image,
w, h,
minX, maxX, minY, maxY
);
clearFilling(
fillImage,
w, h,
minX, maxX, minY, maxY
);
}
}
}
}
int bytesToMx(mxArray **mx, const char *byteArray, int byteArrayLength) {
int ii;
int nInfoBytes=0, nFreeBytes=0, nBytesRead=0;
mxGramInfo info;
// for complex types
int jj;
int nElements;
mxArray *elementData;
mxArray *elementName;
const char *elementByteArray;
int elementBytesRead;
char **fieldNames;
mxArray *callMatlabError;
info.gramBytes = (char *)byteArray;
nInfoBytes = readInfoFieldsFromBytes(&info, byteArray, byteArrayLength);
nBytesRead += nInfoBytes;
info.dataBytes = (char *)byteArray + MX_GRAM_OFFSET_DATA;
nFreeBytes = byteArrayLength - MX_GRAM_OFFSET_DATA;
//printMxGramInfo(&info);
//printBytes(info.gramBytes, info.gramLength);
if (info.gramType==mxGramDouble) {
*mx = mxCreateDoubleMatrix(info.dataM, info.dataN, mxREAL);
nBytesRead += readMxDoubleDataFromBytes(*mx, &info, nFreeBytes);
} else if (info.gramType==mxGramChar) {
mwSize dims[2];
dims[0] = info.dataM;
dims[1] = info.dataN;
*mx = mxCreateCharArray(2, dims);
nBytesRead += readMxCharDataFromBytes(*mx, &info, nFreeBytes);
} else if (info.gramType==mxGramLogical) {
*mx = mxCreateLogicalMatrix(info.dataM, info.dataN);
nBytesRead += readMxLogicalDataFromBytes(*mx, &info, nFreeBytes);
} else if (info.gramType==mxGramCell) {
*mx = mxCreateCellMatrix(info.dataM, info.dataN);
nElements = info.dataM * info.dataN;
elementByteArray = info.dataBytes;
elementBytesRead = 0;
for (ii=0; ii<nElements; ii++) {
elementBytesRead = bytesToMx(&elementData, elementByteArray, nFreeBytes);
if (elementBytesRead > 0) {
mxSetCell(*mx, ii, elementData);
nBytesRead += elementBytesRead;
elementByteArray += elementBytesRead;
nFreeBytes -= elementBytesRead;
}
}
} else if (info.gramType==mxGramStruct) {
// struct arrays may have size 1xn, with m fields
nElements = info.dataM;
fieldNames = mxMalloc(nElements*sizeof(char*));
// recur to read out fieldNames
elementByteArray = info.dataBytes;
for (ii=0; ii<nElements; ii++) {
elementBytesRead = bytesToMx(&elementName, elementByteArray, nFreeBytes);
fieldNames[ii] = mxArrayToString(elementName);
mxDestroyArray(elementName);
nBytesRead += elementBytesRead;
elementByteArray += elementBytesRead;
nFreeBytes -= elementBytesRead;
}
*mx = mxCreateStructMatrix(1, info.dataN, info.dataM, (const char **)fieldNames);
mxFree(fieldNames);
// recur to fill in field data
for (ii=0; ii<nElements; ii++) {
for (jj=0; jj<info.dataN; jj++) {
elementBytesRead = bytesToMx(&elementData, elementByteArray, nFreeBytes);
mxSetFieldByNumber(*mx, jj, ii, elementData);
nBytesRead += elementBytesRead;
elementByteArray += elementBytesRead;
nFreeBytes -= elementBytesRead;
}
}
} else if (info.gramType==mxGramFunctionHandle) {
// recur to read out stringified version of function
elementBytesRead = bytesToMx(&elementData, info.dataBytes, nFreeBytes);
if (elementBytesRead > 0) {
nBytesRead += elementBytesRead;
callMatlabError = mexCallMATLABWithTrap(1, mx, 1, &elementData, MX_GRAM_STRING_TO_FUNCTION);
}
} else {
*mx = mxCreateDoubleScalar(-1);
nBytesRead = 0;
}
return(nBytesRead);
}
void mexFunction(int nlhs, /* number of expected outputs */
mxArray *plhs[], /* mxArray output pointer array */
int nrhs, /* number of inputs */
const mxArray *prhs[] /* mxArray input pointer array */)
{
unsigned int nodes;
unsigned int pairs;
bool *labels;
unsigned int *source;
unsigned int *destination;
float *pairValue;
float *unaryValue;
float *flow;
mxArray *sourceMxArray;
mxArray *destinationMxArray;
mxArray *pairValueMxArray;
/** Input validation */
if( nrhs != 2 )
mexErrMsgTxt( USAGE_NOTIFICATION );
const mxArray *A = prhs[ 0 ];
const mxArray *T = prhs[ 1 ];
if( !mxIsStruct( A ) )
{
mexErrMsgTxt( USAGE_NOTIFICATION );
}
else
{
sourceMxArray = mxGetField( A, 0, "source" );
destinationMxArray = mxGetField( A, 0, "destination" );
pairValueMxArray = mxGetField( A, 0, "value" );
if( mxGetClassID( sourceMxArray ) != mxUINT32_CLASS ||
mxGetClassID( destinationMxArray ) != mxUINT32_CLASS ||
mxGetClassID( pairValueMxArray ) != mxSINGLE_CLASS )
{
mexErrMsgTxt( USAGE_NOTIFICATION );
}
if( mxIsComplex( sourceMxArray ) ||
mxIsComplex( destinationMxArray ) ||
mxIsComplex( pairValueMxArray ) )
{
mexErrMsgTxt( USAGE_NOTIFICATION );
}
pairs = mxGetNumberOfElements( sourceMxArray );
if( pairs != mxGetNumberOfElements( destinationMxArray ) ||
pairs != mxGetNumberOfElements( pairValueMxArray ) )
{
mexErrMsgTxt( USAGE_NOTIFICATION );
}
}
if( mxGetClassID( T ) != mxSINGLE_CLASS )
{
mexErrMsgTxt( USAGE_NOTIFICATION );
}
else
{
if( mxIsComplex( T ) )
mexErrMsgTxt( USAGE_NOTIFICATION );
nodes = mxGetM( T );
if( mxGetN( T ) != 2 )
mexErrMsgTxt( USAGE_NOTIFICATION );
}
/** End of input validation */
source = ( unsigned int * )mxGetData( sourceMxArray );
destination = ( unsigned int * )mxGetData( destinationMxArray );
pairValue = ( float * )mxGetData( pairValueMxArray );
unaryValue = ( float * )mxGetData( T );
typedef Graph< float, float, float > GraphType;
GraphType *graph = new GraphType( nodes, pairs );
graph->add_node( nodes );
/** Add pairwise potentials */
for( int edge = 0; edge < pairs; edge++ )
graph->add_edge( source[ edge ], destination[ edge ], pairValue[ edge ], pairValue[ edge ] );
for( int node = 0; node < nodes; node++ )
graph->add_tweights( node, unaryValue[ node ], unaryValue[ node + nodes ] );
/** Create outputs */
plhs[ 0 ] = mxCreateNumericMatrix( 1, 1, mxSINGLE_CLASS, mxREAL );
plhs[ 1 ] = mxCreateLogicalMatrix( nodes, 1 );
flow = ( float * )mxGetData( plhs[ 0 ] );
labels = ( bool * )mxGetData( plhs[ 1 ] );
*flow = graph->maxflow();
for( int node = 0; node < nodes; node++ )
labels[ node ] = graph->what_segment( node );
delete graph;
}
