/* tz_fcmp(x1, x2) compare two double float numbers.
* tz_fcmp(x1, x2, ep) compare the double float numbers at the accuracy ep,
* which is 1e-5 by default.
*/
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
check_argin(nrhs, prhs);
double epsilon = 1e-5;
if (nrhs == 3) {
epsilon = *mxGetPr(prhs[2]);
}
mwSize length;
mwSize length1 = tz_mxGetL(prhs[0]);
mwSize length2 = tz_mxGetL(prhs[1]);
const mxArray *array = NULL;
if (mxIsScalar(prhs[0]) && mxIsScalar(prhs[1])) {
plhs[0] = mxCreateNumericMatrix(1, 1, mxINT8_CLASS, mxREAL);
length = 1;
} else if (!mxIsScalar(prhs[0]) && !mxIsScalar(prhs[1])) {
plhs[0] = mxCreateNumericArray(mxGetNumberOfDimensions(prhs[0]),
mxGetDimensions(prhs[0]), mxINT8_CLASS,
mxREAL);
length = length1;
} else {
if (length1 == 1) {
array = prhs[1];
length = length2;
} else {
array = prhs[0];
length = length1;
}
plhs[0] = mxCreateNumericArray(mxGetNumberOfDimensions(array),
mxGetDimensions(array), mxINT8_CLASS,
mxREAL);
}
INT8_T *y = (INT8_T *) mxGetPr(plhs[0]);
double *x1 = mxGetPr(prhs[0]);
double *x2 = mxGetPr(prhs[1]);
mwSize i;
if (length1 == length2) {
for (i = 0; i < length; i++) {
y[i] = (INT8_T) gsl_fcmp(x1[i], x2[i], epsilon);
}
} else if (length1 > length2) {
for (i = 0; i < length; i++) {
y[i] = (INT8_T) gsl_fcmp(x1[i], x2[0], epsilon);
}
} else {
for (i = 0; i < length; i++) {
y[i] = (INT8_T) gsl_fcmp(x1[0], x2[i], epsilon);
}
}
}
/*
* h a s O p t i o n s V a l u e
*/
BooleanType hasOptionsValue( const mxArray* optionsPtr, const char* const optionString, double** optionValue )
{
mxArray* optionName = mxGetField( optionsPtr,0,optionString );
if ( optionName == 0 )
{
char msg[MAX_STRING_LENGTH];
snprintf(msg, MAX_STRING_LENGTH, "Option struct does not contain entry '%s', using default value instead!", optionString );
mexWarnMsgTxt( msg );
return BT_FALSE;
}
if ( ( mxIsEmpty(optionName) == false ) && ( mxIsScalar( optionName ) == true ) )
{
*optionValue = mxGetPr( optionName );
return BT_TRUE;
}
else
{
char msg[MAX_STRING_LENGTH];
snprintf(msg, MAX_STRING_LENGTH, "Option '%s' is not a scalar, using default value instead!", optionString );
mexWarnMsgTxt( msg );
return BT_FALSE;
}
}
/* gft1d(sig, windowType)
*
* Performs a 1d GFT using an asymmetric partitioning
* scheme for real signals
*/
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
if (nrhs != 1)
mexErrMsgTxt("gft1dRealPartitions: Wrong number of inputs");
else if (nlhs>1)
mexErrMsgTxt("gft1dRealPartitions: Too many output arguments");
mwSize n = mxGetNumberOfDimensions(prhs[0]);
if (!mxIsDouble(prhs[0]) || mxIsComplex(prhs[0]) || !mxIsScalar(prhs[0]))
mexErrMsgTxt("gft1dRealPartitions: requires scalar argument");
mwSize Nele;
double *inpr = mxGetPr(prhs[0]);
Nele = inpr[0];
int *pars = gft_1dRealPartitions(Nele);
int i = 0;
int parlength;
/* count number of elements in partition vector */
while (pars[i]>0) i++;
parlength = i;
mxArray *out = mxCreateNumericMatrix(1, parlength, mxDOUBLE_CLASS, mxREAL);
double *outpr = mxGetPr(out);
for(i=0; i<parlength; i++)
{
outpr[i] = pars[i];
}
free(pars);
plhs[0] = out;
}
static int ID_check(int arg, const mxArray *prhs[]){
int i;
if(mxINT32_CLASS != mxGetClassID(prhs[arg]) || !mxIsScalar(prhs[arg])){
mexErrMsgIdAndTxt("S4:invalidInput", "Expected handle argument after command.");
}
i = *(int*)(mxGetData(prhs[arg]));
return i;
}
static void complex_check(int arg, const mxArray *prhs[], const char *argname, double *ret){
if(!mxIsNumeric(prhs[arg]) || !mxIsScalar(prhs[arg])){
mexErrMsgIdAndTxt("S4:invalidInput", "Expected complex number.");
}
ret[0] = *(mxGetPr(prhs[arg]));
if(mxIsComplex(prhs[arg])){
ret[1] = *(mxGetPi(prhs[arg]));
}
}
static void S4mex_Simulation_SetMaterial(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
S4_Simulation *S = Sptr_check(prhs);
S4_real eps[18] = {0};
S4_MaterialID M;
if(3 != nrhs){
mexErrMsgIdAndTxt("S4:invalidInput", "Expected two additional arguments to 'set_material'.");
}
if(mxIsScalar(prhs[2])){
eps[0] = *(mxGetPr(prhs[2]));
if(mxIsComplex(prhs[2])){
eps[1] = *(mxGetPi(prhs[2]));
M = S4_Simulation_SetMaterial(S, -1, NULL, S4_MATERIAL_TYPE_SCALAR_COMPLEX, eps);
}else{
M = S4_Simulation_SetMaterial(S, -1, NULL, S4_MATERIAL_TYPE_SCALAR_REAL, eps);
}
}else if(3 == mxGetM(prhs[2]) && 3 == mxGetN(prhs[2])){
int i, j;
const double *p = mxGetPr(prhs[3]);
for(j = 0; j < 3; ++j){
for(i = 0; i < 3; ++i){
eps[2*(i+j*3)+0] = p[i+j*3];
}
}
if(mxIsComplex(prhs[2])){
p = mxGetPi(prhs[2]);
for(j = 0; j < 3; ++j){
for(i = 0; i < 3; ++i){
eps[2*(i+j*3)+1] = p[i+j*3];
}
}
}
{
S4_real abcde[10] = {
eps[0], eps[1], eps[6], eps[7],
eps[2], eps[3], eps[8], eps[9],
eps[16], eps[17]
};
for(i = 0; i < 10; ++i){
eps[i] = abcde[i];
}
}
M = S4_Simulation_SetMaterial(S, -1, NULL, S4_MATERIAL_TYPE_XYTENSOR_COMPLEX, eps);
}else{
mexErrMsgIdAndTxt("S4:invalidInput", "Third additional argument (epsilon) must be a scalar or 3x3 matrix.");
}
MEX_TRACE("> S4mex_Simulation_SetMaterial(S = %p, epsilon = %f ...)\n", S, eps[0]);
plhs[0] = mxCreateNumericMatrix(1, 1, mxINT32_CLASS, mxREAL);
*(int*)(mxGetData(plhs[0])) = M;
MEX_TRACE("< S4mex_Simulation_SetMaterial %d\n", M);
}
static int S_check(const mxArray *prhs[]){
int i;
if(mxINT32_CLASS != mxGetClassID(prhs[1]) || !mxIsScalar(prhs[1])){
mexErrMsgIdAndTxt("S4:invalidInput", "Expected simulation handle argument after command.");
}
i = *(int*)(mxGetData(prhs[1]));
if(i < 0 || i >= nS_alloc){
mexErrMsgIdAndTxt("S4:invalidInput", "Invalid simulation handle argument after command.");
}
return i;
}
void mexFunction (int nlhs, mxArray * plhs[], int nrhs, const mxArray * prhs[]) {
int rc;
double delay;
/* this function will be called upon unloading of the mex file */
mexAtExit(exitFun);
if (nrhs<1) {
/* show the status of the delayed exit */
pthread_mutex_lock(&mutextimer);
pthread_mutex_lock(&mutexstatus);
if (timerStatus) {
delay = difftime(timer-time(NULL));
if (nlhs<1)
mexPrintf("delayed exit scheduled at %d seconds\n", (int)delay);
else
plhs[0] = mxCreateDoubleScalar(delay);
}
else {
if (nlhs<1)
mexPrintf("no delayed exit scheduled\n");
else
plhs[0] = mxCreateDoubleScalar(mxGetInf());
}
pthread_mutex_unlock(&mutexstatus);
pthread_mutex_unlock(&mutextimer);
}
else {
/* the first argument is the delay in seconds */
if (!mxIsScalar(prhs[0]))
mexErrMsgTxt ("invalid input argument #1");
delay = mxGetScalar(prhs[0]);
/* set or update the timer */
pthread_mutex_lock(&mutextimer);
timer = time(NULL) + (unsigned int)delay;
pthread_mutex_unlock(&mutextimer);
/* start the thread */
rc = pthread_create(&timerThread, NULL, checktimer, (void *)NULL);
if (rc)
mexErrMsgTxt("problem with return code from pthread_create()");
}
} /* main */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
char msg[2000];
const char *docstring ="";
/* check proper input and output */
if(nrhs!=3){
sprintf(msg, ">> Need 3 inputs.\n%s\n", docstring);
mexErrMsgTxt(msg);
} else if(!mxIsDouble(prhs[0]) || !is_mex_vector(prhs[0]) ){
sprintf(msg, ">> First Input must be double vector.\n%s\n", docstring);
mexErrMsgTxt(msg);
} else if( !(mxIsScalar(prhs[1]) && mxIsScalar(prhs[2])) ){
sprintf(msg, ">> Second and Third Inputs must be scalars.\n%s\n", docstring);
mexErrMsgTxt(msg);
}
/* getting the appropriate array representation */
Array *s = mex_mxarray_to_array( prhs[0] );
int m=(int)get_mex_double( prhs[1]);
int tau=(int)get_mex_double( prhs[2]);
double *x=s->data;
int n=s->size[0];
/* computation */
TimeDelayReconstruction* td = tdelay_init ( m, tau, x, n );
Array *tda = tdelay_to_array( td );
/* conversion to MATLAB */
plhs[0]=mex_array_to_mxarray( tda );
/* cleaning up */
array_free( s );
tdelay_free( td );
return;
}
static void S4mex_Simulation_SetFrequency(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
S4_Simulation *S = Sptr_check(prhs);
S4_real freq[2] = {0, 0};
if(3 != nrhs){
mexErrMsgIdAndTxt("S4:invalidInput", "Expected two additional arguments to 'set_frequency'.");
}
if(!mxIsScalar(prhs[2])){
mexErrMsgIdAndTxt("S4:invalidInput", "First additional argument (frequency) must be a scalar number.");
}
freq[0] = *(mxGetPr(prhs[2]));
MEX_TRACE("> S4mex_Simulation_SetFrequency(S = %p, freq = %f)\n", S, freq[0]);
S4_Simulation_SetFrequency(S, freq);
MEX_TRACE("< S4mex_Simulation_SetFrequency\n");
}
void mexFunction( int nlhs1, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
double w1, w2, w3, eps;
mwSize n1, n2, n3, nDims;
const mwSize *pDims;
const mwSize pDims2[] = {1,1};
unsigned int flags;
double *x;
double *fx;
double *gx;
/* Control of the number of inputs and outputs */
if (nrhs > 6)
mexErrMsgTxt("6 input argument required.");
else if (nlhs1 !=2)
mexErrMsgTxt("2 output argument required.");
/* Control of the inputs */
if (!mxIsNumeric(prhs[0]) || !mxIsDouble(prhs[0]))
mexErrMsgTxt("The first input must be numerical double array.");
if (!mxIsNumeric(prhs[1]) || !mxIsScalar(prhs[1])
|| !mxIsNumeric(prhs[2]) || !mxIsScalar(prhs[2])
|| !mxIsNumeric(prhs[3]) || !mxIsScalar(prhs[3])
|| !mxIsNumeric(prhs[4]) || !mxIsScalar(prhs[4])
|| !mxIsNumeric(prhs[5]) || !mxIsScalar(prhs[5]) )
mexErrMsgTxt("The 5 last input must be scalar.");
if ( (double )(unsigned int )mxGetScalar(prhs[5]) != mxGetScalar(prhs[5]) )
mexErrMsgTxt("The last input flags must be an unsigned integer.");
/* Get the inputs */
w1 = (double )mxGetScalar(prhs[1]);
w2 = (double )mxGetScalar(prhs[2]);
w3 = (double )mxGetScalar(prhs[3]);
eps = (double )mxGetScalar(prhs[4]);
flags = (unsigned int )mxGetScalar(prhs[5]);
flags |= RGL_INTEGRATE_GRADIENT;
nDims = mxGetNumberOfDimensions(prhs[0]);
pDims = mxGetDimensions(prhs[0]);
x = mxGetPr(prhs[0]);
n1 = pDims[0];
n2 = pDims[1];
n3 = pDims[2];
/* Display the inputs */
/* mexPrintf("n1 = %d, n2 = %d, n3 = %d, w1 = %f, w2 = %f, w3 = %f, eps = %f, flags = %d\n", n1, n2, n3, w1, w2, w3, eps, flags); */
/* Create the output arguments */
/* Argument fx */
plhs[0] = mxCreateNumericArray(2, pDims2, mxDOUBLE_CLASS, mxREAL);
fx = mxGetPr(plhs[0]);
/* Argument gx */
plhs[1] = mxCreateNumericArray(nDims, pDims, mxDOUBLE_CLASS, mxREAL);
gx = mxGetPr(plhs[1]);
/* Call to the rgl_tv3d function */
*fx = rgl_tv3d(x,n1,n2,n3,w1,w2,w3,eps,gx,flags);
}
/*
* s e t u p A u x i l i a r y I n p u t s
*/
returnValue setupAuxiliaryInputs( const mxArray* auxInput, uint_t nV, uint_t nC,
HessianType* hessianType, double** x0, double** guessedBounds, double** guessedConstraints, double** R
)
{
mxArray* curField = 0;
/* hessianType */
curField = mxGetField( auxInput,0,"hessianType" );
if ( curField == NULL )
mexWarnMsgTxt( "auxInput struct does not contain entry 'hessianType'!\n Type 'help qpOASES_auxInput' for further information." );
else
{
if ( mxIsEmpty(curField) == true )
{
*hessianType = HST_UNKNOWN;
}
else
{
if ( mxIsScalar(curField) == false )
return RET_INVALID_ARGUMENTS;
double* hessianTypeTmp = mxGetPr(curField);
int_t hessianTypeInt = (int_t)*hessianTypeTmp;
if ( hessianTypeInt < 0 )
hessianTypeInt = 6; /* == HST_UNKNOWN */
if ( hessianTypeInt > 5 )
hessianTypeInt = 6; /* == HST_UNKNOWN */
*hessianType = (REFER_NAMESPACE_QPOASES HessianType)hessianTypeInt;
}
}
/* x0 */
curField = mxGetField( auxInput,0,"x0" );
if ( curField == NULL )
mexWarnMsgTxt( "auxInput struct does not contain entry 'x0'!\n Type 'help qpOASES_auxInput' for further information." );
else
{
*x0 = mxGetPr(curField);
if ( smartDimensionCheck( x0,nV,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
return RET_INVALID_ARGUMENTS;
}
/* guessedWorkingSetB */
curField = mxGetField( auxInput,0,"guessedWorkingSetB" );
if ( curField == NULL )
mexWarnMsgTxt( "auxInput struct does not contain entry 'guessedWorkingSetB'!\n Type 'help qpOASES_auxInput' for further information." );
else
{
*guessedBounds = mxGetPr(curField);
if ( smartDimensionCheck( guessedBounds,nV,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
return RET_INVALID_ARGUMENTS;
}
/* guessedWorkingSetC */
curField = mxGetField( auxInput,0,"guessedWorkingSetC" );
if ( curField == NULL )
mexWarnMsgTxt( "auxInput struct does not contain entry 'guessedWorkingSetC'!\n Type 'help qpOASES_auxInput' for further information." );
else
{
*guessedConstraints = mxGetPr(curField);
if ( smartDimensionCheck( guessedConstraints,nC,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
return RET_INVALID_ARGUMENTS;
}
/* R */
curField = mxGetField( auxInput,0,"R" );
if ( curField == NULL )
mexWarnMsgTxt( "auxInput struct does not contain entry 'R'!\n Type 'help qpOASES_auxInput' for further information." );
else
{
*R = mxGetPr(curField);
if ( smartDimensionCheck( R,nV,nV, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
return RET_INVALID_ARGUMENTS;
}
return SUCCESSFUL_RETURN;
}
/*
* m e x F u n c t i o n
*/
void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] )
{
/* inputs */
char typeString[2];
real_t *g=0, *lb=0, *ub=0, *lbA=0, *ubA=0;
HessianType hessianType = HST_UNKNOWN;
double *x0=0, *R=0, *R_for=0;
double *guessedBounds=0, *guessedConstraints=0;
int_t H_idx=-1, g_idx=-1, A_idx=-1, lb_idx=-1, ub_idx=-1, lbA_idx=-1, ubA_idx=-1;
int_t x0_idx=-1, auxInput_idx=-1;
BooleanType isSimplyBoundedQp = BT_FALSE;
#ifdef SOLVER_MA57
BooleanType isSparse = BT_TRUE; /* This will be set to BT_FALSE later if a dense matrix is encountered. */
#else
BooleanType isSparse = BT_FALSE;
#endif
Options options;
options.printLevel = PL_LOW;
#ifdef __DEBUG__
options.printLevel = PL_HIGH;
#endif
#ifdef __SUPPRESSANYOUTPUT__
options.printLevel = PL_NONE;
#endif
/* dimensions */
uint_t nV=0, nC=0, handle=0;
int_t nWSRin;
real_t maxCpuTimeIn = -1.0;
QPInstance* globalQP = 0;
/* I) CONSISTENCY CHECKS: */
/* 1) Ensure that qpOASES is called with a feasible number of input arguments. */
if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
{
if ( nrhs != 2 )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
}
/* 2) Ensure that first input is a string ... */
if ( mxIsChar( prhs[0] ) != 1 )
{
myMexErrMsgTxt( "ERROR (qpOASES): First input argument must be a string!" );
return;
}
mxGetString( prhs[0], typeString, 2 );
/* ... and if so, check if it is an allowed one. */
if ( ( strcmp( typeString,"i" ) != 0 ) && ( strcmp( typeString,"I" ) != 0 ) &&
( strcmp( typeString,"h" ) != 0 ) && ( strcmp( typeString,"H" ) != 0 ) &&
( strcmp( typeString,"m" ) != 0 ) && ( strcmp( typeString,"M" ) != 0 ) &&
( strcmp( typeString,"e" ) != 0 ) && ( strcmp( typeString,"E" ) != 0 ) &&
( strcmp( typeString,"c" ) != 0 ) && ( strcmp( typeString,"C" ) != 0 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Undefined first input argument!\nType 'help qpOASES_sequence' for further information." );
return;
}
/* II) SELECT RESPECTIVE QPOASES FUNCTION CALL: */
/* 1) Init (without or with initial guess for primal solution). */
if ( ( strcmp( typeString,"i" ) == 0 ) || ( strcmp( typeString,"I" ) == 0 ) )
{
/* consistency checks */
if ( ( nlhs < 1 ) || ( nlhs > 7 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
if ( ( nrhs < 5 ) || ( nrhs > 10 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
g_idx = 2;
if ( mxIsEmpty(prhs[1]) == 1 )
{
H_idx = -1;
nV = (uint_t)mxGetM( prhs[ g_idx ] ); /* row number of Hessian matrix */
}
else
{
H_idx = 1;
nV = (uint_t)mxGetM( prhs[ H_idx ] ); /* row number of Hessian matrix */
}
/* ensure that data is given in double precision */
if ( ( ( H_idx >= 0 ) && ( mxIsDouble( prhs[ H_idx ] ) == 0 ) ) ||
( mxIsDouble( prhs[ g_idx ] ) == 0 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
return;
}
if ( ( H_idx >= 0 ) && ( ( mxGetN( prhs[ H_idx ] ) != nV ) || ( mxGetM( prhs[ H_idx ] ) != nV ) ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Hessian matrix dimension mismatch!" );
return;
}
/* Check for 'Inf' and 'Nan' in Hessian */
if (containsNaNorInf( prhs,H_idx, 0 ) == BT_TRUE)
return;
/* Check for 'Inf' and 'Nan' in gradient */
if (containsNaNorInf(prhs,g_idx, 0 ) == BT_TRUE)
return;
/* determine whether is it a simply bounded QP */
if ( nrhs <= 7 )
isSimplyBoundedQp = BT_TRUE;
else
isSimplyBoundedQp = BT_FALSE;
if ( isSimplyBoundedQp == BT_TRUE )
{
lb_idx = 3;
ub_idx = 4;
if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
return;
/* Check inputs dimensions and assign pointers to inputs. */
nC = 0; /* row number of constraint matrix */
if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,2 ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,3 ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,4 ) != SUCCESSFUL_RETURN )
return;
/* default value for nWSR */
nWSRin = 5*nV;
/* Check whether x0 and options are specified .*/
if ( nrhs >= 6 )
{
if ((!mxIsEmpty(prhs[5])) && (mxIsStruct(prhs[5])))
setupOptions( &options,prhs[5],nWSRin,maxCpuTimeIn );
if ( ( nrhs >= 7 ) && ( !mxIsEmpty(prhs[6]) ) )
{
/* auxInput specified */
if ( mxIsStruct(prhs[6]) )
{
auxInput_idx = 6;
x0_idx = -1;
}
else
{
auxInput_idx = -1;
x0_idx = 6;
}
}
else
{
auxInput_idx = -1;
x0_idx = -1;
}
}
}
else
{
A_idx = 3;
/* ensure that data is given in double precision */
if ( mxIsDouble( prhs[ A_idx ] ) == 0 )
{
myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in double precision!" );
return;
}
/* Check inputs dimensions and assign pointers to inputs. */
nC = (uint_t)mxGetM( prhs[ A_idx ] ); /* row number of constraint matrix */
lb_idx = 4;
ub_idx = 5;
lbA_idx = 6;
ubA_idx = 7;
if (containsNaNorInf( prhs,A_idx, 0 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
return;
if ( ( mxGetN( prhs[ A_idx ] ) != 0 ) && ( mxGetN( prhs[ A_idx ] ) != nV ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Constraint matrix dimension mismatch!" );
return;
}
if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
return;
/* default value for nWSR */
nWSRin = 5*(nV+nC);
/* Check whether x0 and options are specified .*/
if ( nrhs >= 9 )
{
if ((!mxIsEmpty(prhs[8])) && (mxIsStruct(prhs[8])))
setupOptions( &options,prhs[8],nWSRin,maxCpuTimeIn );
if ( ( nrhs >= 10 ) && ( !mxIsEmpty(prhs[9]) ) )
{
/* auxInput specified */
if ( mxIsStruct(prhs[9]) )
{
auxInput_idx = 9;
x0_idx = -1;
}
else
{
auxInput_idx = -1;
x0_idx = 9;
}
}
else
{
auxInput_idx = -1;
x0_idx = -1;
}
}
}
/* check dimensions and copy auxInputs */
if ( smartDimensionCheck( &x0,nV,1, BT_TRUE,prhs,x0_idx ) != SUCCESSFUL_RETURN )
return;
if ( auxInput_idx >= 0 )
setupAuxiliaryInputs( prhs[auxInput_idx],nV,nC, &hessianType,&x0,&guessedBounds,&guessedConstraints,&R_for );
/* convert Cholesky factor to C storage format */
if ( R_for != 0 )
{
R = new real_t[nV*nV];
convertFortranToC( R_for, nV,nV, R );
}
/* check if QP is sparse */
if ( H_idx >= 0 && !mxIsSparse( prhs[H_idx] ) )
isSparse = BT_FALSE;
if ( nC > 0 && A_idx >= 0 && !mxIsSparse( prhs[A_idx] ) )
isSparse = BT_FALSE;
/* allocate instance */
handle = allocateQPInstance( nV,nC,hessianType, isSimplyBoundedQp, isSparse, &options );
globalQP = getQPInstance( handle );
/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
if ( H_idx >= 0 )
setupHessianMatrix( prhs[H_idx],nV, &(globalQP->H),&(globalQP->Hir),&(globalQP->Hjc),&(globalQP->Hv) );
/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
if ( ( nC > 0 ) && ( A_idx >= 0 ) )
setupConstraintMatrix( prhs[A_idx],nV,nC, &(globalQP->A),&(globalQP->Air),&(globalQP->Ajc),&(globalQP->Av) );
/* Create output vectors and assign pointers to them. */
allocateOutputs( nlhs,plhs, nV,nC,1,handle );
/* Call qpOASES. */
if ( isSimplyBoundedQp == BT_TRUE )
{
QProblemB_init( handle,
globalQP->H,g,
lb,ub,
nWSRin,maxCpuTimeIn,
x0,&options,
nlhs,plhs,
guessedBounds,R
);
}
else
{
SQProblem_init( handle,
globalQP->H,g,globalQP->A,
lb,ub,lbA,ubA,
nWSRin,maxCpuTimeIn,
x0,&options,
nlhs,plhs,
guessedBounds,guessedConstraints,R
);
}
if (R != 0) delete R;
return;
}
/* 2) Hotstart. */
if ( ( strcmp( typeString,"h" ) == 0 ) || ( strcmp( typeString,"H" ) == 0 ) )
{
/* consistency checks */
if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
if ( ( nrhs < 5 ) || ( nrhs > 8 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
/* determine whether is it a simply bounded QP */
if ( nrhs < 7 )
isSimplyBoundedQp = BT_TRUE;
else
isSimplyBoundedQp = BT_FALSE;
if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
return;
}
/* get QP instance */
handle = (uint_t)mxGetScalar( prhs[1] );
globalQP = getQPInstance( handle );
if ( globalQP == 0 )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
return;
}
nV = globalQP->getNV();
g_idx = 2;
lb_idx = 3;
ub_idx = 4;
if (containsNaNorInf( prhs,g_idx, 0 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
return;
/* Check inputs dimensions and assign pointers to inputs. */
if ( isSimplyBoundedQp == BT_TRUE )
{
nC = 0;
if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
return;
/* default value for nWSR */
nWSRin = 5*nV;
/* Check whether options are specified .*/
if ( nrhs == 6 )
if ( ( !mxIsEmpty( prhs[5] ) ) && ( mxIsStruct( prhs[5] ) ) )
setupOptions( &options,prhs[5],nWSRin,maxCpuTimeIn );
}
else
{
nC = globalQP->getNC( );
lbA_idx = 5;
ubA_idx = 6;
if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
return;
if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
return;
/* default value for nWSR */
nWSRin = 5*(nV+nC);
/* Check whether options are specified .*/
if ( nrhs == 8 )
if ( ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
setupOptions( &options,prhs[7],nWSRin,maxCpuTimeIn );
}
/* Create output vectors and assign pointers to them. */
allocateOutputs( nlhs,plhs, nV,nC );
/* call qpOASES */
if ( isSimplyBoundedQp == BT_TRUE )
{
QProblemB_hotstart( handle, g,
lb,ub,
nWSRin,maxCpuTimeIn,
&options,
nlhs,plhs
);
}
else
{
QProblem_hotstart( handle, g,
lb,ub,lbA,ubA,
nWSRin,maxCpuTimeIn,
&options,
nlhs,plhs
);
}
return;
}
/* 3) Modify matrices. */
if ( ( strcmp( typeString,"m" ) == 0 ) || ( strcmp( typeString,"M" ) == 0 ) )
{
/* consistency checks */
if ( ( nlhs < 1 ) || ( nlhs > 6 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
if ( ( nrhs < 9 ) || ( nrhs > 10 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
return;
}
/* get QP instance */
handle = (uint_t)mxGetScalar( prhs[1] );
globalQP = getQPInstance( handle );
if ( globalQP == 0 )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
return;
}
/* Check inputs dimensions and assign pointers to inputs. */
g_idx = 3;
if ( mxIsEmpty(prhs[2]) == 1 )
{
H_idx = -1;
nV = (uint_t)mxGetM( prhs[ g_idx ] ); /* if Hessian is empty, row number of gradient vector */
}
else
{
H_idx = 2;
nV = (uint_t)mxGetM( prhs[ H_idx ] ); /* row number of Hessian matrix */
}
A_idx = 4;
nC = (uint_t)mxGetM( prhs[ A_idx ] ); /* row number of constraint matrix */
lb_idx = 5;
ub_idx = 6;
lbA_idx = 7;
ubA_idx = 8;
/* ensure that data is given in double precision */
if ( ( ( H_idx >= 0 ) && ( mxIsDouble( prhs[H_idx] ) == 0 ) ) ||
( mxIsDouble( prhs[g_idx] ) == 0 ) ||
( mxIsDouble( prhs[A_idx] ) == 0 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): All data has to be provided in real_t precision!" );
return;
}
/* check if supplied data contains 'NaN' or 'Inf' */
if (containsNaNorInf(prhs,H_idx, 0) == BT_TRUE)
return;
if (containsNaNorInf( prhs,g_idx, 0 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,A_idx, 0 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
return;
/* Check that dimensions are consistent with existing QP instance */
if (nV != (uint_t) globalQP->getNV () || nC != (uint_t) globalQP->getNC ())
{
myMexErrMsgTxt( "ERROR (qpOASES): QP dimensions must be constant during a sequence! Try creating a new QP instance instead." );
return;
}
if ( ( H_idx >= 0 ) && ( ( mxGetN( prhs[ H_idx ] ) != nV ) || ( mxGetM( prhs[ H_idx ] ) != nV ) ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Hessian matrix dimension mismatch!" );
return;
}
if ( ( mxGetN( prhs[ A_idx ] ) != 0 ) && ( mxGetN( prhs[ A_idx ] ) != nV ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Constraint matrix dimension mismatch!" );
return;
}
if ( smartDimensionCheck( &g,nV,1, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lb,nV,1, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ub,nV,1, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lbA,nC,1, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ubA,nC,1, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
return;
/* default value for nWSR */
nWSRin = 5*(nV+nC);
/* Check whether options are specified .*/
if ( nrhs > 9 )
if ( ( !mxIsEmpty( prhs[9] ) ) && ( mxIsStruct( prhs[9] ) ) )
setupOptions( &options,prhs[9],nWSRin,maxCpuTimeIn );
globalQP->deleteQPMatrices( );
/* make a deep-copy of the user-specified Hessian matrix (possibly sparse) */
if ( H_idx >= 0 )
setupHessianMatrix( prhs[H_idx],nV, &(globalQP->H),&(globalQP->Hir),&(globalQP->Hjc),&(globalQP->Hv) );
/* make a deep-copy of the user-specified constraint matrix (possibly sparse) */
if ( ( nC > 0 ) && ( A_idx >= 0 ) )
setupConstraintMatrix( prhs[A_idx],nV,nC, &(globalQP->A),&(globalQP->Air),&(globalQP->Ajc),&(globalQP->Av) );
/* Create output vectors and assign pointers to them. */
allocateOutputs( nlhs,plhs, nV,nC );
/* Call qpOASES */
SQProblem_hotstart( handle, globalQP->H,g,globalQP->A,
lb,ub,lbA,ubA,
nWSRin,maxCpuTimeIn,
&options,
nlhs,plhs
);
return;
}
/* 4) Solve current equality constrained QP. */
if ( ( strcmp( typeString,"e" ) == 0 ) || ( strcmp( typeString,"E" ) == 0 ) )
{
/* consistency checks */
if ( ( nlhs < 1 ) || ( nlhs > 4 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
if ( ( nrhs < 7 ) || ( nrhs > 8 ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
return;
}
/* get QP instance */
handle = (uint_t)mxGetScalar( prhs[1] );
globalQP = getQPInstance( handle );
if ( globalQP == 0 )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid handle to QP instance!" );
return;
}
/* Check inputs dimensions and assign pointers to inputs. */
int_t nRHS = (int_t)mxGetN(prhs[2]);
nV = globalQP->getNV( );
nC = globalQP->getNC( );
real_t *x_out, *y_out;
g_idx = 2;
lb_idx = 3;
ub_idx = 4;
lbA_idx = 5;
ubA_idx = 6;
/* check if supplied data contains 'NaN' or 'Inf' */
if (containsNaNorInf(prhs,g_idx, 0) == BT_TRUE)
return;
if (containsNaNorInf( prhs,lb_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ub_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,lbA_idx, 1 ) == BT_TRUE)
return;
if (containsNaNorInf( prhs,ubA_idx, 1 ) == BT_TRUE)
return;
if ( smartDimensionCheck( &g,nV,nRHS, BT_FALSE,prhs,g_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lb,nV,nRHS, BT_TRUE,prhs,lb_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ub,nV,nRHS, BT_TRUE,prhs,ub_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &lbA,nC,nRHS, BT_TRUE,prhs,lbA_idx ) != SUCCESSFUL_RETURN )
return;
if ( smartDimensionCheck( &ubA,nC,nRHS, BT_TRUE,prhs,ubA_idx ) != SUCCESSFUL_RETURN )
return;
/* Check whether options are specified .*/
if ( ( nrhs == 8 ) && ( !mxIsEmpty( prhs[7] ) ) && ( mxIsStruct( prhs[7] ) ) )
{
nWSRin = 5*(nV+nC);
setupOptions( &options,prhs[7],nWSRin,maxCpuTimeIn );
globalQP->sqp->setOptions( options );
}
/* Create output vectors and assign pointers to them. */
plhs[0] = mxCreateDoubleMatrix( nV, nRHS, mxREAL );
x_out = mxGetPr(plhs[0]);
if (nlhs >= 2)
{
plhs[1] = mxCreateDoubleMatrix( nV+nC, nRHS, mxREAL );
y_out = mxGetPr(plhs[1]);
if (nlhs >= 3)
{
plhs[2] = mxCreateDoubleMatrix( nV, nRHS, mxREAL );
real_t* workingSetB = mxGetPr(plhs[2]);
globalQP->sqp->getWorkingSetBounds(workingSetB);
if ( nlhs >= 4 )
{
plhs[3] = mxCreateDoubleMatrix( nC, nRHS, mxREAL );
real_t* workingSetC = mxGetPr(plhs[3]);
globalQP->sqp->getWorkingSetConstraints(workingSetC);
}
}
}
else
y_out = new real_t[nV+nC];
/* Solve equality constrained QP */
returnValue returnvalue = globalQP->sqp->solveCurrentEQP( nRHS,g,lb,ub,lbA,ubA, x_out,y_out );
if (nlhs < 2)
delete[] y_out;
if (returnvalue != SUCCESSFUL_RETURN)
{
char msg[MAX_STRING_LENGTH];
snprintf(msg, MAX_STRING_LENGTH, "ERROR (qpOASES): Couldn't solve current EQP (code %d)!", returnvalue);
myMexErrMsgTxt(msg);
return;
}
return;
}
/* 5) Cleanup. */
if ( ( strcmp( typeString,"c" ) == 0 ) || ( strcmp( typeString,"C" ) == 0 ) )
{
/* consistency checks */
if ( nlhs != 0 )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of output arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
if ( nrhs != 2 )
{
myMexErrMsgTxt( "ERROR (qpOASES): Invalid number of input arguments!\nType 'help qpOASES_sequence' for further information." );
return;
}
if ( ( mxIsDouble( prhs[1] ) == false ) || ( mxIsScalar( prhs[1] ) == false ) )
{
myMexErrMsgTxt( "ERROR (qpOASES): Expecting a handle to QP object as second argument!\nType 'help qpOASES_sequence' for further information." );
return;
}
/* Cleanup SQProblem instance. */
handle = (uint_t)mxGetScalar( prhs[1] );
deleteQPInstance( handle );
return;
}
}
/* Convert an array of MATLAB keypoint structs to a Keypoint_store. mx must be
* a vector, and each struct element must have type double. */
int mx2kp(const mxArray *const mx, Keypoint_store *const kp) {
const mwSize *mxDims, *coordsDims, *oriDims;
mxArray *mxCoords, *mxScale, *mxOri, *mxOctave, *mxLevel;
mwSize numKp, kpRows, kpCols;
int i, coordsNum, scaleNum, oriNum, octaveNum, levelNum;
// Verify the number of dimensions
if (mxGetNumberOfDimensions(mx) != 2)
return SIFT3D_FAILURE;
// Parse the input dimensions
mxDims = mxGetDimensions(mx);
kpRows = mxDims[0];
kpCols = mxDims[1];
if (kpRows == 1)
numKp = kpCols;
else if (kpCols == 1)
numKp = kpRows;
else
return SIFT3D_FAILURE;
// Verify the struct size
if (!mxIsStruct(mx) || mxGetNumberOfFields(mx) != kpNFields)
return SIFT3D_FAILURE;
// Get the field indices
if ((coordsNum = mxGetFieldNumber(mx, COORDS_NAME)) < 0 ||
(scaleNum = mxGetFieldNumber(mx, SCALE_NAME)) < 0 ||
(oriNum = mxGetFieldNumber(mx, ORI_NAME)) < 0 ||
(octaveNum = mxGetFieldNumber(mx, OCTAVE_NAME)) < 0 ||
(levelNum = mxGetFieldNumber(mx, LEVEL_NAME)) < 0)
return SIFT3D_FAILURE;
// Get the fields of the first keypoint
if ((mxCoords = mxGetFieldByNumber(mx, 0, coordsNum)) == NULL ||
(mxScale = mxGetFieldByNumber(mx, 0, scaleNum)) == NULL ||
(mxOri = mxGetFieldByNumber(mx, 0, oriNum)) == NULL ||
(mxOctave = mxGetFieldByNumber(mx, 0, octaveNum)) == NULL ||
(mxLevel = mxGetFieldByNumber(mx, 0, levelNum)) == NULL)
return SIFT3D_FAILURE;
// Verify the type
if (!isDouble(mxCoords) ||
!isDouble(mxScale) ||
!isDouble(mxOri) ||
!isDouble(mxOctave) ||
!isDouble(mxLevel))
return SIFT3D_FAILURE;
// Verify the number of dimensions
if (mxGetNumberOfDimensions(mxCoords) != 2 ||
mxGetNumberOfDimensions(mxScale) != 2 ||
mxGetNumberOfDimensions(mxOri) != 2 ||
mxGetNumberOfDimensions(mxOctave) != 2 ||
mxGetNumberOfDimensions(mxLevel) != 2)
return SIFT3D_FAILURE;
// Verify the scalars
if (!mxIsScalar(mxScale) ||
!mxIsScalar(mxOctave) ||
!mxIsScalar(mxLevel))
return SIFT3D_FAILURE;
// Verify the coordinate vector dimensions
coordsDims = mxGetDimensions(mxCoords);
if ((coordsDims[0] != 1 && coordsDims[1] != 1) ||
coordsDims[0] * coordsDims[1] != IM_NDIMS)
return SIFT3D_FAILURE;
// Verify the orientation matrix dimensions
oriDims = mxGetDimensions(mxOri);
if (oriDims[0] != IM_NDIMS || oriDims[1] != IM_NDIMS)
return SIFT3D_FAILURE;
// Allocate space in the keypoint store
if (resize_Keypoint_store(kp, (size_t) numKp))
return SIFT3D_FAILURE;
// Copy the data
for (i = 0; i < (int) numKp; i++) {
double *coordsData;
Keypoint *const key = kp->buf + i;
const mwIndex idx = (mwIndex) i;
// Get the matrices
if ((mxCoords = mxGetFieldByNumber(mx, idx, coordsNum)) ==
NULL ||
(mxScale = mxGetFieldByNumber(mx, idx, scaleNum)) ==
NULL ||
(mxOri = mxGetFieldByNumber(mx, idx, oriNum)) ==
NULL ||
(mxOctave = mxGetFieldByNumber(mx, idx, octaveNum)) ==
NULL ||
(mxLevel = mxGetFieldByNumber(mx, idx, levelNum)) ==
NULL)
return SIFT3D_FAILURE;
// Copy the scalars
key->sd = mxGetScalar(mxScale);
key->o = (int) mxGetScalar(mxOctave);
key->s = (int) mxGetScalar(mxLevel);
// Get the coordinate data
if ((coordsData = mxGetData(mxCoords)) == NULL)
return SIFT3D_FAILURE;
// Copy the coordinate vector
key->xd = coordsData[0];
key->yd = coordsData[1];
key->zd = coordsData[2];
// Initialize the orientation matrix
if (init_Mat_rm_p(&key->R, key->r_data, IM_NDIMS, IM_NDIMS,
FLOAT, SIFT3D_FALSE))
return SIFT3D_FAILURE;
// Copy the orientation matrix
if (mx2mat(mxOri, &key->R))
return SIFT3D_FAILURE;
}
return SIFT3D_SUCCESS;
}
void mexFunction (int nlhs, mxArray * plhs[], int nrhs, const mxArray * prhs[]) {
int rc, enabled;
UINT32_T masterid = 0;
time_t timallow = 0;
UINT64_T memallow = 0;
UINT64_T rss, vs;
/* this function will be called upon unloading of the mex file */
mexAtExit(exitFun);
if (nrhs<3)
mexErrMsgTxt ("invalid number of input arguments");
if (mxIsScalar(prhs[0]))
masterid = mxGetScalar(prhs[0]);
else if (mxIsEmpty(prhs[0]))
masterid = 0;
else
mexErrMsgTxt ("invalid input argument #1");
if (mxIsScalar(prhs[1]))
timallow = mxGetScalar(prhs[1]);
else if (mxIsEmpty(prhs[1]))
timallow = 0;
else
mexErrMsgTxt ("invalid input argument #2");
if (mxIsScalar(prhs[2]))
memallow = mxGetScalar(prhs[2]);
else if (mxIsEmpty(prhs[2]))
memallow = 0;
else
mexErrMsgTxt ("invalid input argument #3");
if (masterid!=0 || timallow!=0 || memallow!=0) {
enabled = 1;
/* in this case the mex file is not allowed to be cleared from memory */
if (!mexIsLocked())
mexLock();
}
if (masterid==0 && timallow==0 && memallow==0) {
enabled = 0;
/* in this case the mex file is allowed to be cleared from memory */
#ifdef SOLUTION_FOR_UNEXPLAINED_CRASH
if (mexIsLocked())
mexUnlock();
#endif
}
if (!peerInitialized) {
mexPrintf("watchdog: init\n");
peerinit(NULL);
peerInitialized = 1;
}
/* start the discover thread */
pthread_mutex_lock(&mutexstatus);
if (!discoverStatus) {
pthread_mutex_unlock(&mutexstatus);
mexPrintf("watchdog: spawning discover thread\n");
rc = pthread_create(&discoverThread, NULL, discover, (void *)NULL);
if (rc)
mexErrMsgTxt("problem with return code from pthread_create()");
else {
/* wait until the thread has properly started */
pthread_mutex_lock(&mutexstatus);
if (!discoverStatus)
pthread_cond_wait(&condstatus, &mutexstatus);
pthread_mutex_unlock(&mutexstatus);
}
}
else {
pthread_mutex_unlock(&mutexstatus);
}
/* start the expire thread */
pthread_mutex_lock(&mutexstatus);
if (!expireStatus) {
pthread_mutex_unlock(&mutexstatus);
mexPrintf("watchdog: spawning expire thread\n");
rc = pthread_create(&expireThread, NULL, expire, (void *)NULL);
if (rc)
mexErrMsgTxt("problem with return code from pthread_create()");
else {
/* wait until the thread has properly started */
pthread_mutex_lock(&mutexstatus);
if (!expireStatus)
pthread_cond_wait(&condstatus, &mutexstatus);
pthread_mutex_unlock(&mutexstatus);
}
}
else {
pthread_mutex_unlock(&mutexstatus);
}
if (timallow>0) {
/* timallow should be relative to now */
timallow += time(NULL);
}
if (memallow>0) {
/* memallow should be in absolute numbers, add the current memory footprint */
getmem(&rss, &vs);
memallow += rss;
}
/* enable the watchdog: the expire thread will exit if the master is not seen any more */
pthread_mutex_lock(&mutexwatchdog);
watchdog.enabled = enabled;
watchdog.evidence = 0;
watchdog.masterid = masterid;
watchdog.memory = memallow;
watchdog.time = timallow;
pthread_mutex_unlock(&mutexwatchdog);
if (enabled)
mexPrintf("watchdog: enabled for masterid = %lu, time = %d, memory = %lu\n", masterid, timallow, memallow);
else
mexPrintf("watchdog: disabled for masterid = %lu, time = %d, memory = %lu\n", masterid, timallow, memallow);
return;
} /* main */
static double real_check(int arg, const mxArray *prhs[], const char *argname){
if(!mxIsNumeric(prhs[arg]) || !mxIsScalar(prhs[arg]) || mxIsComplex(prhs[arg])){
mexErrMsgIdAndTxt("S4:invalidInput", "Expected number.");
}
return *(mxGetPr(prhs[arg]));
}
void mexFunction( int nlhs, mxArray *plhs[] , int nrhs, const mxArray *prhs[] )
{
int ny = 24 , nx = 24;
double *scale;
int Nscale = 0;
int nF;
unsigned int *F;
int *dimsF;
/* Input 1 */
if ((nrhs > 0) && (int)mxIsScalar(prhs[0]) )
{
ny = (int) mxGetScalar(prhs[0]);
}
if(ny < 3)
{
mexErrMsgTxt("ny must be >= 3");
}
/* Input 2 */
if ((nrhs > 1) && (int)mxIsScalar(prhs[1]) )
{
nx = (int) mxGetScalar(prhs[1]);
if(nx < 3)
{
mexErrMsgTxt("nx must be >= 3");
}
}
else
{
nx = ny;
}
if ((nrhs > 2) && !mxIsEmpty(prhs[2]) )
{
if(mxGetM(prhs[2]) !=2)
{
mexErrMsgTxt("scale must be a (2 x Nscale) matrix");
}
scale = mxGetPr(prhs[2]);
Nscale = mxGetN(prhs[2]);
}
nF = number_mblbp_features(ny , nx , scale , Nscale);
/*------------------------ Output ----------------------------*/
dimsF = (int *)mxMalloc(2*sizeof(int));
dimsF[0] = 5;
dimsF[1] = nF;
plhs[0] = mxCreateNumericArray(2 , dimsF , mxUINT32_CLASS , mxREAL);
F = (unsigned int *)mxGetPr(plhs[0]);
/*------------------------ Main Call ----------------------------*/
mblbp_featlist(ny , nx , scale , Nscale, F);
/*----------------- Free Memory --------------------------------*/
mxFree(dimsF);
}
static void S4mex_Simulation_New(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
S4_real Lr[4] = { 0, 0, 0, 0 };
int nbases = 1;
int is1d = 0;
if(3 != nrhs){
mexErrMsgIdAndTxt("S4:invalidInput", "Expected two additional arguments to 'new'.");
}
if(!mxIsNumeric(prhs[1]) || mxIsComplex(prhs[1]) || !((is1d = mxIsScalar(prhs[1])) || (2 == mxGetM(prhs[1]) && 2 == mxGetN(prhs[1])))){
mexErrMsgIdAndTxt("S4:invalidInput", "First additional argument (lattice) must be scalar or a 2x2 matrix.");
}
if(is1d){
Lr[0] = mxGetScalar(prhs[1]);
}else{
double *p = mxGetPr(prhs[1]);
Lr[0] = p[0];
Lr[1] = p[1];
Lr[2] = p[2];
Lr[3] = p[3];
if(0 == Lr[1] && 0 == Lr[2] && 0 == Lr[3]){
is1d = 1;
}
}
if(!mxIsNumeric(prhs[2]) || mxIsComplex(prhs[2]) || !mxIsScalar(prhs[2])){
mexErrMsgIdAndTxt("S4:invalidInput", "Second additional argument (#bases) must be a positive real integer.");
}
if(mxIsDouble(prhs[2])){
double *p = mxGetPr(prhs[2]);
nbases = (int)p[0];
}else{
void *p = mxGetData(prhs[2]);
switch(mxGetClassID(prhs[2])){
case mxSINGLE_CLASS:
{ float *pp = (float*)p; nbases = (int)pp[0]; break; }
case mxINT8_CLASS:
{ char *pp = (char*)p; nbases = (int)pp[0]; break; }
case mxUINT8_CLASS:
{ unsigned char *pp = (unsigned char*)p; nbases = (int)pp[0]; break; }
case mxINT16_CLASS:
{ short *pp = (short*)p; nbases = (int)pp[0]; break; }
case mxUINT16_CLASS:
{ unsigned short *pp = (unsigned short*)p; nbases = (int)pp[0]; break; }
case mxINT32_CLASS:
{ int *pp = (int*)p; nbases = (int)pp[0]; break; }
case mxUINT32_CLASS:
{ unsigned int *pp = (unsigned int*)p; nbases = (int)pp[0]; break; }
case mxINT64_CLASS:
{ long long *pp = (long long*)p; nbases = (int)pp[0]; break; }
case mxUINT64_CLASS:
{ unsigned long long *pp = (unsigned long long*)p; nbases = (int)pp[0]; break; }
default:
mexErrMsgIdAndTxt("S4:invalidInput", "Second additional argument (#bases) is an unknown class.");
break;
}
}
if(nbases < 1){
mexErrMsgIdAndTxt("S4:invalidInput", "Second additional argument (#bases) must be a positive integer.");
}
MEX_TRACE("> S4mex_Simulation_New(lattice = [%f, %f; %f, %f], bases = %d)\n", Lr[0], Lr[1], Lr[2], Lr[3], nbases);
{
int *p;
S4_Simulation *S = S4_Simulation_New(Lr, nbases, NULL);
plhs[0] = mxCreateNumericMatrix(1, 1, mxINT32_CLASS, mxREAL);
p = mxGetData(plhs[0]);
p[0] = S_add(S);
SS[p[0]].is1d = is1d;
MEX_TRACE("< S4mex_Simulation_New %d\n", p[0]);
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
// This function is called like: [tSubsampledAndDoubledUp, ySubsampledAndDoubledUp]=minMaxDownsampleMex(t,y,r)
// t a double column vector of length nScans
// y a nScans x nChannels matrix of doubles
// r a double scalar holding a positive integer value, or empty
//
// If r is empty, is means "don't downsample", just return t and y as-is.
// Load in the arguments, checking them thoroughly
// prhs[0]: t
if ( mxIsDouble(prhs[0]) && !mxIsComplex(prhs[0]) && mxGetNumberOfDimensions(prhs[0])==2 && mxGetN(prhs[0])==1 ) {
// all is well
}
else {
mexErrMsgIdAndTxt("ws:minMaxDownSample:tNotRight",
"Argument t must be a non-complex double column vector.");
}
mwSize nScans = mxGetM(prhs[0]) ;
double *t = mxGetPr(prhs[0]); // "Convert" to a C++ array
// prhs[1]: y
//bool isDouble = mxIsDouble(prhs[1]) ;
//bool isReal = mxIsComplex(prhs[1]) ;
//mwSize nDims = mxGetNumberOfDimensions(prhs[1]) ;
//mwSize nRows = mxGetM(prhs[1]) ;
if ( mxIsDouble(prhs[1]) && !mxIsComplex(prhs[1]) && mxGetNumberOfDimensions(prhs[1])==2 && mxGetM(prhs[1])==nScans ) {
// all is well
}
else {
mexErrMsgIdAndTxt("ws:minMaxDownSample:yNotRight",
"Argument y must be a non-complex double matrix with the same number of rows as t.");
}
mwSize nChannels = mxGetN(prhs[1]);
double *y = mxGetPr(prhs[1]); // "Convert" to a C++ array (sort of: it's still in col-major order)
// prhs[1]: r
bool rIsEmpty ;
if ( mxIsEmpty(prhs[2]) ) {
// this is always OK
rIsEmpty = true ;
} else {
rIsEmpty = false ;
// non-empty
if ( mxIsDouble(prhs[2]) && !mxIsComplex(prhs[2]) && mxIsScalar(prhs[2]) ) {
// this is ok
}
else {
mexErrMsgIdAndTxt("ws:minMaxDownSample:rNotRight",
"Argument r must be empty or be a non-complex double scalar.");
}
}
double rAsDouble = -1.0 ; // should not be used if rIsEmpty
mwSize r = 0 ; // should not be used if rIsEmpty
if (!rIsEmpty) {
rAsDouble = mxGetScalar(prhs[2]) ;
if ( floor(rAsDouble)!=ceil(rAsDouble) || rAsDouble<=0 ) {
mexErrMsgIdAndTxt("ws:minMaxDownSample:rNotRight",
"Argument r, if a scalar, must be a positive integer.");
}
r = (mwSize) rAsDouble ;
}
// At this point, all args have been read and validated
int effectiveNLHS = (nlhs>1)?nlhs:1 ; // even if nlhs==0, still safe to assign to plhs[0], and should do this, so ans gets assigned
double* tSubsampledAndDoubled ;
double* ySubsampledAndDoubled ;
double* target ;
double* targetEnd ;
double* source ;
if (rIsEmpty) {
// just copy the inputs to the outputs
if ( effectiveNLHS>=1 ) {
plhs[0] = mxCreateDoubleMatrix(nScans, (mwSize)1, mxREAL) ;
tSubsampledAndDoubled = mxGetPr(plhs[0]);
//for (mwSize i=0 ; i<nScans; ++i) {
// tSubsampledAndDoubled[i] = t[i] ;
//}
target = tSubsampledAndDoubled ;
source = t ;
targetEnd = tSubsampledAndDoubled + nScans ;
while (target!=targetEnd) {
*target = *source ;
++target ;
++source ;
}
}
if ( effectiveNLHS>=2 ) {
plhs[1] = mxCreateDoubleMatrix(nScans, nChannels, mxREAL) ;
ySubsampledAndDoubled = mxGetPr(plhs[1]);
target = ySubsampledAndDoubled ;
source = y ;
mwSize nElements = nChannels * nScans ;
targetEnd = ySubsampledAndDoubled + nElements ;
//for (mwSize i=0 ; i<nElements; ++i, ++source, ++target) {
// *target = *source ;
//}
while (target!=targetEnd) {
*target = *source ;
++target ;
++source ;
}
}
return ;
}
// If get here, r is a scalar, which is the interesting case
mwSize nScansSubsampled = (mwSize) (ceil(((double)nScans)/rAsDouble)) ;
// Create the subsampled timeline, decimating t by the factor r
mxArray* tSubsampledMxArray = mxCreateDoubleMatrix(nScansSubsampled, (mwSize)1, mxREAL) ;
double* tSubsampled = mxGetPr(tSubsampledMxArray) ;
source = t ;
target = tSubsampled ;
//for (mwSize i=0 ; i<nScansSubsampled; ++i, source+=r, ++target) {
// *target = *source ;
//}
targetEnd = tSubsampled + nScansSubsampled ;
while (target!=targetEnd) {
*target = *source ;
++target ;
source+=r ;
}
// Now "double-up" time, with two copies of each time point
mxArray* tSubsampledAndDoubledUpMxArray = mxCreateDoubleMatrix(2*nScansSubsampled, (mwSize)1, mxREAL) ;
double* tSubsampledAndDoubledUp = mxGetPr(tSubsampledAndDoubledUpMxArray) ;
target = tSubsampledAndDoubledUp ;
source = tSubsampled ;
targetEnd = tSubsampledAndDoubledUp + 2*nScansSubsampled ;
//for (source = tSubsampled; source!=(tSubsampled+nScansSubsampled); ++source) {
while (target!=targetEnd) {
double tSource = *source ;
*target = tSource ;
++target ;
*target = tSource ;
++target ;
++source ;
}
// Now set up for return (always assign this one)
plhs[0] = tSubsampledAndDoubledUpMxArray ;
// If that's all the return values the user wanted, return now
if ( effectiveNLHS<2 ) {
return ;
}
// Subsample y at each subsampled scan, getting the max and the min of the r samples for that point in the original y
mxArray* ySubsampledMaxMxArray = mxCreateDoubleMatrix(nScansSubsampled, nChannels, mxREAL) ;
double* ySubsampledMax = mxGetPr(ySubsampledMaxMxArray) ;
mxArray* ySubsampledMinMxArray = mxCreateDoubleMatrix(nScansSubsampled, nChannels, mxREAL) ;
double* ySubsampledMin = mxGetPr(ySubsampledMinMxArray) ;
double maxSoFar ;
double minSoFar ;
mwSize iWithinTheR ;
//mwSize iScanSubsampled ;
double* minTarget ;
double* maxTarget ;
double* sourceEnd ;
source = y ;
double yThis ; // for holding the current value of the y array
for (mwSize iChannel=0 ; iChannel<nChannels; ++iChannel) {
maxTarget = ySubsampledMax + iChannel*nScansSubsampled ;
minTarget = ySubsampledMin + iChannel*nScansSubsampled ;
source = y + iChannel*nScans ;
sourceEnd = source + nScans ;
iWithinTheR = 0 ;
while (source!=sourceEnd) {
if (iWithinTheR==0) {
yThis = *source ;
maxSoFar = yThis ;
minSoFar = yThis ;
} else {
yThis = *source ;
if (yThis>maxSoFar) {
maxSoFar = yThis ;
} else {
if (yThis<minSoFar) {
minSoFar = yThis ;
}
}
}
if (iWithinTheR+1 == r) {
// Write to the elements of the subsampled arrays
*maxTarget = maxSoFar ;
*minTarget = minSoFar ;
++minTarget ;
++maxTarget ;
iWithinTheR = 0 ;
} else {
++iWithinTheR ;
}
++source ;
}
if ( iWithinTheR!=0 && nScansSubsampled>0 ) {
// This means r does not evenly divide nScans, so need to write the current minSoFar, maxSoFar to last el of ySubsampledMax, ySampledMin
*maxTarget = maxSoFar ;
*minTarget = minSoFar ;
}
}
// Now for the y's, max, min, max, min, etc.
mwSize nScansSubsampledAndDoubledUp = 2*nScansSubsampled ;
mxArray* ySubsampledAndDoubledUpMxArray = mxCreateDoubleMatrix(nScansSubsampledAndDoubledUp, (mwSize)nChannels, mxREAL) ;
double* ySubsampledAndDoubledUp = mxGetPr(ySubsampledAndDoubledUpMxArray) ;
target = ySubsampledAndDoubledUp ;
targetEnd = ySubsampledAndDoubledUp + nChannels*nScansSubsampledAndDoubledUp ;
double* minSource = ySubsampledMin ;
double* maxSource = ySubsampledMax ;
//for (mwSize iChannel=0 ; iChannel<nChannels; ++iChannel) {
// for (mwSize iScanSubsampled=0; iScanSubsampled<nScansSubsampled; ++iScanSubsampled) {
// //*(ySubsampledAndDoubledUp+iChannel*nScansSubsampled+2*iScanSubsampled) = *(ySubsampledMax+iChannel*nScansSubsampled+iScanSubsampled) ;
// *target = *maxSource ;
// ++maxSource ;
// ++target ;
// //*(ySubsampledAndDoubledUp+iChannel*nScansSubsampled+2*iScanSubsampled+1) = *(ySubsampledMin+iChannel*nScansSubsampled+iScanSubsampled) ;
// *target = *minSource ;
// ++minSource ;
// ++target ;
// }
//}
while (target!=targetEnd) {
*target = *maxSource ;
++maxSource ;
++target ;
*target = *minSource ;
++minSource ;
++target ;
}
// Now set up for return
plhs[1] = ySubsampledAndDoubledUpMxArray ;
}
