void EdgeTable::__buildEdgeTableForOneEdge(const int triID, const int v0, const int v1, void *userdata)
{
//if exist in one list, then already processed this edge
_EdgeTableElementStruct *pedge;
_VertexEdgeTablePointer *p = inEdgeList(m_ppVETP[v1], v0);
if (p){
pedge=p->m_pEdgeTableElement;
if (pedge->m_nT1==-1)
pedge->m_nT1 = triID;
return;
}
//if not, insert into both vertices
const int va = _MIN_(v0, v1);
const int vb = _MAX_(v0, v1);
pedge = allocEdgeElement(va, vb, triID, -1, userdata);
//alloc a pointer and insert into the list
_VertexEdgeTablePointer *pp = allocEdgePointor();
pp->m_pEdgeTableElement = pedge;
pp->m_pNext = m_ppVETP[v0], m_ppVETP[v0] = pp;
pp = allocEdgePointor();
pp->m_pEdgeTableElement = pedge;
pp->m_pNext = m_ppVETP[v1], m_ppVETP[v1] = pp;
}
void ViewSlicer::_computeDistance2View(const void *_vert, const int floatsize, const int nv, float &distMin, float &distMax)
{
//Manage the buffer
if (m_nDistArray==0){
m_pDistArray = new float[nv];
m_nDistArray = nv;
assert(m_pDistArray!=NULL);
}
else if (m_nDistArray<nv){
SafeDeleteArray(m_pDistArray);
m_pDistArray = new float[nv];
m_nDistArray = nv;
}
const float dist1 = DotProd(m_viewdir, m_viewpos);
distMin = 1e32f;
distMax = -distMin;
if (floatsize==8){//double
const Vector3d *vert = (const Vector3d*)_vert;
for (int i=0; i<nv; i++){
const Vector3d * _p = &vert[i];
Vector3f v((float)_p->x, (float)_p->y, (float)_p->z);
const float dist0 = DotProd(m_viewdir, v);
register float dd = dist0 - dist1;
distMin = _MIN_(dd, distMin);
distMax = _MAX_(dd, distMax);
m_pDistArray[i] = dd;
}
}
else{//float
assert(floatsize==4);
const Vector3f *vert = (const Vector3f*)_vert;
for (int i=0; i<nv; i++){
const Vector3f v = vert[i];
const float dist0 = DotProd(m_viewdir, v);
register float dd = dist0 - dist1;
distMin = _MIN_(dd, distMin);
distMax = _MAX_(dd, distMax);
m_pDistArray[i] = dd;
}
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *Prhs[])
{
register int i;
int n, ncon, m, mcon, cnp, pnp, mnp, nvars, nprojs, minnvars;
int status, reftype=BA_MOTSTRUCT, itmax, verbose=0, havejac, havedynproj, havedynprojac;
int len, nopts, nextra, nreserved, covlen;
double *p0, *p, *x, *covx=NULL;
double opts[SBA_OPTSSZ]={SBA_INIT_MU, SBA_STOP_THRESH, SBA_STOP_THRESH, SBA_STOP_THRESH, 0.0};
double info[SBA_INFOSZ];
char *vmask, *str;
register double *pdbl;
mxArray **prhs=(mxArray **)&Prhs[0];
struct mexdata mdata;
const int min1=MININARGS-1;
static char *reftypename[]={"motion & structure", "motion only", "structure only"};
clock_t start_time, end_time;
/* parse input args; start by checking their number */
if(nrhs<MININARGS)
matlabFmtdErrMsgTxt("sba: at least %d input arguments required (got %d).", MININARGS, nrhs);
if(nlhs<MINOUTARGS)
matlabFmtdErrMsgTxt("sba: at least %d output arguments required (got %d).", MINOUTARGS, nlhs);
/** n **/
/* the first argument must be a scalar */
if(!mxIsDouble(prhs[0]) || mxIsComplex(prhs[0]) || mxGetM(prhs[0])!=1 || mxGetN(prhs[0])!=1)
mexErrMsgTxt("sba: n must be a scalar.");
n=(int)mxGetScalar(prhs[0]);
/** ncon **/
/* the second argument must be a scalar */
if(!mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) || mxGetM(prhs[1])!=1 || mxGetN(prhs[1])!=1)
mexErrMsgTxt("sba: ncon must be a scalar.");
ncon=(int)mxGetScalar(prhs[1]);
/** m **/
/* the third argument must be a scalar */
if(!mxIsDouble(prhs[2]) || mxIsComplex(prhs[2]) || mxGetM(prhs[2])!=1 || mxGetN(prhs[2])!=1)
mexErrMsgTxt("sba: m must be a scalar.");
m=(int)mxGetScalar(prhs[2]);
/** mcon **/
/* the fourth argument must be a scalar */
if(!mxIsDouble(prhs[3]) || mxIsComplex(prhs[3]) || mxGetM(prhs[3])!=1 || mxGetN(prhs[3])!=1)
mexErrMsgTxt("sba: mcon must be a scalar.");
mcon=(int)mxGetScalar(prhs[3]);
/** mask **/
/* the fifth argument must be a nxm matrix */
if(!mxIsDouble(prhs[4]) || mxIsComplex(prhs[4]) || mxGetM(prhs[4])!=n || mxGetN(prhs[4])!=m)
matlabFmtdErrMsgTxt("sba: mask must be a %dx%d matrix (got %dx%d).", n, m, mxGetM(prhs[4]), mxGetN(prhs[4]));
if(mxIsSparse(prhs[4])) vmask=getVMaskSparse(prhs[4]);
else vmask=getVMaskDense(prhs[4]);
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "SBA: %s point visibility mask\n", mxIsSparse(prhs[4])? "sparse" : "dense");
#endif /* DEBUG */
/** p **/
/* the sixth argument must be a vector */
if(!mxIsDouble(prhs[5]) || mxIsComplex(prhs[5]) || !(mxGetM(prhs[5])==1 || mxGetN(prhs[5])==1))
mexErrMsgTxt("sba: p must be a real vector.");
p0=mxGetPr(prhs[5]);
/* determine if we have a row or column vector and retrieve its
* size, i.e. the number of parameters
*/
if(mxGetM(prhs[5])==1){
nvars=mxGetN(prhs[5]);
mdata.isrow_p0=1;
}
else{
nvars=mxGetM(prhs[5]);
mdata.isrow_p0=0;
}
/* copy input parameter vector to avoid destroying it */
p=(double*) mxMalloc(nvars*sizeof(double));
/*
for(i=0; i<nvars; ++i)
p[i]=p0[i];
*/
dblcopy_(p, p0, nvars);
/** cnp **/
/* the seventh argument must be a scalar */
if(!mxIsDouble(prhs[6]) || mxIsComplex(prhs[6]) || mxGetM(prhs[6])!=1 || mxGetN(prhs[6])!=1)
mexErrMsgTxt("sba: cnp must be a scalar.");
cnp=(int)mxGetScalar(prhs[6]);
/** pnp **/
/* the eighth argument must be a scalar */
if(!mxIsDouble(prhs[7]) || mxIsComplex(prhs[7]) || mxGetM(prhs[7])!=1 || mxGetN(prhs[7])!=1)
mexErrMsgTxt("sba: pnp must be a scalar.");
pnp=(int)mxGetScalar(prhs[7]);
/* check that p has the right dimension */
if(nvars!=m*cnp + n*pnp)
matlabFmtdErrMsgTxt("sba: p must have %d elements (got %d).", m*cnp + n*pnp, nvars);
/** x **/
/* the ninth argument must be a vector */
if(!mxIsDouble(prhs[8]) || mxIsComplex(prhs[8]) || !(mxGetM(prhs[8])==1 || mxGetN(prhs[8])==1))
mexErrMsgTxt("sba: x must be a real vector.");
x=mxGetPr(prhs[8]);
nprojs=_MAX_(mxGetM(prhs[8]), mxGetN(prhs[8]));
/* covx (optional) */
/* check if the tenth argument is a vector */
if(mxIsDouble(prhs[9]) && !mxIsComplex(prhs[9]) && (mxGetM(prhs[9])==1 || mxGetN(prhs[9])==1)){
covlen=_MAX_(mxGetM(prhs[9]), mxGetN(prhs[9]));
if(covlen>1){ /* make sure that argument is not a scalar */
covx=mxGetPr(prhs[9]);
++prhs;
--nrhs;
}
}
/** mnp **/
/* the tenth required argument must be a scalar */
if(!mxIsDouble(prhs[9]) || mxIsComplex(prhs[9]) || mxGetM(prhs[9])!=1 || mxGetN(prhs[9])!=1)
mexErrMsgTxt("sba: mnp must be a scalar.");
mnp=(int)mxGetScalar(prhs[9]);
nprojs/=mnp;
/* check that x has the correct dimension, comparing with the elements in vmask */
for(i=len=0; i<m*n; ++i)
if(vmask[i]) ++len;
if(nprojs!=len)
matlabFmtdErrMsgTxt("sba: the size of x should agree with the number of non-zeros in vmask (got %d and %d).", nprojs, len);
/* if supplied, check that covx has the correct dimension comparing with the elements in vmask */
if(covx && covlen!=len*mnp*mnp)
matlabFmtdErrMsgTxt("sba: covx must be a real vector of size %d (got %d).", len*mnp*mnp, covlen);
/** proj **/
/* the eleventh required argument must be a string , i.e. a char row vector */
if(mxIsChar(prhs[10])!=1)
mexErrMsgTxt("sba: proj argument must be a string.");
if(mxGetM(prhs[10])!=1)
mexErrMsgTxt("sba: proj argument must be a string (i.e. char row vector).");
/* retrieve supplied name */
len=mxGetN(prhs[10])+1;
status=mxGetString(prhs[10], mdata.projname, _MIN_(len, MAXNAMELEN));
if(status!=0)
mexErrMsgTxt("sba: not enough space. String is truncated.");
/* check if we have a name@library pair */
if((str=strchr(mdata.projname, '@'))){
*str++='\0';
/* copy the library name */
strcpy(mdata.projlibname, str);
/* attempt to load the library */
#ifdef _WIN32
mdata.projlibhandle=LoadLibrary(mdata.projlibname);
if(!mdata.projlibhandle)
#else
mdata.projlibhandle=dlopen(mdata.projlibname, RTLD_LAZY);
if(!mdata.projlibhandle)
#endif /* _WIN32 */
matlabFmtdErrMsgTxt("sba: error loading dynamic library %s!\n", mdata.projlibname);
havedynproj=1;
}
else{
mdata.projlibhandle=NULL;
havedynproj=0;
}
/** jac (optional) **/
havejac=havedynprojac=0;
/* check whether the twelfth argument is a nonempty string */
if(mxIsChar(prhs[11])==1){
switch(mxGetM(prhs[11])){
case 1:
/* store supplied name */
len=mxGetN(prhs[11])+1;
status=mxGetString(prhs[11], mdata.projacname, _MIN_(len, MAXNAMELEN));
if(status!=0)
mexErrMsgTxt("sba: not enough space. String is truncated.");
havejac=1;
/* check if we have a name@library pair */
if((str=strchr(mdata.projacname, '@'))){
*str++='\0';
/* copy the library name */
strcpy(mdata.projaclibname, str);
if(!havedynproj || strcmp(mdata.projlibname, mdata.projaclibname)){ /* is this a different library from that for the proj. function? */
/* yes, attempt to load it */
# ifdef _WIN32
mdata.projaclibhandle=LoadLibrary(mdata.projaclibname);
if(!mdata.projaclibhandle)
# else
mdata.projaclibhandle=dlopen(mdata.projaclibname, RTLD_LAZY);
if(!mdata.projaclibhandle)
# endif /* _WIN32 */
matlabFmtdErrMsgTxt("sba: error loading dynamic library %s!\n", mdata.projaclibname);
}
else /* proj. function and Jacobian come from the same library */
mdata.projaclibhandle=mdata.projlibhandle;
havedynprojac=1;
}
else{
mdata.projaclibhandle=NULL;
havedynprojac=0;
}
/* falling through! */
case 0: /* empty string, ignore */
++prhs;
--nrhs;
break;
default:
matlabFmtdErrMsgTxt("sba: projac argument must be a string (i.e. row vector); got %dx%d.",
mxGetM(prhs[11]), mxGetN(prhs[11]));
}
}
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "SBA: %s analytic Jacobian\n", havejac? "with" : "no");
#endif /* DEBUG */
/** itmax **/
/* the twelfth required argument must be a scalar */
if(!mxIsDouble(prhs[11]) || mxIsComplex(prhs[11]) || mxGetM(prhs[11])!=1 || mxGetN(prhs[11])!=1)
mexErrMsgTxt("sba: itmax must be a scalar.");
itmax=(int)mxGetScalar(prhs[11]);
/* all arguments below this point are optional */
/* check if we have a scalar argument; if yes, this is taken to be the 'verbose' argument */
if(nrhs>=MININARGS){
if(mxIsDouble(prhs[min1]) && !mxIsComplex(prhs[min1]) && mxGetM(prhs[min1])==1 && mxGetN(prhs[min1])==1){
verbose=(int)mxGetScalar(prhs[min1]);
++prhs;
--nrhs;
}
}
/* check if we have a vector argument; if yes, this is taken to be the 'opts' argument */
if(nrhs>=MININARGS && mxIsDouble(prhs[min1]) && !mxIsComplex(prhs[min1]) && ((mxGetM(prhs[min1])==1 || mxGetN(prhs[min1])==1)
|| (mxGetM(prhs[min1])==0 && mxGetN(prhs[min1])==0))){
pdbl=mxGetPr(prhs[min1]);
nopts=_MAX_(mxGetM(prhs[min1]), mxGetN(prhs[min1]));
if(nopts!=0){ /* if opts==[], nothing needs to be done and the defaults are used */
if(nopts>SBA_OPTSSZ)
matlabFmtdErrMsgTxt("sba: opts must have at most %d elements, got %d.", SBA_OPTSSZ, nopts);
else if(nopts<SBA_OPTSSZ)
matlabFmtdWarnMsgTxt("sba: only the %d first elements of opts specified, remaining set to defaults.", nopts);
for(i=0; i<nopts; ++i)
opts[i]=pdbl[i];
}
#ifdef DEBUG
else{
fflush(stderr);
fprintf(stderr, "SBA: empty options vector, using defaults\n");
}
#endif /* DEBUG */
++prhs;
--nrhs;
}
/* check if we have a string argument; if yes, this is taken to be the 'reftype' argument */
if(nrhs>=MININARGS && mxIsChar(prhs[min1])==1 && mxGetM(prhs[min1])==1){
char *refhowto;
/* examine supplied name */
len=mxGetN(prhs[min1])+1;
refhowto=(char*) mxCalloc(len, sizeof(char));
status=mxGetString(prhs[min1], refhowto, len);
if(status!=0)
mexErrMsgTxt("sba: not enough space. String is truncated.");
for(i=0; refhowto[i]; ++i)
refhowto[i]=tolower(refhowto[i]);
if(!strcmp(refhowto, "motstr")) reftype=BA_MOTSTRUCT;
else if(!strcmp(refhowto, "mot")) reftype=BA_MOT;
else if(!strcmp(refhowto, "str")) reftype=BA_STRUCT;
else matlabFmtdErrMsgTxt("sba: unknown minimization type '%s'.", refhowto);
mxFree(refhowto);
++prhs;
--nrhs;
}
else
reftype=BA_MOTSTRUCT;
/* arguments below this point are assumed to be extra arguments passed
* to every invocation of the projection function and its Jacobian
*/
nextra=nrhs-MININARGS+1;
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "SBA: %d extra args\n", nextra);
#endif /* DEBUG */
/* handle any extra args and allocate memory for
* passing them to matlab/C
*/
if(!havedynproj || !havedynprojac){ /* at least one of the projection and Jacobian functions are in matlab */
nreserved=4; /* j, i, aj, bi */
mdata.nrhs=nextra+nreserved;
mdata.rhs=(mxArray **)mxMalloc(mdata.nrhs*sizeof(mxArray *));
for(i=0; i<nextra; ++i)
mdata.rhs[i+nreserved]=(mxArray *)prhs[nrhs-nextra+i]; /* discard 'const' modifier */
mdata.rhs[0]=mxCreateDoubleMatrix(1, 1, mxREAL); /* camera index */
mdata.rhs[1]=mxCreateDoubleMatrix(1, 1, mxREAL); /* point index */
mdata.rhs[2]=mxCreateDoubleMatrix(1, cnp, mxREAL); /* camera parameters */
mdata.rhs[3]=mxCreateDoubleMatrix(1, pnp, mxREAL); /* point parameters */
mdata.dynadata=NULL;
}
else
mdata.rhs=NULL;
mdata.dynadata=NULL;
if(havedynproj || havedynprojac){ /* at least one of the projection and Jacobian functions are from a dynlib */
if(nextra>0){
mdata.dynadata=(double **)mxMalloc(nextra*sizeof(double *));
for(i=0; i<nextra; ++i)
mdata.dynadata[i]=mxGetPr(prhs[nrhs-nextra+i]);
}
}
#ifdef DEBUG
fprintf(stderr, "Projection function: %s, Jacobian: %s, %s\n", havedynproj? "dynamic" : "matlab",
havejac? "present" : "not present", havedynprojac? "dynamic" : "matlab");
#endif
mdata.cnp=cnp; mdata.pnp=pnp;
mdata.mnp=mnp;
/* ensure that the supplied matlab function & Jacobian are as expected */
if((!havedynproj || !havedynprojac) && /* at least one in matlab */
checkFuncAndJacobianMATLAB(0, 0, p, p+m*cnp, !havedynproj, havejac && !havedynprojac, reftype, &mdata)){ /* check using first camera & first point */
status=SBA_ERROR;
goto cleanup;
}
/* invoke sba */
start_time=clock();
switch(reftype){
case BA_MOTSTRUCT:
minnvars=nvars;
mdata.pa=mdata.pb=NULL; /* not needed */
status=sba_motstr_levmar(n, ncon, m, mcon, vmask, p, cnp, pnp, x, covx, mnp,
(havedynproj)? proj_motstrDL : proj_motstrMATLAB, (havejac)? (havedynprojac? projac_motstrDL : projac_motstrMATLAB) : NULL,
(void *)&mdata, itmax, verbose>1, opts, info);
break;
case BA_MOT:
minnvars=m*cnp;
mdata.pa=NULL; /* not needed */
mdata.pb=p+m*cnp;
/* note: only the first part of p is used in the call below (i.e. first m*cnp elements) */
status=sba_mot_levmar(n, m, mcon, vmask, p, cnp, x, covx, mnp,
(havedynproj)? proj_motDL : proj_motMATLAB, (havejac)? (havedynprojac? projac_motDL : projac_motMATLAB) : NULL,
(void *)&mdata, itmax, verbose>1, opts, info);
break;
case BA_STRUCT:
minnvars=n*pnp;
mdata.pa=p;
mdata.pb=NULL; /* not needed */
status=sba_str_levmar(n, ncon, m, vmask, p+m*cnp, pnp, x, covx, mnp,
(havedynproj)? proj_strDL : proj_strMATLAB, (havejac)? (havedynprojac? projac_strDL : projac_strMATLAB) : NULL,
(void *)&mdata, itmax, verbose>1, opts, info);
break;
default: /* should not reach this point */
matlabFmtdErrMsgTxt("sba: unknown refinement type %d requested.", reftype);
}
end_time=clock();
if(verbose){
fflush(stdout);
mexPrintf("\nSBA using %d 3D pts, %d frames and %d image projections, %d variables\n",
n, m, nprojs, minnvars);
mexPrintf("\nRefining %s, %s Jacobian\n\n", reftypename[reftype], havejac? "analytic" : "approximate");
mexPrintf("SBA returned %d in %g iter, reason %g, error %g [initial %g], %d/%d func/fjac evals, %d lin. systems\n",
status, info[5], info[6], info[1]/nprojs, info[0]/nprojs, (int)info[7], (int)info[8], (int)info[9]);
mexPrintf("Elapsed time: %.2lf seconds, %.2lf msecs\n", ((double) (end_time - start_time)) / CLOCKS_PER_SEC,
((double) (end_time - start_time)) / CLOCKS_PER_MSEC);
fflush(stdout);
}
/* copy back return results */
/** ret **/
plhs[0]=mxCreateDoubleMatrix(1, 1, mxREAL);
pdbl=mxGetPr(plhs[0]);
*pdbl=(double)status;
/** p **/
plhs[1]=(mdata.isrow_p0==1)? mxCreateDoubleMatrix(1, nvars, mxREAL) : mxCreateDoubleMatrix(nvars, 1, mxREAL);
pdbl=mxGetPr(plhs[1]);
/*
for(i=0; i<nvars; ++i)
pdbl[i]=p[i];
*/
dblcopy_(pdbl, p, nvars);
/** info **/
if(nlhs>MINOUTARGS){
plhs[2]=mxCreateDoubleMatrix(1, SBA_INFOSZ, mxREAL);
pdbl=mxGetPr(plhs[2]);
/*
for(i=0; i<SBA_INFOSZ; ++i)
pdbl[i]=info[i];
*/
dblcopy_(pdbl, info, SBA_INFOSZ);
}
cleanup:
/* cleanup */
mxFree(vmask);
mxFree(p);
if(mdata.rhs){
for(i=0; i<nreserved; ++i)
mxDestroyArray(mdata.rhs[i]);
mxFree(mdata.rhs);
}
if(mdata.dynadata) mxFree(mdata.dynadata);
/* unload libraries */
if(havedynproj){
#ifdef _WIN32
i=FreeLibrary(mdata.projlibhandle);
if(i==0)
#else
i=dlclose(mdata.projlibhandle);
if(i!=0)
#endif /* _WIN32 */
matlabFmtdErrMsgTxt("sba: error unloading dynamic library %s!\n", mdata.projlibname);
}
if(havedynprojac){
if(mdata.projaclibhandle!=mdata.projlibhandle){
#ifdef _WIN32
i=FreeLibrary(mdata.projaclibhandle);
if(i==0)
#else
i=dlclose(mdata.projaclibhandle);
if(i!=0)
#endif /* _WIN32 */
matlabFmtdErrMsgTxt("sba: error unloading dynamic library %s!\n", mdata.projaclibname);
}
}
}
/* check the supplied matlab projection function and its Jacobian. Returns 1 on error, 0 otherwise */
static int checkFuncAndJacobianMATLAB(int j, int i, double *aj, double *bi, int chkproj, int chkjac, int mintype, struct mexdata *dat)
{
mxArray *lhs[2]={NULL, NULL};
register int k;
int nlhs, ret=0;
double *mp;
mexSetTrapFlag(1); /* handle errors in the MEX-file */
mp=mxGetPr(dat->rhs[0]); *mp=j;
mp=mxGetPr(dat->rhs[1]); *mp=i;
mp=mxGetPr(dat->rhs[2]);
for(k=0; k<dat->cnp; ++k)
mp[k]=aj[k];
mp=mxGetPr(dat->rhs[3]);
for(k=0; k<dat->pnp; ++k)
mp[k]=bi[k];
if(chkproj){
/* attempt to call the supplied proj */
k=mexCallMATLAB(1, lhs, dat->nrhs, dat->rhs, dat->projname);
if(k){
fprintf(stderr, "sba: error calling '%s'.\n", dat->projname);
ret=1;
}
else if(!lhs[0] || !mxIsDouble(lhs[0]) || mxIsComplex(lhs[0]) || !(mxGetM(lhs[0])==1 || mxGetN(lhs[0])==1) ||
_MAX_(mxGetM(lhs[0]), mxGetN(lhs[0]))!=dat->mnp){
fprintf(stderr, "sba: '%s' should produce a real vector with %d elements (got %d).\n",
dat->projname, dat->mnp, _MAX_(mxGetM(lhs[0]), mxGetN(lhs[0])));
ret=1;
}
/* delete the vector created by matlab */
mxDestroyArray(lhs[0]);
}
if(chkjac){
lhs[0]=lhs[1]=NULL;
nlhs=(mintype==BA_MOTSTRUCT)? 2 : 1;
/* attempt to call the supplied jac */
k=mexCallMATLAB(nlhs, lhs, dat->nrhs, dat->rhs, dat->projacname);
if(k){
fprintf(stderr, "sba: error calling '%s'.\n", dat->projacname);
ret=1;
}
else if(mintype==BA_MOTSTRUCT || mintype==BA_MOT){
if(!lhs[0] || !mxIsDouble(lhs[0]) || mxIsComplex(lhs[0]) ||
_MIN_(mxGetM(lhs[0]), mxGetN(lhs[0]))!=1 ||
_MAX_(mxGetM(lhs[0]), mxGetN(lhs[0]))!=dat->mnp*dat->cnp){
fprintf(stderr, "sba: '%s' should produce a real %d row or column vector as its first output arg (got %dx%d).\n",
dat->projacname, dat->mnp*dat->cnp, mxGetM(lhs[0]), mxGetN(lhs[0]));
ret=1;
}
}
else{ /* BA_STRUCT */
if(!lhs[0] || !mxIsDouble(lhs[0]) || mxIsComplex(lhs[0]) ||
_MIN_(mxGetM(lhs[0]), mxGetN(lhs[0]))!=1 ||
_MAX_(mxGetM(lhs[0]), mxGetN(lhs[0]))!=dat->mnp*dat->pnp){
fprintf(stderr, "sba: '%s' should produce a real %d row or column vector as its first output arg (got %dx%d).\n",
dat->projacname, dat->mnp*dat->pnp, mxGetM(lhs[0]), mxGetN(lhs[0]));
ret=1;
}
}
if(lhs[0] && mxIsSparse(lhs[0])){
fprintf(stderr, "sba: '%s' should produce a real dense vector as its first output arg, not a sparse one.\n");
ret=1;
}
if(nlhs==2){ /* BA_MOTSTRUCT */
if(!lhs[1] || !mxIsDouble(lhs[1]) || mxIsComplex(lhs[1]) ||
_MIN_(mxGetM(lhs[1]), mxGetN(lhs[1]))!=1 ||
_MAX_(mxGetM(lhs[1]), mxGetN(lhs[1]))!=dat->mnp*dat->pnp){
fprintf(stderr, "sba: '%s' should produce a real %d row or column vector as its second output arg (got %dx%d).\n",
dat->projacname, dat->mnp*dat->pnp, mxGetM(lhs[1]), mxGetN(lhs[1]));
ret=1;
}
else if(lhs[1] && mxIsSparse(lhs[1])){
fprintf(stderr, "sba: '%s' should produce a real dense vector as its second output arg, not a sparse one.\n");
ret=1;
}
}
/* delete the vectors created by matlab */
for(k=0; k<nlhs; ++k)
mxDestroyArray(lhs[k]);
}
mexSetTrapFlag(0); /* on error terminate the MEX-file and return control to the MATLAB prompt */
return ret;
}
inline void
getElementNearFar(float *pDistArray, const int *pelm, const int elmID, const int elmtype, float& distmin, float& distmax)
{
distmin=1e32f;
distmax=-distmin;
if (elmtype==4){
const Vector4i* ptet = (const Vector4i*)pelm;
const Vector4i tet = ptet[elmID];
float t = pDistArray[tet.x];
distmin = _MIN_(distmin, t);
distmax = _MAX_(distmax, t);
t = pDistArray[tet.y];
distmin = _MIN_(distmin, t);
distmax = _MAX_(distmax, t);
t = pDistArray[tet.z];
distmin = _MIN_(distmin, t);
distmax = _MAX_(distmax, t);
t = pDistArray[tet.w];
distmin = _MIN_(distmin, t);
distmax = _MAX_(distmax, t);
}
else{
const Vector8i* phex = (const Vector8i*)pelm;
const Vector8i hex = phex[elmID];
float t = pDistArray[hex.x];
distmin = _MIN_(distmin, t);
distmax = _MAX_(distmax, t);
t = pDistArray[hex.y];
distmin = _MIN_(distmin, t);
distmax = _MAX_(distmax, t);
t = pDistArray[hex.z];
distmin = _MIN_(distmin, t);
distmax = _MAX_(distmax, t);
t = pDistArray[hex.w];
distmin = _MIN_(distmin, t);
distmax = _MAX_(distmax, t);
t = pDistArray[hex.x1];
distmin = _MIN_(distmin, t);
distmax = _MAX_(distmax, t);
t = pDistArray[hex.y1];
distmin = _MIN_(distmin, t);
distmax = _MAX_(distmax, t);
t = pDistArray[hex.z1];
distmin = _MIN_(distmin, t);
distmax = _MAX_(distmax, t);
t = pDistArray[hex.w1];
distmin = _MIN_(distmin, t);
distmax = _MAX_(distmax, t);
}
}
