/* Function: mdlInitializeConditions ========================================
* Abstract:
* Initialize states to zeros.
*/
static void mdlInitializeConditions(SimStruct *S)
{
int_T i;
InputRealPtrsType M, q;
/* checks the input matrices for NaN and Inf */
/* checking M */
M = ssGetInputPortRealSignalPtrs(S,0);
for (i=0; i<NSTATES(S)*NSTATES(S); i++) {
if ( !mxIsFinite(*M[i]) ) {
ssSetErrorStatus(S, "lcp_sfun: No 'NaN' or 'Inf' terms are allowed in input matrix M.");
return;
}
}
/* checking q */
q = ssGetInputPortRealSignalPtrs(S,1);
for (i=0; i<NSTATES(S); i++) {
if ( !mxIsFinite(*q[i]) ) {
ssSetErrorStatus(S, "lcp_sfun: No 'NaN' or 'Inf' terms are allowed in input vector q.");
return;
}
}
}
void assignmentsuboptimal2(double *assignment, double *cost, double *distMatrixIn, int nOfRows, int nOfColumns)
{
int n, row, col, tmpRow, tmpCol, nOfElements;
double value, minValue, *distMatrix, inf;
inf = mxGetInf();
/* make working copy of distance Matrix */
nOfElements = nOfRows * nOfColumns;
distMatrix = (double *)mxMalloc(nOfElements * sizeof(double));
for(n=0; n<nOfElements; n++)
distMatrix[n] = distMatrixIn[n];
/* initialization */
*cost = 0;
for(row=0; row<nOfRows; row++)
#ifdef ONE_INDEXING
assignment[row] = 0.0;
#else
assignment[row] = -1.0;
#endif
/* recursively search for the minimum element and do the assignment */
while(true)
{
/* find minimum distance observation-to-track pair */
minValue = inf;
for(row=0; row<nOfRows; row++)
for(col=0; col<nOfColumns; col++)
{
value = distMatrix[row + nOfRows*col];
if(mxIsFinite(value) && (value < minValue))
{
minValue = value;
tmpRow = row;
tmpCol = col;
}
}
if(mxIsFinite(minValue))
{
#ifdef ONE_INDEXING
assignment[tmpRow] = tmpCol+ 1;
#else
assignment[tmpRow] = tmpCol;
#endif
*cost += minValue;
for(n=0; n<nOfRows; n++)
distMatrix[n + nOfRows*tmpCol] = inf;
for(n=0; n<nOfColumns; n++)
distMatrix[tmpRow + nOfRows*n] = inf;
}
else
break;
} /* while(true) */
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
int i, j, m, n;
double s1=0.0, s2=0.0;
double *dat;
MAPTYPE *map, *get_maps();
static double M[] = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1};
if (nrhs != 1 || nlhs > 1)
{
mexErrMsgTxt("Incorrect usage.");
}
map = get_maps(prhs[0], &j);
if (j != 1)
{
free_maps(map, j);
mexErrMsgTxt("Incorrect usage.");
}
n = (map->dim[0])*(map->dim[1]);
dat = (double *)mxCalloc(n, sizeof(double));
s1 = 0.0;
m = 0;
for (i=0; i<map->dim[2]; i++)
{
M[14] = i+1;
slice(M, dat, map->dim[0],map->dim[1], map, 0,0);
for(j=0;j<n; j++)
if (mxIsFinite(dat[j]))
{
s1 += dat[j];
m ++;
}
}
s1/=(8.0*m);
s2=0.0;
m =0;
for (i=0; i<map->dim[2]; i++)
{
M[14] = i+1;
slice(M, dat, map->dim[0],map->dim[1], map, 0,0);
for(j=0;j<n; j++)
if (mxIsFinite(dat[j]) && dat[j]>s1)
{
m++;
s2+=dat[j];
}
}
s2/=m;
plhs[0] = mxCreateDoubleMatrix(1,1,mxREAL);
mxGetPr(plhs[0])[0]=s2;
free_maps(map, 1);
}
static void mdlRTW(SimStruct *S){
int_T type = P_TYPE;
int_T id = P_ID;
DTypeId dataType = ssGetInputPortDataType(S,0);
int_T length =ssGetDataTypeSize(S,dataType) * ssGetInputPortWidth(S,0);
int32_T endian = P_ENDIAN;
/* -- Write Invariant Parameter Settings -- */
ssWriteRTWParamSettings(S,4,
SSWRITE_VALUE_DTYPE_NUM, "TYPE",
&type,
DTINFO(SS_UINT32,0),
SSWRITE_VALUE_DTYPE_NUM, "ID",
&id,
DTINFO(SS_UINT32,0),
SSWRITE_VALUE_DTYPE_NUM, "LENGTH",
&length,
DTINFO(SS_UINT16,0),
SSWRITE_VALUE_DTYPE_NUM, "Endian",
&endian,
DTINFO(SS_UINT32,0)
);
/* -- Write Invariant Signal Settings -- */
if ( !mxIsFinite( ssGetSampleTime( S, 0 ) ) ) {
CAN_FRAME * frame = (CAN_FRAME *) ssGetOutputPortSignal(S,0);
CAN_write_rtw_frame(S,frame);
}
}
void computeassignmentcost(double *assignment, double *cost, double *distMatrix, int nOfRows)
{
int row, col;
#ifdef CHECK_FOR_INF
double value;
#endif
for(row=0; row<nOfRows; row++)
{
#ifdef ONE_INDEXING
col = assignment[row]-1; /* MATLAB-Indexing */
#else
col = assignment[row];
#endif
if(col >= 0)
{
#ifdef CHECK_FOR_INF
value = distMatrix[row + nOfRows*col];
if(mxIsFinite(value))
*cost += value;
else
#ifdef ONE_INDEXING
assignment[row] = 0.0;
#else
assignment[row] = -1.0;
#endif
#else
*cost += distMatrix[row + nOfRows*col];
#endif
}
}
}
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);
}
}
}
/* Compute the inverse deformation field within a single cube */
static void invert_it(int x0, int x1, int x2, float *y0, float *y1, float *y2,
int dim_f[3], float *iy0, float *iy1, float *iy2, REAL U[4][3], REAL V[4][3])
{
int i, j, k, vox[MAXV][3], nvox;
REAL Y0[4][3], Y[4][3], M[4][3], IM[4][3];
/* Determine tetrahedral arrangement */
k = (x0%2)==(x1%2)==(x2%2);
for(i=0; i<5; i++) /* Five tetrahedra within a cube */
{
/* Find the vertices (in mm space) */
Y0[0][0] = y0[off[k][0][i]]; Y0[0][1] = y1[off[k][0][i]]; Y0[0][2] = y2[off[k][0][i]];
Y0[1][0] = y0[off[k][1][i]]; Y0[1][1] = y1[off[k][1][i]]; Y0[1][2] = y2[off[k][1][i]];
Y0[2][0] = y0[off[k][2][i]]; Y0[2][1] = y1[off[k][2][i]]; Y0[2][2] = y2[off[k][2][i]];
Y0[3][0] = y0[off[k][3][i]]; Y0[3][1] = y1[off[k][3][i]]; Y0[3][2] = y2[off[k][3][i]];
/* Convert vertex co-ordinates to voxels */
mulMX(Y, U, Y0);
/* Compute affine transform mapping vertices together */
getM(Y, ix[k][i], M, x0, x1, x2);
if (mxIsFinite(M[0][0])) /* Prevent from bombing out when NaNs are encountered */
{
/* Find integer co-ordinates within tetrahedron */
scan_tetrahedron(Y, &nvox, vox, MAXV);
if (nvox>0)
{
/* Invert the affine mapping */
invertM(M, IM);
/* Convert the mapping from voxels to mm */
mulMM(M, V, IM);
/* Insert the new mappings into each voxel within the tetrahedron */
for(j=0; j<nvox; j++)
{
if ((vox[j][0]>=1) && (vox[j][0]<=dim_f[0]) &&
(vox[j][1]>=1) && (vox[j][1]<=dim_f[1]) &&
(vox[j][2]>=1) && (vox[j][2]<=dim_f[2]))
{
int o = vox[j][0]+dim_f[0]*(vox[j][1]+dim_f[1]*vox[j][2]);
iy0[o] = M[0][0]*vox[j][0] + M[1][0]*vox[j][1] + M[2][0]*vox[j][2] + M[3][0];
iy1[o] = M[0][1]*vox[j][0] + M[1][1]*vox[j][1] + M[2][1]*vox[j][2] + M[3][1];
iy2[o] = M[0][2]*vox[j][0] + M[1][2]*vox[j][1] + M[2][2]*vox[j][2] + M[3][2];
}
}
}
}
}
}
/* Function: mdlCheckParameters =============================================
* Abstract:
* Validate our parameters to verify they are okay.
*/
static void mdlCheckParameters(SimStruct *S)
{
int i;
bool badParam = false;
/* All parameters must be positive non-zero integers */
for (i = 0; i < ssGetSFcnParamsCount(S); i++) {
const mxArray *m = ssGetSFcnParam(S,i);
if (mxGetNumberOfElements(m) != 1 ||
!mxIsNumeric(m) || !mxIsDouble(m) || mxIsLogical(m) ||
mxIsComplex(m) || mxIsSparse(m) || !mxIsFinite(mxGetPr(m)[0])) {
badParam = true;
break;
}
if (i < NKPARAMS) {
int ival;
if (((ival=(int_T)mxGetPr(m)[0]) <= 0) ||
(real_T)ival != mxGetPr(m)[0]) {
badParam = true;
break;
}
} else if (mxGetPr(m)[0] <= 0.0) {
badParam = true;
break;
}
}
if (badParam) {
ssSetErrorStatus(S,"All parameters must be positive non-zero "
"values. First three must be integers and the "
"optional fourth must be a discrete sample "
"time");
return;
}
}
/***************************************************************************************
Deconvolve the B-spline basis functions from the image volume
vol - a handle for the volume to deconvolve
c - the coefficients (arising from the deconvolution)
d - the spline degree
splinc0, splinc1, splinc2 - functions for 1D deconvolutions
*/
static int vol_coeffs(MAPTYPE *vol, double c[], int d[], void (*splinc[])())
{
double p[4], *cp;
int np;
int i, j, k, n;
double f[10240];
/* Check that dimensions don't exceed size of f */
if (vol->dim[1]>10240 ||vol->dim[2]>10240)
return(1);
/* Do a straight copy */
cp = c;
for(k=0; k<vol->dim[2]; k++)
{
double dk = k+1;
for(j=0; j<vol->dim[1]; j++)
{
double dj = j+1;
for(i=0;i<vol->dim[0];i++, cp++)
{
double di = i+1;
resample(1,vol,cp,&di,&dj,&dk,0, 0.0);
/* Not sure how best to handle NaNs */
if (!mxIsFinite(*cp)) *cp = 0.0;
}
}
}
/* Deconvolve along the fastest dimension (X) */
if (d[0]>1 && vol->dim[0]>1)
{
if (get_poles(d[0], &np, p)) return(1);
for(k=0; k<vol->dim[2]; k++)
{
/* double dk = k+1; */
for(j=0; j<vol->dim[1]; j++)
{
cp = &c[vol->dim[0]*(j+vol->dim[1]*k)];
splinc[0](cp, vol->dim[0], p, np);
}
}
}
/* Deconvolve along the middle dimension (Y) */
if (d[1]>1 && vol->dim[1]>1)
{
if (get_poles(d[1], &np, p)) return(1);
n =vol->dim[0];
for(k=0; k<vol->dim[2]; k++)
{
for(i=0;i<vol->dim[0];i++)
{
cp = &c[i+vol->dim[0]*vol->dim[1]*k];
for(j=0; j<vol->dim[1]; j++, cp+=n)
f[j] = *cp;
splinc[1](f, vol->dim[1], p, np);
cp = &c[i+vol->dim[0]*vol->dim[1]*k];
for(j=0; j<vol->dim[1]; j++, cp+=n)
*cp = f[j];
}
}
}
/* Deconvolve along the slowest dimension (Z) */
if (d[2]>1 && vol->dim[2]>1)
{
if (get_poles(d[2], &np, p)) return(1);
n = vol->dim[0]*vol->dim[1];
for(j=0; j<vol->dim[1]; j++)
{
for(i=0;i<vol->dim[0];i++)
{
cp = &c[i+vol->dim[0]*j];
for(k=0; k<vol->dim[2]; k++, cp+=n)
f[k] = *cp;
splinc[2](f, vol->dim[2], p, np);
cp = &c[i+vol->dim[0]*j];
for(k=0; k<vol->dim[2]; k++, cp+=n)
*cp = f[k];
}
}
}
return(0);
}
void assignmentsuboptimal1(double *assignment, double *cost, double *distMatrixIn, int nOfRows, int nOfColumns)
{
bool infiniteValueFound, finiteValueFound, repeatSteps, allSinglyValidated, singleValidationFound;
int n, row, col, tmpRow, tmpCol, nOfElements;
int *nOfValidObservations, *nOfValidTracks;
double value, minValue, *distMatrix, inf;
inf = mxGetInf();
/* make working copy of distance Matrix */
nOfElements = nOfRows * nOfColumns;
distMatrix = (double *)mxMalloc(nOfElements * sizeof(double));
for(n=0; n<nOfElements; n++)
distMatrix[n] = distMatrixIn[n];
/* initialization */
*cost = 0;
#ifdef ONE_INDEXING
for(row=0; row<nOfRows; row++)
assignment[row] = 0.0;
#else
for(row=0; row<nOfRows; row++)
assignment[row] = -1.0;
#endif
/* allocate memory */
nOfValidObservations = (int *)mxCalloc(nOfRows, sizeof(int));
nOfValidTracks = (int *)mxCalloc(nOfColumns, sizeof(int));
/* compute number of validations */
infiniteValueFound = false;
finiteValueFound = false;
for(row=0; row<nOfRows; row++)
for(col=0; col<nOfColumns; col++)
if(mxIsFinite(distMatrix[row + nOfRows*col]))
{
nOfValidTracks[col] += 1;
nOfValidObservations[row] += 1;
finiteValueFound = true;
}
else
infiniteValueFound = true;
if(infiniteValueFound)
{
if(!finiteValueFound)
return;
repeatSteps = true;
while(repeatSteps)
{
repeatSteps = false;
/* step 1: reject assignments of multiply validated tracks to singly validated observations */
for(col=0; col<nOfColumns; col++)
{
singleValidationFound = false;
for(row=0; row<nOfRows; row++)
if(mxIsFinite(distMatrix[row + nOfRows*col]) && (nOfValidObservations[row] == 1))
{
singleValidationFound = true;
break;
}
if(singleValidationFound)
{
for(row=0; row<nOfRows; row++)
if((nOfValidObservations[row] > 1) && mxIsFinite(distMatrix[row + nOfRows*col]))
{
distMatrix[row + nOfRows*col] = inf;
nOfValidObservations[row] -= 1;
nOfValidTracks[col] -= 1;
repeatSteps = true;
}
}
}
/* step 2: reject assignments of multiply validated observations to singly validated tracks */
if(nOfColumns > 1)
{
for(row=0; row<nOfRows; row++)
{
singleValidationFound = false;
for(col=0; col<nOfColumns; col++)
if(mxIsFinite(distMatrix[row + nOfRows*col]) && (nOfValidTracks[col] == 1))
{
singleValidationFound = true;
break;
}
if(singleValidationFound)
{
for(col=0; col<nOfColumns; col++)
if((nOfValidTracks[col] > 1) && mxIsFinite(distMatrix[row + nOfRows*col]))
{
distMatrix[row + nOfRows*col] = inf;
nOfValidObservations[row] -= 1;
nOfValidTracks[col] -= 1;
repeatSteps = true;
}
}
}
}
} /* while(repeatSteps) */
/* for each multiply validated track that validates only with singly validated */
/* observations, choose the observation with minimum distance */
for(row=0; row<nOfRows; row++)
{
if(nOfValidObservations[row] > 1)
{
allSinglyValidated = true;
minValue = inf;
for(col=0; col<nOfColumns; col++)
{
value = distMatrix[row + nOfRows*col];
if(mxIsFinite(value))
{
if(nOfValidTracks[col] > 1)
{
allSinglyValidated = false;
break;
}
else if((nOfValidTracks[col] == 1) && (value < minValue))
{
tmpCol = col;
minValue = value;
}
}
}
if(allSinglyValidated)
{
#ifdef ONE_INDEXING
assignment[row] = tmpCol + 1;
#else
assignment[row] = tmpCol;
#endif
*cost += minValue;
for(n=0; n<nOfRows; n++)
distMatrix[n + nOfRows*tmpCol] = inf;
for(n=0; n<nOfColumns; n++)
distMatrix[row + nOfRows*n] = inf;
}
}
}
/* for each multiply validated observation that validates only with singly validated */
/* track, choose the track with minimum distance */
for(col=0; col<nOfColumns; col++)
{
if(nOfValidTracks[col] > 1)
{
allSinglyValidated = true;
minValue = inf;
for(row=0; row<nOfRows; row++)
{
value = distMatrix[row + nOfRows*col];
if(mxIsFinite(value))
{
if(nOfValidObservations[row] > 1)
{
allSinglyValidated = false;
break;
}
else if((nOfValidObservations[row] == 1) && (value < minValue))
{
tmpRow = row;
minValue = value;
}
}
}
if(allSinglyValidated)
{
#ifdef ONE_INDEXING
assignment[tmpRow] = col + 1;
#else
assignment[tmpRow] = col;
#endif
*cost += minValue;
for(n=0; n<nOfRows; n++)
distMatrix[n + nOfRows*col] = inf;
for(n=0; n<nOfColumns; n++)
distMatrix[tmpRow + nOfRows*n] = inf;
}
}
}
} /* if(infiniteValueFound) */
/* now, recursively search for the minimum element and do the assignment */
while(true)
{
/* find minimum distance observation-to-track pair */
minValue = inf;
for(row=0; row<nOfRows; row++)
for(col=0; col<nOfColumns; col++)
{
value = distMatrix[row + nOfRows*col];
if(mxIsFinite(value) && (value < minValue))
{
minValue = value;
tmpRow = row;
tmpCol = col;
}
}
if(mxIsFinite(minValue))
{
#ifdef ONE_INDEXING
assignment[tmpRow] = tmpCol+ 1;
#else
assignment[tmpRow] = tmpCol;
#endif
*cost += minValue;
for(n=0; n<nOfRows; n++)
distMatrix[n + nOfRows*tmpCol] = inf;
for(n=0; n<nOfColumns; n++)
distMatrix[tmpRow + nOfRows*n] = inf;
}
else
break;
} /* while(true) */
/* free allocated memory */
mxFree(nOfValidObservations);
mxFree(nOfValidTracks);
}
void mrqminbd(double x[], // x values of dataset
double y[], // y values of dataset
double sig[], // individual std. deviations
int npts, // number of data points
double p[], // function parameters p
double lb[], // lower bounds on function parameters
double ub[], // upper bounds on function parameters
int npar, // number of function parameters
int ip[], // indices of parameters to fit
int nfit, // number of parameters to fit
double **covar, // returned covariance matrix
double **alpha, // returned curvature matrix
double beta[], // returned chi^2 partials matrix
double *chisq, // returned chi^2 value
double (*funcs)(double, // function x value, returns f(p, x)
double [], // function parameters p
double [], // returned df/dp values
int), // number of parameters
double *lambda) { // returned curvature diagonal scaling factor
// declare variables
// some are static because of repeated calls
static double ochisq; // old chi^2
static double *ptry; // function parameters to test
static double *dp; // parameter increments
int inverr; // matrix inversion error
int j, k; // loop counters
// first call has scaling < 0 for initialization
if (*lambda < 0.0) {
ptry = (double *) mxCalloc(npar, sizeof(double));
dp = (double *) mxCalloc(npar, sizeof(double));
// assign initial parameters
for (j = 0; j < npar; j++) {
ptry[j] = p[j];
}
// return initial chi^2, curvature & chi^2 partial derivatives
*lambda = FIRST_ALAMBDA;
mrqcof(x, y, sig, npts, ptry, npar, ip, nfit, alpha, beta, chisq, funcs);
ochisq = (*chisq);
return;
}
// augment diagonal curvature values with lambda
for (j = 0; j < nfit; j++) {
for (k = 0; k < nfit; k++) {
covar[j][k] = alpha[j][k];
}
covar[j][j] *= 1.0 + (*lambda);
dp[j] = beta[j];
}
// solve for covariance & parameter increments
// trial is unsuccessful if matrix inversion error
inverr = gaussj(covar, dp, nfit);
if (inverr < 0) {
*lambda *= INC_ALAMBDA;
return;
}
// last call has lambda = 0
// rearrange matrices & free allocated space
if (*lambda == 0.0) {
if (nfit < npar) {
ArrangeFixed(alpha, beta, npar, ip, nfit, SORT_BKWD);
ArrangeFixed(covar, dp, npar, ip, nfit, SORT_BKWD);
}
mxFree((void *) dp);
mxFree((void *) ptry);
return;
}
// increment parameters to see if trial succeeded
// trial is unsuccessful if bounds are exceeded
for (j = 0; j < nfit; j++) {
ptry[ip[j]] = p[ip[j]] + dp[j];
if (mxIsFinite(lb[ip[j]]) && (ptry[ip[j]] <= lb[ip[j]])) {
*lambda *= INC_ALAMBDA;
return;
}
else if (mxIsFinite(ub[ip[j]]) && (ptry[ip[j]] >= ub[ip[j]])) {
*lambda *= INC_ALAMBDA;
return;
}
}
// obtain updated chi^2, curvature & chi^2 partial derivatives
// note that covar & dp are used to preserve alpha & beta if step fails
mrqcof(x, y, sig, npts, ptry, npar, ip, nfit, covar, dp, chisq, funcs);
// if successful, accept new parameters
// decrease lambda
if (*chisq < ochisq) {
*lambda *= DEC_ALAMBDA;
ochisq = (*chisq);
for (j = 0; j < nfit; j++) {
for (k = 0; k < nfit; k++) {
alpha[j][k] = covar[j][k];
}
beta[j] = dp[j];
p[ip[j]] = ptry[ip[j]];
}
}
// otherwise reject new parameters
// increase lambda
else {
*lambda *= INC_ALAMBDA;
*chisq = ochisq;
}
}
void assignmentoptimal(double *assignment, double *cost, double *distMatrixIn, int nOfRows, int nOfColumns)
{
double *distMatrix, *distMatrixTemp, *distMatrixEnd, *columnEnd, value, minValue;
bool *coveredColumns, *coveredRows, *starMatrix, *newStarMatrix, *primeMatrix;
int nOfElements, minDim, row, col;
#ifdef CHECK_FOR_INF
bool infiniteValueFound;
double maxFiniteValue, infValue;
#endif
/* initialization */
*cost = 0;
for(row=0; row<nOfRows; row++)
#ifdef ONE_INDEXING
assignment[row] = 0.0;
#else
assignment[row] = -1.0;
#endif
/* generate working copy of distance Matrix */
/* check if all matrix elements are positive */
nOfElements = nOfRows * nOfColumns;
distMatrix = (double *)mxMalloc(nOfElements * sizeof(double));
distMatrixEnd = distMatrix + nOfElements;
for(row=0; row<nOfElements; row++)
{
value = distMatrixIn[row];
if(mxIsFinite(value) && (value < 0))
mexErrMsgTxt("All matrix elements have to be non-negative.");
distMatrix[row] = value;
}
#ifdef CHECK_FOR_INF
/* check for infinite values */
maxFiniteValue = -1;
infiniteValueFound = false;
distMatrixTemp = distMatrix;
while(distMatrixTemp < distMatrixEnd)
{
value = *distMatrixTemp++;
if(mxIsFinite(value))
{
if(value > maxFiniteValue)
maxFiniteValue = value;
}
else
infiniteValueFound = true;
}
if(infiniteValueFound)
{
if(maxFiniteValue == -1) /* all elements are infinite */
return;
/* set all infinite elements to big finite value */
if(maxFiniteValue > 0)
infValue = 10 * maxFiniteValue * nOfElements;
else
infValue = 10;
distMatrixTemp = distMatrix;
while(distMatrixTemp < distMatrixEnd)
if(mxIsInf(*distMatrixTemp++))
*(distMatrixTemp-1) = infValue;
}
#endif
/* memory allocation */
coveredColumns = (bool *)mxCalloc(nOfColumns, sizeof(bool));
coveredRows = (bool *)mxCalloc(nOfRows, sizeof(bool));
starMatrix = (bool *)mxCalloc(nOfElements, sizeof(bool));
primeMatrix = (bool *)mxCalloc(nOfElements, sizeof(bool));
newStarMatrix = (bool *)mxCalloc(nOfElements, sizeof(bool)); /* used in step4 */
/* preliminary steps */
if(nOfRows <= nOfColumns)
{
minDim = nOfRows;
for(row=0; row<nOfRows; row++)
{
/* find the smallest element in the row */
distMatrixTemp = distMatrix + row;
minValue = *distMatrixTemp;
distMatrixTemp += nOfRows;
while(distMatrixTemp < distMatrixEnd)
{
value = *distMatrixTemp;
if(value < minValue)
minValue = value;
distMatrixTemp += nOfRows;
}
/* subtract the smallest element from each element of the row */
distMatrixTemp = distMatrix + row;
while(distMatrixTemp < distMatrixEnd)
{
*distMatrixTemp -= minValue;
distMatrixTemp += nOfRows;
}
}
/* Steps 1 and 2a */
for(row=0; row<nOfRows; row++)
for(col=0; col<nOfColumns; col++)
if(distMatrix[row + nOfRows*col] == 0)
if(!coveredColumns[col])
{
starMatrix[row + nOfRows*col] = true;
coveredColumns[col] = true;
break;
}
}
else /* if(nOfRows > nOfColumns) */
{
minDim = nOfColumns;
for(col=0; col<nOfColumns; col++)
{
/* find the smallest element in the column */
distMatrixTemp = distMatrix + nOfRows*col;
columnEnd = distMatrixTemp + nOfRows;
minValue = *distMatrixTemp++;
while(distMatrixTemp < columnEnd)
{
value = *distMatrixTemp++;
if(value < minValue)
minValue = value;
}
/* subtract the smallest element from each element of the column */
distMatrixTemp = distMatrix + nOfRows*col;
while(distMatrixTemp < columnEnd)
*distMatrixTemp++ -= minValue;
}
/* Steps 1 and 2a */
for(col=0; col<nOfColumns; col++)
for(row=0; row<nOfRows; row++)
if(distMatrix[row + nOfRows*col] == 0)
if(!coveredRows[row])
{
starMatrix[row + nOfRows*col] = true;
coveredColumns[col] = true;
coveredRows[row] = true;
break;
}
for(row=0; row<nOfRows; row++)
coveredRows[row] = false;
}
/* move to step 2b */
step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
/* compute cost and remove invalid assignments */
computeassignmentcost(assignment, cost, distMatrixIn, nOfRows);
/* free allocated memory */
mxFree(distMatrix);
mxFree(coveredColumns);
mxFree(coveredRows);
mxFree(starMatrix);
mxFree(primeMatrix);
mxFree(newStarMatrix);
return;
}
/*this function does the job of averaging.*/
void doaveraging(const double *data, const double *dataerr,
const double energy, const double dist,
const double xresol, const double yresol,
const double bcx, const double bcy, const mxLogical *mask,
const double *q, double *I, double *E, double *A,
const mwSize M, const mwSize N, const mwSize Noutput,
const double par1, const double par2)
/* Directions:
* +-----------------------------------------> Y
* | 1,1 1,2 1,3 1,4 ... 1,N
* | 2,1 2,2 2,3 2,4 ... 1,N
* | 3,1 3,2 3,3 3,4 ... 1,N
* | 4,1 4,2 4,3 4,4 ... 1,N
* | . . . . . .
* | . . . . . .
* | . . . . . .
* | M,1 M,2 M,3 M,4 ... M,N
* |
* | X
* v
*
*/
{
mwIndex i,j,k;
#if INT_MODE==ASIM
double rmin, rmax;
double *phi0, *dphi;
double dmax2,dmin2; /*dmax and dmin to the second, this is used for TEST2*/
#else /*for both RAD and SECTOR*/
double *rmin,*rmax; /*rmin and rmax: distances from the origin
corresponding to the current q-ring*/
double *dmax2,*dmin2; /*dmax and dmin to the second, this is used for TEST2*/
# if INT_MODE==SECTOR
double phi0, dphi; /*phi0 is the beginning angle of the sector, dphi is
the length of it. Both are expressed in radians.*/
# endif
#endif
double xc,yc,x0,y0,x1,y1; /*for each pixel, xc,yc is its center,
x0,y0,x1,y1 are the coordinates for the
corners*/
double x,y; /*loop variables for subpixel resolution*/
unsigned long n; /*counter for subpixel resolution*/
double p; /*used for subpixel resolution*/
double avgresol=sqrt(xresol*xresol+yresol*yresol); /*the average resolution*/
/*"decode" par1 and par2*/
#if INT_MODE==ASIM
/*these are the distances from the origin, corresponding to the lower and
high bounds of the q-ring for azimuthal averaging.*/
rmin=tan(asin(par1*HC/energy/4/M_PI)*2)*dist;
rmax=tan(asin(par2*HC/energy/4/M_PI)*2)*dist;
/*limits for TEST2*/
dmax2=pow(rmax+avgresol,2);
dmin2=pow(((rmin-avgresol)<0)?0:(rmin-avgresol),2);
phi0=mxMalloc(sizeof(double)*Noutput);
dphi=mxMalloc(sizeof(double)*Noutput);
#else /*RAD or SECTOR*/
rmin=mxMalloc(sizeof(double)*Noutput);
rmax=mxMalloc(sizeof(double)*Noutput);
dmax2=mxMalloc(sizeof(double)*Noutput);
dmin2=mxMalloc(sizeof(double)*Noutput);
# if INT_MODE==SECTOR
phi0=par1; /*these are now in radians*/
dphi=par2;
# endif /*INT_MODE==SECTOR*/
#endif /*INT_MODE==ASIM*/
for (i=0; i<Noutput; i++) /*this loop cleans all the q-bins and
initializes rmin,rmax or phi0,dphi
depending on the mode. The
initialization of those array seems to
be a better idea than calculating these
values in every q-bin, as this is done
once, that would be done about M*N
times. Think about a 1024x1024 matrix...*/
{
double q1,q2; /*the left and right hand side of the i-th q-bin.*/
A[i]=0; /*clear the bin*/
I[i]=0;
E[i]=0;
/********************************************************************
* Bin borders (marked by "x"):
*
* x x x x x x
* | | | | | |
* q[0] q[1] q[2] q[3] ... q[Noutput-2] q[Noutput-1]
*
*
********************************************************************/
if (i>0) /*if this is not the first bin*/
q1=0.5*(q[i-1]+q[i]);
else /*if this is the first*/
q1=q[0];
if (i<(Noutput-1)) /*if this is not the last bin*/
q2=0.5*(q[i]+q[i+1]);
else /*if this is the last bin*/
q2=q[Noutput-1];
#if INT_MODE==ASIM
/*the starting angle and the length of the bin. Note, that
for angles, not the min-max interval method is used, as for q-s,
as problems can occur around 2*pi.*/
phi0[i]=q1;
dphi[i]=fmod((q2-q1)+10*M_PI,2*M_PI);
/*rmin, rmax, dmax2 and dmin2 are already defined, and they are SCALARS.*/
#else /*RADINT and SECTORINT*/
/*find the minimal and the maximal radius for the given q-bin.
For ASIMINT, it was done outside the q-loop*/
/*phi0 and dphi is already defined for SECTORINT, as scalars. They are
not used for RADINT*/
rmin[i]=tan(asin(q1*HC/energy/4/M_PI)*2)*dist;
rmax[i]=tan(asin(q2*HC/energy/4/M_PI)*2)*dist;
dmax2[i]=(rmax[i]+avgresol)*(rmax[i]+avgresol);
dmin2[i]=(((rmin[i]-avgresol)<0)?0:(rmin[i]-avgresol))*
(((rmin[i]-avgresol)<0)?0:(rmin[i]-avgresol));
#endif
}
/*go through the pixels.*/
for(j=0; j<M; j++) /*rows*/
for(k=0; k<N; k++) /*columns*/
{
double d1,d2,d3,d4; /*these contain the SQUARES of the distances of
the corners of the current pixel, from the
origin*/
#if INT_MODE==ASIM || INT_MODE==SECTOR
double phi1,phi2,phi3,phi4; /*these contain the polar angles of the
corners of the current pixel, with
respect to angle 0 (axis x, which is
downwards).*/
#endif
unsigned long proportion_added=0; /*this counts how much
sub-pixels were
distributed to q-bins. If
this reaches
NSUBDIVX*NSUBDIVY, the
q-loop is terminated.*/
/*the next two arrays are used in the subpixel loops.*/
double rr[NSUBDIVX*NSUBDIVY]; /*radius to the second for each
sub-pixel*/
double phi[NSUBDIVX*NSUBDIVY]; /*polar angle for each sub-pixel*/
/*TASK #1: eliminate unneeded pixels.*/
/*the current pixel is data[k*M+j] as Octave and Matlab
represents the matrices column-wise, as Fortran.*/
if (mask[k*M+j]!=0) /*if the pixel is masked, continue with
the next one.*/
continue;
/*if the pixel data or error is NaN or infinite, disregard
that pixel as well.*/
if (!mxIsFinite(data[k*M+j]) || !mxIsFinite(dataerr[k*M+j]))
continue;
/*TASK #2: calculate pixel coordinates and all the data which
have to be calculated here, not inside the q-loop.*/
/*now calculate the coordinates. All coordinates are
relative to the beam-center!*/
xc=(j-(bcx-1))*xresol; /*the coordinates of the center of
the pixel.*/
yc=(k-(bcy-1))*yresol;
/*the coordinates of the corners*/
x0=xc-xresol*0.5; y0=yc-yresol*0.5;
x1=xc+xresol*0.5; y1=yc+yresol*0.5;
/*the squared distances of the corners from the origin.*/
d1=x0*x0+y0*y0; d2=x0*x0+y1*y1;
d3=x1*x1+y1*y1; d4=x1*x1+y0*y0;
#if (INT_MODE==SECTOR) || (INT_MODE==ASIM)
/*the polar angles of the corners, with respect to the +x axis*/
phi1=atan2(y0,x0); phi2=atan2(y1,x0);
phi3=atan2(y1,x1); phi4=atan2(y0,x1);
#endif
/*TASK #3: throw away pixels which for sure fall outside the
integration range. This means the sector for SECTORINT
(defined by phi0 and dphi) and the ring for ASIMINT (given
by rmin and rmax).*/
#if (INT_MODE==SECTOR)
/*for sector integration only, because for azimuthal integration,
this is carried out inside the a-loop.*/
/*TEST 3: For sector and asimuthal averaging methods. If neither
of the lines defining the sector intersects none of the edges
of the pixel, ignore the pixel. For an edge and a line, the
condition of intersection is when the following equation is
true for the angle of the line and one end of the edge (a1),
the angle of the line and the other end of the edge (a2) and
the angle of the two ends of the edge (b):
a1<b and a2<b
By "angle of two objects" I mean the smallest possible
positive angle between 0 and pi/2.*/
if (!test3intersect(phi1,phi2,phi0) &&
!test3intersect(phi2,phi3,phi0) &&
!test3intersect(phi3,phi4,phi0) &&
!test3intersect(phi4,phi1,phi0) &&
!test3intersect(phi1,phi2,phi0+dphi) &&
!test3intersect(phi2,phi3,phi0+dphi) &&
!test3intersect(phi3,phi4,phi0+dphi) &&
!test3intersect(phi4,phi1,phi0+dphi))
continue;
#elif (INT_MODE==ASIM)
/*For ASIMINT, we already have all the parameters needed to perform
TEST1 and TEST2. */
/*TEST 1: Eliminate pixels, for which the distance of all
four corners from the origin is greater than
rmax+sqrt(xresol^2+yresol^2). Thus these pixels fall entirely
outside the given q-range.
*/
if (d1>dmax2 && d2>dmax2 && d3>dmax2 && d4>dmax2)
continue;
/*TEST 2: The same as above, except for rmin.*/
if (d1<dmin2 && d2<dmin2 && d3<dmin2 && d4<dmin2)
continue;
#endif
/*TASK #4: if we reached here, we should try to fit the pixel
into q-ranges.*/
/*TASK #4a: calculate rr and phi values for each sub-pixel.*/
n=0; /*this indexes the rr and phi arrays*/
for (x=x0+0.5*xresol/(double)NSUBDIVX; x<x1;
x+=xresol/(double)NSUBDIVX)
for (y=y0+0.5*yresol/(double)NSUBDIVY; y<y1;
y+=yresol/(double)NSUBDIVY)
{
rr[n]=x*x+y*y; /*the squared distance of the
subdivision point from the
beam-center*/
phi[n]=atan2(y,x); /*the angle of the subdivision
point. 0 is the +x axis (pointing
down), positive
counterclockwise. This phi is in
[-pi,pi]*/
n++;
}
proportion_added=0;
for (i=0;
((i<Noutput) && (proportion_added<NSUBDIVX*NSUBDIVY));
i++) /*this is the q-loop, which goes through all points
of the independent value (q or angle). The
independent value is called "q", even for
asimuthal integration. Thus we can use the same
lines of code.*/
{
unsigned long l; /*loop variable*/
/*TASK #4b: in the current q-bin, we can eliminate other
pixels, which were not eliminated before this.*/
#if INT_MODE==RAD || INT_MODE==SECTOR
/*We perform TEST1 and TEST2 ONLY FOR RADINT and SECTORINT!
Because for ASIMINT, it is done outside the q-loop.*/
/*TEST 1: Eliminate pixels, for which the distance of all
four corners from the origin is greater than
rmax+sqrt(xresol^2+yresol^2). Thus these pixels fall entirely
outside the given q-range.
*/
if (d1>dmax2[i] && d2>dmax2[i] && d3>dmax2[i] && d4>dmax2[i])
continue; /*next q-bin*/
/*TEST 2: The same as above, except for rmin.*/
if (d1<dmin2[i] && d2<dmin2[i] && d3<dmin2[i] && d4<dmin2[i])
continue; /*next q-bin*/
#elif (INT_MODE==ASIM)
/*for azimuthal integration only, because for sector integration,
this has already been carried out outside the q-loop.*/
/*TEST 3: For sector and asimuthal averaging methods. If neither
of the lines defining the sector intersects none of the edges
of the pixel, ignore the pixel. For an edge and a line, the
condition of intersection is when the following equation is
true for the angle of the line and one end of the edge (a1),
the angle of the line and the other end of the edge (a2) and
the angle of the two ends of the edge (b):
a1<b and a2<b
By "angle of two objects" I mean the smallest possible
positive angle between 0 and pi/2.*/
if (!test3intersect(phi1,phi2,phi0[i]) &&
!test3intersect(phi2,phi3,phi0[i]) &&
!test3intersect(phi3,phi4,phi0[i]) &&
!test3intersect(phi4,phi1,phi0[i]) &&
!test3intersect(phi1,phi2,phi0[i]+dphi[i]) &&
!test3intersect(phi2,phi3,phi0[i]+dphi[i]) &&
!test3intersect(phi3,phi4,phi0[i]+dphi[i]) &&
!test3intersect(phi4,phi1,phi0[i]+dphi[i]))
continue; /*next q-bin*/
#endif
/*TASK #4c: subdividing.*/
n=0; /*this will contain the number of hits, ie. the number of
subpixels falling into the current q-bin*/
for (l=0; l<NSUBDIVX*NSUBDIVY; l++)
{
/*now decide if hit or not. The subdivision point
should be counted, if it is between the two
circles and:
1. for RADINT, no other properties needed
2. in SECTORINT and ASIMINT modes, the angle
difference of the point (phi) and the beginning of
the sector (phi0) should be less than dphi.*/
#if INT_MODE==RAD
if (rr[l]>rmin[i]*rmin[i] && rr[l]<=rmax[i]*rmax[i])
n++;
#elif INT_MODE==SECTOR
if ((rr[l]>rmin[i]*rmin[i] && rr[l]<=rmax[i]*rmax[i]) &&
(fmod(phi[l]-phi0+10*M_PI,2*M_PI)<dphi))
n++;
#elif INT_MODE==ASIM
if ((rr[l]>rmin*rmin && rr[l]<=rmax*rmax) &&
(fmod(phi[l]-phi0[i]+10*M_PI,2*M_PI)<dphi[i]))
n++;
#endif
}
proportion_added+=n;
/*TASK #4d: add the intensity to the bin*/
p=n/(double)(NSUBDIVX*NSUBDIVY); /*this is the proportion
of the intensity in the
current pixel, which
should be added to the
current bin.*/
A[i]+=p; /*count the area. Its
dimension is pixels.*/
I[i]+=data[k*M+j]*p; /*add that part of intensity*/
/*the squares of the errors are summed. At the end, a
square-root will be taken.*/
E[i]+=(dataerr[k*M+j]*p)*(dataerr[k*M+j]*p);
} /*end of q-loop*/
/*next pixel*/
} /*end of double loop for pixels*/
/*TASK #5: post-processing of the averaged data: normalizing by
effective area*/
for (i=0; i<Noutput; i++)
{
if (A[i]!=0) /*if the area for this q is not zero, divide, thus
making an average from the integral.*/
{
I[i]/=A[i];
/* E[i]/=A[i]*A[i];*/ /*eliminated on suggestion of Ulla Vainio*/
/* E[i]=sqrt(E[i]);*/
E[i]=sqrt(E[i])/A[i]; /*this is definitely faster. Thanks.*/
}
}
/*TASK #6: cleanup*/
#if INT_MODE==ASIM
mxFree(phi0);
mxFree(dphi);
#else /*RAD or SECTOR*/
mxFree(rmin);
mxFree(rmax);
mxFree(dmax2);
mxFree(dmin2);
#endif
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
MAPTYPE *map, *get_maps();
mwSize m,n,k;
mwIndex i;
int nn, E[3], F[3], P, C;
double *R, r[3], *img;
int *curr, *prev, *tmpp;
if (nrhs != 2 || nlhs > 1)
{
mexErrMsgTxt("Incorrect usage.");
}
map = get_maps(prhs[0], &nn);
if (nn!=1)
{
free_maps(map, nn);
mexErrMsgTxt("Bad image handle dimensions.");
}
if (!mxIsNumeric(prhs[1]) || mxIsComplex(prhs[1]) || !mxIsDouble(prhs[1]))
{
free_maps(map, 1);
mexErrMsgTxt("Second argument must be numeric, real, full and double.");
}
if (mxGetM(prhs[1])*mxGetN(prhs[1]) != 3)
{
free_maps(map, nn);
mexErrMsgTxt("Second argument must contain three elements.");
}
r[0] = 1.0/mxGetPr(prhs[1])[0];
r[1] = 1.0/mxGetPr(prhs[1])[1];
r[2] = 1.0/mxGetPr(prhs[1])[2];
plhs[0] = mxCreateDoubleMatrix(4,1,mxREAL);
R = mxGetPr(plhs[0]);
m = map->dim[0];
n = map->dim[1];
k = map->dim[2];
curr = (int *)mxCalloc((m+1)*(n+1),sizeof(int)); /* current plane */
prev = (int *)mxCalloc((m+1)*(n+1),sizeof(int)); /* previous plane */
img = (double *)mxCalloc((m+1)*(n+1),sizeof(double));
P = C = E[0] = E[1] = E[2] = F[0] = F[1] = F[2] = 0;
for(i=0; i<k; i++)
{
int i1, j1;
static double mat[16] = {
1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0};
mat[14] = i+1.0;
slice(mat, img, m, n, map, 0, 0);
for(i1=0;i1<m; i1++)
for(j1=0; j1<n; j1++)
if (mxIsFinite(img[i1+m*j1]) && img[i1+m*j1])
curr[i1+1+(m+1)*(j1+1)] = 1;
else
curr[i1+1+(m+1)*(j1+1)] = 0;
/* count edges, faces etc */
resel_fun(curr,prev,m+1,n+1, &P, E, F, &C);
/* make current plane previous */
tmpp = prev; prev = curr; curr = tmpp;
}
(void)mxFree((char *)curr);
(void)mxFree((char *)prev);
(void)mxFree((char *)img);
free_maps(map, 1);
R[0] = P - (E[0]+E[1]+E[2])+(F[0]+F[1]+F[2])-C;
R[1] = (E[0]-F[0]-F[1]+C)*r[0] + (E[1]-F[0]-F[2]+C)*r[1] + (E[2]-F[1]-F[2]+C)*r[2];
R[2] = (F[0]-C)*r[0]*r[1] + (F[1]-C)*r[0]*r[2] + (F[2]-C)*r[1]*r[2];
R[3] = C*r[0]*r[1]*r[2];
}
