void
mexFunction (int nlhs, mxArray* plhs[], int nrhs,
const mxArray* prhs[])
{
int i;
mwIndex j;
mxArray *v;
const char *keys[] = { "this", "that" };
if (nrhs != 1 || ! mxIsStruct (prhs[0]))
mexErrMsgTxt ("expects struct");
for (i = 0; i < mxGetNumberOfFields (prhs[0]); i++)
for (j = 0; j < mxGetNumberOfElements (prhs[0]); j++)
{
mexPrintf ("field %s(%d) = ",
mxGetFieldNameByNumber (prhs[0], i), j);
v = mxGetFieldByNumber (prhs[0], j, i);
mexCallMATLAB (0, 0, 1, &v, "disp");
}
v = mxCreateStructMatrix (2, 2, 2, keys);
mxSetFieldByNumber (v, 0, 0, mxCreateString ("this1"));
mxSetFieldByNumber (v, 0, 1, mxCreateString ("that1"));
mxSetFieldByNumber (v, 1, 0, mxCreateString ("this2"));
mxSetFieldByNumber (v, 1, 1, mxCreateString ("that2"));
mxSetFieldByNumber (v, 2, 0, mxCreateString ("this3"));
mxSetFieldByNumber (v, 2, 1, mxCreateString ("that3"));
mxSetFieldByNumber (v, 3, 0, mxCreateString ("this4"));
mxSetFieldByNumber (v, 3, 1, mxCreateString ("that4"));
if (nlhs)
plhs[0] = v;
}
void mexProblem::ObtainElementFromMxArray(Element *X, const mxArray *Xmx)
{
// copy the main data from mxArray to X
double *Xmxptr = mxGetPr(GetFieldbyName(Xmx, 0, "main"));
integer lengthX = X->Getlength();
integer inc = 1;
double *Xptr = X->ObtainWriteEntireData();
dcopy_(&lengthX, Xmxptr, &inc, Xptr, &inc);
// copy temp data from mxArray to X
integer nfields = mxGetNumberOfFields(Xmx);
const char **fnames;
mxArray *tmp;
fnames = static_cast<const char **> (mxCalloc(nfields, sizeof(*fnames)));
for (integer i = 0; i < nfields; i++)
{
fnames[i] = mxGetFieldNameByNumber(Xmx, i);
if (strcmp(fnames[i], "main") != 0)
{
tmp = GetFieldbyName(Xmx, 0, fnames[i]);
double *tmpptr = mxGetPr(tmp);
integer length = mxGetM(tmp);
SharedSpace *Sharedtmp = new SharedSpace(1, length);
double *Sharedtmpptr = Sharedtmp->ObtainWriteEntireData();
dcopy_(&length, tmpptr, &inc, Sharedtmpptr, &inc);
X->AddToTempData(fnames[i], Sharedtmp);
}
}
};
ostrm& json( ostrm& S, const mxArray *M ) {
// convert Matlab mxArray to JSON string
siz i, j, m, n=mxGetNumberOfElements(M);
void *A=mxGetData(M); ostrm *nms;
switch( mxGetClassID(M) ) {
case mxDOUBLE_CLASS: return json(S,(double*) A,n);
case mxSINGLE_CLASS: return json(S,(float*) A,n);
case mxINT64_CLASS: return json(S,(int64_t*) A,n);
case mxUINT64_CLASS: return json(S,(uint64_t*) A,n);
case mxINT32_CLASS: return json(S,(int32_t*) A,n);
case mxUINT32_CLASS: return json(S,(uint32_t*) A,n);
case mxINT16_CLASS: return json(S,(int16_t*) A,n);
case mxUINT16_CLASS: return json(S,(uint16_t*) A,n);
case mxINT8_CLASS: return json<int8_t,int32_t>(S,(int8_t*) A,n);
case mxUINT8_CLASS: return json<uint8_t,uint32_t>(S,(uint8_t*) A,n);
case mxLOGICAL_CLASS: return json<uint8_t,uint32_t>(S,(uint8_t*) A,n);
case mxCHAR_CLASS: return json(S,mxArrayToString(M));
case mxCELL_CLASS:
S << "["; for(i=0; i<n-1; i++) json(S,mxGetCell(M,i)) << ",";
if(n>0) json(S,mxGetCell(M,n-1)); S << "]"; return S;
case mxSTRUCT_CLASS:
if(n==0) { S<<"{}"; return S; } m=mxGetNumberOfFields(M);
if(m==0) { S<<"["; for(i=0; i<n; i++) S<<"{},"; S<<"]"; return S; }
if(n>1) S<<"["; nms=new ostrm[m];
for(j=0; j<m; j++) json(nms[j],mxGetFieldNameByNumber(M,j));
for(i=0; i<n; i++) for(j=0; j<m; j++) {
if(j==0) S << "{"; S << nms[j].str() << ":";
json(S,mxGetFieldByNumber(M,i,j)) << ((j<m-1) ? "," : "}");
if(j==m-1 && i<n-1) S<<",";
}
if(n>1) S<<"]"; delete [] nms; return S;
default:
mexErrMsgTxt( "Unknown type." ); return S;
}
}
std::string MxArray::fieldname(int index) const
{
const char *f = mxGetFieldNameByNumber(p_, index);
if (!f)
mexErrMsgIdAndTxt("mexopencv:error",
"Failed to get field name at %d\n", index);
return std::string(f);
}
value_struct* get_structure_malloc(const mxArray* prhsi, int *numb_fields)
{
if (!mxIsStruct(prhsi))
{
mexErrMsgTxt("Ask the programmer of this soft to clear up this issue");
}
*numb_fields = mxGetNumberOfFields(prhsi);
value_struct* structure = malloc(*numb_fields*sizeof(value_struct));
int k=0;
for (k=0;k<*numb_fields;k++)
{
structure[k].name=malloc(sizeof(char)*strlen(mxGetFieldNameByNumber(prhsi,k)));
structure[k].name=mxGetFieldNameByNumber(prhsi,k);
structure[k].value=(mxGetPr(mxGetField(prhsi,0,mxGetFieldNameByNumber(prhsi,k))))[0];
}
return structure;
}
/*
* sample file that shows how to transform an array of structs
* containing (id, handle) pairs to a callback list and return the
* pointer to the data. the callback list can thus be passed to the
* second file in this example (exec_callbacks.cpp).
*
* The passed matlab functions must not have any argument as this example here
* doesn't take care of those arguments.
*/
void
mexFunction (int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if (nrhs != 1)
mexErrMsgTxt("Function requires exactly one argument.\n");
if (nlhs != 1)
mexErrMsgTxt("Function returns exactly one value.\n");
if (!mxIsStruct(prhs[0]))
mexErrMsgTxt("Function requires a structure array as argument\n");
int nfields = mxGetNumberOfFields(prhs[0]);
mwSize nstructs = mxGetNumberOfElements(prhs[0]);
std::vector<callback*> *cb_list = new std::vector<callback*>();
// parse each struct
for (mwIndex i = 0; i < nstructs; i++) {
bool id_found = false;
bool fn_found = false;
callback *cb = new callback();
for (int j = 0; j < nfields; j++) {
mxArray *field = mxGetFieldByNumber(prhs[0], i, j);
if (!field) {
mexWarnMsgTxt("Invalid structure found\n");
continue;
}
const char *fname = mxGetFieldNameByNumber(prhs[0], j);
if (!strncmp(fname, "id", sizeof("id"))) {
if (!mxIsNumeric(field) || mxGetNumberOfElements(field) != 1) {
mexWarnMsgTxt("Invalid id (must be numeric scalar).\n");
continue;
}
id_found = true;
cb->id = (int)mxGetScalar(field);
}
else if (!strncmp(fname, "handle", sizeof("handle"))) {
if (!mxIsClass(field, "function_handle")) {
mexWarnMsgTxt("Invalid handle (must be function_handle).\n");
continue;
}
fn_found = true;
cb->fn = field;
}
}
if (!id_found || !fn_found)
delete cb;
else
cb_list->push_back(cb);
}
// return the list
plhs[0] = create_mex_obj(cb_list);
}
std::string MxArray::fieldname(int fieldnumber) const
{
if (!isStruct())
mexErrMsgIdAndTxt("mexopencv:error", "MxArray is not struct");
const char *fname = mxGetFieldNameByNumber(p_, fieldnumber);
if (!fname)
mexErrMsgIdAndTxt("mexopencv:error",
"Failed to get field name at %d\n", fieldnumber);
return std::string(fname);
}
void setCameraProperty(const EdsCameraRef handle, const mxArray* mxstruct) {
if (mxIsStruct(mxstruct)) {
unsigned numProperties = mxGetNumberOfFields(mxstruct);
for (unsigned iterProperty = 0; iterProperty < numProperties; ++iterProperty) {
setCameraProperty(handle, stringToCameraProperty(mxGetFieldNameByNumber(mxstruct, iterProperty)), \
mxGetFieldByNumber(mxstruct, 0, iterProperty));
}
} else if (!mxIsEmpty(mxstruct)) {
mexErrMsgIdAndTxt(ERROR_ID, "For setting multiple attributes, a struct array must be provided.");
}
}
mxArray *mexProblem::GetFieldbyName(const mxArray *S, integer idxstruct, const char *name)
{
integer nfields = mxGetNumberOfFields(S);
for (integer i = 0; i < nfields; i++)
{
if (strcmp(mxGetFieldNameByNumber(S, i), name) == 0)
{
return mxGetFieldByNumber(S, idxstruct, i);
}
}
return nullptr;
};
void mexProblem::ObtainMxArrayFromElement(mxArray *&Xmx, const Element *X)
{
integer sizeoftempdata = X->GetSizeofTempData();
integer nfields = sizeoftempdata + 1;
std::string *fnames = new std::string[nfields];
X->ObtainTempNames(fnames);
fnames[sizeoftempdata].assign("main");
char **names = new char *[nfields];
for (integer i = 0; i < nfields; i++)
{
names[i] = new char[30];
if (fnames[i].size() >= 30)
{
mexErrMsgTxt("The lengths of field names should be less than 30!");
}
strcpy(names[i], fnames[i].c_str());
}
mxArray *tmp;
const char *name;
const SharedSpace *Sharedtmp;
integer lengthtmp, inc = 1;
const double *Sharedtmpptr;
double *tmpptr;
Xmx = mxCreateStructMatrix(1, 1, nfields, const_cast<const char **> (names));
for (integer i = 0; i < nfields; i++)
{
name = mxGetFieldNameByNumber(Xmx, i);
if (strcmp(name, "main") != 0)
{
Sharedtmp = X->ObtainReadTempData(name);
Sharedtmpptr = Sharedtmp->ObtainReadData();
lengthtmp = Sharedtmp->Getlength();
}
else
{
Sharedtmpptr = X->ObtainReadData();
lengthtmp = X->Getlength();
}
tmp = mxCreateDoubleMatrix(lengthtmp, 1, mxREAL);
tmpptr = mxGetPr(tmp);
dcopy_(&lengthtmp, const_cast<double *> (Sharedtmpptr), &inc, tmpptr, &inc);
mxSetFieldByNumber(Xmx, 0, i, tmp);
}
for (integer i = 0; i < nfields; i++)
delete[] names[i];
delete[] names;
delete[] fnames;
};
/* Pass analyze_structure a pointer to a structure mxArray. Each element
in a structure mxArray holds one or more fields; each field holds zero
or one mxArray. analyze_structure accesses every field of every
element and displays information about it. */
static void
analyze_structure(const mxArray *structure_array_ptr)
{
mwSize total_num_of_elements;
mwIndex index;
int number_of_fields, field_index;
const char *field_name;
const mxArray *field_array_ptr;
mexPrintf("\n");
total_num_of_elements = mxGetNumberOfElements(structure_array_ptr);
number_of_fields = mxGetNumberOfFields(structure_array_ptr);
/* Walk through each structure element. */
for (index=0; index<total_num_of_elements; index++) {
/* For the given index, walk through each field. */
for (field_index=0; field_index<number_of_fields; field_index++) {
mexPrintf("\n\t\t");
display_subscript(structure_array_ptr, index);
field_name = mxGetFieldNameByNumber(structure_array_ptr,
field_index);
mexPrintf(".%s\n", field_name);
field_array_ptr = mxGetFieldByNumber(structure_array_ptr,
index,
field_index);
if (field_array_ptr == NULL) {
mexPrintf("\tEmpty Field\n");
} else {
/* Display a top banner. */
mexPrintf("------------------------------------------------\n");
get_characteristics(field_array_ptr);
analyze_class(field_array_ptr);
mexPrintf("\n");
}
}
mexPrintf("\n\n");
}
}
static void set_object_data(const mxArray *data)
{
OBJECT *obj;
mxArray *pId = mxGetField(data,0,"id");
char id[256];
if (pId==NULL)
output_error("set_object_data(const mxArray *data={...}) did not find a required object id field");
else if (mxGetString(pId,id,sizeof(id)))
output_error("set_object_data(const mxArray *data={...}) couldn't read the object id field");
else if ((obj=object_find_name(id))==NULL)
output_error("set_object_data(const mxArray *data={id='%s',...}) couldn't find object id", id);
else
{
int i;
for (i=0; i<mxGetNumberOfFields(data); i++)
{
const char *name;
const mxArray *pField = mxGetFieldByNumber(data,0,i);
char value[4096];
if ((name=mxGetFieldNameByNumber(data,i))==NULL)
output_error("set_object_data(const mxArray *data={id='%s',...}) couldn't read the name of field %d", id, i);
else if (strcmp(name,"id")==0 || strcmp(name,"class")==0)
{ /* these may not be changed */ }
else if (pField==NULL)
output_error("set_object_data(const mxArray *data={id='%s',...}) couldn't read the object field '%s' for object '%s'", id, name);
else if (mxIsChar(pField) && mxGetString(pField,value,sizeof(value)))
output_error("set_object_data(const mxArray *data={id='%s',...}) couldn't read the string value '%s' from field '%s'", id, value, name);
else if (mxIsDouble(pField) && sprintf(value,"%lg",*(double*)mxGetPr(pField))<1)
output_error("set_object_data(const mxArray *data={id='%s',...}) couldn't read the double value '%lg' from field '%s'", id, *(double*)mxGetPr(pField), name);
else if (mxIsComplex(pField) && sprintf(value,"%lg%+lgi",*(double*)mxGetPr(pField),*(double*)mxGetPi(pField))<1)
output_error("set_object_data(const mxArray *data={id='%s',...}) couldn't read the complex value '%lg%+lgi' from field '%s'", id, *(double*)mxGetPr(pField), *(double*)mxGetPi(pField), name);
else if (mxIsUint32(pField) && sprintf(value,"%lu",*(unsigned int*)mxGetPr(pField))<1)
output_error("set_object_data(const mxArray *data={id='%s',...}) couldn't read the uint32 value '%lu' from field '%s'", id, *(unsigned int*)mxGetPr(pField), name);
else if (strcmp(value,ERROR)==0)
output_error("set_object_data(const mxArray *data={id='%s',...}) couldn't use error value '%s'", id, value);
else if (strcmp(value,NONE)==0 && strcpy(value,"")==NULL)
output_error("set_object_data(const mxArray *data={id='%s',...}) couldn't clear empty value '%s'", id, value);
else if (!object_set_value_by_name(obj,name,value))
output_error("set_object_data(const mxArray *data={id='%s',...}) couldn't read the value '%s' into property '%s'", id, value, name);
}
}
}
bool parse(const mxArray* opts_in)
{
if (!mxIsStruct(opts_in)) {
mexErrMsgTxt("Options parameter must be a structure.\n");
return (false);
}
int num_fields = mxGetNumberOfFields(opts_in);
for (int fn=0; fn<num_fields; fn++) {
const char* opt_name = mxGetFieldNameByNumber(opts_in, fn);
std::string opt_name_str = opt_name;
mxArray *opt_val = mxGetFieldByNumber(opts_in, 0, fn);
if (opt_name_str == "num_max_iter") {
num_max_iter = static_cast<size_t>(mxGetScalar(opt_val));
}
else if (opt_name_str == "rho") {
rho = mxGetScalar(opt_val);
}
else if (opt_name_str == "eps_gnorm") {
eps_gnorm = mxGetScalar(opt_val);
}
else if (opt_name_str == "solver") {
std::string solver_name = GetMatlabString(opt_val);
if (solver_name == "fistadescent") {
solver = SOLVER_FISTADESCENT;
}
else if (solver_name == "lbfgs") {
solver = SOLVER_LBFGS;
}
else {
mexErrMsgTxt("No solver with this name.\n");
return false;
}
}
else {
mexErrMsgTxt("Name of the option is invalid.\n");
return (false);
}
}
return true;
}
bool ConnectionHeader::fromMatlab(const mxArray *array)
{
data_.reset();
if (!mxIsStruct(array) && !mxGetNumberOfElements(array) == 1) return false;
data_.reset(new ros::M_string);
int numberOfFields = mxGetNumberOfFields(array);
for(int i = 0; i < numberOfFields; i++) {
std::string key = mxGetFieldNameByNumber(array, i);
const mxArray *value = mxGetFieldByNumber(array, 0, i);
if (Options::isString(value)) {
data_->insert(ros::StringPair(key, Options::getString(value)));
} else if (Options::isDoubleScalar(value)) {
data_->insert(ros::StringPair(key, boost::lexical_cast<std::string>(Options::getDoubleScalar(value))));
} else if (Options::isIntegerScalar(value)) {
data_->insert(ros::StringPair(key, boost::lexical_cast<std::string>(Options::getIntegerScalar(value))));
} else if (Options::isLogicalScalar(value)) {
data_->insert(ros::StringPair(key, boost::lexical_cast<std::string>(Options::getLogicalScalar(value))));
}
}
return true;
}
void object(mxArray *ma, int i, json_object **jo){
int j = 0;
int n = mxGetNumberOfFields(ma);
mxArray *tmpma;
json_object *tmpobj;
*jo = json_object_new_object();
for(; j < n; j++){
bool needDestroy = false;
tmpma = mxGetFieldByNumber(ma, i, j);
if (tmpma == NULL){
/* Object is NULL, create an empty matrix instead */
tmpma = mxCreateNumericMatrix(0, 0, mxDOUBLE_CLASS, mxREAL);
needDestroy = true;
}
parse(tmpma, &tmpobj);
json_object_object_add(*jo, mxGetFieldNameByNumber(ma, j), tmpobj);
if (needDestroy)
mxDestroyArray(tmpma);
}
}
void mexFunction
(
/* === Parameters ======================================================= */
int nlhs, /* number of left-hand sides */
mxArray *plhs [], /* left-hand side matrices */
int nrhs, /* number of right--hand sides */
const mxArray *prhs [] /* right-hand side matrices */
)
{
Zmat A;
ZAMGlevelmat *PRE;
ZILUPACKparam *param;
integer n;
const char **fnames;
const mwSize *dims;
mxClassID *classIDflags;
mxArray *tmp, *fout, *A_input , *b_input, *x0_input, *options_input,
*PRE_input, *options_output, *x_output;
char *pdata, *input_buf, *output_buf;
mwSize mrows, ncols, buflen, ndim,nnz;
int ifield, status, nfields, ierr,i,j,k,l,m;
size_t sizebuf;
double dbuf, *A_valuesR, *A_valuesI, *convert, *sr, *pr, *pi;
doublecomplex *sol, *rhs;
mwIndex *irs, *jcs,
*A_ja, /* row indices of input matrix A */
*A_ia; /* column pointers of input matrix A */
if (nrhs != 5)
mexErrMsgTxt("five input arguments required.");
else if (nlhs !=2)
mexErrMsgTxt("Too many output arguments.");
else if (!mxIsStruct(prhs[2]))
mexErrMsgTxt("Third input must be a structure.");
else if (!mxIsNumeric(prhs[0]))
mexErrMsgTxt("First input must be a matrix.");
/* The first input must be a square matrix.*/
A_input = (mxArray *) prhs [0] ;
/* get size of input matrix A */
mrows = mxGetM (A_input) ;
ncols = mxGetN (A_input) ;
nnz = mxGetNzmax(A_input);
if (mrows!=ncols) {
mexErrMsgTxt("First input must be a square matrix.");
}
if (!mxIsSparse (A_input))
{
mexErrMsgTxt ("ILUPACK: input matrix must be in sparse format.") ;
}
/* copy input matrix to sparse row format */
A.nc=A.nr=mrows;
A.ia=(integer *) MAlloc((size_t)(A.nc+1)*sizeof(integer),"ZGNLSYMilupacksolver");
A.ja=(integer *) MAlloc((size_t)nnz *sizeof(integer),"ZGNLSYMilupacksolver");
A. a=(doublecomplex *) MAlloc((size_t)nnz *sizeof(doublecomplex), "ZGNLSYMilupacksolver");
A_ja = (mwIndex *) mxGetIr (A_input) ;
A_ia = (mwIndex *) mxGetJc (A_input) ;
A_valuesR = (double *) mxGetPr(A_input);
if (mxIsComplex(A_input))
A_valuesI = (double *) mxGetPi(A_input);
/* -------------------------------------------------------------------- */
/* .. Convert matrix from 0-based C-notation to Fortran 1-based */
/* notation. */
/* -------------------------------------------------------------------- */
/*
for (i = 0 ; i < ncols ; i++)
for (j = A_ia[i] ; j < A_ia[i+1] ; j++)
printf("i=%d j=%d A.real=%e\n", i+1, A_ja[j]+1, A_valuesR[j]);
*/
for (i=0; i<=A.nr; i++)
A.ia[i]=0;
/* remember that MATLAB uses storage by columns and NOT by rows! */
for (i=0; i<A.nr; i++) {
for (j=A_ia[i]; j<A_ia[i+1]; j++) {
k=A_ja[j];
A.ia[k+1]++;
}
}
/* now we know how many entries are located in every row */
/* switch to pointer structure */
for (i=0; i<A.nr; i++)
A.ia[i+1]+=A.ia[i];
if (mxIsComplex(A_input)) {
for (i=0; i<ncols; i++) {
for (j=A_ia[i]; j<A_ia[i+1]; j++) {
/* row index l in C-notation */
l=A_ja[j];
/* where does row l currently start */
k=A.ia[l];
/* column index will be i in FORTRAN notation */
A.ja[k]=i+1;
A.a [k].r =A_valuesR[j];
A.a [k++].i=A_valuesI[j];
A.ia[l]=k;
}
}
}
else {
for (i=0; i<ncols; i++) {
for (j=A_ia[i]; j<A_ia[i+1]; j++) {
/* row index l in C-notation */
l=A_ja[j];
/* where does row l currently start */
k=A.ia[l];
/* column index will be i in FORTRAN notation */
A.ja[k]=i+1;
A.a [k].r =A_valuesR[j];
A.a [k++].i=0;
A.ia[l]=k;
}
}
}
/* switch to FORTRAN style */
for (i=A.nr; i>0; i--)
A.ia[i]=A.ia[i-1]+1;
A.ia[0]=1;
/*
for (i = 0 ; i < A.nr ; i++)
for (j = A.ia[i]-1 ; j < A.ia[i+1]-1 ; j++)
printf("i=%d j=%d A.real=%e A.imag=%e\n", i+1, A.ja[j], A.a[j].r, A.a[j].i);
*/
/* import pointer to the preconditioner */
PRE_input = (mxArray*) prhs [1] ;
/* get number of levels of input preconditioner structure `PREC' */
/* nlev=mxGetN(PRE_input); */
nfields = mxGetNumberOfFields(PRE_input);
/* allocate memory for storing pointers */
fnames = mxCalloc((size_t)nfields, (size_t)sizeof(*fnames));
for (ifield = 0; ifield < nfields; ifield++) {
fnames[ifield] = mxGetFieldNameByNumber(PRE_input,ifield);
/* check whether `PREC.ptr' exists */
if (!strcmp("ptr",fnames[ifield])) {
/* field `ptr' */
tmp = mxGetFieldByNumber(PRE_input,0,ifield);
pdata = mxGetData(tmp);
memcpy(&PRE, pdata, (size_t)sizeof(size_t));
}
else if (!strcmp("param",fnames[ifield])) {
/* field `param' */
tmp = mxGetFieldByNumber(PRE_input,0,ifield);
pdata = mxGetData(tmp);
memcpy(¶m, pdata, (size_t)sizeof(size_t));
}
}
mxFree(fnames);
/* rescale input matrix */
/* obsolete
for (i=0; i <A.nr; i++) {
for (j=A.ia[i]-1; j<A.ia[i+1]-1; j++) {
A.a[j].r*=PRE->rowscal[i].r*PRE->colscal[A.ja[j]-1].r;
A.a[j].i*=PRE->rowscal[i].r*PRE->colscal[A.ja[j]-1].r;
}
}
*/
/* Get third input argument `options' */
options_input=(mxArray*)prhs[2];
nfields = mxGetNumberOfFields(options_input);
/* Allocate memory for storing classIDflags */
classIDflags = (mxClassID *) mxCalloc((size_t)nfields+1, (size_t)sizeof(mxClassID));
/* allocate memory for storing pointers */
fnames = mxCalloc((size_t)nfields+1, (size_t)sizeof(*fnames));
/* Get field name pointers */
j=-1;
for (ifield = 0; ifield < nfields; ifield++) {
fnames[ifield] = mxGetFieldNameByNumber(options_input,ifield);
/* check whether `options.niter' already exists */
if (!strcmp("niter",fnames[ifield]))
j=ifield;
}
if (j==-1)
fnames[nfields]="niter";
/* mexPrintf("search for niter completed\n"); fflush(stdout); */
/* import data */
for (ifield = 0; ifield < nfields; ifield++) {
/* mexPrintf("%2d\n",ifield+1); fflush(stdout); */
tmp = mxGetFieldByNumber(options_input,0,ifield);
classIDflags[ifield] = mxGetClassID(tmp);
ndim = mxGetNumberOfDimensions(tmp);
dims = mxGetDimensions(tmp);
/* Create string/numeric array */
if (classIDflags[ifield] == mxCHAR_CLASS) {
/* Get the length of the input string. */
buflen = (mxGetM(tmp) * mxGetN(tmp)) + 1;
/* Allocate memory for input and output strings. */
input_buf = (char *) mxCalloc((size_t)buflen, (size_t)sizeof(char));
/* Copy the string data from tmp into a C string
input_buf. */
status = mxGetString(tmp, input_buf, buflen);
if (!strcmp("amg",fnames[ifield])) {
if (strcmp(param->amg,input_buf)) {
param->amg=(char *)MAlloc((size_t)buflen*sizeof(char),"ilupacksolver");
strcpy(param->amg,input_buf);
}
}
else if (!strcmp("presmoother",fnames[ifield])) {
if (strcmp(param->presmoother,input_buf)) {
param->presmoother=(char *)MAlloc((size_t)buflen*sizeof(char),
"ilupacksolver");
strcpy(param->presmoother,input_buf);
}
}
else if (!strcmp("postsmoother",fnames[ifield])) {
if (strcmp(param->postsmoother,input_buf)) {
param->postsmoother=(char *)MAlloc((size_t)buflen*sizeof(char),
"ilupacksolver");
strcpy(param->postsmoother,input_buf);
}
}
else if (!strcmp("typecoarse",fnames[ifield])) {
if (strcmp(param->typecoarse,input_buf)) {
param->typecoarse=(char *)MAlloc((size_t)buflen*sizeof(char),
"ilupacksolver");
strcpy(param->typecoarse,input_buf);
}
}
else if (!strcmp("typetv",fnames[ifield])) {
if (strcmp(param->typetv,input_buf)) {
param->typetv=(char *)MAlloc((size_t)buflen*sizeof(char),
"ilupacksolver");
strcpy(param->typetv,input_buf);
}
}
else if (!strcmp("FCpart",fnames[ifield])) {
if (strcmp(param->FCpart,input_buf)) {
param->FCpart=(char *)MAlloc((size_t)buflen*sizeof(char),
"ilupacksolver");
strcpy(param->FCpart,input_buf);
}
}
else if (!strcmp("solver",fnames[ifield])) {
if (strcmp(param->solver,input_buf)) {
param->solver=(char *)MAlloc((size_t)buflen*sizeof(char),
"ilupacksolver");
strcpy(param->solver,input_buf);
}
}
else if (!strcmp("ordering",fnames[ifield])) {
if (strcmp(param->ordering,input_buf)) {
param->ordering=(char *)MAlloc((size_t)buflen*sizeof(char),
"ilupacksolver");
strcpy(param->ordering,input_buf);
}
}
else {
/* mexPrintf("%s ignored\n",fnames[ifield]);fflush(stdout); */
}
}
else {
if (!strcmp("elbow",fnames[ifield])) {
param->elbow=*mxGetPr(tmp);
}
else if (!strcmp("lfilS",fnames[ifield])) {
param->lfilS=*mxGetPr(tmp);
}
else if (!strcmp("lfil",fnames[ifield])) {
param->lfil=*mxGetPr(tmp);
}
else if (!strcmp("maxit",fnames[ifield])) {
param->maxit=*mxGetPr(tmp);
}
else if (!strcmp("droptolS",fnames[ifield])) {
param->droptolS=*mxGetPr(tmp);
}
else if (!strcmp("droptolc",fnames[ifield])) {
param->droptolc=*mxGetPr(tmp);
}
else if (!strcmp("droptol",fnames[ifield])) {
param->droptol=*mxGetPr(tmp);
}
else if (!strcmp("condest",fnames[ifield])) {
param->condest=*mxGetPr(tmp);
}
else if (!strcmp("restol",fnames[ifield])) {
param->restol=*mxGetPr(tmp);
}
else if (!strcmp("npresmoothing",fnames[ifield])) {
param->npresmoothing=*mxGetPr(tmp);
}
else if (!strcmp("npostmoothing",fnames[ifield])) {
param->npostsmoothing=*mxGetPr(tmp);
}
else if (!strcmp("ncoarse",fnames[ifield])) {
param->ncoarse=*mxGetPr(tmp);
}
else if (!strcmp("matching",fnames[ifield])) {
param->matching=*mxGetPr(tmp);
}
else if (!strcmp("nrestart",fnames[ifield])) {
param->nrestart=*mxGetPr(tmp);
}
else if (!strcmp("damping",fnames[ifield])) {
param->damping.r=*mxGetPr(tmp);
if (mxIsComplex(tmp))
param->damping.i=*mxGetPi(tmp);
else
param->damping.i=0;
}
else if (!strcmp("mixedprecision",fnames[ifield])) {
param->mixedprecision=*mxGetPr(tmp);
}
else {
/* mexPrintf("%s ignored\n",fnames[ifield]);fflush(stdout); */
}
}
}
/* copy right hand side `b' */
b_input = (mxArray *) prhs [3] ;
/* get size of input matrix A */
rhs=(doublecomplex*) MAlloc((size_t)A.nr*sizeof(doublecomplex),"ZGNLSYMilupacksolver:rhs");
pr=mxGetPr(b_input);
if (!mxIsComplex(b_input)) {
for (i=0; i<A.nr; i++) {
rhs[i].r=pr[i];
rhs[i].i=0;
}
}
else {
pi=mxGetPi(b_input);
for (i=0; i<A.nr; i++) {
rhs[i].r=pr[i];
rhs[i].i=pi[i];
}
}
/* copy initial solution `x0' */
x0_input = (mxArray *) prhs [4] ;
/* numerical solution */
sol=(doublecomplex *)MAlloc((size_t)A.nr*sizeof(doublecomplex),"ZGNLSYMilupacksolver:sol");
pr=mxGetPr(x0_input);
if (!mxIsComplex(x0_input)) {
for (i=0; i<A.nr; i++) {
sol[i].r=pr[i];
sol[i].i=0;
}
}
else {
pi=mxGetPi(x0_input);
for (i=0; i<A.nr; i++) {
sol[i].r=pr[i];
sol[i].i=pi[i];
}
}
/* set bit 2, transpose */
param->ipar[4]|=4;
/* set bit 3, conjugate */
param->ipar[4]|=8;
ierr=ZGNLSYMAMGsolver(&A, PRE, param, rhs, sol);
/* Create a struct matrices for output */
nlhs=2;
if (j==-1)
plhs[1] = mxCreateStructMatrix((mwSize)1, (mwSize)1, (mwSize)nfields+1, fnames);
else
plhs[1] = mxCreateStructMatrix((mwSize)1, (mwSize)1, (mwSize)nfields, fnames);
if (plhs[1]==NULL)
mexErrMsgTxt("Could not create structure mxArray\n");
options_output=plhs[1];
/* export data */
for (ifield = 0; ifield<nfields; ifield++) {
tmp = mxGetFieldByNumber(options_input,0,ifield);
classIDflags[ifield] = mxGetClassID(tmp);
ndim = mxGetNumberOfDimensions(tmp);
dims = mxGetDimensions(tmp);
/* Create string/numeric array */
if (classIDflags[ifield] == mxCHAR_CLASS) {
if (!strcmp("amg",fnames[ifield])) {
output_buf = (char *) mxCalloc((size_t)strlen(param->amg)+1, (size_t)sizeof(char));
strcpy(output_buf,param->amg);
fout = mxCreateString(output_buf);
}
else if (!strcmp("presmoother",fnames[ifield])) {
output_buf = (char *) mxCalloc((size_t)strlen(param->presmoother)+1, (size_t)sizeof(char));
strcpy(output_buf,param->presmoother);
fout = mxCreateString(output_buf);
}
else if (!strcmp("postsmoother",fnames[ifield])) {
output_buf = (char *) mxCalloc((size_t)strlen(param->postsmoother)+1, (size_t)sizeof(char));
strcpy(output_buf,param->postsmoother);
fout = mxCreateString(output_buf);
}
else if (!strcmp("typecoarse",fnames[ifield])) {
output_buf = (char *) mxCalloc((size_t)strlen(param->typecoarse)+1, (size_t)sizeof(char));
strcpy(output_buf,param->typecoarse);
fout = mxCreateString(output_buf);
}
else if (!strcmp("typetv",fnames[ifield])) {
output_buf = (char *) mxCalloc((size_t)strlen(param->typetv)+1, (size_t)sizeof(char));
strcpy(output_buf,param->typetv);
fout = mxCreateString(output_buf);
}
else if (!strcmp("FCpart",fnames[ifield])) {
output_buf = (char *) mxCalloc((size_t)strlen(param->FCpart)+1, (size_t)sizeof(char));
strcpy(output_buf,param->FCpart);
fout = mxCreateString(output_buf);
}
else if (!strcmp("solver",fnames[ifield])) {
output_buf = (char *) mxCalloc((size_t)strlen(param->solver)+1, (size_t)sizeof(char));
strcpy(output_buf, param->solver);
fout = mxCreateString(output_buf);
}
else if (!strcmp("ordering",fnames[ifield])) {
output_buf = (char *) mxCalloc((size_t)strlen(param->ordering)+1, (size_t)sizeof(char));
strcpy(output_buf, param->ordering);
fout = mxCreateString(output_buf);
}
else {
/* Get the length of the input string. */
buflen = (mxGetM(tmp) * mxGetN(tmp)) + 1;
/* Allocate memory for input and output strings. */
input_buf = (char *) mxCalloc((size_t)buflen, (size_t)sizeof(char));
output_buf = (char *) mxCalloc((size_t)buflen, (size_t)sizeof(char));
/* Copy the string data from tmp into a C string
input_buf. */
status = mxGetString(tmp, input_buf, buflen);
sizebuf = (size_t)buflen*sizeof(char);
memcpy(output_buf, input_buf, sizebuf);
fout = mxCreateString(output_buf);
}
}
else {
/* real case */
if (mxGetPi(tmp)==NULL && strcmp("damping",fnames[ifield]))
fout = mxCreateNumericArray(ndim, dims,
classIDflags[ifield], mxREAL);
else { /* complex case */
fout = mxCreateNumericArray(ndim, dims,
classIDflags[ifield], mxCOMPLEX);
}
pdata = mxGetData(fout);
sizebuf = mxGetElementSize(tmp);
if (!strcmp("elbow",fnames[ifield])) {
dbuf=param->elbow;
memcpy(pdata, &dbuf, sizebuf);
}
else if (!strcmp("lfilS",fnames[ifield])) {
dbuf=param->lfilS;
memcpy(pdata, &dbuf, sizebuf);
}
else if (!strcmp("lfil",fnames[ifield])) {
dbuf=param->lfil;
memcpy(pdata, &dbuf, sizebuf);
}
else if (!strcmp("maxit",fnames[ifield])) {
dbuf=param->maxit;
memcpy(pdata, &dbuf, sizebuf);
}
else if (!strcmp("droptolS",fnames[ifield])) {
dbuf=param->droptolS;
memcpy(pdata, &dbuf, sizebuf);
}
else if (!strcmp("droptolc",fnames[ifield])) {
dbuf=param->droptolc;
memcpy(pdata, &dbuf, sizebuf);
}
else if (!strcmp("droptol",fnames[ifield])) {
dbuf=param->droptol;
memcpy(pdata, &dbuf, sizebuf);
}
else if (!strcmp("condest",fnames[ifield])) {
dbuf=param->condest;
memcpy(pdata, &dbuf, sizebuf);
}
else if (!strcmp("restol",fnames[ifield])) {
dbuf=param->restol;
memcpy(pdata, &dbuf, sizebuf);
}
else if (!strcmp("npresmoothing",fnames[ifield])) {
dbuf=param->npresmoothing;
memcpy(pdata, &dbuf, sizebuf);
}
else if (!strcmp("npostmoothing",fnames[ifield])) {
dbuf=param->npostsmoothing;
memcpy(pdata, &dbuf, sizebuf);
}
else if (!strcmp("ncoarse",fnames[ifield])) {
dbuf=param->ncoarse;
memcpy(pdata, &dbuf, sizebuf);
}
else if (!strcmp("matching",fnames[ifield])) {
dbuf=param->matching;
memcpy(pdata, &dbuf, sizebuf);
}
else if (!strcmp("nrestart",fnames[ifield])) {
dbuf=param->nrestart;
memcpy(pdata, &dbuf, sizebuf);
}
else if (!strcmp("damping",fnames[ifield])) {
pr=mxGetPr(fout);
pi=mxGetPi(fout);
*pr=param->damping.r;
*pi=param->damping.i;
}
else if (!strcmp("mixedprecision",fnames[ifield])) {
dbuf=param->mixedprecision;
memcpy(pdata, &dbuf, sizebuf);
}
else {
memcpy(pdata, mxGetData(tmp), sizebuf);
}
}
/* Set each field in output structure */
mxSetFieldByNumber(options_output, (mwIndex)0, ifield, fout);
}
/* store number of iteration steps */
if (j==-1)
ifield=nfields;
else
ifield=j;
fout=mxCreateDoubleMatrix((mwSize)1,(mwSize)1, mxREAL);
pr=mxGetPr(fout);
*pr=param->ipar[26];
/* set each field in output structure */
mxSetFieldByNumber(options_output, (mwIndex)0, ifield, fout);
mxFree(fnames);
mxFree(classIDflags);
/* mexPrintf("options exported\n"); fflush(stdout); */
/* export approximate solution */
plhs[0] = mxCreateDoubleMatrix((mwSize)A.nr, (mwSize)1, mxCOMPLEX);
x_output=plhs[0];
pr=mxGetPr(x_output);
pi=mxGetPi(x_output);
for (i=0; i<A.nr; i++) {
pr[i]=sol[i].r;
pi[i]=sol[i].i;
}
/* release right hand side */
free(rhs);
/* release solution */
free(sol);
/* release input matrix */
free(A.ia);
free(A.ja);
free(A.a);
switch (ierr) {
case 0: /* perfect! */
break;
case -1: /* too many iteration steps */
mexPrintf("!!! ILUPACK Warning !!!\n");
mexPrintf("number of iteration steps exceeded its limit.\nEither increase `options.maxit'\n or recompute ILUPACK preconditioner using a smaller `options.droptol'");
break;
case -2: /* weird, should not occur */
mexErrMsgTxt("not enough workspace provided.");
plhs[0]=NULL;
break;
case -3: /* breakdown */
mexErrMsgTxt("iterative solver breaks down.\nMost likely you need to recompute ILUPACK preconditioner using a smaller `options.droptol'");
plhs[0]=NULL;
break;
default: /* zero pivot encountered at step number ierr */
mexPrintf("iterative solver exited with error code %d",ierr);
mexErrMsgTxt(".");
plhs[0]=NULL;
break;
} /* end switch */
return;
}
MP_Dict_c * mp_create_dict_from_mxDict(const mxArray *mxDict)
{
const char *func = "mp_create_dict_from_mxDict";
const char *fieldName;
map<string,string,mp_ltstring> *paramMap;
MP_Dict_c *dict = NULL;
mxArray *mxBlockCell,*mxBlock,*mxTmp;
mwSize mwNumDimension;
const mwSize *mwDimension;
size_t numFields;
int nBlocks;
int iIndexDimension,iDimension;
char *fieldValue;
double *dTable = NULL;
string szDataString,szReturnString;
if (NULL==mxDict)
{
mp_error_msg(func,"input is NULL\n");
return(NULL);
}
// Check that the input dictionary structure has the right fields
mxBlockCell = mxGetField(mxDict,0,"block");
if (NULL==mxBlockCell)
{
mp_error_msg(func,"the dict.block field is missing\n");
return(NULL);
}
nBlocks = (int)mxGetNumberOfElements(mxBlockCell);
if (0==nBlocks)
{
mp_error_msg(func,"the number of blocks should be at least one\n");
return(NULL);
}
if(!mxIsCell(mxBlockCell))
{
mp_error_msg(func,"the dict.block is not a cell array\n");
return(NULL);
}
// Reach all blocks
for (int i = 0; i < nBlocks; i++ )
{
mxBlock = mxGetCell(mxBlockCell,i);
if (NULL==mxBlock)
{
// This should never happen
mp_error_msg(func,"dict.block{%d} could not be retrieved\n",i+1);
// Clean the house
if(NULL!=dict) delete dict;
return(NULL);
}
numFields = mxGetNumberOfFields(mxBlock);
if (0==numFields)
{
mp_error_msg(func,"the number of fields %d should be at least one in dict.block{%d}\n",numFields,i+1);
// Clean the house
if(NULL!=dict) delete dict;
return(NULL);
}
// Reach all fields of the block and put them in a map
paramMap = new map<string, string, mp_ltstring>();
if(NULL==paramMap)
{
mp_error_msg(func,"could not allocate paramMap\n");
// Clean the house
if(NULL!=dict) delete dict;
return(NULL);
}
for (unsigned int j= 0; j <numFields ; j++)
{
fieldName = mxGetFieldNameByNumber(mxBlock,j);
if (fieldName == NULL)
{
mp_error_msg(func,"field number %d in dict.block{%d} could not be retrieved\n",j+1,i+1);
// Clean the house
if(NULL!=dict) delete dict;
return(NULL);
}
// Retrieve the field value
mxTmp = mxGetField(mxBlock,0,fieldName);
if(mxTmp == NULL)
{
mp_error_msg(func,"value of field number %d in dict.block{%d} could not be retrieved\n",j+1,i+1);
// Clean the house
if(NULL!=dict) delete dict;
return(NULL);
}
if(mxIsDouble(mxTmp))
{
// Retrieve the dimension of the double
mwNumDimension = mxGetNumberOfDimensions(mxTmp);
mwDimension = mxGetDimensions(mxTmp);
iDimension = 1;
for(iIndexDimension = 0; iIndexDimension < (int)mwNumDimension; iIndexDimension++)
iDimension *= mwDimension[iIndexDimension];
// Add the dimension of a double
iDimension = iDimension * sizeof(double);
// Getting the storage field
if((dTable = (MP_Real_t*)malloc(iDimension)) == NULL)
{
mp_error_msg(func,"The double storage has not been allocaed\n");
return(NULL);
}
// Loading the mxArray
if(!mp_get_anywave_datas_from_mxAnywaveTable(mxBlock,dTable))
{
mp_error_msg(func,"A double value has been found but could not be retrieved\n");
// Clean the house
if(NULL!=dict) delete dict;
return(NULL);
}
// Store it in the map and free
(*paramMap)["doubledata"] = string((char *)dTable, iDimension);
}
else
{
fieldValue = mxArrayToString(mxTmp);
if(fieldValue == NULL)
{
mp_error_msg(func,"string value of field number %d in dict.block{%d} could not be retrieved\n",j+1,i+1);
// Clean the house
if(NULL!=dict) delete dict;
return(NULL);
}
// Store it in the map and free
(*paramMap)[string(fieldName)]=string(fieldValue);
mxFree(fieldValue);
fieldValue = NULL;
}
}
// Retrieve the block creator
MP_Block_c* (*blockCreator)( MP_Signal_c *setSignal, map<string, string, mp_ltstring> * paramMap ) = NULL;
blockCreator = MP_Block_Factory_c::get_block_creator((*paramMap)["type"].c_str());
if (NULL == blockCreator)
{
mp_error_msg(func,"the block factory does not contain type %s of dict.block{%d}\n",(*paramMap)["type"].c_str(),i+1);
// Clean the house
if(NULL!=dict) delete dict;
delete paramMap;
return(NULL);
}
// Create the block
MP_Block_c *block = blockCreator(NULL, paramMap);
if (NULL == block)
{
mp_error_msg(func,"the dict.block{%d}, of type %s was not successfully created\n",i+1,(*paramMap)["type"].c_str());
// Clean the house
if(NULL!=dict) delete dict;
delete paramMap;
return(NULL);
}
// Create the dictionary if needed (i.e. when adding first block)
if (NULL==dict)
{
dict = MP_Dict_c::init();
if (NULL==dict)
{
mp_error_msg(func,"Failed to create an empty dictionary\n");
delete paramMap;
delete block;
return(NULL);
}
}
// Add the block to the dictionary
dict->add_block(block);
if(dTable) free(dTable);
delete paramMap;
}
return(dict);
}
void mexFunction
(
/* === Parameters ======================================================= */
int nlhs, /* number of left-hand sides */
mxArray *plhs [], /* left-hand side matrices */
int nrhs, /* number of right--hand sides */
const mxArray *prhs [] /* right-hand side matrices */
)
{
Zmat A;
ZAMGlevelmat *PRE;
CAMGlevelmat *SPRE;
ZILUPACKparam *param;
const char **fnames; /* pointers to field names */
mxClassID *classIDflags;
mxArray *tmp, *PRE_input;
mwSize ndim, buflen;
char *pdata, *input_buf;
int ifield, status, nfields;
size_t mrows, ncols, sizebuf;
double dbuf;
if (nrhs != 1)
mexErrMsgTxt("One input arguments required.");
else if (nlhs > 0)
mexErrMsgTxt("No output arguments.");
else if (!mxIsStruct(prhs[0]))
mexErrMsgTxt("Input must be a structure.");
/* import pointer to the preconditioner */
PRE_input = (mxArray*) prhs [0] ;
/* get number of levels of input preconditioner structure `PREC' */
/* nlev=mxGetN(PRE_input); */
nfields = mxGetNumberOfFields(PRE_input);
/* allocate memory for storing pointers */
fnames = mxCalloc((size_t)nfields, (size_t)sizeof(*fnames));
for (ifield = 0; ifield < nfields; ifield++) {
fnames[ifield] = mxGetFieldNameByNumber(PRE_input,ifield);
/* check whether `PREC.ptr' exists */
if (!strcmp("ptr",fnames[ifield])) {
/* field `ptr' */
tmp = mxGetFieldByNumber(PRE_input,0,ifield);
pdata = mxGetData(tmp);
memcpy(&PRE, pdata, (size_t)sizeof(size_t));
}
else if (!strcmp("param",fnames[ifield])) {
/* field `param' */
tmp = mxGetFieldByNumber(PRE_input,0,ifield);
pdata = mxGetData(tmp);
memcpy(¶m, pdata, (size_t)sizeof(size_t));
}
else if (!strcmp("n",fnames[ifield])) {
/* get size of the initial system */
tmp = mxGetFieldByNumber(PRE_input,0,ifield);
A.nr=A.nc=*mxGetPr(tmp);
A.ia=A.ja=NULL;
A.a=NULL;
}
}
mxFree(fnames);
/* finally release memory of the preconditioner */
ZGNLAMGdelete(&A,PRE,param);
return;
}
/* the gateway routine. */
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
/* create a pointer to the real data in the input matrix */
int nfields = mxGetNumberOfFields(prhs[0]); // number of fields in each constraint
mwSize NStructElems = mxGetNumberOfElements(prhs[0]); // number of constraints
const char **fnames = (const char **)mxCalloc(nfields, sizeof(*fnames)); /* pointers to field names */
double *sw = mxGetPr(prhs[1]); /* switch vector */
double *xs = mxGetPr(prhs[2]); /* linearisation point */
long unsigned int nrow = mxGetM(prhs[2]);
double *ptr = mxGetPr(prhs[3]); /* number of cameras */
int ncams = ptr[0];
/* initialise a PwgOptimiser object */
PwgOptimiser *Object; // pointer initialisation
Object = new PwgOptimiser (ncams, nrow-6*ncams) ; // pointer initialisation
/* get constraints from Matlab to C++ and initialise a constraint */
double *pr;
mxArray *tmp;
int cam, kpt;
std::vector<double> p1(2), z(2), R(4,0.0);//, Y(49,0.0), y(7,0.0);
std::string str1 ("cam");
std::string str2 ("kpt");
std::string str3 ("p1");
std::string str4 ("z");
std::string str5 ("R");
std::string str6 ("y");
std::string str7 ("Y");
Eigen::MatrixXd yz = Eigen::MatrixXd::Zero(7,1);
Eigen::VectorXd Yz = Eigen::MatrixXd::Zero(7,7);
for (mwIndex jstruct = 0; jstruct < NStructElems; jstruct++) { /* loop the constraints */
for (int ifield = 0; ifield < nfields; ifield++) { /* loop the fields */
fnames[ifield] = mxGetFieldNameByNumber(prhs[0],ifield); // get field name
tmp = mxGetFieldByNumber(prhs[0], jstruct, ifield); // get field
pr = (double *)mxGetData(tmp); // get the field data pointer
if (str1.compare(fnames[ifield]) == 0) // cam
cam = pr[0];
if (str2.compare(fnames[ifield]) == 0) // kpt
kpt = pr[0];
if (str3.compare(fnames[ifield]) == 0){ // p1
p1[0] = pr[0]; p1[1] = pr[1];}
if (str4.compare(fnames[ifield]) == 0){ // z
z[0] = pr[0]; z[1] = pr[1];}
if (str5.compare(fnames[ifield]) == 0){ // R
R[0] = pr[0]; R[3] = pr[3];}
} // end of nfields loop
Object->initialise_a_constraint(cam, kpt, p1, z, R, Yz, yz, (int)sw[jstruct]) ; // using pointer to object
} // end of NStructElems loop
// optimise constraints information
Object->optimise_constraints_image_inverse_depth_Mviews( xs ) ;
/* get output state vector */
int ncol = (Object->xhat).size();
plhs[0] = mxCreateDoubleMatrix(ncol, 1, mxREAL);
double *vec_out = mxGetPr(plhs[0]);
for (int i=0; i<ncol; i++)
vec_out[i] = (Object->xhat).coeffRef(i);
/* get output sparse covariance matrix */
ncol = (Object->Phat).outerSize();
long unsigned int nzmax = (Object->Phat).nonZeros();
plhs[1] = mxCreateSparse(ncol, ncol, nzmax, mxREAL);
double *mat_out = mxGetPr(plhs[1]);
long unsigned int *ir2 = mxGetIr(plhs[1]);
long unsigned int *jc2 = mxGetJc(plhs[1]);
int index=0;
for (int k=0; k < (Object->Phat).outerSize(); ++k)
{
jc2[k] = index;
for (Eigen::SparseMatrix<double>::InnerIterator it(Object->Phat,k); it; ++it)
{
ir2[index] = it.row();
mat_out[index] = it.value();
index++;
}
}
jc2[(Object->Phat).outerSize()] = index;
/* Free memory */
mxFree((void *)fnames);
delete Object; // delete class pointer
//mxFree(tmp);
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[]){
int n_fields, f;
const char *field_name;
int n_dims= 0, c;
long n_tri, t, n_xyz, n, p;
double *xyz_e;
int *ijk;
long *new_ids;
double threshold;
long last_id;
double *DATA;
double *newDATA;
int n_cols;
double distance;
double d;
long nn_xyz;
mxArray *mxNewDATA;
int N[3];
threshold= *( mxGetPr( prhs[1] ) );
threshold= threshold*threshold;
n_xyz = mxGetM( mxGetField( prhs[0], 0, "xyz") );
/* // mexPrintf("%d\n",n_xyz); myFlush(); */
ijk= mxMalloc( 3 * n_xyz * sizeof(int) );
SCAMCOORD( ijk , prhs[0], mxGetPr( mxGetField( prhs[0], 0, "xyz") ) , n_xyz, N);
/* // for( n=0; n<n_xyz; n++){
// mexPrintf( "%d %d %d\n",
// ijk[n],ijk[n+n_xyz],ijk[n+2*n_xyz]);
// } */
xyz_e = mxMalloc( n_xyz*sizeof( double ) );
n_fields= mxGetNumberOfFields( prhs[0] );
n_dims= 0;
for( f=0 ; f < n_fields ; f++ ){
/* // mexPrintf("%d\n",f); myFlush(); */
field_name= mxGetFieldNameByNumber( prhs[0] , f );
if( !strncmp( field_name,"xyz",3 ) || !strcmp( field_name, "uv") ){
DATA = mxGetPr( mxGetField( prhs[0], 0, field_name ) );
n_cols= mxGetN( mxGetField( prhs[0], 0, field_name ) );
xyz_e= mxRealloc( xyz_e, (n_dims+n_cols)*n_xyz*sizeof( double ) );
for( c=0 ; c < n_cols ; c++ ){
for( n= 0; n< n_xyz ; n++ ){
xyz_e[ n_dims*n_xyz + n ] = DATA[ c*n_xyz + n ];
}
n_dims++;
}
}
}
/* // mexPrintf("\n\nPorAca1\n\n"); */
new_ids= mxMalloc( n_xyz*sizeof(long) );
for( n=0 ; n < n_xyz ; n++ ){
new_ids[n]= -1;
}
/* // mexPrintf("\n\nPorAca2\n\n"); */
last_id= 0;
for( n=0; n < n_xyz ; n++ ){
/* // mexPrintf( "n: %d of %d\n" , n , n_xyz ); myFlush(); */
if( new_ids[n] != -1 ){
continue;
}
new_ids[n] = last_id;
for( p=n+1 ; p<n_xyz ; p++ ){
/* // mexPrintf("n: %d p: %d de %d", n,p,n_xyz); myFlush(); */
/* // mexPrintf("%d %d %f\n", ijk[p+2*n_xyz],ijk[n+2*n_xyz],fabs(1)); */
if( new_ids[p] != -1 ) { continue; }
if( fabs(ijk[p ] - ijk[n ]) > 1 ) { continue; }
if( fabs(ijk[p+ n_xyz] - ijk[n+ n_xyz]) > 1 ) { continue; }
if( fabs(ijk[p+2*n_xyz] - ijk[n+2*n_xyz]) > 1 ) { continue; }
/* // mexPrintf(" distance0 - " ); myFlush(); */
distance=0;
for( c=0 ; c<n_dims & distance<=threshold ; c++ ){
d = ( xyz_e[ c*n_xyz + p] - xyz_e[ c*n_xyz + n] );
distance += d*d;
}
/* // mexPrintf(" distance: %f ", distance); myFlush(); */
/* // mexPrintf(" new_ids: %d ", new_ids[ p ]); myFlush(); */
if( distance <= threshold ){
new_ids[ p ]= last_id;
/* // mexPrintf("last_id : %d\n",last_id); myFlush(); */
}
/* // mexPrintf("\n"); myFlush(); */
}
last_id++;
}
nn_xyz= last_id;
plhs[0]= mxDuplicateArray( prhs[0] );
for( f=0 ; f<n_fields ; f++ ){
field_name= mxGetFieldNameByNumber( plhs[0] , f );
/* // mexPrintf("%s ",field_name); */
if( !strncmp( field_name,"xyz",3 ) || !strcmp( field_name, "uv") ){
DATA = mxGetPr( mxGetField( plhs[0], 0, field_name ) );
n_cols = mxGetN( mxGetField( plhs[0], 0, field_name ) );
mxNewDATA= mxCreateDoubleMatrix( nn_xyz , n_cols , mxREAL );
newDATA = mxGetPr( mxNewDATA );
last_id= 0;
for( n=0 ; n<n_xyz ; n++ ){
if( new_ids[n] != last_id ) { continue; }
for( c=0 ; c<n_cols ; c++ ){
newDATA[ c*nn_xyz + last_id ]= DATA[ c*n_xyz + n ];
}
last_id++;
}
mxSetField( plhs[0], 0, field_name, mxNewDATA );
}
if( !strcmp( field_name,"tri" ) ){
DATA = mxGetPr( mxGetField( plhs[0], 0, field_name ) );
n_tri = mxGetM( mxGetField( plhs[0], 0, field_name ) );
for( t=0 ; t<n_tri ; t++ ){
DATA[t ]=new_ids[(long)(DATA[t ]-1)]+1;
DATA[t+ n_tri]=new_ids[(long)(DATA[t+ n_tri]-1)]+1;
DATA[t+2*n_tri]=new_ids[(long)(DATA[t+2*n_tri]-1)]+1;
}
}
if( !strcmp( field_name,"PLOC" ) ){
mxRemoveField( plhs[0],f);
n_fields--; f--;
}
if( !strcmp( field_name,"PSUP" ) ){
mxRemoveField( plhs[0],f);
n_fields--; f--;
}
if( !strcmp( field_name,"ESUP" ) ){
mxRemoveField( plhs[0],f);
n_fields--; f--;
}
/* // mexPrintf("ok\n"); */
}
mxFree(ijk);
mxFree(new_ids);
mxFree(xyz_e);
}
static PyObject* mat2py(const mxArray *a) {
size_t ndims = mxGetNumberOfDimensions(a);
const mwSize *dims = mxGetDimensions(a);
mxClassID cls = mxGetClassID(a);
size_t nelem = mxGetNumberOfElements(a);
char *data = (char*) mxGetData(a);
char *imagData = (char*) mxGetImagData(a);
if (debug) mexPrintf("cls = %d, nelem = %d, ndims = %d, dims[0] = %d, dims[1] = %d\n", cls, nelem, ndims, dims[0], dims[1]);
if (cls == mxCHAR_CLASS) {
char *str = mxArrayToString(a);
PyObject *o = PyString_FromString(str);
mxFree(str);
return o;
} else if (mxIsStruct(a)) {
PyObject *o = PyDict_New();
PyObject *list;
PyObject *pyItem;
mxArray *item;
int i, j;
const char *fieldName;
int nfields = mxGetNumberOfFields(a);
if (debug) mexPrintf("nfields = %d, nelem = %d\n", nfields, nelem);
for(i = 0; i < nfields; i++) {
fieldName = mxGetFieldNameByNumber(a, i);
list = PyList_New(nelem);
for(j = 0; j < nelem; j++) {
item = mxGetFieldByNumber(a, j, i);
if(item == NULL)
{
Py_DECREF(list);
Py_DECREF(o);
mexErrMsgIdAndTxt("matpy:NullFieldValue", "Null field in struct");
}
pyItem = mat2py(item);
if(pyItem == NULL)
{
Py_DECREF(list);
Py_DECREF(o);
mexErrMsgIdAndTxt("matpy:UnsupportedVariableType", "Unsupported variable type in struct");
}
PyList_SetItem(list, j, pyItem);
}
PyDict_SetItemString(o, fieldName, list);
}
Py_DECREF(list);
return o;
}
PyObject *list = PyList_New(nelem);
const char *dtype = NULL;
if (mxIsCell(a))
{
dtype = "object";
for (int i = 0; i < nelem; i++)
{
PyObject *item = mat2py(mxGetCell(a, i));
if(NULL != item)
{
PyList_SetItem(list, i, item);
}
else // Failure
{
Py_DECREF(list);
mexErrMsgIdAndTxt("matpy:UnsupportedVariableType", "Unsupported variable type in a cell");
}
}
return list;
} else {
if (imagData == NULL) {
#define CASE(cls,c_type,d_type,py_ctor) case cls: for (int i = 0; i < nelem; i++) { \
dtype=d_type; \
PyObject *item = py_ctor(*((c_type*) data)); \
if (debug) mexPrintf("Setting %d to %f (item = 0x%08X)\n", i, (double) *((c_type*) data), item); \
data += sizeof(c_type); \
if (PyList_SetItem(list, i, item) == -1) PyErr_Print(); } \
break
switch(cls) {
CASE(mxLOGICAL_CLASS, bool, "bool", PyBool_FromLong);
CASE(mxDOUBLE_CLASS, double, "float64", PyFloat_FromDouble);
CASE(mxSINGLE_CLASS, float, "float32", PyFloat_FromDouble);
CASE(mxINT8_CLASS, char, "int8", PyInt_FromLong);
CASE(mxUINT8_CLASS, unsigned char, "uint8", PyInt_FromLong);
CASE(mxINT16_CLASS, short, "int16", PyInt_FromLong);
CASE(mxUINT16_CLASS, unsigned short, "uint16", PyInt_FromLong);
CASE(mxINT32_CLASS, int, "int32", PyInt_FromLong);
CASE(mxUINT32_CLASS, unsigned int, "uint32", PyLong_FromLongLong);
CASE(mxINT64_CLASS, long long, "int64", PyLong_FromLongLong);
CASE(mxUINT64_CLASS, unsigned long long, "uint64", PyLong_FromUnsignedLongLong);
default:
mexErrMsgIdAndTxt("matpy:UnsupportedVariableType", "Unsupported variable type");
}
} else {
#undef CASE
#define CASE(cls,c_type,d_type) case cls: for (int i = 0; i < nelem; i++) { \
dtype = d_type; \
PyObject *item = PyComplex_FromDoubles(*((c_type*) data), *((c_type*) imagData)); \
data += sizeof(c_type); \
imagData += sizeof(c_type); \
PyList_SetItem(list, i, item); } \
break
switch(cls) {
CASE(mxDOUBLE_CLASS, double, "complex128");
CASE(mxSINGLE_CLASS, float, "complex64");
CASE(mxINT8_CLASS, char, "complex64");
CASE(mxUINT8_CLASS, unsigned char, "complex64");
CASE(mxINT16_CLASS, short, "complex64");
CASE(mxUINT16_CLASS, unsigned short, "complex64");
CASE(mxINT32_CLASS, int, "complex128");
CASE(mxUINT32_CLASS, unsigned int, "complex128");
CASE(mxINT64_CLASS, long long, "complex128");
CASE(mxUINT64_CLASS, unsigned long long, "complex128");
default:
mexErrMsgIdAndTxt("matpy:UnsupportedVariableType", "Unsupported variable type");
}
}
}
//parse the matlab struct to get all the options for training
CvRTParams* parse_struct_to_forest_config(const mxArray *trainingOptions) {
int numFields = 0;
if (trainingOptions != NULL) {
numFields = mxGetNumberOfFields(trainingOptions);
}
mexPrintf("%d training fields provided\n", numFields);
//these should be the default values that we would get from instantiating
//a CvRTParams object.
int maxDepth = DEFAULT_MAX_DEPTH;
int minSampleCount = DEFAULT_MIN_SAMPLE_COUNT;
float regressionAccuracy = DEFAULT_REGRESSION_ACCURACY;
bool useSurrogates = DEFAULT_USE_SURROGATES;
int maxCategories = DEFAULT_MAX_CATEGORIES;
float* priors = DEFAULT_PRIORS;
bool calcVarImportance = DEFAULT_CALC_VAR_IMPORTANCE;
int numActiveVars = DEFAULT_NUM_ACTIVE_VARS;
int maxTreeCount = DEFAULT_MAX_TREE_COUNT;
float forestAccuracy = DEFAULT_FOREST_ACCURACY;
int termCriteriaType = DEFAULT_TERM_CRITERIA_TYPE;
for (int i=0; i < numFields; i++) {
const char *name = mxGetFieldNameByNumber(trainingOptions, i);
mexPrintf("field %d is %s\n",i,name);
mxArray *field = mxGetFieldByNumber(trainingOptions, 0, i);
if (num_elements(field) != 1) {
mexWarnMsgIdAndTxt("train_random_forest:non_scalar_option",
"field is not a scalar! cannot parse into Random Forest options.");
continue;
}
//now need to compare against all the names we recognise
if (strcmp(name, "max_depth") == 0) {
if (mxIsInt32(field)) {
maxDepth = SCALAR_GET_SINT32(field);
}
else {
mexWarnMsgIdAndTxt("train_random_forest:not_int32","max_depth field must be a int32. Skipping...\n");
}
continue;
}
else if (strcmp(name, "min_sample_count") == 0) {
if (mxIsInt32(field)) {
minSampleCount = SCALAR_GET_SINT32(field);
}
else {
mexWarnMsgIdAndTxt("train_random_forest:not_int32","min_sample_count field must be a int32. Skipping...\n");
}
continue;
}
else if (strcmp(name, "regression_accuracy") == 0) {
if (mxIsDouble(field)) {
regressionAccuracy = (float)SCALAR_GET_DOUBLE(field);
}
else if (mxIsSingle(field)) {
regressionAccuracy = SCALAR_GET_SINGLE(field);
}
else {
mexWarnMsgIdAndTxt("train_random_forest:not_float","regression_accuracy field must be a single or double. Skipping...\n");
}
continue;
}
else if (strcmp(name, "use_surrogates") == 0) {
if (mxIsLogicalScalar(field)) {
useSurrogates = mxIsLogicalScalarTrue(field);
}
else {
mexWarnMsgIdAndTxt("train_random_forest:not_bool","use_surrogates field must be a boolean/logical. skipping...\n");
}
continue;
}
else if (strcmp(name, "max_categories") == 0) {
if (mxIsInt32(field)) {
maxCategories = SCALAR_GET_SINT32(field);
}
else {
mexWarnMsgIdAndTxt("train_random_forest:not_int32","max_categories field must be an int32. Skipping...\n");
}
continue;
}
else if (strcmp(name, "calc_var_importance") == 0) {
if (mxIsLogicalScalar(field)) {
calcVarImportance = mxIsLogicalScalarTrue(field);
}
else {
mexWarnMsgIdAndTxt("train_random_forest:not_bool","calc_var_importance field must be a boolean/logical. skipping...\n");
}
continue;
}
else if (strcmp(name, "num_active_vars") == 0) {
if (mxIsInt32(field)) {
numActiveVars = SCALAR_GET_SINT32(field);
}
else {
mexWarnMsgIdAndTxt("train_random_forest:not_int32","num_active_vars field must be an int32. Skipping...\n");
}
continue;
}
else if (strcmp(name, "max_tree_count") == 0) {
if (mxIsInt32(field)) {
maxTreeCount = SCALAR_GET_SINT32(field);
}
else {
mexWarnMsgIdAndTxt("train_random_forest:not_int32","max_tree_count field must be an int32. Skipping...\n");
}
continue;
}
else if (strcmp(name, "forest_accuracy") == 0) {
if (mxIsDouble(field)) {
forestAccuracy = (float)SCALAR_GET_DOUBLE(field);
}
else if (mxIsSingle(field)) {
forestAccuracy = SCALAR_GET_SINGLE(field);
}
else {
mexWarnMsgIdAndTxt("train_random_forest:not_float","forest_accuracy field must be a float. Skipping...\n");
}
continue;
}
else if (strcmp(name, "term_criteria_type") == 0) {
if (mxIsInt32(field)) {
termCriteriaType = SCALAR_GET_SINT32(field);
}
else {
mexWarnMsgIdAndTxt("train_random_forest:not_int32","term_critera_type field must be an int32. Skipping...\n");
}
continue;
}
else {
char msgbuf[ERR_MSG_SIZE];
sprintf(msgbuf,"field provided was not recognised: %s",name);
mexWarnMsgIdAndTxt("train_random_forest:unrecognised_field",msgbuf);
}
}
return new CvRTParams(maxDepth, minSampleCount, regressionAccuracy,
useSurrogates, maxCategories, priors,
calcVarImportance, numActiveVars, maxTreeCount,
forestAccuracy, termCriteriaType);
}
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
int i,j,m,n;
int ifield, jstruct, *classIDflags;
int NStructElems, nfields, ndim, nsignal, nvals;
const char *field_name, *str;
const mxArray *field_array_ptr;
double dbl;
float *leadSnds[MAXCARRIERS], *lagSnds[MAXCARRIERS];
float *locations, *azimuths, *rove, *scalesLead, *scalesLag, *attensLead, *tmp;
div_t div_result;
InfoStruct recInfo;
/* Check for proper number of arguments */
if (nrhs != 9)
mexErrMsgTxt("9 input arguments required).");
else if(!mxIsStruct(prhs[0]))
mexErrMsgTxt("Input1 must be a structure.");
// deal with the structure (arg0)
nfields = mxGetNumberOfFields(prhs[0]);
NStructElems = mxGetNumberOfElements(prhs[0]);
/* check each field. */
for(ifield=0; ifield<nfields; ifield++)
{
field_name = mxGetFieldNameByNumber(prhs[0], ifield);
field_array_ptr = mxGetFieldByNumber(prhs[0], 0, ifield);
if (field_array_ptr == NULL)
mexPrintf(".%s is empty\n", field_name);
else
{
str = mxArrayToString(field_array_ptr);
dbl = mxGetScalar(field_array_ptr);
if(strncmp("FN1", field_name, 3) == 0)
strncpy(recInfo.outFN1, str, MAX_FILENAME_LEN);
if (strncmp("FN2",field_name, 3) ==0)
strncpy(recInfo.outFN2, str, MAX_FILENAME_LEN);
if (strncmp("record",field_name, 6) ==0)
recInfo.record = (unsigned int) dbl;
if (strncmp("latten",field_name, 6) ==0)
recInfo.latten = (float) dbl;
if (strncmp("ratten",field_name, 6) ==0)
recInfo.ratten = (float) dbl;
if (strncmp("n_trials",field_name,8) ==0)
recInfo.n_trials = (unsigned int) dbl;
if (strncmp("decimateFactor",field_name, 14) ==0)
recInfo.decimateFactor = (unsigned int) dbl;
if (strncmp("buf_pts",field_name, 7) ==0)
recInfo.buf_pts = (long) dbl;
if (strncmp("nptsTotalPlay",field_name, 13) ==0)
recInfo.nptsTotalPlay = (long) dbl;
if (strncmp("ISI",field_name, 3) ==0)
recInfo.ISI = (unsigned int) dbl;
if (strncmp("max_signal_jitter",field_name, 17) ==0)
recInfo.max_signal_jitter = (unsigned int) dbl;
if (strncmp("num_carriers",field_name, 12) ==0)
recInfo.num_carriers = (unsigned int) dbl;
if (strncmp("n_speakers",field_name, 10) ==0)
recInfo.n_speakers = (unsigned int) dbl;
if (strncmp("sound_onset",field_name, 11) ==0)
recInfo.sound_onset = (long) dbl;
if (strncmp("trials_to_show",field_name, 14) ==0)
recInfo.trials_to_show = (long) dbl;
if (strncmp("hab_loc",field_name, 7) ==0)
recInfo.hab_loc = (float) dbl;
if (strncmp("test_trial_freq",field_name, 15) ==0)
recInfo.test_trial_freq = (float) dbl;
}
}
// check buf_pts = (n * 2^decimateFactor);
div_result = div( recInfo.buf_pts, pow(2,recInfo.decimateFactor) );
if(div_result.rem)
{
recInfo.buf_pts = pow(2,recInfo.decimateFactor) * (div_result.quot +1);
mexPrintf("buf_pts and decimateFactor incompatable. buf_pts increased to %d \n",recInfo.buf_pts);
}
// check nptsTotalPlay = (n * buf_pts)
div_result = div( recInfo.nptsTotalPlay, recInfo.buf_pts );
if(div_result.rem)
{
recInfo.nptsTotalPlay = recInfo.buf_pts * (div_result.quot +1);
mexPrintf("buf_pts and totalpts incompatable. nptsTotalPlay increased to %d \n",recInfo.nptsTotalPlay);
}
div_result = div( recInfo.nptsTotalPlay, (recInfo.buf_pts * (recInfo.ISI+1)) );
recInfo.n_trials = (int) div_result.quot +1;
// get LEAD and LAG sounds (should be <= 50)
// matlab arrays should be buf_pts x num_carriers
// LEAD
tmp=(float *) mxGetPr(prhs[1]);
m=mxGetM(prhs[1]); n=mxGetN(prhs[1]);
if(recInfo.buf_pts != m || recInfo.num_carriers != n) {
mexPrintf("array is %i x %i",m,n);
mexErrMsgTxt("Sound arrays must be buf_pts x num_carriers");
}
for(i=0;i<recInfo.num_carriers;++i)
leadSnds[i] = &tmp[i*recInfo.buf_pts];
// LEAD
tmp=(float *) mxGetPr(prhs[2]);
m=mxGetM(prhs[2]); n=mxGetN(prhs[2]);
if(recInfo.buf_pts != m || recInfo.num_carriers != n) {
mexPrintf("array is %i x %i",m,n);
mexErrMsgTxt("Sound arrays must be buf_pts x num_carriers");
}
for(i=0;i<recInfo.num_carriers;++i)
lagSnds[i] = &tmp[i*recInfo.buf_pts];
// locations is a 1 x n (n>=n_trials) array of spkr positions to switch between (0-3)
nsignal = mxGetN(prhs[3]);
if ( mxGetM(prhs[3])!=1 || nsignal < recInfo.n_trials)
mexErrMsgTxt("locations must be a 1 x n (n > n_trials), row vector.");
locations = (float*) mxGetPr(prhs[3]);
/* azimuths is a 1 x N array of azimuths (N = no. of speakers used) */
azimuths = (float*) mxGetPr(prhs[4]);
// rove is a 1 x n (n>=n_trials) array of sound IDs for carrier roving
nsignal = mxGetN(prhs[5]);
if ( mxGetM(prhs[5])!=1 || nsignal < recInfo.n_trials)
mexErrMsgTxt("rove sequence must be a 1 x n (n > n_trials), row vector.");
rove = (float*) mxGetPr(prhs[5]);
/* scalesLead is a 1 x (# lag speakers) array of scales for the lead speaker */
nvals = mxGetN(prhs[6]);
if ( mxGetM(prhs[6])!=1 || nvals != (recInfo.n_speakers - 1) )
mexErrMsgTxt("scales for lead speaker must by 1 x (# lag speakers), row vector.");
scalesLead = (float*) mxGetPr(prhs[6]);
/* scalesLag is a 1 x (# lag speakers) array of scales for the lag speakers */
nvals = mxGetN(prhs[7]);
if ( mxGetM(prhs[7])!=1 || nvals != (recInfo.n_speakers - 1) )
mexErrMsgTxt("scales for lag speaker must by 1 x (# lag speakers), row vector.");
scalesLag = (float*) mxGetPr(prhs[7]);
/* attensLead is a 1 x (# lag speakers) array of attens for the lead speaker */
nvals = mxGetN(prhs[8]);
if ( mxGetM(prhs[8])!=1 || nvals != (recInfo.n_speakers - 1) )
mexErrMsgTxt("attens for the lead speaker must be a 1 x (# lag speakers), row vector.");
attensLead = (float*) mxGetPr(prhs[8]);
/* The double_buffer subroutine */
double_buffer( &recInfo, leadSnds, lagSnds, locations, azimuths, rove, scalesLead, scalesLag, attensLead, nsignal);
return;
}
/* Function: WriteMatlabStructureToRTWFile =====================================
* Abstract:
* This routine writes out a matlab structure to the the rtw file. Nested
* structures inside structures are also supported, i.e., any field(s) of
* the mxArray passed in can be structures. However, the basic atomic
* units have to be either row strings or non-sparse, numeric vectors or
* matrices. For example, fields cannot be things like a matrix of strings
* or nd arrays.
*
* In order to avoid name clashes with records (espeically ones that might
* be added in future) written out by Simulink, your structure name should
* start with the string "SFcn". For example, instead of naming your
* structure "ParseTree" you should name it "SFcnParseTree".
*
* Returns:
* 1 -on success
* 0 -on failure
*/
bool WriteMatlabStructureToRTWFile(SimStruct *S,
const mxArray *mlStruct,
const char *structName,
char *strBuffer,
const int strBufferLen)
{
int numStructs;
int numFields;
int i;
if (mlStruct == NULL) {
return(1);
}
numStructs = mxGetNumberOfElements(mlStruct);
numFields = mxGetNumberOfFields(mlStruct);
/* make sure strBuffer is long enough for sprintf, be conservative */
if ( strlen(structName)+5 >= ((size_t) strBufferLen) ) {
return(0);
}
for (i=0; i<numStructs; i++) {
int j;
(void) sprintf(strBuffer, "%s {",structName);
if ( !ssWriteRTWStr(S, strBuffer) ) {
return(0);
}
for (j=0; j<numFields; j++) {
const char *fieldName = mxGetFieldNameByNumber(mlStruct, j);
const mxArray *field = mxGetFieldByNumber(mlStruct, i, j);
int nRows;
int nCols;
int nElements;
int nDims;
if (field == NULL) {
continue;
}
nRows = mxGetM(field);
nCols = mxGetN(field);
nElements = mxGetNumberOfElements(field);
nDims = mxGetNumberOfDimensions(field);
if ( mxIsStruct(field) ) { /* struct param */
if ( !WriteMatlabStructureToRTWFile(S,
field,
fieldName,
strBuffer,
strBufferLen) ) {
return(0);
}
} else if ( mxIsChar(field) ) { /* string param */
/* can handle only "row" strings */
if ( nDims > 2 || nRows > 1 ) {
return(0);
}
if ( mxGetString(field,strBuffer,strBufferLen) ) {
return(0);
}
if ( !ssWriteRTWStrParam(S,fieldName,strBuffer) ) {
return(0);
}
} else if ( mxIsNumeric(field) ) { /* numeric param */
const void *rval = mxGetData(field);
int isCmplx = mxIsComplex(field);
DTypeId dTypeId = ssGetDTypeIdFromMxArray(field);
int dtInfo = DTINFO(dTypeId, isCmplx);
const void *ival = (isCmplx) ? mxGetImagData(field) : NULL;
/* can handle only non-sparse, numeric vectors or matrices */
if (nDims > 2 || nRows*nCols != nElements || mxIsSparse(field)){
return(0);
}
if (nRows == 1 || nCols == 1) { /* vector param */
if ( !ssWriteRTWMxVectParam(S, fieldName, rval, ival,
dtInfo, nElements) ) {
return(0);
}
} else { /* matrix param */
if ( !ssWriteRTWMx2dMatParam(S, fieldName, rval, ival,
dtInfo, nRows, nCols) ) {
return(0);
}
}
} else {
return(0);
}
}
if ( !ssWriteRTWStr(S, "}") ) {
return(0);
}
}
return(1);
} /* end WriteMatlabStructureToRTWFile */
// Takes a MATLAB variable and writes in in Mathematica form to link
void toMma(const mxArray *var, MLINK link) {
// the following may occur when retrieving empty struct fields
// it showsup as [] in MATLAB so we return {}
// note that non-existent variables are caught and handled in eng_get()
if (var == NULL) {
MLPutFunction(link, "List", 0);
return;
}
// get size information
mwSize depth = mxGetNumberOfDimensions(var);
const mwSize *mbDims = mxGetDimensions(var);
// handle zero-size arrays
if (mxIsEmpty(var)) {
if (mxIsChar(var))
MLPutString(link, "");
else
MLPutFunction(link, "List", 0);
return;
}
// translate dimension information to Mathematica order
std::vector<int> mmDimsVec(depth);
std::reverse_copy(mbDims, mbDims + depth, mmDimsVec.begin());
int *mmDims = &mmDimsVec[0];
int len = mxGetNumberOfElements(var);
// numerical (sparse or dense)
if (mxIsNumeric(var)) {
mxClassID classid = mxGetClassID(var);
// verify that var is of a supported class
switch (classid) {
case mxDOUBLE_CLASS:
case mxSINGLE_CLASS:
case mxINT32_CLASS:
case mxINT16_CLASS:
case mxUINT16_CLASS:
case mxINT8_CLASS:
case mxUINT8_CLASS:
break;
default:
putUnknown(var, link);
return;
}
if (mxIsSparse(var)) {
// Note: I realised that sparse arrays can only hold double precision numerical types
// in MATLAB R2013a. I will leave the below implementation for single precision & integer
// types in case future versions of MATLAB will add support for them.
int ncols = mxGetN(var); // number of columns
mwIndex *jc = mxGetJc(var);
mwIndex *ir = mxGetIr(var);
int nnz = jc[ncols]; // number of nonzeros
MLPutFunction(link, CONTEXT "matSparseArray", 4);
mlpPutIntegerList(link, jc, ncols + 1);
mlpPutIntegerList(link, ir, nnz);
// if complex, put as im*I + re
if (mxIsComplex(var)) {
MLPutFunction(link, "Plus", 2);
MLPutFunction(link, "Times", 2);
MLPutSymbol(link, "I");
switch (classid) {
case mxDOUBLE_CLASS:
MLPutReal64List(link, mxGetPi(var), nnz); break;
case mxSINGLE_CLASS:
MLPutReal32List(link, (float *) mxGetImagData(var), nnz); break;
case mxINT16_CLASS:
MLPutInteger16List(link, (short *) mxGetImagData(var), nnz); break;
case mxINT32_CLASS:
MLPutInteger32List(link, (int *) mxGetImagData(var), nnz); break;
default:
assert(false); // should never reach here
}
}
switch (classid) {
case mxDOUBLE_CLASS:
MLPutReal64List(link, mxGetPr(var), nnz); break;
case mxSINGLE_CLASS:
MLPutReal32List(link, (float *) mxGetData(var), nnz); break;
case mxINT16_CLASS:
MLPutInteger16List(link, (short *) mxGetData(var), nnz); break;
case mxINT32_CLASS:
MLPutInteger32List(link, (int *) mxGetData(var), nnz); break;
default:
assert(false); // should never reach here
}
MLPutInteger32List(link, mmDims, depth);
}
else // not sparse
{
MLPutFunction(link, CONTEXT "matArray", 2);
// if complex, put as im*I + re
if (mxIsComplex(var)) {
MLPutFunction(link, "Plus", 2);
MLPutFunction(link, "Times", 2);
MLPutSymbol(link, "I");
switch (classid) {
case mxDOUBLE_CLASS:
MLPutReal64Array(link, mxGetPi(var), mmDims, NULL, depth); break;
case mxSINGLE_CLASS:
MLPutReal32Array(link, (float *) mxGetImagData(var), mmDims, NULL, depth); break;
case mxINT32_CLASS:
MLPutInteger32Array(link, (int *) mxGetImagData(var), mmDims, NULL, depth); break;
case mxINT16_CLASS:
MLPutInteger16Array(link, (short *) mxGetImagData(var), mmDims, NULL, depth); break;
case mxUINT16_CLASS:
{
int *arr = new int[len];
unsigned short *mbData = (unsigned short *) mxGetImagData(var);
std::copy(mbData, mbData + len, arr);
MLPutInteger32Array(link, arr, mmDims, NULL, depth);
delete [] arr;
break;
}
case mxINT8_CLASS:
{
short *arr = new short[len];
char *mbData = (char *) mxGetImagData(var);
std::copy(mbData, mbData + len, arr);
MLPutInteger16Array(link, arr, mmDims, NULL, depth);
delete [] arr;
break;
}
case mxUINT8_CLASS:
{
short *arr = new short[len];
unsigned char *mbData = (unsigned char *) mxGetImagData(var);
std::copy(mbData, mbData + len, arr);
MLPutInteger16Array(link, arr, mmDims, NULL, depth);
delete [] arr;
break;
}
default:
assert(false); // should never reach here
}
}
switch (classid) {
case mxDOUBLE_CLASS:
MLPutReal64Array(link, mxGetPr(var), mmDims, NULL, depth); break;
case mxSINGLE_CLASS:
MLPutReal32Array(link, (float *) mxGetData(var), mmDims, NULL, depth); break;
case mxINT32_CLASS:
MLPutInteger32Array(link, (int *) mxGetData(var), mmDims, NULL, depth); break;
case mxINT16_CLASS:
MLPutInteger16Array(link, (short *) mxGetData(var), mmDims, NULL, depth); break;
case mxUINT16_CLASS:
{
int *arr = new int[len];
unsigned short *mbData = (unsigned short *) mxGetData(var);
std::copy(mbData, mbData + len, arr);
MLPutInteger32Array(link, arr, mmDims, NULL, depth);
delete [] arr;
break;
}
case mxINT8_CLASS:
{
short *arr = new short[len];
char *mbData = (char *) mxGetData(var);
std::copy(mbData, mbData + len, arr);
MLPutInteger16Array(link, arr, mmDims, NULL, depth);
delete [] arr;
break;
}
case mxUINT8_CLASS:
{
short *arr = new short[len];
unsigned char *mbData = (unsigned char *) mxGetData(var);
std::copy(mbData, mbData + len, arr);
MLPutInteger16Array(link, arr, mmDims, NULL, depth);
delete [] arr;
break;
}
default:
assert(false); // should never reach here
}
MLPutInteger32List(link, mmDims, depth);
}
}
// logical (sparse or dense)
else if (mxIsLogical(var))
if (mxIsSparse(var)) {
int ncols = mxGetN(var); // number of columns
mwIndex *jc = mxGetJc(var);
mwIndex *ir = mxGetIr(var);
mxLogical *logicals = mxGetLogicals(var);
int nnz = jc[ncols]; // number of nonzeros
MLPutFunction(link, CONTEXT "matSparseLogical", 4);
mlpPutIntegerList(link, jc, ncols + 1);
mlpPutIntegerList(link, ir, nnz);
short *integers = new short[nnz];
std::copy(logicals, logicals+nnz, integers);
MLPutInteger16List(link, integers, nnz);
MLPutInteger32List(link, mmDims, depth);
delete [] integers;
}
else // not sparse
{
mxLogical *logicals = mxGetLogicals(var);
short *integers = new short[len];
std::copy(logicals, logicals+len, integers);
MLPutFunction(link, CONTEXT "matLogical", 2);
MLPutInteger16Array(link, integers, mmDims, NULL, depth);
MLPutInteger32List(link, mmDims, depth);
delete [] integers;
}
// char array
else if (mxIsChar(var)) {
assert(sizeof(mxChar) == sizeof(unsigned short));
// 1 by N char arrays (row vectors) are sent as a string
if (depth == 2 && mbDims[0] == 1) {
const mxChar *str = mxGetChars(var);
MLPutFunction(link, CONTEXT "matString", 1);
MLPutUTF16String(link, reinterpret_cast<const unsigned short *>(str), len); // cast may be required on other platforms: (mxChar *) str
}
// general char arrays are sent as an array of characters
else {
MLPutFunction(link, CONTEXT "matCharArray", 2);
const mxChar *str = mxGetChars(var);
MLPutFunction(link, "List", len);
for (int i=0; i < len; ++i)
MLPutUTF16String(link, reinterpret_cast<const unsigned short *>(str + i), 1);
MLPutInteger32List(link, mmDims, depth);
}
}
// struct
else if (mxIsStruct(var)) {
int nfields = mxGetNumberOfFields(var);
MLPutFunction(link, CONTEXT "matStruct", 2);
MLPutFunction(link, "List", len);
for (int j=0; j < len; ++j) {
MLPutFunction(link, "List", nfields);
for (int i=0; i < nfields; ++i) {
const char *fieldname;
fieldname = mxGetFieldNameByNumber(var, i);
MLPutFunction(link, "Rule", 2);
MLPutString(link, fieldname);
toMma(mxGetFieldByNumber(var, j, i), link);
}
}
MLPutInteger32List(link, mmDims, depth);
}
// cell
else if (mxIsCell(var)) {
MLPutFunction(link, CONTEXT "matCell", 2);
MLPutFunction(link, "List", len);
for (int i=0; i < len; ++i)
toMma(mxGetCell(var, i), link);
MLPutInteger32List(link, mmDims, depth);
}
// unknown or failure; TODO distinguish between unknown and failure
else
{
putUnknown(var, link);
}
}
/* Function: mdlCheckParameters =============================================
* Abstract:
* Validate our parameters to verify they are okay.
*/
static void mdlCheckParameters(SimStruct *S)
{
int_T i;
static char_T msg[100];
/* for RTW we do not need these variables */
#ifdef MATLAB_MEX_FILE
const char *fname;
mxArray *fval;
int_T nfields;
#endif
/* Check 1st parameter: number of states */
{
if ( !mxIsDouble(ssGetSFcnParam(S,0)) ||
mxIsEmpty(ssGetSFcnParam(S,0)) ||
mxGetNumberOfElements(ssGetSFcnParam(S,0))!=1 ||
mxGetScalar(ssGetSFcnParam(S,0))<=0 ||
mxIsNaN(*mxGetPr(ssGetSFcnParam(S,0))) ||
mxIsInf(*mxGetPr(ssGetSFcnParam(S,0))) ||
mxIsSparse(ssGetSFcnParam(S,0)) ) {
ssSetErrorStatus(S,"lcp_sfun: Number of states must be of type double, not empty, scalar and finite.");
return;
}
}
/* Check 2nd parameter: sample time */
{
if ( !mxIsDouble(ssGetSFcnParam(S,1)) ||
mxIsEmpty(ssGetSFcnParam(S,1)) ||
mxGetNumberOfElements(ssGetSFcnParam(S,1))!=1 ||
mxGetScalar(ssGetSFcnParam(S,1))<=0 ||
mxIsNaN(*mxGetPr(ssGetSFcnParam(S,1))) ||
mxIsInf(*mxGetPr(ssGetSFcnParam(S,1))) ||
mxIsSparse(ssGetSFcnParam(S,1)) ) {
ssSetErrorStatus(S,"lcp_sfun: Sample time must be of type double, not empty, scalar and finite.");
return;
}
}
#ifdef MATLAB_MEX_FILE
/* Check 3rd parameter: options */
if (ssGetSFcnParamsCount(S)==3)
{
if (!mxIsStruct(ssGetSFcnParam(S,2))) {
ssSetErrorStatus(S, "lcp_sfun: Options must be in STRUCT format.");
return;
}
else if (mxGetNumberOfElements(ssGetSFcnParam(S,2))>1) {
ssSetErrorStatus(S,"lcp_sfun: Only one option structure is allowed.");
return;
}
/* checking each option individually */
nfields = mxGetNumberOfFields(ssGetSFcnParam(S,2));
for (i=0; i<nfields; i++) {
fname = mxGetFieldNameByNumber(ssGetSFcnParam(S,2), i);
fval = mxGetField(ssGetSFcnParam(S,2), 0, fname);
/* check for proper field names */
if (!( (strcmp(fname, "zerotol")==0) || (strcmp(fname, "maxpiv")==0) ||
(strcmp(fname, "lextol")==0) || (strcmp(fname, "nstepf")==0) ||
(strcmp(fname, "clock")==0) || (strcmp(fname, "verbose")==0) ||
(strcmp(fname, "routine")==0) || (strcmp(fname, "timelimit")==0) ||
(strcmp(fname, "normalize")==0) || (strcmp(fname, "normalizethres")==0) )) {
strcpy(msg,"");
strcat(msg, "lcp_sfun: The field '");
strcat(msg, fname);
strcat(msg, "' is not allowed in the options structure.");
ssSetErrorStatus(S, msg);
return;
}
/* some options must be nonnegative */
if (strcmp(fname,"zerotol")==0 || strcmp(fname,"maxpiv")==0 ||
strcmp(fname,"timelimit")==0 || strcmp(fname,"lextol")==0 ||
(strcmp(fname,"nstepf")==0) || (strcmp(fname, "normalizethres")==0) ) {
if (!mxIsDouble(fval) || mxIsEmpty(fval) || (mxGetM(fval)*mxGetN(fval))!=1 ||
(mxGetScalar(fval)<=0) ) {
strcpy(msg,"");
strcat(msg, "lcp_sfun: Option value '");
strcat(msg, fname);
strcat(msg, "' must be of type double, nonempty, scalar, and nonnegative.");
ssSetErrorStatus(S, msg);
return;
}
}
/* some can be zeros */
if (strcmp(fname,"clock")==0 || strcmp(fname,"verbose")==0 ||
strcmp(fname,"routine")==0 || strcmp(fname,"normalize")==0) {
if (!mxIsDouble(fval) || mxIsEmpty(fval) || (mxGetM(fval)*mxGetN(fval))!=1 ) {
strcpy(msg,"");
strcat(msg, "lcp_sfun: Option value '");
strcat(msg, fname);
strcat(msg, "' must be of type double, nonempty, scalar, and integer valued.");
ssSetErrorStatus(S, msg);
return;
}
}
/* No NaN values allowed */
if ( mxIsNaN(*mxGetPr(fval)) ) {
ssSetErrorStatus(S, "lcp_sfun: No 'NaN' are allowed in the options structure.");
return;
}
/* Inf value allowed only for timelimit */
if ( strcmp(fname,"timelimit")!=0 && mxIsInf(*mxGetPr(fval)) ) {
ssSetErrorStatus(S, "lcp_sfun: No 'Inf' terms allowed except for 'timelimit' option.");
return;
}
}
}
#endif
}
void mexFunction
(
/* === Parameters ======================================================= */
int nlhs, /* number of left-hand sides */
mxArray *plhs [], /* left-hand side matrices */
int nrhs, /* number of right--hand sides */
const mxArray *prhs [] /* right-hand side matrices */
)
{
Dmat A;
DILUPACKparam param;
const char **fnames; /* pointers to field names */
const mwSize *dims;
mxClassID *classIDflags;
mwSize buflen, mrows, ncols, ndim;
mxArray *tmp, *fout, *A_input, *options_input, *options_output;
char *pdata, *input_buf, *output_buf;
int ifield, status, nfields;
size_t sizebuf;
double dbuf;
if (nrhs != 2)
mexErrMsgTxt("Two input arguments required.");
else if (nlhs > 1)
mexErrMsgTxt("Too many output arguments.");
else if (!mxIsStruct(prhs[1]))
mexErrMsgTxt("Second input must be a structure.");
else if (!mxIsNumeric(prhs[0]))
mexErrMsgTxt("First input must be a matrix.");
/* The first input must be a square matrix.*/
A_input=(mxArray *)prhs[0];
mrows = mxGetM(A_input);
ncols = mxGetN(A_input);
if (mrows!=ncols) {
mexErrMsgTxt("First input must be a square matrix.");
}
A.nc=A.nr=mrows;
A.ia=A.ja=NULL;
A.a=NULL;
DSYMAMGinit(&A,¶m);
/* Get second input arguments */
options_input=(mxArray *)prhs[1];
nfields = mxGetNumberOfFields(options_input);
/* Allocate memory for storing pointers */
fnames = mxCalloc((size_t)nfields, (size_t)sizeof(*fnames));
/* Allocate memory for storing classIDflags */
classIDflags = (mxClassID *) mxCalloc((size_t)nfields, (size_t)sizeof(mxClassID));
/* Get field name pointers */
for (ifield = 0; ifield < nfields; ifield++) {
fnames[ifield] = mxGetFieldNameByNumber(options_input,ifield);
}
/* Create a 1x1 struct matrix for output */
nlhs=1;
plhs[0] = mxCreateStructMatrix((mwSize)1, (mwSize)1, nfields, fnames);
options_output=plhs[0];
/* copy data */
for (ifield = 0; ifield < nfields; ifield++) {
tmp = mxGetFieldByNumber(options_input,(mwIndex)0,ifield);
classIDflags[ifield] = mxGetClassID(tmp);
ndim = mxGetNumberOfDimensions(tmp);
dims = mxGetDimensions(tmp);
/* Create string/numeric array */
if (classIDflags[ifield] == mxCHAR_CLASS) {
/* Get the length of the input string. */
buflen = (mxGetM(tmp) * mxGetN(tmp)) + 1;
if (!strcmp("amg",fnames[ifield])) {
output_buf = (char *) mxCalloc((size_t)strlen(param.amg)+1, (size_t)sizeof(char));
strcpy(output_buf,param.amg);
fout = mxCreateString(output_buf);
}
else if (!strcmp("presmoother",fnames[ifield])) {
output_buf = (char *) mxCalloc((size_t)strlen(param.presmoother)+1, (size_t)sizeof(char));
strcpy(output_buf,param.presmoother);
fout = mxCreateString(output_buf);
}
else if (!strcmp("postsmoother",fnames[ifield])) {
output_buf = (char *) mxCalloc((size_t)strlen(param.postsmoother)+1, (size_t)sizeof(char));
strcpy(output_buf,param.postsmoother);
fout = mxCreateString(output_buf);
}
else if (!strcmp("typecoarse",fnames[ifield])) {
output_buf = (char *) mxCalloc((size_t)strlen(param.typecoarse)+1, (size_t)sizeof(char));
strcpy(output_buf,param.typecoarse);
fout = mxCreateString(output_buf);
}
else if (!strcmp("typetv",fnames[ifield])) {
output_buf = (char *) mxCalloc((size_t)strlen(param.typetv)+1, (size_t)sizeof(char));
strcpy(output_buf,param.typetv);
fout = mxCreateString(output_buf);
}
else if (!strcmp("FCpart",fnames[ifield])) {
output_buf = (char *) mxCalloc((size_t)strlen(param.FCpart)+1, (size_t)sizeof(char));
strcpy(output_buf,param.FCpart);
fout = mxCreateString(output_buf);
}
else if (!strcmp("solver",fnames[ifield])) {
output_buf = (char *) mxCalloc((size_t)strlen(param.solver)+1, (size_t)sizeof(char));
strcpy(output_buf, param.solver);
fout = mxCreateString(output_buf);
}
else if (!strcmp("ordering",fnames[ifield])) {
output_buf = (char *) mxCalloc((size_t)strlen(param.ordering)+1, (size_t)sizeof(char));
strcpy(output_buf, param.ordering);
fout = mxCreateString(output_buf);
}
else {
/* Allocate memory for input and output strings. */
input_buf = (char *) mxCalloc((size_t)buflen, (size_t)sizeof(char));
output_buf = (char *) mxCalloc((size_t)buflen, (size_t)sizeof(char));
/* Copy the string data from tmp into a C string
input_buf. */
status = mxGetString(tmp, input_buf, buflen);
sizebuf = buflen*sizeof(char);
memcpy(output_buf, input_buf, (size_t)sizebuf);
fout = mxCreateString(output_buf);
}
}
else {
fout = mxCreateNumericArray((mwSize)ndim, dims,
classIDflags[ifield], mxREAL);
pdata = mxGetData(fout);
sizebuf = mxGetElementSize(tmp);
if (!strcmp("elbow",fnames[ifield])) {
dbuf=param.elbow;
memcpy(pdata, &dbuf, (size_t)sizebuf);
}
else if (!strcmp("lfilS",fnames[ifield])) {
dbuf=param.lfilS;
memcpy(pdata, &dbuf, (size_t)sizebuf);
}
else if (!strcmp("lfil",fnames[ifield])) {
dbuf=param.lfil;
memcpy(pdata, &dbuf, (size_t)sizebuf);
}
else if (!strcmp("maxit",fnames[ifield])) {
dbuf=param.maxit;
memcpy(pdata, &dbuf, (size_t)sizebuf);
}
else if (!strcmp("droptolS",fnames[ifield])) {
dbuf=param.droptolS;
memcpy(pdata, &dbuf, (size_t)sizebuf);
}
else if (!strcmp("droptolc",fnames[ifield])) {
dbuf=param.droptolc;
memcpy(pdata, &dbuf, (size_t)sizebuf);
}
else if (!strcmp("droptol",fnames[ifield])) {
dbuf=param.droptol;
memcpy(pdata, &dbuf, (size_t)sizebuf);
}
else if (!strcmp("condest",fnames[ifield])) {
dbuf=param.condest;
memcpy(pdata, &dbuf, (size_t)sizebuf);
}
else if (!strcmp("restol",fnames[ifield])) {
dbuf=param.restol;
memcpy(pdata, &dbuf, (size_t)sizebuf);
}
else if (!strcmp("npresmoothing",fnames[ifield])) {
dbuf=param.npresmoothing;
memcpy(pdata, &dbuf, (size_t)sizebuf);
}
else if (!strcmp("npostmoothing",fnames[ifield])) {
dbuf=param.npostsmoothing;
memcpy(pdata, &dbuf, (size_t)sizebuf);
}
else if (!strcmp("ncoarse",fnames[ifield])) {
dbuf=param.ncoarse;
memcpy(pdata, &dbuf, (size_t)sizebuf);
}
else if (!strcmp("matching",fnames[ifield])) {
dbuf=param.matching;
memcpy(pdata, &dbuf, (size_t)sizebuf);
}
else if (!strcmp("nrestart",fnames[ifield])) {
dbuf=param.nrestart;
memcpy(pdata, &dbuf, (size_t)sizebuf);
}
else if (!strcmp("damping",fnames[ifield])) {
dbuf=param.damping;
memcpy(pdata, &dbuf, (size_t)sizebuf);
}
else if (!strcmp("mixedprecision",fnames[ifield])) {
dbuf=param.mixedprecision;
memcpy(pdata, &dbuf, (size_t)sizebuf);
}
else {
memcpy(pdata, mxGetData(tmp), (size_t)sizebuf);
}
}
/* Set each field in output structure */
mxSetFieldByNumber(options_output, (mwIndex)0, ifield, fout);
}
mxFree(fnames);
return;
}
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if (!(nlhs==1))
{
mexErrMsgIdAndTxt("MyToolbox:arrayProduct:nlhs","One output required.");
}
if (!(nrhs==2 || nrhs==3))
{
mexErrMsgIdAndTxt("MyToolbox:arrayProduct:nrhs","Two double matrix and one optional structure are required.");
}
int i = 1;
float tau = PAR_DEFAULT_TAU;
float lambda = PAR_DEFAULT_LAMBDA;
float theta = PAR_DEFAULT_THETA;
int nscales = PAR_DEFAULT_NSCALES;
float zfactor = PAR_DEFAULT_ZFACTOR;
int nwarps = PAR_DEFAULT_NWARPS;
float epsilon = PAR_DEFAULT_EPSILON;
int verbose = PAR_DEFAULT_VERBOSE;
if (nrhs==3)
{
int tmp=0;
double* ptr;
for( tmp=0; tmp<mxGetNumberOfFields(prhs[2]);tmp++)
{
if ( strcmp(mxGetFieldNameByNumber(prhs[2],tmp),"tau")==0)
{
if (!(mxIsDouble(mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],0)))))
{
mexErrMsgTxt("A double argument was expected.");
}
if (mxGetNumberOfElements((mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],0))))!=1)
{
mexErrMsgTxt("Only one value was expected.");
}
ptr=mxGetPr(mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],tmp)));
if (ptr[0]> 0 && ptr[0]< 0.25)
{
tau=ptr[0];
}
else
{
mexErrMsgTxt("The tau value is not acceptable. For more information, type \"help tvl1\"");
}
}
if ( strcmp(mxGetFieldNameByNumber(prhs[2],tmp),"lambda")==0)
{
if (!(mxIsDouble(mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],0)))))
{
mexErrMsgTxt("A double argument was expected.");
}
if (mxGetNumberOfElements((mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],0))))!=1)
{
mexErrMsgTxt("Only one value was expected.");
}
ptr=mxGetPr(mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],tmp)));
if (ptr[0]>0)
{
lambda=ptr[0];
}
else
{
mexErrMsgTxt("The lambda value is not acceptable. For more information, type \"help tvl1\"");
}
}
if ( strcmp(mxGetFieldNameByNumber(prhs[2],tmp),"theta")==0)
{
if (!(mxIsDouble(mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],0)))))
{
mexErrMsgTxt("A double argument was expected.");
}
if (mxGetNumberOfElements((mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],0))))!=1)
{
mexErrMsgTxt("Only one value was expected.");
}
ptr=mxGetPr(mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],tmp)));
if (ptr[0]>0)
{
theta=ptr[0];
}
else
{
mexErrMsgTxt("The theta value is not acceptable. For more information, type \"help tvl1\"");
}
}
if ( strcmp(mxGetFieldNameByNumber(prhs[2],tmp),"nscales")==0)
{
if (!(mxIsDouble(mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],0)))))
{
mexErrMsgTxt("A double argument was expected.");
}
if (mxGetNumberOfElements((mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],0))))!=1)
{
mexErrMsgTxt("Only one value was expected.");
}
ptr=mxGetPr(mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],tmp)));
if (ptr[0]>0)
{
nscales=ptr[0];
}
else
{
mexErrMsgTxt("The nscales value is not acceptable. For more information, type \"help tvl1\"");
}
}
if ( strcmp(mxGetFieldNameByNumber(prhs[2],tmp),"zfactor")==0)
{
if (!(mxIsDouble(mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],0)))))
{
mexErrMsgTxt("A double argument was expected.");
}
if (mxGetNumberOfElements((mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],0))))!=1)
{
mexErrMsgTxt("Only one value was expected.");
}
ptr=mxGetPr(mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],tmp)));
if (ptr[0]>0)
{
zfactor=ptr[0];
}
else
{
mexErrMsgTxt("The zfactor value is not acceptable. For more information, type \"help tvl1\"");
}
}
if ( strcmp(mxGetFieldNameByNumber(prhs[2],tmp),"nwarps")==0)
{
if (!(mxIsDouble(mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],0)))))
{
mexErrMsgTxt("A double argument was expected.");
}
if (mxGetNumberOfElements((mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],0))))!=1)
{
mexErrMsgTxt("Only one value was expected.");
}
ptr=mxGetPr(mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],tmp)));
if (ptr[0]>0)
{
nwarps=ptr[0];
}
else
{
mexErrMsgTxt("The nwarps value is not acceptable. For more information, type \"help tvl1\"");
}
}
if ( strcmp(mxGetFieldNameByNumber(prhs[2],tmp),"epsilon")==0)
{
if (!(mxIsDouble(mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],0)))))
{
mexErrMsgTxt("A double argument was expected.");
}
if (mxGetNumberOfElements((mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],0))))!=1)
{
mexErrMsgTxt("Only one value was expected.");
}
ptr=mxGetPr(mxGetField(prhs[2],0,mxGetFieldNameByNumber(prhs[2],tmp)));
if (ptr[0]>0)
{
epsilon=ptr[0];
}
else
{
mexErrMsgTxt("The epsilon value is not acceptable. For more information, type \"help tvl1\"");
}
}
}
}
if (tau <= 0 || tau > 0.25) {
tau = PAR_DEFAULT_TAU;
if (verbose) mexPrintf("warning: "
"tau changed to %g\n", tau);
}
if (lambda <= 0) {
lambda = PAR_DEFAULT_LAMBDA;
if (verbose) mexPrintf("warning: "
"lambda changed to %g\n", lambda);
}
if (theta <= 0) {
theta = PAR_DEFAULT_THETA;
if (verbose) mexPrintf("warning: "
"theta changed to %g\n", theta);
}
if (nscales <= 0) {
nscales = PAR_DEFAULT_NSCALES;
if (verbose) mexPrintf("warning: "
"nscales changed to %d\n", nscales);
}
if (zfactor <= 0 || zfactor >= 1) {
zfactor = PAR_DEFAULT_ZFACTOR;
if (verbose) mexPrintf( "warning: "
"zfactor changed to %g\n", zfactor);
}
if (nwarps <= 0) {
nwarps = PAR_DEFAULT_NWARPS;
if (verbose) mexPrintf( "warning: "
"nwarps changed to %d\n", nwarps);
}
if (epsilon <= 0) {
epsilon = PAR_DEFAULT_EPSILON;
if (verbose) mexPrintf("warning: "
"epsilon changed to %f\n", epsilon);
}
int nx = mxGetN(prhs[0]);
int ny = mxGetM(prhs[0]);
int nx2 = mxGetN(prhs[1]);
int ny2 = mxGetM(prhs[1]);
float *I0=malloc(nx*ny*sizeof(float));
int tmpx, tmpy;
for (tmpx=0; tmpx<nx; tmpx++)
for (tmpy=0; tmpy<ny; tmpy++)
I0[tmpx+tmpy*nx] = (float)(mxGetPr(prhs[0]))[tmpy+tmpx*ny];
float *I1=malloc(nx2*ny2*sizeof(float));
for (tmpx=0; tmpx<nx2; tmpx++)
for (tmpy=0; tmpy<ny2; tmpy++)
I1[tmpx+tmpy*nx] = (float)(mxGetPr(prhs[1]))[tmpy+tmpx*ny];
if (nx == nx2 && ny == ny2)
{
const float N = 1 + log(hypot(nx, ny)/16.0) / log(1/zfactor);
if (N < nscales)
nscales = N;
if (verbose)
{
mexPrintf(
"tau=%f lambda=%f theta=%f nscales=%d "
"zfactor=%f nwarps=%d epsilon=%g\n",
tau, lambda, theta, nscales,
zfactor, nwarps, epsilon);
}
float *u = xmalloc(2 * nx * ny * sizeof(*u));
float *v = u + nx*ny;
Dual_TVL1_optic_flow_multiscale(
I0, I1, u, v, nx, ny, tau, lambda, theta,
nscales, zfactor, nwarps, epsilon, verbose
);
mwSize dim = 3;
const mwSize dims[3]={ny,nx,2};
plhs[0]=mxCreateNumericArray(dim, dims,mxDOUBLE_CLASS,mxREAL);
double* pointeur=(double*)mxGetPr(plhs[0]);
for (tmpy=0; tmpy<ny; tmpy++)
for (tmpx=0; tmpx<nx; tmpx++)
{
pointeur[tmpy+tmpx*ny + 0 ]=(double)u[tmpx+tmpy*nx];
pointeur[tmpy+tmpx*ny + nx*ny]=(double)v[tmpx+tmpy*nx];
}
free(I0);
free(I1);
free(u);
}
else
{
mexErrMsgTxt("ERROR: input images size mismatch ");
}
}
int mxToBytes(const mxArray *mx, char *byteArray, int byteArrayLength) {
int ii;
int nInfoBytes=0, nDataBytes=0, nFreeBytes=0;
mxGramInfo info;
// for complex types
int jj;
int nElements;
mxArray *elementData, *elementName;
char *elementByteArray;
int elementGramLength;
mxArray *callMatlabError;
if (mx == NULL)
return(0);
info.gramBytes = byteArray;
setInfoFieldsFromMx(&info, mx);
info.dataBytes = byteArray + MX_GRAM_OFFSET_DATA;
nFreeBytes = byteArrayLength - MX_GRAM_OFFSET_DATA;
if (info.gramType==mxGramDouble) {
nDataBytes = writeMxDoubleDataToBytes(mx, &info, nFreeBytes);
} else if (info.gramType==mxGramChar) {
nDataBytes = writeMxCharDataToBytes(mx, &info, nFreeBytes);
} else if (info.gramType==mxGramLogical) {
nDataBytes = writeMxLogicalDataToBytes(mx, &info, nFreeBytes);
} else if (info.gramType==mxGramCell) {
// recur to write data for each cell element
nElements = info.dataM * info.dataN;
elementByteArray = info.dataBytes;
elementGramLength = 0;
for (ii=0; ii<nElements; ii++) {
elementData = mxGetCell(mx, ii);
elementGramLength = mxToBytes((const mxArray*)elementData, elementByteArray, nFreeBytes);
if (elementGramLength < 0)
return(elementGramLength);
nDataBytes += elementGramLength;
elementByteArray += elementGramLength;
nFreeBytes -= elementGramLength;
}
} else if (info.gramType==mxGramStruct) {
// struct arrays resized to 1xn, with m fields
nElements = mxGetNumberOfFields(mx);
info.dataM = nElements;
info.dataN = mxGetM(mx) * mxGetN(mx);
// recur to write data for each field name
elementByteArray = info.dataBytes;
elementGramLength = 0;
for (ii=0; ii<nElements; ii++) {
elementName = mxCreateString(mxGetFieldNameByNumber(mx, ii));
elementGramLength = mxToBytes(elementName, elementByteArray, nFreeBytes);
mxDestroyArray(elementName);
if (elementGramLength < 0)
return(elementGramLength);
nDataBytes += elementGramLength;
elementByteArray += elementGramLength;
nFreeBytes -= elementGramLength;
}
// recur to write data for each field datum
for (ii=0; ii<nElements; ii++) {
for (jj=0; jj<info.dataN; jj++) {
elementData = mxGetFieldByNumber(mx, jj, ii);
elementGramLength = mxToBytes((const mxArray*)elementData, elementByteArray, nFreeBytes);
if (elementGramLength < 0)
return(elementGramLength);
nDataBytes += elementGramLength;
elementByteArray += elementGramLength;
nFreeBytes -= elementGramLength;
}
}
} else if (info.gramType==mxGramFunctionHandle) {
// recur to write stringified version of function
callMatlabError = mexCallMATLABWithTrap(1, &elementData, 1, (mxArray**)&mx, MX_GRAM_FUNCTION_TO_STRING);
if (callMatlabError == NULL && elementData != NULL) {
nDataBytes = mxToBytes((const mxArray*)elementData, info.dataBytes, nFreeBytes);
if (nDataBytes < 0)
return(nDataBytes);
} else
return(-1);
} else {
return(mxGramUnsupported);
}
//mexPrintf("nDataBytes = %d\n", nDataBytes);
if (nDataBytes < 0)
return(nDataBytes);
info.gramLength = MX_GRAM_OFFSET_DATA + nDataBytes;
nInfoBytes = writeInfoFieldsToBytes(&info, byteArray, byteArrayLength);
//mexPrintf("nInfoBytes = %d\n", nInfoBytes);
//printMxGramInfo(&info);
//printBytes(info.gramBytes, info.gramLength);
return(info.gramLength);
}
/* Function: mdlOutputs =======================================================
* do the main optimization routine here
* no discrete states are considered
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
int_T i, j, Result, qinfeas=0;
real_T *z = ssGetOutputPortRealSignal(S,0);
real_T *w = ssGetOutputPortRealSignal(S,1);
real_T *I = ssGetOutputPortRealSignal(S,2);
real_T *exitflag = ssGetOutputPortRealSignal(S,3);
real_T *pivots = ssGetOutputPortRealSignal(S,4);
real_T *time = ssGetOutputPortRealSignal(S,5);
InputRealPtrsType M = ssGetInputPortRealSignalPtrs(S,0);
InputRealPtrsType q = ssGetInputPortRealSignalPtrs(S,1);
ptrdiff_t pivs, info, m, n, inc=1;
char_T T='N';
double total_time, alpha=1.0, tmp=-1.0, *x, *Mn, *qn, *r, *c, s=0.0, sn=0.0;
PT_Matrix pA; /* Problem data A = [M -1 q] */
PT_Basis pB; /* The basis */
T_Options options; /* options structure defined in lcp_matrix.h */
/* for RTW we do not need these variables */
#ifdef MATLAB_MEX_FILE
const char *fname;
mxArray *fval;
int_T nfields;
clock_t t1,t2;
#endif
UNUSED_ARG(tid); /* not used in single tasking mode */
/* default options */
options.zerotol = 1e-10; /* zero tolerance */
options.lextol = 1e-10; /* lexicographic tolerance - a small treshold to determine if values are equal */
options.maxpiv = INT_MAX; /* maximum number of pivots */
/* if LUMOD routine is chosen, this options refactorizes the basis after n steps using DGETRF
routine from lapack to avoid numerical problems */
options.nstepf = 50;
options.clock = 0; /* 0 or 1 - to print computational time */
options.verbose = 0; /* 0 or 1 - verbose output */
/* which routine in Basis_solve should solve a set of linear equations:
0 - corresponds to LUmod package that performs factorization in the form L*A = U. Depending on
the change in A factors L, U are updated. This is the fastest method.
1 - corresponds to DGESV simple driver
which solves the system AX = B by factorizing A and overwriting B with the solution X
2 - corresponds to DGELS simple driver
which solves overdetermined or underdetermined real linear systems min ||b - Ax||_2
involving an M-by-N matrix A, or its transpose, using a QR or LQ factorization of A. */
options.routine = 0;
options.timelimit = 3600; /* time limit in seconds to interrupt iterations */
options.normalize = 1; /* 0 or 1 - perform scaling of input matrices M, q */
options.normalizethres = 1e6;
/* If the normalize option is on, then the matrix scaling is performed
only if 1 norm of matrix M (maximum absolute column sum) is above this threshold.
This enforce additional control over normalization since it seems to be more
aggressive also for well-conditioned problems. */
#ifdef MATLAB_MEX_FILE
/* overwriting default options by the user */
if (ssGetSFcnParamsCount(S)==3) {
nfields = mxGetNumberOfFields(ssGetSFcnParam(S,2));
for(i=0; i<nfields; i++){
fname = mxGetFieldNameByNumber(ssGetSFcnParam(S,2), i);
fval = mxGetField(ssGetSFcnParam(S,2), 0, fname);
if ( strcmp(fname,"zerotol")==0 )
options.zerotol = mxGetScalar(fval);
if ( strcmp(fname,"lextol")==0 )
options.lextol = mxGetScalar(fval);
if ( strcmp(fname,"maxpiv")==0 ) {
if (mxGetScalar(fval)>=(double)INT_MAX)
options.maxpiv = INT_MAX;
else
options.maxpiv = (int_T)mxGetScalar(fval);
}
if ( strcmp(fname,"nstepf")==0 )
options.nstepf = (int_T)mxGetScalar(fval);
if ( strcmp(fname,"timelimit")==0 )
options.timelimit = mxGetScalar(fval);
if ( strcmp(fname,"clock")==0 )
options.clock = (int_T)mxGetScalar(fval);
if ( strcmp(fname,"verbose")==0 )
options.verbose = (int_T)mxGetScalar(fval);
if ( strcmp(fname,"routine")==0 )
options.routine = (int_T)mxGetScalar(fval);
if ( strcmp(fname,"normalize")==0 )
options.normalize = (int_T)mxGetScalar(fval);
if ( strcmp(fname, "normalizethres")==0 )
options.normalizethres = mxGetScalar(fval);
}
}
#endif
/* Normalize M, q to avoid numerical problems if possible
Mn = diag(r)*M*diag(c) , qn = diag(r)*q */
/* initialize Mn, qn */
Mn = (double *)ssGetPWork(S)[0];
qn = (double *)ssGetPWork(S)[1];
/* initialize vectors r, c */
r = (double *)ssGetPWork(S)[2];
c = (double *)ssGetPWork(S)[3];
/* initialize auxiliary vector x */
x = (double *)ssGetPWork(S)[4];
/* initialize to ones */
for (i=0; i<NSTATES(S); i++) {
r[i] = 1.0;
c[i] = 1.0;
}
m = NSTATES(S);
n = m*m;
/* write data to Mn = M */
memcpy(Mn, *M, n*sizeof(double));
/* write data to qn = q */
memcpy(qn, *q, m*sizeof(double));
/* check out the 1-norm of matrix M (maximum column sum) */
for (i=0; i<m; i++) {
sn = dasum(&m, &Mn[i*m], &inc);
if (sn>s) {
s = sn;
}
}
/* scale matrix M, q and write scaling factors to r (rows) and c (columns) */
if (options.normalize && s>options.normalizethres) {
NormalizeMatrix (m, m, Mn, qn, r, c, options);
}
/* Setup the problem */
pA = ssGetPWork(S)[5];
/* A(:,1:m) = M */
memcpy(pMAT(pA), Mn, n*sizeof(double));
/* A(:,1:m) = -A(:,1:m) */
dscal(&n, &tmp, pMAT(pA), &inc);
/* A(:,m+1) = -1 */
for(i=0;i<m;i++)
C_SEL(pA,i,m) = -1.0;
/* A(:,m+2) = q */
memcpy(&(C_SEL(pA,0,m+1)),qn,m*sizeof(double));
/* initialize basis */
pB = ssGetPWork(S)[6];
/* check if the problem is not feasible at the beginning */
for (i=0; i<m; i++) {
if (qn[i]<-options.zerotol) {
qinfeas = 1;
break;
}
}
/* Solve the LCP */
if (qinfeas) {
#ifdef MATLAB_MEX_FILE
t1 = clock();
#endif
/* main LCP rouinte */
Result = lcp(pB, pA, &pivs, options);
#ifdef MATLAB_MEX_FILE
t2 = clock();
total_time = ((double)(t2-t1))/CLOCKS_PER_SEC;
#else
total_time = -1;
#endif
} else {
pivs = 0;
total_time = 0;
Result = LCP_FEASIBLE;
}
#ifdef MATLAB_MEX_FILE
if (options.clock) {
printf("Time needed to perform pivoting:\n time= %i (%lf seconds)\n",
t2-t1,total_time);
printf("Pivots: %ld\n", pivs);
printf("CLOCKS_PER_SEC = %i\n",CLOCKS_PER_SEC);
}
#endif
/* initialize values to 0 */
for(i=0;i<NSTATES(S);i++)
{
w[i] = 0.0;
z[i] = 0.0;
I[i] = 0.0;
}
/* for a feasible basis, compute the solution */
if ( Result == LCP_FEASIBLE || Result == LCP_PRETERMINATED )
{
#ifdef MATLAB_MEX_FILE
t1 = clock();
#endif
info = Basis_Solve(pB, &(C_SEL(pA,0,m+1)), x, options);
for (j=0,i=0;i<Index_Length(pB->pW);i++,j++)
{
w[Index_Get(pB->pW,i)] = x[j];
/* add 1 due to matlab 1-indexing */
I[j] = Index_Get(pB->pW,i)+1;
}
for(i=0;i<Index_Length(pB->pZ);i++,j++)
{
/* take only positive values */
if (x[j] > options.zerotol ) {
z[Index_Get(pB->pZ,i)] = x[j];
}
/* add 1 due to matlab 1-indexing */
I[j] = Index_Get(pB->pZ, i)+m+1;
}
#ifdef MATLAB_MEX_FILE
t2 = clock();
total_time+=(double)(t2-t1)/(double)CLOCKS_PER_SEC;
if (options.clock) {
printf("Time in total needed to solve LCP: %lf seconds\n",
total_time + (double)(t2-t1)/(double)CLOCKS_PER_SEC);
}
#endif
if (options.normalize) {
/* do the backward normalization */
/* z = diag(c)*zn */
for (i=0; i<m; i++) {
z[i] = c[i]*z[i];
}
/* since the normalization does not compute w properly, we recalculate it from
* recovered z */
/* write data to Mn = M */
memcpy(Mn, *M, n*sizeof(double));
/* write data to qn = q */
memcpy(qn, *q, m*sizeof(double));
/* copy w <- q; */
dcopy(&m, qn, &inc, w, &inc);
/* compute w = M*z + q */
dgemv(&T, &m, &m, &alpha, Mn, &m, z, &inc, &alpha, w, &inc);
/* if w is less than eps, consider it as zero */
for (i=0; i<m; i++) {
if (w[i]<options.zerotol) {
w[i] = 0.0;
}
}
}
}
/* outputs */
*exitflag = (real_T )Result;
*pivots =(real_T)pivs;
*time = (real_T)total_time;
/* reset dimensions and values in basis for a recursive call */
Reinitialize_Basis(m, pB);
}
