double row_or_column_major (struct GMT_BCR *bcr, float *data, double *wx, double *wy, int ij, int row_maj) {
int i, j;
double x, y, retval, wsum, w;
retval = wsum = 0.0;
if (row_maj) { /* C order */
for (j = 0; j < bcr->n; j++) {
for (i = 0; i < bcr->n; i++) {
if (!mxIsNaN(data[ij+i])) {
w = wx[i] * wy[j];
retval += data[ij+i] * w;
wsum += w;
}
}
ij += bcr->mx;
}
}
else { /* Matlab (fortran) ordering */
for (i = 0; i < bcr->n; i++) {
for (j = 0; j < bcr->n; j++) {
if (!mxIsNaN(data[ij-j])) {
w = wx[i] * wy[j];
retval += data[ij-j] * w;
wsum += w;
}
}
ij += bcr->my;
}
}
return ( ((wsum + GMT_CONV_LIMIT - bcr->threshold) > 0.0) ? retval / wsum : mxGetNaN());
}
void get_data(const mxArray *data, int *pp, int *pn, int *pN, double **y, int **miss)
{
int i;
double *ydata = mxGetPr(data);
if(pN) {
int yndims = mxGetNumberOfDimensions(data);
const int *ydims = mxGetDimensions(data);
int p = *pp = ydims[0];
int n = *pn = ydims[1];
int N = *pN = (yndims > 2) ? ydims[2] : 1;
*miss = (int *)0;
for(i=0; i<p*N*n; i++)
if(mxIsNaN(ydata[i])) {
*miss = g_miss = (int *)mxCalloc(p*n, sizeof(int));
break;
}
if(N > 1) {
int j, t;
double *yiter = *y = g_y = (double *)mxCalloc(p*N*n, sizeof(double));
for(t=0; t<n; t++, yiter += p*N)
for(i=0; i<p; i++) {
if(*miss) (*miss)[i+t*p] = false;
for(j=0; j<N; j++) {
yiter[i+j*p] = ydata[i+t*p+j*p*n];
if(*miss && !(*miss)[i+t*p] && mxIsNaN(yiter[i+j*p])) (*miss)[i+t*p] = true;
}
}
}
else {
*y = ydata;
if(*miss) for(i=0; i<p*n; i++) (*miss)[i] = mxIsNaN((*y)[i]);
}
}
else {
int p = *pp = mxGetM(data);
int n = *pn = mxGetN(data);
*miss = (int *)0;
for(i=0; i<p*n; i++)
if(mxIsNaN(ydata[i])) {
*miss = g_miss = (int *)mxCalloc(p*n, sizeof(int));
break;
}
*y = ydata;
if(*miss) for(i=0; i<p*n; i++) (*miss)[i] = mxIsNaN((*y)[i]);
}
}
static double user_function(unsigned n, const double *x,
double *gradient, /* NULL if not needed */
void *d_)
{
user_function_data *d = (user_function_data *) d_;
double f;
d->plhs[0] = d->plhs[1] = NULL;
memcpy(mxGetPr(d->prhs[d->xrhs]), x, n * sizeof(double));
CHECK0(0 == mexCallMATLAB(gradient ? 2 : 1, d->plhs,
d->nrhs, d->prhs, d->f),
"error calling user function");
CHECK0(mxIsNumeric(d->plhs[0]) && !mxIsComplex(d->plhs[0])
&& mxGetM(d->plhs[0]) * mxGetN(d->plhs[0]) == 1,
"user function must return real scalar");
f = mxGetScalar(d->plhs[0]);
mxDestroyArray(d->plhs[0]);
if (gradient) {
CHECK0(mxIsDouble(d->plhs[1]) && !mxIsComplex(d->plhs[1])
&& (mxGetM(d->plhs[1]) == 1 || mxGetN(d->plhs[1]) == 1)
&& mxGetM(d->plhs[1]) * mxGetN(d->plhs[1]) == n,
"gradient vector from user function is the wrong size");
memcpy(gradient, mxGetPr(d->plhs[1]), n * sizeof(double));
mxDestroyArray(d->plhs[1]);
}
d->neval++;
if (d->verbose) mexPrintf("nlopt_optimize eval #%d: %g\n", d->neval, f);
if (mxIsNaN(f)) nlopt_force_stop(d->opt);
return f;
}
void nnInter2D(double *Tc, const double *T,
const double *omega, const double *m, int N, const double *X,
double *h, bool boundary) {
// Increments in X and Y direction
double x, y;
int i, xf, yf, i2 = m[0];
#pragma omp parallel for default(shared) private(i, x, y, xf, yf)
for (i=0; i<N; i++) {
// map x from [h/2,omega-h/2] -> [0,m-1],
x = (X[i] - omega[0]) / h[0] + .5 - 1;
y = (X[i+N] - omega[2]) / h[1] + .5 - 1;
xf = round(x);
yf = round(y);
if (boundary) {
// Replicate boundary
Tc[i] = T[min(m[0]-1,max(0,xf)) + i2 * min(m[1]-1,max(0,yf))];
} else {
// Zero padding
Tc[i] = (xf<0 || yf<0 || xf>m[0]-1 || yf>m[1]-1)? 0: T[xf+i2*yf];
}
if (mxIsNaN(Tc[i]))
mexPrintf("Is NAN");
}
}
mxArray *
pfstring2mxArray( char *in )
{
mxArray *result;
mxArray *input[1];
mxArray *output[1];
input[0] = mxCreateString( in );
mexCallMATLAB( 1, output, 1, input, "str2double" );
if( mxIsNaN( mxGetScalar( output[0] ) ) ) {
result = input[0];
mxDestroyArray( output[0] );
} else {
result = output[0];
mxDestroyArray( input[0] );
}
return result;
}
void nnInter1D(double *Tc, const double *T,
const double *omega, const double *m, int N, const double *X,
double *h, bool boundary) {
double x;
int i, xf;
#pragma omp parallel for default(shared) private(i, x, xf)
for (i=0; i<N; i++) {
// map x from [h/2,omega-h/2] -> [0,m-1],
x = (X[i] - omega[0]) / h[0] + .5 - 1;
xf = round(x);
if (boundary) {
// Replicate boundary
Tc[i] = T[min(m[0]-1,max(0,xf))];
} else {
// Zero padding
Tc[i] = (xf<0 || xf>m[0]-1)? 0: T[xf];
}
if (mxIsNaN(Tc[i]))
mexPrintf("Is NAN");
}
}
void mexFunction(int nlhs, mxArray *out[], int nrhs, const mxArray *input[])
{
if (nlhs == 0) {
return;
} else if (nlhs != 1 || nrhs != 3) {
mexErrMsgTxt("Error. Usage: [dzdx] = regionToPixel_backward(boxCount, spMap, dzdy)");
return;
}
// Get pointers
const mxArray* boxCountMx = input[0];
const mxArray* spMapMx = input[1];
const mxArray* dzdyMx = input[2];
// Check inputs
if (!mxIsDouble(boxCountMx) || !isScalar(boxCountMx)) {
mexErrMsgTxt("Error: boxCount must be a scalar double!");
}
if (!mxIsDouble(spMapMx) || mxGetNumberOfDimensions(spMapMx) != 2) {
mexErrMsgTxt("Error: spMap must be double with format labelCount x spCount!");
}
int labelCount = mxGetM(spMapMx);
int spCount = mxGetN(spMapMx);
const mwSize* dzdyDims = mxGetDimensions(dzdyMx);
if (!mxIsSingle(dzdyMx) || dzdyDims[0] != 1 || dzdyDims[1] != 1 || dzdyDims[2] != labelCount ||
(!(mxGetNumberOfDimensions(dzdyMx) == 4 && dzdyDims[3] == spCount)
&& !(mxGetNumberOfDimensions(dzdyMx) == 3))) {
mexErrMsgTxt("Error: dzdy must be single with format 1 x 1 x labelCount x spCount!");
}
// Get arrays
int boxCount = (int) mxGetScalar(boxCountMx);
double* spMap = (double*) mxGetData(spMapMx);
float* dzdy = (float*) mxGetData(dzdyMx);
// Create output and initialize it to all zeros (in mxCreateNumericArray)
mwSize dzdxSize[4];
dzdxSize[0] = 1;
dzdxSize[1] = 1;
dzdxSize[2] = labelCount;
dzdxSize[3] = boxCount;
out[0] = mxCreateNumericArray(4, dzdxSize, mxSINGLE_CLASS, mxREAL);
float* dzdx = (float*) mxGetData(out[0]);
for (int spIdx = 0; spIdx < spCount; spIdx++) {
for (int labelIdx = 0; labelIdx < labelCount; labelIdx++) {
// We can safely ignore the first two dimensions of these
// matrices as they are always 1
int spMapIdx = labelIdx + spIdx * labelCount;
double boxIdxD = spMap[spMapIdx];
int boxIdx = (int) boxIdxD - 1; // Convert from Matlab to C indexing
if (!mxIsNaN(boxIdxD)) {
int dzdxIdx = labelIdx + boxIdx * labelCount;
dzdx[dzdxIdx] = dzdx[dzdxIdx] + dzdy[spMapIdx];
}
}
}
}
void Matrix66Pass(double *r, double *M, int num_particles)
{ int c, c6;
for(c = 0;c<num_particles;c++)
{ c6 = c*6;
if(!mxIsNaN(r[c6]))
ATmultmv(r+c*6,M);
}
}
void splineInter2D(double *Tc, double *dT, const double *T,
const double *omega, const double *m, int N, const double *X,
double *h, bool doDerivative, bool boundary) {
// Increments in X and Y direction
int xf, yf, i, j, k, m0 = m[0], m1 = m[1];
double x, y, p;
//for each value X compute the value in the spline
#pragma omp parallel for default(shared) private(x, y, xf, yf, p, i, j, k)
for (i=0;i<N;i++) {
Tc[i] = 0;
x = (X[i] - omega[0]) / h[0] + .5 - 1;
y = (X[i+N] - omega[2]) / h[1] + .5 - 1;
//check if it is a valid x
if (!boundary && (x<=-2 || y<=-2 || x>=m0+1 || y>=m1+1))
continue;
xf = floor(x);
yf = floor(y);
x = x - xf;
y = y - yf;
if (doDerivative) {
dT[i] = 0;
dT[i+N] = 0;
}
for (j=-1;j<3;j++) {
for (k=-1;k<3;k++) {
if (boundary) {
p = T[min(m0-1,max(0,xf+j))+m0*(min(m1-1,max(0,yf+k)))];
} else {
p = (xf+j<0 || xf+j>m0-1 ||
yf+k<0 || yf+k>m1-1)? 0: T[xf+j+m0*(yf+k)];
}
Tc[i] += p*b0(3-j,x-j)*b0(3-k,y-k);
if (doDerivative) {
dT[i] += p*db0(3-j,x-j)* b0(3-k,y-k);
dT[i+N] += p* b0(3-j,x-j)*db0(3-k,y-k);
}
}
}
if (mxIsNaN(Tc[i]))
mexPrintf("Is NAN. ");
if (doDerivative) {
dT[i] = dT[i]/h[0];
dT[i+N] = dT[i+N]/h[1];
}
}
}
void QuadLinearPass(double *r, double le, double kv, double *T1, double *T2, double *R1, double *R2, int num_particles)
{ int c;
double *r6;
bool useT1, useT2, useR1, useR2;
if(T1==NULL)
useT1=false;
else
useT1=true;
if(T2==NULL)
useT2=false;
else
useT2=true;
if(R1==NULL)
useR1=false;
else
useR1=true;
if(R2==NULL)
useR2=false;
else
useR2=true;
for(c = 0;c<num_particles;c++)
{ r6 = r+c*6;
if(!mxIsNaN(r6[0]) & mxIsFinite(r6[4]))
/*
function quad6 internally calculates the square root
of the energy deviation of the particle
To protect against DOMAIN and OVERFLOW error, check if the
fifth component of the phase spacevector r6[4] is finite
*/
{
/* Misalignment at entrance */
if(useT1)
ATaddvv(r6,T1);
if(useR1)
ATmultmv(r6,R1);
quad6(r6,le,kv);
/* Misalignment at exit */
if(useR2)
ATmultmv(r6,R2);
if(useT2)
ATaddvv(r6,T2);
}
}
}
inline void do_nan_check(_T *F,int num_elements)
{
for (int p = 0; p < num_elements; p++)
{
if (mxIsNaN(F[p]))
{
mexErrMsgIdAndTxt("Images:watershed:expectedNonNaN",
"%s",
"Input image may not contain NaNs.");
}
}
}
void jacdist(T *x, mwSize m, mwSize n, T *d)
{
/*
nz = XI ~= 0 | XJ ~= 0;
ne = XI ~= XJ;
d = sum(ne&nz,2) ./ sum(nz,2); % Jaccard
*/
mwIndex i,j,k;
T theSum,nz;
T *XI, *XJ, *XI0;
T *theNaN = (T*)mxCalloc(m,sizeof(T));
/* Determine which rows of X have missing data.
*/
XI = x;
for (i=0; i<m; i++) {
for (k=0;k<n;k++,XI++) if (mxIsNaN(*XI)) theNaN[i] = *XI;
}
XI = x;
for (i=0; i<m; i++) {
XI0 = XI;
XJ = XI+n;
for (j=i+1; j<m; j++) {
/* XI = x + i*n; XJ = x + j*n; */
XI = XI0;
/* If XI or XJ have missing data, set their distance to NaN.
*/
if (theNaN[i] || theNaN[j]) {
XI += n;
XJ += n;
*(d++) = theNaN[i] + theNaN[j];
} else {
theSum = 0;
nz = 0;
for (k=0;k<n;k++,XI++,XJ++){
if ((*XI) || (*XJ)) {
nz++;
if ((*XI)!=(*XJ)) {
theSum++;
}
}
}
if (nz) {
*(d++) = theSum/nz;
} else {
*(d++) = (T)mxGetNaN();
}
}
}
}
mxFree(theNaN);
}
/*
* c o n t a i n s N a N
*/
BooleanType containsNaN( const real_t* const data, uint_t dim )
{
uint_t i;
if ( data == 0 )
return BT_FALSE;
for ( i = 0; i < dim; ++i )
if ( mxIsNaN(data[i]) == 1 )
return BT_TRUE;
return BT_FALSE;
}
void numeric(double dbl, json_object **jo){
int intgr = round(dbl);
if(mxIsNaN(dbl) || mxIsInf(dbl)){
*jo = NULL;
}
else{
if(fabs(dbl-(double)intgr) > 1e-18){
*jo = json_object_new_double(dbl);
}
else{
*jo = json_object_new_int(intgr);
}
}
}
void chedist(T *x, mwSize m, mwSize n, T *d)
{
/*
d = max(abs(XI-XJ),[],2);
*/
mwIndex i,j,k;
T theMax,Y;
T *XI, *XJ, *XI0;
T *theNaN = (T*)mxCalloc(m,sizeof(T));
/* Determine which rows of X have missing data.
*/
XI = x;
for (i=0; i<m; i++) {
for (k=0;k<n;k++,XI++) if (mxIsNaN(*XI)) theNaN[i] = *XI;
}
XI = x;
for (i=0; i<m; i++) {
XI0 = XI;
XJ = XI+n;
for (j=i+1; j<m; j++) {
/* XI = x + i*n; XJ = x + j*n; */
XI = XI0;
/* If XI or XJ have missing data, set their distance to NaN.
*/
if (theNaN[i] || theNaN[j]) {
XI += n;
XJ += n;
*(d++) = theNaN[i] + theNaN[j];
} else {
theMax = 0;
for (k=0;k<n;k++,XI++,XJ++){
Y = (T)fabs((*XI)-(*XJ));
if (Y>theMax) {
theMax = Y;
}
}
*(d++) = theMax;
}
}
}
mxFree(theNaN);
}
void splineInter1D(double *Tc, double *dT, const double *T,
const double *omega, const double *m, int N, const double *X,
double *h, bool doDerivative, bool boundary) {
int xf, i, j, m0 = m[0];
double x, p;
#pragma omp parallel for default(shared) private(x, xf, p, i, j)
for (i=0;i<N;i++) {
Tc[i] = 0;
x = (X[i] - omega[0]) / h[0] + .5 - 1; //subtract 1 for indexing purposes in C
//check if it is a valid x
if (!boundary && (x<=-2 || x>=m0+1))
continue;
xf = floor(x);
x = x - xf;
if (doDerivative) {
dT[i] = 0;
}
for (j=-1;j<3;j++) {
if (boundary) {
p = T[min(m0-1,max(0,xf+j))];
} else {
p = (xf+j<0 || xf+j>m0-1)? 0: T[xf+j];
}
Tc[i] += p*b0(3-j,x-j);
if (doDerivative) {
dT[i] += p*db0(3-j,x-j);
}
}
if (mxIsNaN(Tc[i]))
mexPrintf("Is NAN. ");
if (doDerivative) {
dT[i] = dT[i]/h[0];
}
}
}
void hamdist(T *x, mwSize m, mwSize n, T *d)
{
/*
d = sum(XI ~= XJ,2) / size(XI,2); % Hamming
*/
mwIndex i,j,k;
T theSum;
T *XI, *XJ, *XI0;
T *theNaN = (T*)mxCalloc(m,sizeof(T));
/* Determine which rows of X have missing data.
*/
XI = x;
for (i=0; i<m; i++) {
for (k=0;k<n;k++,XI++) if (mxIsNaN(*XI)) theNaN[i] = *XI;
}
XI = x;
for (i=0; i<m; i++) {
XI0 = XI;
XJ = XI+n;
for (j=i+1; j<m; j++) {
/* XI = x + i*n; XJ = x + j*n; */
XI = XI0;
/* If XI or XJ have missing data, set their distance to NaN.
*/
if (theNaN[i] || theNaN[j]) {
XI += n;
XJ += n;
*(d++) = theNaN[i] + theNaN[j];
} else {
theSum = 0;
for (k=0;k<n;k++,XI++,XJ++){
if ((*XI)!=(*XJ)) {
theSum++;
}
}
*(d++) = theSum/n;
}
}
}
mxFree(theNaN);
}
void mexFunction(
int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
double *pitch, *wave, pitchRate;
int fs, pitchLen, waveLen;
cAudio myAudio;
/* Check for proper number of arguments */
if (nrhs!=3){
char message[200];
strcpy(message, mexFunctionName());
strcat(message, " requires 3 input arguments: pitch, pitchRate, fs.");
mexErrMsgTxt(message);
}
/* Dimensions of the input matrix */
pitchLen = mxGetM(PITCH)*mxGetN(PITCH);
/* Assign pointers to the various parameters */
pitch = mxGetPr(PITCH);
pitchRate = mxGetScalar(PITCHRATE);
fs = (int)mxGetScalar(FS);
/* If some of the elements are nan/inf, change them to 0 */
double *pitch2=(double *)malloc(pitchLen*sizeof(double));
memcpy(pitch2, pitch, pitchLen*sizeof(double));
for (int i=0; i<pitchLen; i++)
if (mxIsNaN(pitch[i])||mxIsInf(pitch[i]))
pitch2[i]=0.0;
/* Create a matrix for the return argument */
waveLen=(int)floor(fs*pitchLen/pitchRate+0.5);
WAVE = mxCreateDoubleMatrix(waveLen, 1, mxREAL);
wave = mxGetPr(WAVE);
myAudio.pitch2wave(pitch2, pitchLen, pitchRate, fs, wave, waveLen);
free(pitch2);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
/* MATLAB USAGE:
* [X,Y] = meshgrid(...,...);
* Z = mxgridtrimesh(F,V,X,Y);
*/
int nx, ny, n, m, i, j, k;
double *X, *Y, *Z, *F, *V;
double v1x, v1y, v1z, v2x, v2y, v2z, v3x, v3y, v3z;
double w1, w2, w3, z;
double minx, maxx, miny, maxy;
int east, west, north, south;
/* dimensions of input data */
m = mxGetM(prhs[0]);
n = mxGetM(prhs[1]);
nx = mxGetN(prhs[2]);
ny = mxGetM(prhs[2]);
/* pointers to input data */
F = mxGetPr(prhs[0]);
V = mxGetPr(prhs[1]);
X = mxGetPr(prhs[2]);
Y = mxGetPr(prhs[3]);
/* create mxArray and pointfor the output data */
plhs[0] = mxCreateDoubleMatrix(ny, nx, mxREAL);
/* pointer to the output data */
Z = mxGetPr(plhs[0]);
/* initialise output */
for (i = 0; i < nx*ny; i++) Z[i] = mxGetNaN();
/* consider every triangle, projected to the x-y plane and determine whether gridpoints lie inside */
for (i = 0; i < m; i++) {
v1x = V[(int)(F[i]-1)]; v1y = V[(int)(F[i]-1+n)]; v1z = V[(int)(F[i]-1+2*n)];
v2x = V[(int)(F[m+i]-1)]; v2y = V[(int)(F[m+i]-1+n)]; v2z = V[(int)(F[m+i]-1+2*n)];
v3x = V[(int)(F[2*m+i]-1)]; v3y = V[(int)(F[2*m+i]-1+n)]; v3z = V[(int)(F[2*m+i]-1+2*n)];
/* we'll use the projected triangle's bounding box: of the form (minx,maxx) x (miny,maxy) */
minx = v1x; if (v2x < minx) minx = v2x; if (v3x < minx) minx = v3x;
maxx = v1x; if (v2x > maxx) maxx = v2x; if (v3x > maxx) maxx = v3x;
/* find smallest x-grid value > minx, and largest x-grid value < maxx */
east = mxGetNaN(); west = mxGetNaN();
j = 0; while (j <= (nx-1)*ny && X[j] < minx) j = j + ny; if (j <= (nx-1)*ny) west = j/ny;
j = (nx-1)*ny; while (j >= 0 && X[j] > maxx) j = j - ny; if (j >= 0) east = j/ny;
/* if there are gridpoints strictly inside bounds (minx,maxx), continue */
if (!(mxIsNaN(west)) && !(mxIsNaN(east)) && east-west >= 0) {
miny = v1y; if (v2y < miny) miny = v2y; if (v3y < miny) miny = v3y;
maxy = v1y; if (v2y > maxy) maxy = v2y; if (v3y > maxy) maxy = v3y;
/* find smallest y-grid value > miny, and largest y-grid value < maxy */
north = mxGetNaN(); south = mxGetNaN();
j = 0; while (j < ny && Y[j] < miny) j++; if (j < ny) north = j;
j = ny-1; while (j >= 0 && Y[j] > maxy) j--; if (j >= 0) south = j;
/* if, further, there are gridpoints strictly inside bounds (miny,maxy), continue */
if (!(mxIsNaN(north)) && !(mxIsNaN(south)) && south-north >= 0)
/* we now know that there might be gridpoints bounded by (west,east) x (north,south)
that lie inside the current triangle, so we'll test each of them */
for (j = west; j <= east; j++)
for (k = north; k <= south; k++) {
/* calculate barycentric coordinates of gridpoint w.r.t. current (projected) triangle */
w1 = (v2y*v3x - v2x*v3y - v2y*X[j*ny+k] + v3y*X[j*ny+k] + v2x*Y[j*ny+k] - v3x*Y[j*ny+k])/(v1y*v2x - v1x*v2y - v1y*v3x + v2y*v3x + v1x*v3y - v2x*v3y);
w2 = (-(v3y*X[j*ny+k]) + v1y*(-v3x + X[j*ny+k]) + v1x*(v3y - Y[j*ny+k]) + v3x*Y[j*ny+k])/(v1y*(v2x - v3x) + v2y*v3x - v2x*v3y + v1x*(-v2y + v3y));
w3 = (v1y*v2x - v1x*v2y - v1y*X[j*ny+k] + v2y*X[j*ny+k] + v1x*Y[j*ny+k] - v2x*Y[j*ny+k])/(v1y*v2x - v1x*v2y - v1y*v3x + v2y*v3x + v1x*v3y - v2x*v3y);
if (w1 > 0 && w2 > 0 && w3 > 0) {
/* use barycentric coordinates to calculate z-value */
z = w1*v1z + w2*v2z + w3*v3z;
if (mxIsNaN(Z[j*ny+k]) || z > Z[j*ny+k]) Z[j*ny+k] = z;
}
}
}
}
}
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
/* ************** Inputs *******************/
double *IV;
unsigned int *e, Niter;
/* ************** Outputs *******************/
double *VV;
/* ************** Others ******************* */
unsigned int nV, nE, n0, n1, niter, ne, nv;
double vertvals[100], dotval;
double *x, *y, *z;
/* Inputs */
e = (unsigned int*)mxGetPr(prhs[0]);
IV = mxGetPr(prhs[1]);
Niter = *mxGetPr(prhs[2]);
//mexPrintf("I[%d]\n",Niter);
nV = mxGetM(prhs[1]);
nE = mxGetM(prhs[0]);
/* Outputs */
plhs[0] = mxCreateDoubleMatrix(nV,1,mxREAL);
VV = mxGetPr(plhs[0]);
/*temp */
x = (double *)mxCalloc(nV,sizeof(double));
for(n0=0;n0<nV;n0++)
{
VV[n0] = IV[n0];
x[n0] = VV[n0];
}
for(niter=0;niter<Niter;niter++)
{
ne = 0;
for(n0=0;n0<nV;n0++)
{
nv = 0;
if (!mxIsNaN(VV[n0]))
{
vertvals[nv++] = VV[n0];
}
for(;ne<nE & e[ne]==n0;ne++)
{
if (!mxIsNaN(VV[e[nE+ne]]))
{
vertvals[nv++] = VV[e[nE+ne]];
}
}
if (nv>0)
{
qsort(vertvals,nv,sizeof(double),compare);
//for (n1=0;n1<nv;n1++)
//mexPrintf("%f ",vertvals[n1]);
if (nv%2 == 0)
x[n0] = (vertvals[nv/2]+vertvals[nv/2-1])/2.0;
else
x[n0] = vertvals[(int)floor((double)nv/2.0)];
//mexPrintf(":Med[%f]\n",x[n0]);
/*
for(n1=0;n1<nv;n1++)
VV[n0] += vertvals[n1];
VV[n0] /= nv;
*/
}
}
dotval = 0.0;
for(n0=0;n0<nV;n0++)
{
//dotval += (VV[n0]-x[n0])*(VV[n0]-x[n0]);
VV[n0] = x[n0];
}
//mexPrintf("dotval[%f]\n",dotval);
}
}
/* here comes the main function */
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
/* default settings */
/* connectivity pattern: 2 */
const double DEF_clconn = 2.0;
/* threshold 1 (consider also single voxel clusters) */
const double DEF_kthr = 1.0;
/* setting pointers (for argument read-out) */
const double *clconn = NULL;
const double *kthr = NULL;
/* settings (internal) */
/* max directions */
unsigned int md = 2;
/* cluster threshold */
unsigned int kthrl = 1;
/* sizes and position */
const int *dim = NULL;
int idim[3] = {1, 1, 1};
int tdim[3] = {1, 1, 1};
/* offset (must be variable, as copy might not require additional slices) */
unsigned int toff[3] = {1, 1, 1};
/* dimX, dimY, dimZ, dimXY, next position */
unsigned int dx = 0, dy = 0, dz = 0, dxy = 0, dxyz = 0, np = 0;
/* data pointers */
/* (enlarged) copy */
unsigned char *copy = NULL;
/* list of cluster voxels */
unsigned int clvoxcnt;
unsigned int clnxtvox;
signed int *clvox = NULL;
/* list of cluster sizes and total number of clustered voxels */
unsigned int mxclnum = 0;
unsigned int clnum = 0;
unsigned int *clsiz = NULL;
unsigned int cltvox = 0, cltvox2 = 0, cltvox3 = 0;
/* clustered volume (internal) */
unsigned int *clvol = NULL;
/* direction increments (as index differences) */
signed int dlxyz[26] =
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
/* next positions (signed!) */
signed int nextp = 0, nextpi = 0;
/* counters */
signed char dc = 0;
unsigned int cx = 0, cy = 0, cz = 0;
/* output pointer(s) */
mxArray *ma = NULL;
double *op = NULL, *sop = NULL;
/* variable output text */
/* char vstr[256]; */
/* code starts here */
/* nr. of arguments check */
if ((nrhs < 1) ||
(nlhs > 4))
mexErrMsgTxt("Bad number of input/output arguments.");
/* first argument class and size check */
if (!mxIsLogical(prhs[0]) ||
(mxGetNumberOfElements(prhs[0]) < 8) ||
(mxGetNumberOfDimensions(prhs[0]) > 3))
mexErrMsgTxt("Invalid argument type or dimension.");
/* check connectivity flag */
if ((nrhs < 2) ||
!mxIsDouble(prhs[1]) ||
(mxGetNumberOfElements(prhs[1]) != 1))
clconn = &DEF_clconn;
else {
clconn = (const double*) mxGetPr(prhs[1]);
if (mxIsNaN(*clconn) ||
mxIsInf(*clconn) ||
((*clconn) < 1.0) ||
((*clconn) > 5.0))
clconn = &DEF_clconn;
}
/* check threshold */
if ((nrhs < 3) ||
!mxIsDouble(prhs[2]) ||
(mxGetNumberOfElements(prhs[2]) != 1))
kthr = &DEF_kthr;
else {
kthr = (const double*) mxGetPr(prhs[2]);
if (mxIsNaN(*kthr) ||
mxIsInf(*kthr) ||
((*kthr) < 1.0) ||
((*kthr) > (0.5 * mxGetNumberOfElements(prhs[0]))))
kthr = &DEF_kthr;
}
kthrl = (unsigned int) *kthr;
/* get & check argument content */
dim = mxGetDimensions(prhs[0]);
idim[0] = dim[0];
idim[1] = dim[1];
if (mxGetNumberOfDimensions(prhs[0]) > 2)
idim[2] = dim[2];
dx = idim[0];
dy = idim[1];
dz = idim[2];
dxy = dx * dy;
dxyz = dxy * dz;
mxclnum = (unsigned int) (((double) (dxyz + 3)) / 4.0);
/* set connectivity flag -> max directions (opposite direction later) */
/* face connectivity -> three main directions */
if ((*clconn >= 1.0) && (*clconn < 1.5))
md = 3;
/* edge connectivity -> add 6 edge diagonals */
else if ((*clconn >= 1.5) && (*clconn < 2.5))
md = 9;
/* vertex connectivity -> add 4 3D diagonals */
else if ((*clconn >= 2.5) && (*clconn < 3.5))
md = 13;
/* otherwise, leave at 2, we stay "in-plane" */
/* reserve space for next cluster list and cluster sizes */
clvox = (signed int *) mxCalloc(dxyz, sizeof(signed int));
if (clvox == NULL)
mexErrMsgTxt("Error allocating next cluster voxel list.");
clsiz = (unsigned int *) mxCalloc(mxclnum, sizeof(signed int));
if (clsiz == NULL) {
mxFree(clvox);
mexErrMsgTxt("Error allocating next cluster voxel list.");
}
/* increase numbers accordingly */
if (idim[0] > 1)
dx += 2;
else
toff[0] = 0;
if (idim[1] > 1)
dy += 2;
else
toff[1] = 0;
if (idim[2] > 1)
dz += 2;
else
toff[2] = 0;
/* compute new plane/vol numel */
dxy = dx * dy;
dxyz = dxy * dz;
/* reserve space for clustered volume (if needed) */
if (nlhs > 1) {
clvol = (unsigned int *) mxCalloc(dxyz, sizeof(unsigned int));
if (clvol == NULL) {
mxFree(clsiz);
mxFree(clvox);
mexErrMsgTxt("Error allocating clustered volume.");
}
}
/* set temp dim array */
tdim[0] = dx;
tdim[1] = dy;
tdim[2] = dz;
/* fill direction increment array */
np = 0;
for (dc = 0; dc < md; ++dc) {
if (((idim[0] > 1) ||
(dlx[dc] == 0)) &&
((idim[1] > 1) ||
(dly[dc] == 0)) &&
(((idim[2] > 1) &&
(*clconn < 3.5)) ||
(dlz[dc] == 0))) {
dlxyz[np] = dlx[dc] + dly[dc] * dx + dlz[dc] * dxy;
dlxyz[np+1] = -dlxyz[np];
np += 2;
}
}
if ((*clconn >= 4.5) &&
(idim[0] > 1) &&
(idim[1] > 1)) {
for (dc = 3; dc < 5; ++dc) {
dlxyz[np] = dlx[dc] + dly[dc] * dx + dlz[dc] * dxy;
dlxyz[np+1] = -dlxyz[np];
np += 2;
}
}
/* re-set */
md = np - 1;
/* create temp. copy array */
copy = (unsigned char *) mxCalloc(dxyz, sizeof(unsigned char));
if (copy == NULL) {
mxFree(clsiz);
mxFree(clvox);
mexErrMsgTxt("Error allocating temporary memory");
}
/* copy into temporary array */
ff_copy_intopart(idim[0], idim[1], idim[2], (unsigned char *) mxGetPr(prhs[0]), tdim, copy, toff);
/* preparations complete, now we can "search"... */
/* iterate while new points found */
for (np = 0; np < dxyz; ++np) {
/* check voxel */
if (copy[np] == 1) {
/* start new cluster */
clvoxcnt = 1;
clvox[0] = np;
/* remove from search volume */
copy[np] = 0;
/* continue until no more neighbors found */
for (clnxtvox = 0; clnxtvox < clvoxcnt; ++clnxtvox) {
/* check neighbors */
nextp = clvox[clnxtvox];
for (dc = md; dc >= 0; --dc) {
/* check voxel with increment */
nextpi = nextp + dlxyz[dc];
if (copy[nextpi] == 1) {
/* equally remove from search volume */
copy[nextpi] = 0;
/* and add to list */
clvox[clvoxcnt++] = nextpi;
}
}
}
/* rest of code only if surpasses threshold */
if (clvoxcnt >= kthrl) {
/* write to list of sizes and increase cluster number count */
clsiz[clnum++] = clvoxcnt;
cltvox += clvoxcnt;
/* write to output volume (if needed) */
if (nlhs > 1) {
for (nextp = clvoxcnt - 1; nextp >= 0; --nextp)
clvol[clvox[nextp]] = clnum;
}
}
}
}
/* we're done with this */
mxFree(copy);
/* create output argument #1 */
CREATE_MxN(plhs[0], 1, clnum);
if (plhs[0] == NULL) {
if (clvol != NULL)
mxFree(clvol);
mxFree(clsiz);
mxFree(clvox);
mexErrMsgTxt("Error creating output argument cs.");
}
/* if requested create other outputs first */
if (nlhs > 1) {
plhs[1] = mxCreateNumericArray(3, idim, mxUINT32_CLASS, mxREAL);
if (plhs[1] == NULL) {
mxDestroyArray(plhs[0]);
mxFree(clvol);
mxFree(clsiz);
mxFree(clvox);
mexErrMsgTxt("Error creating output argument cv.");
}
}
/* if requested create other outputs first */
if (nlhs > 2) {
CREATE_MxN(plhs[2], cltvox, 4);
if (plhs[2] == NULL) {
mxDestroyArray(plhs[1]);
mxDestroyArray(plhs[0]);
mxFree(clvol);
mxFree(clsiz);
mxFree(clvox);
mexErrMsgTxt("Error creating output argument l.");
}
}
/* if requested create other outputs first */
if (nlhs > 3) {
plhs[3] = mxCreateCellMatrix(1, clnum);
if (plhs[3] == NULL) {
mxDestroyArray(plhs[2]);
mxDestroyArray(plhs[1]);
mxDestroyArray(plhs[0]);
mxFree(clvol);
mxFree(clsiz);
mxFree(clvox);
mexErrMsgTxt("Error creating output argument c.");
}
/* put numeric array into each cell */
for (nextp = 0; nextp < clnum; ++nextp) {
CREATE_MxN(ma, clsiz[nextp], 3);
if (ma == NULL) {
mxDestroyArray(plhs[3]);
mxDestroyArray(plhs[2]);
mxDestroyArray(plhs[1]);
mxDestroyArray(plhs[0]);
mxFree(clvol);
mxFree(clsiz);
mxFree(clvox);
mexErrMsgTxt("Error creating output cell contents in argument c.");
}
mxSetCell(plhs[3], nextp, ma);
}
}
/* copy cluster sizes into output */
op = (double *) mxGetPr(plhs[0]);
for (nextp = 0; nextp < clnum; ++nextp)
*op++ = (double) clsiz[nextp];
/* copy volume to output */
if (nlhs > 1) {
ff_copy_frompart(dx, dy, clvol, idim, (unsigned int*) mxGetData(plhs[1]), toff);
mxFree(clvol);
clvol = (unsigned int *) mxGetPr(plhs[1]);
}
/* create list of voxels */
if (nlhs > 2) {
/* get output pointer */
op = (double *) mxGetPr(plhs[2]);
/* 2/3 * total count for fast indexing */
cltvox2 = 2 * cltvox;
cltvox3 = 3 * cltvox;
/* if required re-init counter */
if (nlhs > 3) {
for (nextp = clnum - 1; nextp >= 0; --nextp)
clvox[nextp] = 0;
}
dx = idim[0];
dy = idim[1];
dz = idim[2];
dxy = dx * dy;
for (cz = 0; cz < dz; ++cz) {
dxyz = dxy * cz;
for (cy = 0; cy < dy; ++cy) {
np = dxyz + cy * dx;
for (cx = 0; cx < dx; ++cx) {
clnum = *clvol++;
if (clnum > 0) {
op[cltvox3] = (double) clnum;
op[cltvox2] = (double) (cz + 1);
op[cltvox] = (double) (cy + 1);
*op++ = (double) (cx + 1);
if (nlhs > 3) {
--clnum;
sop = (double *) mxGetPr(mxGetCell(plhs[3], clnum));
sop[2 * clsiz[clnum] + clvox[clnum]] = (double) (cz + 1);
sop[clsiz[clnum] + clvox[clnum]] = (double) (cy + 1);
sop[clvox[clnum]++] = (double) (cx + 1);
}
}
}
}
}
}
/* free other temp arrays */
mxFree(clsiz);
mxFree(clvox);
}
//-------------------------------------------------------------------
void mexFunction(int POutputCount, mxArray* POutput[], int PInputCount, const mxArray *PInputs[])
{
const int *SZ;
double* LInput;
double* LInputI;
double* LOutputSum;
double* LOutputSumI;
double* LOutputCount;
double* LOutputSum2;
double* LOutputSum4;
double x, x2;
unsigned long LCount, LCountI;
double LSum, LSum2, LSum4;
unsigned DIM = 0;
unsigned long D1, D2, D3; // NN; //
unsigned ND, ND2; // number of dimensions: input, output
unsigned long ix1, ix2; // index to input and output
unsigned j, k, l; // running indices
int *SZ2; // size of output
// check for proper number of input and output arguments
if ((PInputCount <= 0) || (PInputCount > 2))
mexErrMsgTxt("SumSkipNan.MEX requires 1 or 2 arguments.");
if (POutputCount > 4)
mexErrMsgTxt("SumSkipNan.MEX has 1 to 4 output arguments.");
// get 1st argument
if(mxIsDouble(PInputs[0]))
LInput = mxGetPr(PInputs[0]);
else
mexErrMsgTxt("First argument must be DOUBLE.");
if(mxIsLogical(PInputs[0]))
LInput = (double*)mxGetLogicals(PInputs[0]);
else if(mxIsNumeric(PInputs[0]))
LInput = mxGetPr(PInputs[0]);
else if(mxIsSparse(PInputs[0]))
LInput = mxGetPr(PInputs[0]);
else if(mxIsInt8(PInputs[0]))
LInput = (double *)mxGetData(PInputs[0]);
else if(mxIsUint8(PInputs[0]))
LInput = (double *)mxGetData(PInputs[0]);
else if(mxIsInt16(PInputs[0]))
LInput = (double *)mxGetData(PInputs[0]);
else if(mxIsUint16(PInputs[0]))
LInput = (double *)mxGetData(PInputs[0]);
else if(mxIsInt32(PInputs[0]))
LInput = (double *)mxGetData(PInputs[0]);
else if(mxIsUint32(PInputs[0]))
LInput = (double *)mxGetData(PInputs[0]);
else
mexErrMsgTxt("First argument must be NUMERIC.");
if(mxIsComplex(PInputs[0]))
LInputI = mxGetPi(PInputs[0]);
if(mxIsComplex(PInputs[0]) & (POutputCount > 3))
mexErrMsgTxt("More than 3 output arguments only supported for REAL data ");
// get 2nd argument
if (PInputCount == 2){
switch (mxGetNumberOfElements(PInputs[1])) {
case 0: x = 0.0; // accept empty element
break;
case 1: x = (mxIsNumeric(PInputs[1]) ? mxGetScalar(PInputs[1]) : -1.0);
break;
default:x = -1.0; // invalid
}
if ((x < 0) || (x > 65535) || (x != floor(x)))
mexErrMsgTxt("Error SUMSKIPNAN.MEX: DIM-argument must be a positive integer scalar");
DIM = (unsigned)floor(x);
}
// get size
ND = mxGetNumberOfDimensions(PInputs[0]);
// NN = mxGetNumberOfElements(PInputs[0]);
SZ = mxGetDimensions(PInputs[0]);
// if DIM==0 (undefined), look for first dimension with more than 1 element.
for (k = 0; (DIM < 1) && (k < ND); k++)
if (SZ[k]>1) DIM = k+1;
if (DIM < 1) DIM=1; // in case DIM is still undefined
ND2 = (ND>DIM ? ND : DIM); // number of dimensions of output
SZ2 = (int*)mxCalloc(ND2, sizeof(int)); // allocate memory for output size
for (j=0; j<ND; j++) // copy size of input;
SZ2[j] = SZ[j];
for (j=ND; j<ND2; j++) // in case DIM > ND, add extra elements 1
SZ2[j] = 1;
for (j=0, D1=1; j<DIM-1; D1=D1*SZ2[j++]); // D1 is the number of elements between two elements along dimension DIM
D2 = SZ2[DIM-1]; // D2 contains the size along dimension DIM
for (j=DIM, D3=1; j<ND; D3=D3*SZ2[j++]); // D3 is the number of blocks containing D1*D2 elements
SZ2[DIM-1] = 1; // size of output is same as size of input but SZ(DIM)=1;
// create outputs
#define TYP mxDOUBLE_CLASS
if(mxIsComplex(PInputs[0]))
{ POutput[0] = mxCreateNumericArray(ND2, SZ2, TYP, mxCOMPLEX);
LOutputSum = mxGetPr(POutput[0]);
LOutputSumI= mxGetPi(POutput[0]);
}
else
{ POutput[0] = mxCreateNumericArray(ND2, SZ2, TYP, mxREAL);
LOutputSum = mxGetPr(POutput[0]);
}
if (POutputCount >= 2){
POutput[1] = mxCreateNumericArray(ND2, SZ2, TYP, mxREAL);
LOutputCount = mxGetPr(POutput[1]);
}
if (POutputCount >= 3){
POutput[2] = mxCreateNumericArray(ND2, SZ2, TYP, mxREAL);
LOutputSum2 = mxGetPr(POutput[2]);
}
if (POutputCount >= 4){
POutput[3] = mxCreateNumericArray(ND2, SZ2, TYP, mxREAL);
LOutputSum4 = mxGetPr(POutput[3]);
}
mxFree(SZ2);
// OUTER LOOP: along dimensions > DIM
for (l = 0; l<D3; l++)
{
ix2 = l*D1; // index for output
ix1 = ix2*D2; // index for input
// Inner LOOP: along dimensions < DIM
for (k = 0; k<D1; k++, ix1++, ix2++)
{
LCount = 0;
LSum = 0.0;
LSum2 = 0.0;
LSum4 = 0.0;
// LOOP along dimension DIM
for (j=0; j<D2; j++)
{
x = LInput[ix1 + j*D1];
if (!mxIsNaN(x))
{
LCount++;
LSum += x;
x2 = x*x;
LSum2 += x2;
LSum4 += x2*x2;
}
}
LOutputSum[ix2] = LSum;
if (POutputCount >= 2)
LOutputCount[ix2] = (double)LCount;
if (POutputCount >= 3)
LOutputSum2[ix2] = LSum2;
if (POutputCount >= 4)
LOutputSum4[ix2] = LSum4;
if(mxIsComplex(PInputs[0]))
{
LSum = 0.0;
LCountI = 0;
LSum2 = 0.0;
for (j=0; j<D2; j++)
{
x = LInputI[ix1 + j*D1];
if (!mxIsNaN(x))
{
LCountI++;
LSum += x;
LSum2 += x*x;
}
}
LOutputSumI[ix2] = LSum;
if (LCount != LCountI)
mexErrMsgTxt("Number of NaNs is different for REAL and IMAG part");
if (POutputCount >= 3)
LOutputSum2[ix2] += LSum2;
}
}
}
}
//Check all inputs for size and type errors
void checkInputs(const mxArray *prhs[], int nrhs)
{
size_t ndec, m, i;
const mxArray *fxd;
double *val;
//Correct number of inputs
if(nrhs < 3)
mexErrMsgTxt("You must supply at least 3 arguments to dsdp (f, A, b)");
//Check we have an objective
if(mxIsEmpty(pF))
mexErrMsgTxt("You must supply an objective function via f!");
//Check we have some constraints
if(nrhs >= (eSDP+1) && mxIsEmpty(pA) && mxIsEmpty(pLB) && mxIsEmpty(pUB) && mxIsEmpty(pSDP))
mexErrMsgTxt("You must supply constraints to this solver!");
else if(nrhs == (eUB+1) && mxIsEmpty(pA) && mxIsEmpty(pLB) && mxIsEmpty(pUB))
mexErrMsgTxt("You must supply constraints to this solver!");
else if(nrhs == (eB+1) && (mxIsEmpty(pA) || mxIsEmpty(pB)))
mexErrMsgTxt("You must supply constraints to this solver!");
//Check options is a structure
if(nrhs > eOPTS && !mxIsStruct(pOPTS))
mexErrMsgTxt("The options argument must be a structure");
//Get Sizes
ndec = mxGetNumberOfElements(pF);
//Check linear constraints
if(!mxIsEmpty(pA)) {
//Check types
if(!mxIsSparse(pA) || mxIsComplex(pA) || !mxIsDouble(pA))
mexErrMsgTxt("A must be real, sparse, double matrix");
if(mxIsSparse(pB) || mxIsComplex(pB) || !mxIsDouble(pB))
mexErrMsgTxt("b must be real, dense, double column vector");
//Check sizes
if(mxGetN(pA) != ndec)
mexErrMsgTxt("A does not have the same number of columns as decision variables!");
if(mxGetM(pA) != mxGetNumberOfElements(pB))
mexErrMsgTxt("A and b do not have the same number of rows!");
}
//Check bounds
if(nrhs > eLB && !mxIsEmpty(pLB)) {
if(mxIsSparse(pLB) || mxIsComplex(pLB) || !mxIsDouble(pLB))
mexErrMsgTxt("lb must be real, dense, double column vector");
if(mxGetNumberOfElements(pLB) != ndec)
mexErrMsgTxt("lb is not the same length as f!");
}
if(nrhs > eUB && !mxIsEmpty(pUB)) {
if(mxIsSparse(pUB) || mxIsComplex(pUB) || !mxIsDouble(pUB))
mexErrMsgTxt("ub must be real, dense, double column vector");
if(mxGetNumberOfElements(pUB) != ndec)
mexErrMsgTxt("ub is not the same length as f!");
}
//Check sdcones
if(nrhs > eSDP && !mxIsEmpty(pSDP)) {
if(mxIsCell(pSDP)) {
for(i=0;i<mxGetNumberOfElements(pSDP);i++)
checkCone(mxGetCell(pSDP,i),ndec,i);
}
else
checkCone(pSDP,ndec,1);
}
//Check y0
if(nrhs > eY0 && !mxIsEmpty(pY0)) {
if(mxIsSparse(pY0) || mxIsComplex(pY0) || !mxIsDouble(pY0))
mexErrMsgTxt("y0 must be real, dense, double column vector");
if(mxGetNumberOfElements(pY0) != ndec)
mexErrMsgTxt("y0 is not the same length as f!");
}
//Check fixed vars option
if(nrhs > eOPTS && !mxIsEmpty(pOPTS) && mxGetField(pOPTS,0,"fixed") && !mxIsEmpty(mxGetField(pOPTS,0,"fixed"))) {
fxd = mxGetField(pOPTS,0,"fixed");
//Check Type
if(mxIsSparse(fxd) || mxIsComplex(fxd) || !mxIsDouble(fxd))
mexErrMsgTxt("The fixed option must be a real, dense, double matrix");
val = mxGetPr(fxd);
m = mxGetM(fxd);
if(mxGetN(fxd) != 2)
mexErrMsgTxt("The fixed option must contain 2 columns");
if(m > ndec)
mexErrMsgTxt("The fixed option must not contain more rows than decision variables");
for(i=0;i<m;i++) {
if(val[i] <= 0 || val[i] > ndec)
mexErrMsgTxt("A variable index in the fixed option is <= 0 or > ndec");
if(mxIsNaN(val[i+m]) || mxIsInf(val[i+m]))
mexErrMsgTxt("A variable value in the fixed option is NaN or Inf");
}
}
}
/* 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(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
struct EULER *p; /* Pointer to array of stage poles */
struct ISOC_SCALE *isoc_scale; /* Pointer to array of time scale */
int error = FALSE, brick = FALSE, age_flow = FALSE;
int multi_flow = FALSE, first_seg = TRUE, invert_rot = FALSE, linear_age = FALSE;
int np_in_seg, switch_xy = FALSE, first = TRUE, n_flow, isoc_c = FALSE, isoc_cm = TRUE;
int n_seg; /* Number of segments in input data */
int m, i_min = 0, int_seg = FALSE, ts_file = FALSE, isoc_p = FALSE, isoc_o = FALSE;
int data_in_input = FALSE, ages_given = FALSE, fake_origin = FALSE;
int i, ii, j, jj, k, l, ll, ndata, n_alloc, ix = 0, iy = 1, ns;
int n_stages; /* Number of stage poles */
int n_isoc; /* Number of isochrons in time scale */
int n_chunk; /* Total length or array returned by libeuler functions */
int argc, n_arg_no_char = 0, np_in, np_out;
double in[2], lon, lat, *tmp, *out, *out2, *out_n_data, *out_n_seg, *out_n_flow;
double upper_age = 0.0; /* Extend oldest age back to this time, in Ma */
double t_zero = 0.0; /* Current age in Ma */
double age = 0.0; /* Age of isochron, in Ma */
double age_shift = 0.0; /* Fake the chron table by shifting it by this amount, in Ma */
double *c; /* Array com os pts de todos os tijolos de cada seg */
double filter_width = 0.001; /* Full width of filter in user's units */
double dl = 0.05; /* Delta resolution in degrees for along segment interpolation */
double ddeg = 0.005; /* Delta resolution in degrees for flow interpolation */
double age_lin_start, age_lin_stop, age_lin_inc, *first_mag;
double ecc2_1 = (1 - 0.0818191908426215 * 0.0818191908426215); /* Parameter for convertion to geocentrics in WGS84 */
char line[MAXCHAR], *newseg = ">";
char *euler_file = NULL; /* Name pointer for file with stage poles */
char *t_scale = NULL; /* Name pointer for file with time scale */
char **argv;
FILE *fp = NULL, *fp_ts = NULL;
argc = nrhs;
for (i = 0; i < nrhs; i++) { /* Check input to find how many arguments are of type char */
if(!mxIsChar(prhs[i])) {
argc--;
n_arg_no_char++; /* Number of arguments that have a type other than char */
}
}
argc++; /* to account for the program's name to be inserted in argv[0] */
/* get the length of the input string */
argv=(char **)mxCalloc(argc, sizeof(char *));
argv[0] = "telha";
for (i = 1; i < argc; i++)
argv[i] = (char *)mxArrayToString(prhs[i+n_arg_no_char-1]);
for (i = 1; i < argc; i++) {
if (argv[i][0] == '-') {
switch (argv[i][1]) {
/* Common parameters */
case '\0':
error = TRUE;
break;
case ':':
switch_xy = TRUE;
break;
case 'A':
age_flow = TRUE;
break;
case 'B':
brick = TRUE;
break;
case 'C':
if (strlen (&argv[i][2]) == 1) { /* use one of default time scales */
if (strcmp (&argv[i][2], "P") == 0) /* Patriat */
isoc_p = TRUE;
else if (strcmp (&argv[i][2], "C") == 0) /* Cande & Kent 95 */
isoc_c = TRUE;
else if (strcmp (&argv[i][2], "O") == 0) /* Cande (old scale given by Patriat)*/
isoc_o = TRUE;
else if (strcmp (&argv[i][2], "M") == 0) /* Cande & Kent 95 + Malinverno 2012 */
isoc_cm = TRUE;
else
mexPrintf ("SYNTAX ERROR -C option: Must choose only between -CP (Patriat) or -CC (Cande)\n");
}
else {
t_scale = &argv[i][2];
ts_file = TRUE;
}
break;
case 'D':
ddeg = atof (&argv[i][2]);
break;
case 'E': /* File with stage poles */
euler_file = &argv[i][2];
break;
case 'F':
filter_width = atof (&argv[i][2]);
break;
case 'G':
multi_flow = TRUE;
break;
case 'I':
invert_rot = TRUE;
break;
case 'L':
sscanf (&argv[i][2], "%lf/%lf/%lf", &age_lin_start, &age_lin_stop, &age_lin_inc);
linear_age = TRUE;
break;
case 'N': /* Extend oldest stage back to this time [no extension] */
upper_age = atof (&argv[i][2]);
break;
case 'O':
age_shift = fabs(atof (&argv[i][2]));
fake_origin = TRUE;
break;
case 'P':
ages_given = TRUE;
break;
case 'S':
dl = atof (&argv[i][2]);
int_seg = TRUE;
break;
case 'T': /* Current age [0 Ma] */
t_zero = atof (&argv[i][2]);
break;
default:
error = TRUE;
break;
}
}
else {
if (!data_in_input && argv[i][0] != ' ') {
if ((fp = fopen(argv[i], "r")) == NULL) {
mexPrintf("%s: Cannot open %s\n", "telha", argv[i]);
error = TRUE;
}
}
}
}
if (argc == 1) { /* Display usage */
mexPrintf ("Telha - 3D magnetic modeling program\n");
mexPrintf("usage: telha infile -E<euler> -N<upper_age> [-A] [-T<t_zero>] [-F<filterwidth>] [-G] [-I] [-B] [-D<inc_deg>] [-S<inc_deg>] [-C[P|C][<time_scale>]] [-L<start/stop/inc>] [-P] [-:]\n\n");
mexPrintf (" infile (ASCII) has 2 columns defining segment limits.\n");
mexPrintf ("\t-E specifies the stage pole file to be used\n");
mexPrintf ("\n\tOPTIONS:\n");
mexPrintf ("\t-A create age flow lines (instead of mag) starting at points as \n\t determined by -S and -G options\n");
mexPrintf ("\t-B output a file with direct/reverse polarity blocks\n");
mexPrintf ("\t-C choose between different isochron time scales. Append:\n");
mexPrintf ("\t M (-CM) to select the Cand & Kent 95 + Mailvento 2012 [Default]\n");
mexPrintf ("\t C (-CC) to select the Cand & Kent 95 time scale\n");
mexPrintf ("\t O (-CO) to select the old Cand time scale\n");
mexPrintf ("\t P (-CP) to select the Patriat time scale\n");
mexPrintf ("\t Altervatively give a file name (-Ctime_scale) with a user provided time scale.\n");
mexPrintf ("\t-D<inc_deg> distance in degrees used to reinterpolate the flow line \n\t equidistantly [default 0.005]\n");
mexPrintf ("\t-F<filterwidth> apply a gaussian filter (width in meters)\n");
mexPrintf ("\t-G create multiple flow lines starting at points defining the \n\t segment OR at interpolated positions controled by -S option\n");
mexPrintf ("\t-I invert sense of rotation as defined in the stage pole file\n");
mexPrintf ("\t-L compute rotated points along a linear 'age-line'.\n");
mexPrintf ("\t-P It means that age points are transmited in input (They MUST be).\n");
mexPrintf ("\t-N<age> extends earliest stage pole back to <upper_age> [no extension]\n");
mexPrintf ("\t-S<inc_deg> reinterpolate the segment at inc_deg spacing\n");
mexPrintf ("\t-T<age> sets the starting age in Ma [0]\n");
mexPrintf("\t-: Expect lat/lon input rather than lon/lat [OFF].\n");
mexErrMsgTxt("\n");
}
if (age_flow && filter_width > 0.001) {
mexPrintf ("%s: Warning -A option overuns -F<filterwidth>\n", "telha");
filter_width = 0.001;
}
if (multi_flow && brick) {
mexPrintf ("%s: Warning -B option overuns -G\n", "telha");
}
if (ts_file) {
if ((fp_ts = fopen (t_scale, "r")) == NULL) {
mexPrintf ("%s: Cannot open file %s\n", argv[0], t_scale);
mexErrMsgTxt("\n");
}
}
if (n_arg_no_char == 0 && ages_given) { /* This will fail if -P and data_in_input */
mexPrintf ("%s: ERROR: -P option requires ages to be transmited in input\n", "telha");
mexErrMsgTxt("\n");
}
if (error) mexErrMsgTxt("\n");
if (switch_xy) {iy = 0; ix = 1;}
if (n_arg_no_char == 1 && !ages_given) { /* Not all combinations are tested */
tmp = mxGetPr(prhs[0]);
np_in = mxGetM(prhs[0]);
data_in_input = TRUE;
}
else if (n_arg_no_char == 1 && ages_given) { /* prhs[0] MUST contain the "rotation ages" */
tmp = mxGetPr(prhs[0]);
n_isoc = MAX(mxGetM(prhs[0]),mxGetN(prhs[0]));
isoc_scale = (struct ISOC_SCALE *) mxCalloc ((size_t) n_isoc, sizeof(struct ISOC_SCALE));
for (i = 0; i < n_isoc; i++)
isoc_scale[i].age = tmp[i];
linear_age = ts_file = isoc_p = isoc_o = isoc_c = FALSE;
}
else if (n_arg_no_char == 2) { /* Here, prhs[0] MUST contain the object to rotate */
/* and prhs[1] the "rotation ages" */
/* The "rotation ages" */
tmp = mxGetPr(prhs[1]);
n_isoc = MAX(mxGetM(prhs[1]),mxGetN(prhs[1]));
isoc_scale = (struct ISOC_SCALE *) mxCalloc ((size_t) n_isoc, sizeof(struct ISOC_SCALE));
for (i = 0; i < n_isoc; i++)
isoc_scale[i].age = tmp[i];
linear_age = ts_file = isoc_p = isoc_o = isoc_c = FALSE;
/* And now the the object to rotate */
tmp = mxGetPr(prhs[0]);
np_in = mxGetM(prhs[0]);
data_in_input = TRUE;
}
if (linear_age) {
n_isoc = (int)floor(fabs(age_lin_stop - age_lin_start) / age_lin_inc + 0.5) + 1;
isoc_scale = (struct ISOC_SCALE *) mxCalloc ((size_t) n_isoc, sizeof(struct ISOC_SCALE));
isoc_scale[0].age = age_lin_start;
for (i = 1; i < n_isoc; i++)
isoc_scale[i].age = isoc_scale[i-1].age + age_lin_inc;
}
else if (ts_file) /* read time scale provided by user */
n_isoc = read_time_scale (fp_ts, isoc_scale);
else if (isoc_p) { /* use Patriat time scale */
n_isoc = sizeof isoc_scale_p / sizeof (struct ISOC_SCALE);
isoc_scale = (struct ISOC_SCALE *) mxCalloc ((size_t)n_isoc, sizeof(struct ISOC_SCALE));
isoc_scale = isoc_scale_p;
}
else if (isoc_cm) { /* use Cande & Kent 95 time scale (DEFAULT) */
n_isoc = sizeof isoc_scale_cm / sizeof (struct ISOC_SCALE);
isoc_scale = (struct ISOC_SCALE *) mxCalloc ((size_t)n_isoc, sizeof(struct ISOC_SCALE));
isoc_scale = isoc_scale_cm;
}
else if (isoc_c) { /* use Cande & Kent 95 time scale */
n_isoc = sizeof isoc_scale_c / sizeof (struct ISOC_SCALE);
isoc_scale = (struct ISOC_SCALE *) mxCalloc ((size_t)n_isoc, sizeof(struct ISOC_SCALE));
isoc_scale = isoc_scale_c;
}
else if (isoc_o) { /* use old Cande time scale (by Patriat) */
n_isoc = sizeof(isoc_scale_o) / sizeof (struct ISOC_SCALE);
isoc_scale = (struct ISOC_SCALE *) mxCalloc ((size_t)n_isoc, sizeof(struct ISOC_SCALE));
isoc_scale = isoc_scale_o;
}
else if (ages_given);
else {
mexPrintf ("%s: ERROR, should not pass here %s\n", argv[0]);
mexErrMsgTxt("\n");
}
n_alloc = CHUNK;
ndata = 0; n_seg = 0; np_in_seg = 0;
if (data_in_input) { /* Data was transmited as arguments. */
data = (struct DATA *) mxCalloc ((size_t) np_in, sizeof(struct DATA));
for (i = 0; i < np_in; i++) {
if (!mxIsNaN (tmp[i])) {
data[ndata].x = tmp[i];
/* Convert to geocentrics */
data[ndata].y = atan2( ecc2_1 * sin(tmp[i+np_in]*D2R), cos(tmp[i+np_in]*D2R) ) / D2R;
ndata++;
np_in_seg++;
}
else if (first) {
first = FALSE;
n_seg++;
}
else {
data[n_seg-1].np = np_in_seg;
n_seg++;
np_in_seg = 0;
}
}
if (first) n_seg++; /* Single segment file without any NaNs */
data[n_seg-1].np = np_in_seg; /* Save npoints of the last segment */
np_in = ndata + n_seg - 1;
}
else { /* Data was not transmited as arguments. So we must read it from file */
data = (struct DATA *) mxCalloc ((size_t) n_alloc, sizeof(struct DATA));
while (fgets (line, MAXCHAR, fp)) { /* Reading file */
if (ns = strncmp(newseg, line, 1)) { /* Multisegment file */
if (sscanf (line, "%lg %lg", &in[0], &in[1]) < 2)
mexPrintf ("ERROR deciphering line %d\n",ndata+1);
if (ndata == n_alloc) {
n_alloc += CHUNK;
data = (struct DATA *) mxRealloc ((void *)data, (size_t)(n_alloc * sizeof(struct DATA)));
}
data[ndata].x = in[ix];
data[ndata].y = in[iy];
ndata++;
np_in_seg++;
}
else if (first) {
first = FALSE;
n_seg++;
}
else {
data[n_seg-1].np = np_in_seg;
n_seg++;
np_in_seg = 0;
}
}
if (first) n_seg++; /* Single segment file without any ">" */
data[n_seg-1].np = np_in_seg; /* Save npoints of the last segment */
fclose(fp);
np_in = ndata + n_seg - 1;
}
if (int_seg) intp_along_seg(n_seg, dl);
/* I'm not sure that the user realy wants this all the times */
if (upper_age == 0.0 && linear_age) upper_age = age_lin_stop;
/* Load in the stage poles */
n_stages = spotter_init (euler_file, &p, 0, upper_age);
if (upper_age == 0.0) upper_age = p[0].t_start;
if (invert_rot) /* Want to revert sense of rotation */
for (i = 0; i < n_stages; i++) {
p[i].omega *= -1; p[i].omega_r *= -1;
}
if (p[n_stages - 1].t_stop > 0) {
mexPrintf ("ERROR: A idade mais nova do polo mais recente \ntem que ser 0 nao %lg como e o caso\n", p[n_stages - 1].t_stop);
return;
}
if (t_zero > 0)
while (isoc_scale[i_min].age < t_zero) i_min++;
if (linear_age || ages_given) { /* The -B (brick) option should not pass here */
n_flow = 0; l = 0;
/* Count number of rotations for each point in segment */
for (jj = 0; jj < n_isoc && isoc_scale[jj].age <= upper_age; jj++) n_flow++;
n_flow -= i_min;
np_out = np_in * n_flow + n_flow;
plhs[0] = mxCreateDoubleMatrix (np_out, 2, mxREAL);
out = mxGetPr(plhs[0]);
for (j = i_min; j < n_flow + i_min; j++) { /* Loop over number of rotations*/
age = isoc_scale[j].age;
m = 0;
for (k = 0; k < n_seg; k++) { /* Loop over segments */
ii = 0;
c = mxCalloc (data[k].np * 2, sizeof (double));
for (i = 0; i < data[k].np; i++) { /* Loop over np in segment */
lon = data[m].x * D2R;
lat = data[m].y * D2R;
n_chunk = spotter_backtrack (&lon, &lat, &age, 1, p, n_stages, 0);
lon *= R2D; lat *= R2D;
if (fabs(lon / 180.) > 1) lon -= 360.;
c[ii++] = lon; c[ii++] = lat;
m++;
}
if (n_flow > 1 || n_seg > 1) {
out[l] = mxGetNaN(); out[l+np_out] = mxGetNaN();
l++;
}
for (ll = 0; ll < data[k].np; ll++) {
out[l] = c[ll*2]; out[l+np_out] = c[ll*2+1];
l++;
}
mxFree(c);
}
}
if (!data_in_input) mxFree ((void *)data);
}
if (nlhs > 1) {
plhs[1] = mxCreateDoubleMatrix (1, 1, mxREAL);
out_n_data = mxGetPr(plhs[1]); out_n_data[0] = ndata;
}
if (nlhs > 2) {
plhs[2] = mxCreateDoubleMatrix (1, 1, mxREAL);
out_n_seg = mxGetPr(plhs[2]); out_n_seg[0] = n_seg;
}
if (nlhs > 3) {
plhs[3] = mxCreateDoubleMatrix (1, 1, mxREAL);
out_n_flow = mxGetPr(plhs[3]); out_n_flow[0] = n_flow;
}
if (!brick) { /* Not very clean code, but we are done here */
for (k = 0; k < np_out; k++) {
lat = out[k+np_out];
if (mxIsNaN(lat))
continue;
else
lat *= D2R;
lat = atan2( sin(lat), ecc2_1 * cos(lat) ) * R2D; /* Convert back to geodetics */
out[k+np_out] = lat;
}
return;
}
n_flow = 0;
/* Count number of rotations for each point in segment */
for (jj = 0; jj < n_isoc && isoc_scale[jj].age <= upper_age; jj++) n_flow++;
n_flow -= i_min;
/* NOTE. Since from the above loop jj is allways < n_isoc, but it also is incremented one
too much before the condition fails, we can safely do the following. The intention is that
we can have also the portion of the last brick between isoc_scale[jj-1].age and upper_age */
if (isoc_scale[jj].age != upper_age) { /* Otherwise we would be repeating last point */
isoc_scale[jj].age = upper_age;
n_flow++;
}
/* For the same reason, that is to account for the portion of the brick from t_start
to the next closest entry in the chronological table, we do this trick. */
if (t_zero > 0 && i_min > 0) {
i_min--;
isoc_scale[i_min].age = t_zero;
n_flow++;
}
for (k = m = 0; k < n_seg; k++) { /* Loop over segments */
c = mxCalloc ((size_t)(n_flow * data[k].np * 2), sizeof(double));
ii = 0; m = 0;
for (i = 0; i < data[k].np; i++) { /* Loop over np in segment */
for (j = i_min; j < n_flow + i_min; j++) { /* Loop over number of rotations*/
lon = data[i].x * D2R;
lat = data[i].y * D2R;
age = isoc_scale[j].age;
n_chunk = spotter_backtrack (&lon, &lat, &age, 1, p, n_stages, 0);
lon *= R2D;
lat = atan2( sin(lat), ecc2_1 * cos(lat) ) * R2D; /* Convert back to geodetics */
if (fabs(lon / 180.) > 1) lon -= 360.;
c[ii++] = lon; c[ii++] = lat;
}
m++;
}
if (brick) { /* Always true because otherwise it has already returned further up (shity code) */
i = 0;
if (first_seg) {
plhs[0] = mxCreateDoubleMatrix (np_in * 2, n_flow, mxREAL);
plhs[1] = mxCreateDoubleMatrix (np_in * 2, n_flow, mxREAL);
out = mxGetPr(plhs[0]);
out2 = mxGetPr(plhs[1]);
if (nlhs > 2) {
plhs[2] = mxCreateDoubleMatrix (1, 1, mxREAL);
first_mag = mxGetPr(plhs[2]);
first_mag[0] = isoc_scale[i_min].mag;
}
}
for (l = 0; l < n_flow-1; l++) {
for (ll = 0; ll < data[k].np; ll++) {
out[i] = c[(l+ll*n_flow)*2];
out2[i] = c[(l+ll*n_flow)*2+1];
i++;
}
for (ll = data[k].np; ll > 0; ll--) {
out[i] = c[((ll-1)*n_flow+l+1)*2];
out2[i] = c[((ll-1)*n_flow+l+1)*2+1];
i++;
}
}
first_seg = FALSE;
}
else
int_perf (c, i_min, n_flow, k, age_flow, multi_flow, ddeg, filter_width, isoc_scale);
mxFree ((void *)c);
}
mxFree ((void *)data);
mxFree ((void *)p);
mxFree ((void *)argv);
}
void mexFunction(
int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[]
)
{
mwSize ndata, ncomp, ntref, i, comp, start = 0, stop = 0;
double *data, *tref, *res, dt2, thresh;
double NaN = mxGetNaN();
mwSignedIndex *starts;
/* Check for proper number of input and output arguments */
if ( nrhs != 4 )
mexErrMsgTxt("Four input arguments required.");
if ( nlhs > 1 )
mexErrMsgTxt("Too many output arguments.");
/* Check data type of input argument */
if ( !(mxIsDouble(prhs[0])) || !(mxIsDouble(prhs[1])) ||
!(mxIsDouble(prhs[2])) )
{
mexErrMsgTxt("Input arguments must be of type double.");
}
if( mxIsEmpty(prhs[0]) )
mexErrMsgTxt("First input argument is empty\n");
if( mxGetNumberOfDimensions(prhs[0]) != 2 )
mexErrMsgTxt("First input arguments must be a 2D matrix.");
if(mxIsEmpty(prhs[1]))
mexErrMsgTxt("Second input argument is empty");
if( mxGetNumberOfDimensions(prhs[1]) != 2 )
mexErrMsgTxt("Second input arguments must be a 2D matrix.");
if(mxIsEmpty(prhs[2]))
mexErrMsgTxt("Third input argument is empty");
if(mxIsEmpty(prhs[3]))
mexErrMsgTxt("Forth input argument is empty");
data = mxGetPr(prhs[0]);
ndata = mxGetM(prhs[0]);
ncomp = mxGetN(prhs[0]);
tref = mxGetPr(prhs[1]);
ntref = mxGetM(prhs[1]);
dt2 = mxGetScalar(prhs[2]);
thresh = mxGetScalar(prhs[3]);
if ( thresh < 0 )
mexErrMsgTxt("Forth input argument must be positive");
/* check is there is a total interval mismatch */
if ( (data[0] > tref[ntref-1] + dt2) || (data[ndata-1] <= tref[0] - dt2) )
mexErrMsgTxt("interval mismatch\n");
/* Create output array */
plhs[0] = mxCreateDoubleMatrix(ntref,ncomp,0);
res = mxGetPr(plhs[0]);
for (i=0; i < ntref; i++)
res[i] = tref[i];
if ( (starts = mxMalloc(ntref*sizeof(mwSize))) == NULL )
mexErrMsgTxt("mxMalloc() failed");
/* first find starts for all intervals */
for (i=0; i < ntref; i++)
{
/* check if we have been through all the data
* or that the data starts after the current interval */
while ( (start<ndata) && (data[start] <= res[i] - dt2) )
start++;
/*
if ( start > stop )
printf("interval(%d) : skipping %d points\n",i,start-stop);
*/
if ( (start==ndata) || (data[start] > res[i] + dt2) )
starts[i] = -1; /* no data */
else
starts[i] = start;
}
for (i=0; i < ntref; i++)
{
if ( starts[i]<0 )
{
for (comp=1; comp<ncomp; comp++)
res[i+comp*ntref] = NaN;
/*
printf("interval(%d) : no data\n",i);
*/
continue;
}
for (comp=1; comp<ncomp; comp++)
{
mwSize cur = starts[i], nav = 0;
double mean = 0.0;
while ( (data[cur] <= res[i] + dt2) && (cur < ndata) )
{
if ( mxIsNaN(data[cur+comp*ndata]) )
{
mean = NaN;
nav = 0;
break;
}
else
{
mean += data[cur+comp*ndata];
nav++;
}
cur++;
}
if ( nav )
mean = mean/(double)nav;
/*
printf("interval(%d) mean : %f, nav : %d\n",i,mean,nav);
*/
if ( cur>stop )
stop = cur;
/* compute std() */
if ( nav && thresh)
{
double std = 0;
for ( cur = starts[i]; cur < stop; cur++ )
std += (data[cur+comp*ndata] - mean)*
(data[cur+comp*ndata] - mean);
std = sqrt(std / (double)(stop-starts[i]-1));
/*
printf("interval(%d) std : %f\n",i,std);
*/
/* compute new average for pints < thresh*sdev */
nav = 0;
for ( cur = starts[i]; cur < stop; cur++ )
if ( fabs( data[cur+comp*ndata] - mean ) <= thresh*std )
{
res[i+comp*ntref] += data[cur+comp*ndata];
nav++;
}
/*
printf("interval(%d) : disregarding %d 0f %d points\n",i,
stop-starts[i]-nav,stop-start);
*/
if ( nav )
res[i+comp*ntref] = res[i+comp*ntref]/(double)nav;
else
res[i+comp*ntref] = NaN;
}
else
res[i+comp*ntref] = mean;
}
}
}
/*
* mexFunction(): entry point for the mex function
*/
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[]) {
// interface to deal with input arguments from Matlab
enum InputIndexType {IN_TRI, IN_X, InputIndexType_MAX};
MatlabImportFilter::Pointer matlabImport = MatlabImportFilter::New();
matlabImport->ConnectToMatlabFunctionInput(nrhs, prhs);
// check that we have at least a filter name and input image
matlabImport->CheckNumberOfArguments(2, InputIndexType_MAX);
// register the inputs for this function at the import filter
typedef MatlabImportFilter::MatlabInputPointer MatlabInputPointer;
MatlabInputPointer inTRI = matlabImport->RegisterInput(IN_TRI, "TRI");
MatlabInputPointer inX = matlabImport->RegisterInput(IN_X, "X");
// interface to deal with outputs to Matlab
enum OutputIndexType {OUT_A, OutputIndexType_MAX};
MatlabExportFilter::Pointer matlabExport = MatlabExportFilter::New();
matlabExport->ConnectToMatlabFunctionOutput(nlhs, plhs);
// check number of outputs the user is asking for
matlabExport->CheckNumberOfArguments(0, OutputIndexType_MAX);
// register the outputs for this function at the export filter
typedef MatlabExportFilter::MatlabOutputPointer MatlabOutputPointer;
MatlabOutputPointer outA = matlabExport->RegisterOutput(OUT_A, "A");
// default coordinates are NaN values, so that the user can spot
// whether there was any problem reading them
Point def(mxGetNaN(), mxGetNaN(), mxGetNaN());
// if any of the inputs is empty, the output is empty too
if (mxIsEmpty(prhs[IN_TRI]) || mxIsEmpty(prhs[IN_X])) {
matlabExport->CopyEmptyArrayToMatlab(outA);
return;
}
// get size of input matrix
mwSize nrowsTri = mxGetM(prhs[IN_TRI]);
mwSize ncolsTri = mxGetN(prhs[IN_TRI]);
mwSize ncolsX = mxGetN(prhs[IN_X]);
if ((ncolsTri != 3) || (ncolsX != 3)) {
mexErrMsgTxt("Both input arguments must have 3 columns");
}
// initialise output
double *area = matlabExport->AllocateColumnVectorInMatlab<double>(outA, nrowsTri);
// read triangular mesh from function
Triangle tri;
mwIndex v0, v1, v2; // indices of the 3 vertices of each triangle
Point x0, x1, x2; // coordinates of the 3 vertices of each triangle
for (mwIndex i = 0; i < nrowsTri; ++i) {
// exit if user pressed Ctrl+C
ctrlcCheckPoint(__FILE__, __LINE__);
// get indices of the 3 vertices of each triangle. These indices
// follow Matlab's convention v0 = 1, 2, ..., n
v0 = matlabImport->ReadScalarFromMatlab<mwIndex>(inTRI, i, 0, mxGetNaN());
v1 = matlabImport->ReadScalarFromMatlab<mwIndex>(inTRI, i, 1, mxGetNaN());
v2 = matlabImport->ReadScalarFromMatlab<mwIndex>(inTRI, i, 2, mxGetNaN());
if (mxIsNaN(v0) || mxIsNaN(v1) || mxIsNaN(v2)) {
mexErrMsgTxt("Input TRI: Vertex index is NaN");
}
// get coordinates of the 3 vertices (substracting 1 so that
// indices follow the C++ convention 0, 1, ..., n-1)
x0 = matlabImport->ReadRowVectorFromMatlab<void, Point>(inX, v0 - 1, def);
x1 = matlabImport->ReadRowVectorFromMatlab<void, Point>(inX, v1 - 1, def);
x2 = matlabImport->ReadRowVectorFromMatlab<void, Point>(inX, v2 - 1, def);
// create triangle from the vertices read at the input
tri = Triangle(x0, x1, x2);
// compute triangle area
area[i] = std::sqrt(tri.squared_area());
// // DEBUG
// std::cout << x0 << "\t" << x1 << "\t" << x2 << "\t" << std::sqrt(tri.squared_area()) << std::endl;
}
}
double erf(double x)
{
const double p1[5] = {
3.20937758913846947e03,
3.77485237685302021e02,
1.13864154151050156e02,
3.16112374387056560e00,
1.85777706184603153e-1};
const double q1[4] = {
2.84423683343917062e03,
1.28261652607737228e03,
2.44024637934444173e02,
2.36012909523441209e01};
const double p2[9] = {
1.23033935479799725e03,
2.05107837782607147e03,
1.71204761263407058e03,
8.81952221241769090e02,
2.98635138197400131e02,
6.61191906371416295e01,
8.88314979438837594e00,
5.64188496988670089e-1,
2.15311535474403846e-8};
const double q2[8] = {
1.23033935480374942e03,
3.43936767414372164e03,
4.36261909014324716e03,
3.29079923573345963e03,
1.62138957456669019e03,
5.37181101862009858e02,
1.17693950891312499e02,
1.57449261107098347e01};
const double p3[6] = {
6.58749161529837803e-4,
1.60837851487422766e-2,
1.25781726111229246e-1,
3.60344899949804439e-1,
3.05326634961232344e-1,
1.63153871373020978e-2};
const double q3[5] = {
2.33520497626869185e-3,
6.05183413124413191e-2,
5.27905102951428412e-1,
1.87295284992346047e00,
2.56852019228982242e00};
int i;
double xval, xx, p, q;
bool NegativeValue;
xval=x;
if (xval<0) {xval=-xval; NegativeValue=true;}
else NegativeValue=false;
if (xval<=0.46875)
{
xx = xval*xval;
p=p1[4];
q=1.0;
for (i=3;i>=0;i--) {p=p*xx+p1[i]; q=q*xx+q1[i];}
xx=p/q;
return(x*xx);
}
else if (xval<=4)
{
xx=xval;
p=p2[8];
q=1.0;
for (i=7;i>=0;i--) {p=p*xx+p2[i]; q=q*xx+q2[i];}
xx=p/q;
xx = exp(-xval*xval)*xx;
if (NegativeValue) return((xx-0.5)-0.5); else return((0.5-xx)+0.5);
}
else if (xval<10)
{
xx=1/(xval*xval);
p=p3[5];
q=1.0;
for (i=4;i>=0;i--) {p=p*xx+p3[i]; q=q*xx+q3[i];}
xx=p/q;
xx = exp(-xval*xval)*(0.7071067811865475-xx)/(xval);
if (mxIsNaN(xx)) xx=0;
if (NegativeValue) return((xx-0.5)-0.5); else return((0.5-xx)+0.5);
}
else
if (NegativeValue) return(-1); else return(1);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
mwSize nrow, ncol; // use mwIndex
mwIndex ix, ixn, row, col, r, c, ixnn;
int coln[4] = {1, 1, 1, 0};
int rown[4] = {-1, 0, 1, 1};
double z, zn, g;
double *cs;
double *dem;
double *G;
double dist[4] = {sqrt(2), 1, sqrt(2), 1 };
if( nrhs > 2 || nlhs > 1 ) {
mexErrMsgTxt("Need 2 inputs and no more than 1 output");
}
dem = mxGetPr(prhs[0]);
nrow = mxGetM(prhs[0]);
ncol = mxGetN(prhs[0]);
cs = mxGetPr(prhs[1]);
/* Create an m-by-n mxArray and either copy initial values into it or create a new one with ones*/
plhs[0] = mxCreateDoubleMatrix((mwSize) nrow,(mwSize) ncol, mxREAL);
/* pointer into drainage basins*/
G = mxGetPr(plhs[0]);
// set cell distances
for( ixn = 0; ixn < 4; ixn++) {
dist[ixn] = cs[0] * dist[ixn];
}
for( row= 0; row<nrow; row++ ) {
for( col= 0; col<ncol; col++) {
ix = (col*nrow) + row;
z = dem[ix];
// nan?
if (mxIsNaN(z)) {
G[ix] = dem[ix];
continue;
}
for ( ixn = 0; ixn < 4; ixn++ ) {
// row index of neighbor
r = row + rown[ixn];
// column index of neighbor
c = col + coln[ixn];
// check if indices may be outside the grid
if (r < 0) {
continue;
}
if (r >= nrow) {
continue;
}
if (c < 0) {
continue;
}
if (c >= ncol) {
continue;
}
ixnn = (c*nrow) + r;
// calculate slope
g = (z - dem[ixnn])/ dist[ixn];
// find maximum slope
if ( g > G[ix] ) {
G[ix] = g;
}
else if (g < 0) {
g = -g;
if ( g > G[ixnn] ) {
G[ixnn] = g;
}
}
}
}
}
}
//CALCR
static void CALCR(int *n, int *p, double *x, int *nf, double *r, int *uiparm, double *ydata, void *ufparm)
{
bool havrnorm = false, stop = false;
int i, stat;
double *fval, rnorm = 0;
clock_t end;
double evaltime;
//Get User Data
user_function_data *fun = (user_function_data*)ufparm;
fun->plhs[0] = NULL;
memcpy(mxGetPr(fun->prhs[fun->xrhs]), x, *p * sizeof(double));
stat = mexCallMATLAB(1, fun->plhs, fun->nrhs, fun->prhs, fun->f);
if(stat)
mexErrMsgTxt("Error calling Objective Function!");
//Get Objective
fval = mxGetPr(fun->plhs[0]);
//Assign Objective + subtract ydata
for(i=0; i<*n; i++) {
r[i] = fval[i]-ydata[i];
//Check for out of bounds, will try a smaller step size
if(mxIsInf(r[i]) || mxIsNaN(r[i]))
*nf = 0;
}
// Clean up Ptr
mxDestroyArray(fun->plhs[0]);
//Iteration Printing
if(uiparm[1] > 1) {
//Get Execution Time
end = clock();
evaltime = ((double)(end-start))/CLOCKS_PER_SEC;
//Calculate Residual Norm
rnorm = 0;
havrnorm = true;
for(i=0; i<*n; i++)
rnorm += r[i]*r[i];
if(uiparm[0] == 1 || !(uiparm[0]%10))
mexPrintf(" feval time sse\n");
mexPrintf("%5d %5.2f %12.5g\n",uiparm[0],evaltime,rnorm);
mexEvalString("drawnow;"); //flush draw buffer
}
//Iteration Callback
if(iterF.enabled)
{
//Calculate sse if we don't have it
if(!havrnorm)
for(i=0; i<*n; i++)
rnorm += r[i]*r[i];;
iterF.plhs[0] = NULL;
memcpy(mxGetData(iterF.prhs[1]), uiparm, sizeof(int));
memcpy(mxGetPr(iterF.prhs[2]), &rnorm, sizeof(double));
memcpy(mxGetPr(iterF.prhs[3]), x, *p * sizeof(double));
stat = mexCallMATLAB(1, iterF.plhs, 4, iterF.prhs, iterF.f);
if(stat)
mexErrMsgTxt("Error calling Callback Function!");
//Collect return argument
stop = *(bool*)mxGetData(iterF.plhs[0]);
// Clean up Ptr
mxDestroyArray(iterF.plhs[0]);
//Warn user stop not implemented
if(stop)
mexWarnMsgTxt("NL2SOL does not implement the stop feature of iterfun");
}
uiparm[0]++;
}
