void mexFunction
(
int nargout,
mxArray *pargout [ ],
int nargin,
const mxArray *pargin [ ]
)
{
Long i, m, n, *Ap, *Ai, *P, nc, result, spumoni, full, *C, Clen ;
double *Pout, *InfoOut, Control [CAMD_CONTROL], Info [CAMD_INFO],
*ControlIn, *Cin ;
mxArray *A ;
/* --------------------------------------------------------------------- */
/* get control parameters */
/* --------------------------------------------------------------------- */
camd_malloc = mxMalloc ;
camd_free = mxFree ;
camd_calloc = mxCalloc ;
camd_realloc = mxRealloc ;
camd_printf = mexPrintf ;
spumoni = 0 ;
if (nargin == 0)
{
/* get the default control parameters, and return */
pargout [0] = mxCreateDoubleMatrix (CAMD_CONTROL, 1, mxREAL) ;
camd_l_defaults (mxGetPr (pargout [0])) ;
if (nargout == 0)
{
camd_l_control (mxGetPr (pargout [0])) ;
}
return ;
}
camd_l_defaults (Control) ;
if (nargin > 1)
{
ControlIn = mxGetPr (pargin [1]) ;
nc = mxGetM (pargin [1]) * mxGetN (pargin [1]) ;
Control [CAMD_DENSE]
= (nc > 0) ? ControlIn [CAMD_DENSE] : CAMD_DEFAULT_DENSE ;
Control [CAMD_AGGRESSIVE]
= (nc > 1) ? ControlIn [CAMD_AGGRESSIVE] : CAMD_DEFAULT_AGGRESSIVE ;
spumoni = (nc > 2) ? (ControlIn [2] != 0) : 0 ;
}
if (spumoni > 0)
{
camd_l_control (Control) ;
}
/* --------------------------------------------------------------------- */
/* get inputs */
/* --------------------------------------------------------------------- */
if (nargout > 2 || nargin > 3)
{
mexErrMsgTxt ("Usage: p = camd (A)\n"
"or [p, Info] = camd (A, Control, C)") ;
}
Clen = 0 ;
C = NULL ;
if (nargin > 2)
{
Cin = mxGetPr (pargin [2]) ;
Clen = mxGetNumberOfElements (pargin [2]) ;
if (Clen != 0)
{
C = (Long *) mxCalloc (Clen, sizeof (Long)) ;
for (i = 0 ; i < Clen ; i++)
{
/* convert c from 1-based to 0-based */
C [i] = (Long) Cin [i] - 1 ;
}
}
}
A = (mxArray *) pargin [0] ;
n = mxGetN (A) ;
m = mxGetM (A) ;
if (spumoni > 0)
{
mexPrintf (" input matrix A is %d-by-%d\n", m, n) ;
}
if (mxGetNumberOfDimensions (A) != 2)
{
mexErrMsgTxt ("camd: A must be 2-dimensional") ;
}
if (m != n)
{
mexErrMsgTxt ("camd: A must be square") ;
}
/* --------------------------------------------------------------------- */
/* allocate workspace for output permutation */
/* --------------------------------------------------------------------- */
P = mxMalloc ((n+1) * sizeof (Long)) ;
/* --------------------------------------------------------------------- */
/* if A is full, convert to a sparse matrix */
/* --------------------------------------------------------------------- */
full = !mxIsSparse (A) ;
if (full)
{
if (spumoni > 0)
{
mexPrintf (
" input matrix A is full (sparse copy of A will be created)\n");
}
mexCallMATLAB (1, &A, 1, (mxArray **) pargin, "sparse") ;
}
Ap = (Long *) mxGetJc (A) ;
Ai = (Long *) mxGetIr (A) ;
if (spumoni > 0)
{
mexPrintf (" input matrix A has %d nonzero entries\n", Ap [n]) ;
}
/* --------------------------------------------------------------------- */
/* order the matrix */
/* --------------------------------------------------------------------- */
result = camd_l_order (n, Ap, Ai, P, Control, Info, C) ;
/* --------------------------------------------------------------------- */
/* if A is full, free the sparse copy of A */
/* --------------------------------------------------------------------- */
if (full)
{
mxDestroyArray (A) ;
}
/* --------------------------------------------------------------------- */
/* print results (including return value) */
/* --------------------------------------------------------------------- */
if (spumoni > 0)
{
camd_l_info (Info) ;
}
/* --------------------------------------------------------------------- */
/* check error conditions */
/* --------------------------------------------------------------------- */
if (result == CAMD_OUT_OF_MEMORY)
{
mexErrMsgTxt ("camd: out of memory") ;
}
else if (result == CAMD_INVALID)
{
mexErrMsgTxt ("camd: input matrix A is corrupted") ;
}
/* --------------------------------------------------------------------- */
/* copy the outputs to MATLAB */
/* --------------------------------------------------------------------- */
/* output permutation, P */
pargout [0] = mxCreateDoubleMatrix (1, n, mxREAL) ;
Pout = mxGetPr (pargout [0]) ;
for (i = 0 ; i < n ; i++)
{
Pout [i] = P [i] + 1 ; /* change to 1-based indexing for MATLAB */
}
mxFree (P) ;
if (nargin > 2) mxFree (C) ;
/* Info */
if (nargout > 1)
{
pargout [1] = mxCreateDoubleMatrix (CAMD_INFO, 1, mxREAL) ;
InfoOut = mxGetPr (pargout [1]) ;
for (i = 0 ; i < CAMD_INFO ; i++)
{
InfoOut [i] = Info [i] ;
}
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
double *data;
int c=0;
mxArray *mxout, *mxin, *mxstr;
double *out;
char *str;
double *yfit;
double median_tc,mad_res;
double mad_res_array;
struct fminestimate *estimate
data=mxGetPr(prhs[0]);
nTp=mxGetM(data);
nVox=mxGetN(data);
mxout=mxCreateDoubleMatrix(1,1,mxREAL);
out=mxGetPr(mxout);
mxstr=mxCreateString("double");
mexCallMatlab(1,mxout,0,mxstr,'REALMIN');
REALMIN=*(mxGetPr(mxout));
mxDestroyArray(mxout);
mxDestroyArray(mxstr);
yfit=malloc(sizeof(double)*nTp);
tc=malloc(sizeof(double)*nTp);
mad_res_array=malloc(sizeof(yfit));
for(c=0;c<nVox;++c)
{
for(i=0;i<nTp;++i)
*(tc+i)=*(data+nTp*c+i);
ylerr=
icatb_fun(xestimate, &yqerr, tc, TR);
icatb_fun(xestimates,&yserr, tc, TR);
//
yerr=ylerr;
miny=1;
if(yerr>yqerr)
miny=2;
if(yerr>yserr)
miny=3;
//
if(miny==1)
yfit=[];
else if(miny==2)
icatb_quadFit(yfit,coff,nTp,TR);
else if(miny==3)
icatb_splFit(yfit,coff,nTp,TR);
// residue
for(i=0;i<nTp;++i)
*(tc+i)-=*(yfit+i);
quickSort(tc,rawOrder,0,nTp-1);
median_tc=median(tc);
for(i=0;i<nTp;++i)
*(mad_res_array+i)=abs(*(tc+i)-median_tc);
mad_res=median(mad_res_array);
sigma=mad_res*pow(PI/2.0,0.5);
for(i=0;i<nTp;++i)
*(tc+i)=*(tc+i)/sigma;
for(t=0;t<nTp;++t)
if(abs(*(tc+t))>c1)
{
*(Data+nTp*c+t)=sign(*(tc+t)) * (c1+((c2-c1) * tanh((abs(*(tc+t)))-c1) / (c2-c1)));
*(Data+nTp*c+t)=*(tc+t)*sigma + *(yfit+t);
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
int i, j;
mxArray *xm, *cell, *xm_cell;
double *src;
double *dest;
double *dest_cell;
int valid_vars;
int steps;
int this_var_errors;
int warned_diags;
int prepare_for_c = 0;
int extra_solves;
const char *status_names[] = {"optval", "gap", "steps", "converged"};
mwSize dims1x1of1[1] = {1};
mwSize dims[1];
const char *var_names[] = {"u_0", "u_1", "u_2", "u_3", "u_4", "u_5", "u_6", "u_7", "u_8", "u_9", "x_1", "x_2", "x_3", "x_4", "x_5", "x_6", "x_7", "x_8", "x_9", "x_10", "u", "x"};
const int num_var_names = 22;
/* Avoid compiler warnings of unused variables by using a dummy assignment. */
warned_diags = j = 0;
extra_solves = 0;
set_defaults();
/* Check we got the right number of arguments. */
if (nrhs == 0)
mexErrMsgTxt("Not enough arguments: You need to specify at least the parameters.\n");
if (nrhs > 1) {
/* Assume that the second argument is the settings. */
if (mxGetField(prhs[1], 0, "eps") != NULL)
settings.eps = *mxGetPr(mxGetField(prhs[1], 0, "eps"));
if (mxGetField(prhs[1], 0, "max_iters") != NULL)
settings.max_iters = *mxGetPr(mxGetField(prhs[1], 0, "max_iters"));
if (mxGetField(prhs[1], 0, "refine_steps") != NULL)
settings.refine_steps = *mxGetPr(mxGetField(prhs[1], 0, "refine_steps"));
if (mxGetField(prhs[1], 0, "verbose") != NULL)
settings.verbose = *mxGetPr(mxGetField(prhs[1], 0, "verbose"));
if (mxGetField(prhs[1], 0, "better_start") != NULL)
settings.better_start = *mxGetPr(mxGetField(prhs[1], 0, "better_start"));
if (mxGetField(prhs[1], 0, "verbose_refinement") != NULL)
settings.verbose_refinement = *mxGetPr(mxGetField(prhs[1], 0,
"verbose_refinement"));
if (mxGetField(prhs[1], 0, "debug") != NULL)
settings.debug = *mxGetPr(mxGetField(prhs[1], 0, "debug"));
if (mxGetField(prhs[1], 0, "kkt_reg") != NULL)
settings.kkt_reg = *mxGetPr(mxGetField(prhs[1], 0, "kkt_reg"));
if (mxGetField(prhs[1], 0, "s_init") != NULL)
settings.s_init = *mxGetPr(mxGetField(prhs[1], 0, "s_init"));
if (mxGetField(prhs[1], 0, "z_init") != NULL)
settings.z_init = *mxGetPr(mxGetField(prhs[1], 0, "z_init"));
if (mxGetField(prhs[1], 0, "resid_tol") != NULL)
settings.resid_tol = *mxGetPr(mxGetField(prhs[1], 0, "resid_tol"));
if (mxGetField(prhs[1], 0, "extra_solves") != NULL)
extra_solves = *mxGetPr(mxGetField(prhs[1], 0, "extra_solves"));
else
extra_solves = 0;
if (mxGetField(prhs[1], 0, "prepare_for_c") != NULL)
prepare_for_c = *mxGetPr(mxGetField(prhs[1], 0, "prepare_for_c"));
}
valid_vars = 0;
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "A");
if (xm == NULL) {
printf("could not find params.A.\n");
} else {
if (!((mxGetM(xm) == 3) && (mxGetN(xm) == 3))) {
printf("A must be size (3,3), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter A must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter A must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter A must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.A;
src = mxGetPr(xm);
for (i = 0; i < 9; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "B");
if (xm == NULL) {
printf("could not find params.B.\n");
} else {
if (!((mxGetM(xm) == 3) && (mxGetN(xm) == 1))) {
printf("B must be size (3,1), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter B must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter B must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter B must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.B;
src = mxGetPr(xm);
for (i = 0; i < 3; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "Ff");
if (xm == NULL) {
printf("could not find params.Ff.\n");
} else {
if (!((mxGetM(xm) == 2) && (mxGetN(xm) == 3))) {
printf("Ff must be size (2,3), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter Ff must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter Ff must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter Ff must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.Ff;
src = mxGetPr(xm);
for (i = 0; i < 6; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "Fu");
if (xm == NULL) {
printf("could not find params.Fu.\n");
} else {
if (!((mxGetM(xm) == 4) && (mxGetN(xm) == 1))) {
printf("Fu must be size (4,1), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter Fu must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter Fu must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter Fu must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.Fu;
src = mxGetPr(xm);
for (i = 0; i < 4; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "Fus");
if (xm == NULL) {
printf("could not find params.Fus.\n");
} else {
if (!((mxGetM(xm) == 2) && (mxGetN(xm) == 1))) {
printf("Fus must be size (2,1), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter Fus must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter Fus must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter Fus must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.Fus;
src = mxGetPr(xm);
for (i = 0; i < 2; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "Fx");
if (xm == NULL) {
printf("could not find params.Fx.\n");
} else {
if (!((mxGetM(xm) == 4) && (mxGetN(xm) == 3))) {
printf("Fx must be size (4,3), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter Fx must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter Fx must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter Fx must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.Fx;
src = mxGetPr(xm);
for (i = 0; i < 12; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "Fxs");
if (xm == NULL) {
printf("could not find params.Fxs.\n");
} else {
if (!((mxGetM(xm) == 2) && (mxGetN(xm) == 3))) {
printf("Fxs must be size (2,3), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter Fxs must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter Fxs must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter Fxs must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.Fxs;
src = mxGetPr(xm);
for (i = 0; i < 6; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "Q");
if (xm == NULL) {
printf("could not find params.Q.\n");
} else {
if (!((mxGetM(xm) == 3) && (mxGetN(xm) == 3))) {
printf("Q must be size (3,3), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter Q must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter Q must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter Q must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.Q;
src = mxGetPr(xm);
for (i = 0; i < 9; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "Q_final");
if (xm == NULL) {
printf("could not find params.Q_final.\n");
} else {
if (!((mxGetM(xm) == 3) && (mxGetN(xm) == 3))) {
printf("Q_final must be size (3,3), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter Q_final must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter Q_final must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter Q_final must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.Q_final;
src = mxGetPr(xm);
for (i = 0; i < 9; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "R");
if (xm == NULL) {
printf("could not find params.R.\n");
} else {
if (!((mxGetM(xm) == 1) && (mxGetN(xm) == 1))) {
printf("R must be size (1,1), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter R must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter R must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter R must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.R;
src = mxGetPr(xm);
for (i = 0; i < 1; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "d");
if (xm == NULL) {
printf("could not find params.d.\n");
} else {
if (!((mxGetM(xm) == 3) && (mxGetN(xm) == 1))) {
printf("d must be size (3,1), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter d must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter d must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter d must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.d;
src = mxGetPr(xm);
for (i = 0; i < 3; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "f");
if (xm == NULL) {
printf("could not find params.f.\n");
} else {
if (!((mxGetM(xm) == 4) && (mxGetN(xm) == 1))) {
printf("f must be size (4,1), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter f must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter f must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter f must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.f;
src = mxGetPr(xm);
for (i = 0; i < 4; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "ff");
if (xm == NULL) {
printf("could not find params.ff.\n");
} else {
if (!((mxGetM(xm) == 2) && (mxGetN(xm) == 1))) {
printf("ff must be size (2,1), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter ff must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter ff must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter ff must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.ff;
src = mxGetPr(xm);
for (i = 0; i < 2; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "fs");
if (xm == NULL) {
printf("could not find params.fs.\n");
} else {
if (!((mxGetM(xm) == 2) && (mxGetN(xm) == 1))) {
printf("fs must be size (2,1), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter fs must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter fs must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter fs must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.fs;
src = mxGetPr(xm);
for (i = 0; i < 2; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "x_0");
if (xm == NULL) {
printf("could not find params.x_0.\n");
} else {
if (!((mxGetM(xm) == 3) && (mxGetN(xm) == 1))) {
printf("x_0 must be size (3,1), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter x_0 must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter x_0 must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter x_0 must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.x_0;
src = mxGetPr(xm);
for (i = 0; i < 3; i++)
*dest++ = *src++;
valid_vars++;
}
}
this_var_errors = 0;
xm = mxGetField(prhs[0], 0, "xt");
if (xm == NULL) {
printf("could not find params.xt.\n");
} else {
if (!((mxGetM(xm) == 3) && (mxGetN(xm) == 1))) {
printf("xt must be size (3,1), not (%d,%d).\n", mxGetM(xm), mxGetN(xm));
this_var_errors++;
}
if (mxIsComplex(xm)) {
printf("parameter xt must be real.\n");
this_var_errors++;
}
if (!mxIsClass(xm, "double")) {
printf("parameter xt must be a full matrix of doubles.\n");
this_var_errors++;
}
if (mxIsSparse(xm)) {
printf("parameter xt must be a full matrix.\n");
this_var_errors++;
}
if (this_var_errors == 0) {
dest = params.xt;
src = mxGetPr(xm);
for (i = 0; i < 3; i++)
*dest++ = *src++;
valid_vars++;
}
}
if (valid_vars != 16) {
printf("Error: %d parameters are invalid.\n", 16 - valid_vars);
mexErrMsgTxt("invalid parameters found.");
}
if (prepare_for_c) {
printf("settings.prepare_for_c == 1. thus, outputting for C.\n");
for (i = 0; i < 3; i++)
printf(" params.x_0[%d] = %.6g;\n", i, params.x_0[i]);
for (i = 0; i < 3; i++)
printf(" params.xt[%d] = %.6g;\n", i, params.xt[i]);
for (i = 0; i < 9; i++)
printf(" params.Q[%d] = %.6g;\n", i, params.Q[i]);
for (i = 0; i < 1; i++)
printf(" params.R[%d] = %.6g;\n", i, params.R[i]);
for (i = 0; i < 6; i++)
printf(" params.Fxs[%d] = %.6g;\n", i, params.Fxs[i]);
for (i = 0; i < 2; i++)
printf(" params.Fus[%d] = %.6g;\n", i, params.Fus[i]);
for (i = 0; i < 2; i++)
printf(" params.fs[%d] = %.6g;\n", i, params.fs[i]);
for (i = 0; i < 9; i++)
printf(" params.Q_final[%d] = %.6g;\n", i, params.Q_final[i]);
for (i = 0; i < 9; i++)
printf(" params.A[%d] = %.6g;\n", i, params.A[i]);
for (i = 0; i < 3; i++)
printf(" params.B[%d] = %.6g;\n", i, params.B[i]);
for (i = 0; i < 3; i++)
printf(" params.d[%d] = %.6g;\n", i, params.d[i]);
for (i = 0; i < 12; i++)
printf(" params.Fx[%d] = %.6g;\n", i, params.Fx[i]);
for (i = 0; i < 4; i++)
printf(" params.Fu[%d] = %.6g;\n", i, params.Fu[i]);
for (i = 0; i < 4; i++)
printf(" params.f[%d] = %.6g;\n", i, params.f[i]);
for (i = 0; i < 6; i++)
printf(" params.Ff[%d] = %.6g;\n", i, params.Ff[i]);
for (i = 0; i < 2; i++)
printf(" params.ff[%d] = %.6g;\n", i, params.ff[i]);
}
/* Perform the actual solve in here. */
steps = solve();
/* For profiling purposes, allow extra silent solves if desired. */
settings.verbose = 0;
for (i = 0; i < extra_solves; i++)
solve();
/* Update the status variables. */
plhs[1] = mxCreateStructArray(1, dims1x1of1, 4, status_names);
xm = mxCreateDoubleMatrix(1, 1, mxREAL);
mxSetField(plhs[1], 0, "optval", xm);
*mxGetPr(xm) = work.optval;
xm = mxCreateDoubleMatrix(1, 1, mxREAL);
mxSetField(plhs[1], 0, "gap", xm);
*mxGetPr(xm) = work.gap;
xm = mxCreateDoubleMatrix(1, 1, mxREAL);
mxSetField(plhs[1], 0, "steps", xm);
*mxGetPr(xm) = steps;
xm = mxCreateDoubleMatrix(1, 1, mxREAL);
mxSetField(plhs[1], 0, "converged", xm);
*mxGetPr(xm) = work.converged;
/* Extract variable values. */
plhs[0] = mxCreateStructArray(1, dims1x1of1, num_var_names, var_names);
/* Create cell arrays for indexed variables. */
dims[0] = 9;
cell = mxCreateCellArray(1, dims);
mxSetField(plhs[0], 0, "u", cell);
dims[0] = 10;
cell = mxCreateCellArray(1, dims);
mxSetField(plhs[0], 0, "x", cell);
xm = mxCreateDoubleMatrix(1, 1, mxREAL);
mxSetField(plhs[0], 0, "u_0", xm);
dest = mxGetPr(xm);
src = vars.u_0;
for (i = 0; i < 1; i++) {
*dest++ = *src++;
}
xm = mxCreateDoubleMatrix(1, 1, mxREAL);
mxSetField(plhs[0], 0, "u_1", xm);
xm_cell = mxCreateDoubleMatrix(1, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 0, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_1;
for (i = 0; i < 1; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(1, 1, mxREAL);
mxSetField(plhs[0], 0, "u_2", xm);
xm_cell = mxCreateDoubleMatrix(1, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 1, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_2;
for (i = 0; i < 1; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(1, 1, mxREAL);
mxSetField(plhs[0], 0, "u_3", xm);
xm_cell = mxCreateDoubleMatrix(1, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 2, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_3;
for (i = 0; i < 1; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(1, 1, mxREAL);
mxSetField(plhs[0], 0, "u_4", xm);
xm_cell = mxCreateDoubleMatrix(1, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 3, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_4;
for (i = 0; i < 1; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(1, 1, mxREAL);
mxSetField(plhs[0], 0, "u_5", xm);
xm_cell = mxCreateDoubleMatrix(1, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 4, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_5;
for (i = 0; i < 1; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(1, 1, mxREAL);
mxSetField(plhs[0], 0, "u_6", xm);
xm_cell = mxCreateDoubleMatrix(1, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 5, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_6;
for (i = 0; i < 1; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(1, 1, mxREAL);
mxSetField(plhs[0], 0, "u_7", xm);
xm_cell = mxCreateDoubleMatrix(1, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 6, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_7;
for (i = 0; i < 1; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(1, 1, mxREAL);
mxSetField(plhs[0], 0, "u_8", xm);
xm_cell = mxCreateDoubleMatrix(1, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 7, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_8;
for (i = 0; i < 1; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(1, 1, mxREAL);
mxSetField(plhs[0], 0, "u_9", xm);
xm_cell = mxCreateDoubleMatrix(1, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "u");
mxSetCell(cell, 8, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.u_9;
for (i = 0; i < 1; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(3, 1, mxREAL);
mxSetField(plhs[0], 0, "x_1", xm);
xm_cell = mxCreateDoubleMatrix(3, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 0, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_1;
for (i = 0; i < 3; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(3, 1, mxREAL);
mxSetField(plhs[0], 0, "x_2", xm);
xm_cell = mxCreateDoubleMatrix(3, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 1, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_2;
for (i = 0; i < 3; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(3, 1, mxREAL);
mxSetField(plhs[0], 0, "x_3", xm);
xm_cell = mxCreateDoubleMatrix(3, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 2, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_3;
for (i = 0; i < 3; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(3, 1, mxREAL);
mxSetField(plhs[0], 0, "x_4", xm);
xm_cell = mxCreateDoubleMatrix(3, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 3, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_4;
for (i = 0; i < 3; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(3, 1, mxREAL);
mxSetField(plhs[0], 0, "x_5", xm);
xm_cell = mxCreateDoubleMatrix(3, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 4, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_5;
for (i = 0; i < 3; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(3, 1, mxREAL);
mxSetField(plhs[0], 0, "x_6", xm);
xm_cell = mxCreateDoubleMatrix(3, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 5, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_6;
for (i = 0; i < 3; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(3, 1, mxREAL);
mxSetField(plhs[0], 0, "x_7", xm);
xm_cell = mxCreateDoubleMatrix(3, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 6, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_7;
for (i = 0; i < 3; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(3, 1, mxREAL);
mxSetField(plhs[0], 0, "x_8", xm);
xm_cell = mxCreateDoubleMatrix(3, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 7, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_8;
for (i = 0; i < 3; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(3, 1, mxREAL);
mxSetField(plhs[0], 0, "x_9", xm);
xm_cell = mxCreateDoubleMatrix(3, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 8, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_9;
for (i = 0; i < 3; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
xm = mxCreateDoubleMatrix(3, 1, mxREAL);
mxSetField(plhs[0], 0, "x_10", xm);
xm_cell = mxCreateDoubleMatrix(3, 1, mxREAL);
cell = mxGetField(plhs[0], 0, "x");
mxSetCell(cell, 9, xm_cell);
dest = mxGetPr(xm);
dest_cell = mxGetPr(xm_cell);
src = vars.x_10;
for (i = 0; i < 3; i++) {
*dest++ = *src;
*dest_cell++ = *src++;
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
double *ia, *ja, *C;
double *is2, *js2, *as2;
double *inbd, *jnbd, *anbd;
int m;
int path;
int *npath;
int nbdindex, nbdsize;
int *len2nbd, len2start;
int index = 0;
int i, j, k, jaj, jak,l;
int isinnbd;
if(nrhs!=4) {
mexErrMsgIdAndTxt("MATAMG:length2strong:nrhs", "LENGTH2STRONG_MEX(IA,JA,IS,JS,AS,C,PATH)");
}
ia = mxGetPr(prhs[0]);
ja = mxGetPr(prhs[1]);
C = mxGetPr(prhs[2]);
path =(int)mxGetScalar(prhs[3]);
m = mxGetN(prhs[2]);
npath = (int*)calloc(m,sizeof(int));
int maxnnzrow = 0;
for ( i = 1; i <= m; i++ ) {
if ((int)(ia[ i ] - ia[ i - 1 ]) > maxnnzrow )
maxnnzrow = (int)(ia[ i ] - ia[ i - 1 ]);
}
plhs[0] = mxCreateDoubleMatrix(maxnnzrow*maxnnzrow*m,1,mxREAL);
plhs[1] = mxCreateDoubleMatrix(maxnnzrow*maxnnzrow*m,1,mxREAL);
plhs[2] = mxCreateDoubleMatrix(maxnnzrow*maxnnzrow*m,1,mxREAL);
plhs[3] = mxCreateDoubleMatrix(1,1,mxREAL);
plhs[4] = mxCreateDoubleMatrix(m+1,1,mxREAL);
plhs[5] = mxCreateDoubleMatrix(maxnnzrow*maxnnzrow*m,1,mxREAL);
len2nbd = (int*)calloc(maxnnzrow*maxnnzrow,sizeof(int));
is2 = mxGetPr(plhs[0]);
js2 = mxGetPr(plhs[1]);
as2 = mxGetPr(plhs[2]);
inbd = mxGetPr(plhs[4]);
jnbd = mxGetPr(plhs[5]);
nbdindex = 0;
/* Compute length 2 connections */
inbd[ 0 ] = 1;
for ( i = 1; i <= m; i++ ) {
nbdsize = 0;
/* Check length 1 connections */
for (j = (int)ia[ i - 1 ]; j < (int)ia[ i ]; j++){
jaj = (int)ja[ j - 1];
if (jaj!=i){
len2nbd[nbdsize] = (int)ja[ j - 1 ];
nbdsize++;
}
}
len2start = nbdsize;
/* Check length 2 connections */
for (j = (int)ia[ i - 1 ]; j < (int)ia[ i ]; j++){
jaj = (int)ja[ j - 1 ];
if ( jaj != i ){
for ( k = (int)ia[ jaj - 1 ]; k < (int)ia[ jaj ] ; k++ ){
isinnbd = 0;
jak = (int)ja[ k - 1 ];
if (jak != i && jak != jaj){
for ( l = 0; l < nbdsize; l++ ) {
if ( len2nbd[ l ] == jak ){
isinnbd = 1;
break;
}
}
if (!isinnbd){
jnbd[ nbdindex ] = jak;
len2nbd[ nbdsize ] = jak;
nbdsize++;
nbdindex++;
}
}
}
}
}
inbd[ i ] = inbd[ i - 1 ] + nbdsize - len2start;
}
index = 0;
for ( i = 1; i <= m; i++ ) {
if ( C[ i - 1 ] ){
for ( j = (int)ia[ i - 1 ]; j < (int)ia[ i ]; j++ ) {
jaj = (int)ja[ j - 1 ];
if ( !C[ jaj - 1 ] ) {
/*mexPrintf(" %d is a F point nbd of %d\n", jaj, i);*/
for ( k = (int)ia [ jaj - 1 ]; k < (int)ia[ jaj ] ; k++ ){
jak = (int)ja[ k - 1 ];
isinnbd = 0;
for ( l = inbd[ i - 1 ]; l < inbd[ i ]; l++ ){
if (jnbd[ l - 1 ] == jak){
isinnbd = 1;
break;
}
}
if (isinnbd) {
if ( C [ jak - 1 ] && jak != i){
npath[ jak - 1 ]++;
}
}
}
}
}
for ( j = (int)ia[ i - 1 ]; j < (int)ia[ i ]; j++ ) {
jaj = (int)ja[ j - 1 ];
if ( !C[ jaj - 1 ] ) {
for ( k = (int)ia [ jaj - 1 ]; k < (int)ia[ jaj ] ; k++ ){
jak = (int)ja[ k - 1 ];
if ( C [ jak - 1 ] && jak != i && npath[ jak - 1 ] > 0 ){
if ( npath[ jak - 1 ] >= path){
is2[ index ] = i;
js2[ index ] = jak;
as2[ index ] = 1;
index++;
}
npath[ jak - 1 ] = 0.0;
}
}
}
}
}
}
free(npath);
free(len2nbd);
*mxGetPr(plhs[3])=(double)index;
return;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
double TT1, TT2, dX, dY, *xVec;
size_t numRow, numVec;
mxArray *retMat;
double *retData;
double GCRS2CIRS[3][3];
double CIRS2GCRS[3][3];
if(nrhs<3||nrhs>4){
mexErrMsgTxt("Wrong number of inputs");
}
if(nlhs>2) {
mexErrMsgTxt("Wrong number of outputs.");
}
checkRealDoubleArray(prhs[0]);
numRow = mxGetM(prhs[0]);
numVec = mxGetN(prhs[0]);
if(!(numRow==3||numRow==6)) {
mexErrMsgTxt("The input vector has a bad dimensionality.");
}
xVec=(double*)mxGetData(prhs[0]);
TT1=getDoubleFromMatlab(prhs[1]);
TT2=getDoubleFromMatlab(prhs[2]);
//If some values from the function getEOP will be needed.
if(nrhs<4||mxIsEmpty(prhs[3])) {
mxArray *retVals[2];
double *dXdY;
mxArray *JulUTCMATLAB[2];
double JulUTC[2];
int retVal;
//Get the time in UTC to look up the parameters by going to TAI and
//then UTC.
retVal=iauTttai(TT1, TT2, &JulUTC[0], &JulUTC[1]);
if(retVal!=0) {
mexErrMsgTxt("An error occurred computing TAI.");
}
retVal=iauTaiutc(JulUTC[0], JulUTC[1], &JulUTC[0], &JulUTC[1]);
switch(retVal){
case 1:
mexWarnMsgTxt("Dubious Date entered.");
break;
case -1:
mexErrMsgTxt("Unacceptable date entered");
break;
default:
break;
}
JulUTCMATLAB[0]=doubleMat2Matlab(&JulUTC[0],1,1);
JulUTCMATLAB[1]=doubleMat2Matlab(&JulUTC[1],1,1);
//Get the Earth orientation parameters for the given date.
mexCallMATLAB(2,retVals,2,JulUTCMATLAB,"getEOP");
mxDestroyArray(JulUTCMATLAB[0]);
mxDestroyArray(JulUTCMATLAB[1]);
//%We do not need the polar motion coordinates.
mxDestroyArray(retVals[0]);
checkRealDoubleArray(retVals[1]);
if(mxGetM(retVals[1])!=2||mxGetN(retVals[1])!=1) {
mxDestroyArray(retVals[1]);
mexErrMsgTxt("Error using the getEOP function.");
return;
}
dXdY=(double*)mxGetData(retVals[1]);
dX=dXdY[0];
dY=dXdY[1];
//Free the returned arrays.
mxDestroyArray(retVals[1]);
} else {//Get the celestial pole offsets
size_t dim1, dim2;
checkRealDoubleArray(prhs[4]);
dim1 = mxGetM(prhs[4]);
dim2 = mxGetN(prhs[4]);
if((dim1==2&&dim2==1)||(dim1==1&&dim2==2)) {
double *dXdY=(double*)mxGetData(prhs[4]);
dX=dXdY[0];
dY=dXdY[1];
} else {
mexErrMsgTxt("The celestial pole offsets have the wrong dimensionality.");
return;
}
}
{
double x, y, s;
//Get the X,Y coordinates of the Celestial Intermediate Pole (CIP) and
//the Celestial Intermediate Origin (CIO) locator s, using the IAU 2006
//precession and IAU 2000A nutation models.
iauXys06a(TT1, TT2, &x, &y, &s);
//Add the CIP offsets.
x += dX;
y += dY;
//Get the GCRS-to-CIRS matrix
iauC2ixys(x, y, s, GCRS2CIRS);
//To go from the CIRS to the GCRS, we need to use the inverse rotation
//matrix, which is just the transpose of the rotation matrix.
iauTr(GCRS2CIRS, CIRS2GCRS);
}
//Allocate space for the return vectors.
retMat=mxCreateDoubleMatrix(numRow,numVec,mxREAL);
retData=(double*)mxGetData(retMat);
{
size_t curVec;
for(curVec=0;curVec<numVec;curVec++) {
//Multiply the position vector with the rotation matrix.
iauRxp(CIRS2GCRS, xVec+numRow*curVec, retData+numRow*curVec);
//If a velocity vector was given.
if(numRow>3) {
double *velCIRS=xVec+numRow*curVec+3;//Velocity in CIRS
double *retDataVel=retData+numRow*curVec+3;
//Convert velocity from CIRS to GCRS.
iauRxp(CIRS2GCRS, velCIRS, retDataVel);
}
}
}
plhs[0]=retMat;
//If the rotation matrix is desired on the output.
if(nlhs>1) {
double *elPtr;
size_t i,j;
plhs[1]=mxCreateDoubleMatrix(3,3,mxREAL);
elPtr=(double*)mxGetData(plhs[1]);
for (i=0;i<3;i++) {
for(j=0;j<3;j++) {
elPtr[i+3*j]=CIRS2GCRS[i][j];
}
}
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
/* Declare variable */
int i,m,n,nzmax,newnnz,col,processed,passed;
int starting_row_index, current_row_index, stopping_row_index;
double *in_pr,*in_pi,*out_pr,*out_pi;
int *in_ir,*in_jc,*out_ir,*out_jc;
double thres;
/* Check for proper number of input and output arguments */
if ((nlhs != 1) || (nrhs != 2)){
mexErrMsgTxt("usage: SPMX = SPARSIFY(MX, THRES).");
}
/* if matrix is complex threshold the norm of the numbers */
if (mxIsComplex(prhs[0])){
/* Check data type of input argument */
if (mxIsSparse(prhs[0])){
/* read input */
in_pr = mxGetPr(prhs[0]);
in_pi = mxGetPi(prhs[0]);
in_ir = mxGetIr(prhs[0]);
in_jc = mxGetJc(prhs[0]);
nzmax = mxGetNzmax(prhs[0]);
m = mxGetM(prhs[0]);
n = mxGetN(prhs[0]);
thres = mxGetScalar(prhs[1]);
/* Count new nonzeros */
newnnz=0;
for(i=0; i<nzmax; i++){
if (sqrt(in_pr[i]*in_pr[i] + in_pi[i]*in_pi[i])>thres) {newnnz++;}
}
if (newnnz>0){
/* create output */
plhs[0] = mxCreateSparse(m,n,newnnz,mxCOMPLEX);
if (plhs[0]==NULL)
mexErrMsgTxt("Could not allocate enough memory!\n");
out_pr = mxGetPr(plhs[0]);
out_pi = mxGetPr(plhs[0]);
out_ir = mxGetIr(plhs[0]);
out_jc = mxGetJc(plhs[0]);
passed = 0;
out_jc[0] = 0;
for (col=0; col<n; col++){
starting_row_index = in_jc[col];
stopping_row_index = in_jc[col+1];
out_jc[col+1] = out_jc[col];
if (starting_row_index == stopping_row_index)
continue;
else {
for (current_row_index = starting_row_index;
current_row_index < stopping_row_index;
current_row_index++) {
if (sqrt(in_pr[current_row_index]*in_pr[current_row_index] +
in_pi[current_row_index]*in_pi[current_row_index] ) > thres){
out_pr[passed]=in_pr[current_row_index];
out_pi[passed]=in_pi[current_row_index];
out_ir[passed]=in_ir[current_row_index];
out_jc[col+1] = out_jc[col+1]+1;
passed++;
}
}
}
}
}
else{
plhs[0] = mxCreateSparse(m,n,0,mxCOMPLEX);
}
}
else{ /* for full matrices */
/* read input */
in_pr = mxGetPr(prhs[0]);
in_pi = mxGetPr(prhs[0]);
m = mxGetM(prhs[0]);
n = mxGetN(prhs[0]);
thres = mxGetScalar(prhs[1]);
/* Count new nonzeros */
newnnz=0;
for(i=0; i<m*n; i++){
if (sqrt(in_pr[i]*in_pr[i] + in_pi[i]*in_pi[i])>thres) {newnnz++;}
}
if (newnnz>0){
/* create output */
plhs[0] = mxCreateSparse(m,n,newnnz,mxCOMPLEX);
if (plhs[0]==NULL)
mexErrMsgTxt("Could not allocate enough memory!\n");
out_pr = mxGetPr(plhs[0]);
out_pi = mxGetPi(plhs[0]);
out_ir = mxGetIr(plhs[0]);
out_jc = mxGetJc(plhs[0]);
passed = 0;
out_jc[0] = 0;
for (col=0; col<n; col++){
out_jc[col+1] = out_jc[col];
for (current_row_index=0; current_row_index<m; current_row_index++){
if (sqrt(in_pr[current_row_index+m*col]*in_pr[current_row_index+m*col] +
in_pi[current_row_index+m*col]*in_pi[current_row_index+m*col]) > thres){
out_pr[passed]=in_pr[current_row_index+m*col];
out_ir[passed]=current_row_index;
out_jc[col+1] = out_jc[col+1]+1;
passed++;
}
}
}
}
else{
plhs[0] = mxCreateSparse(m,n,0,mxCOMPLEX);
}
}
}
else {
/* Check data type of input argument */
if (mxIsSparse(prhs[0])){
/* read input */
in_pr = mxGetPr(prhs[0]);
in_ir = mxGetIr(prhs[0]);
in_jc = mxGetJc(prhs[0]);
nzmax = mxGetNzmax(prhs[0]);
n = mxGetN(prhs[0]);
m = mxGetM(prhs[0]);
thres = mxGetScalar(prhs[1]);
/* Count new nonzeros */
newnnz=0;
for(i=0; i<nzmax; i++){
if ((fabs(in_pr[i]))>thres) {newnnz++;}
}
if (newnnz>0){
/* create output */
plhs[0] = mxCreateSparse(m,n,newnnz,mxREAL);
if (plhs[0]==NULL)
mexErrMsgTxt("Could not allocate enough memory!\n");
out_pr = mxGetPr(plhs[0]);
out_ir = mxGetIr(plhs[0]);
out_jc = mxGetJc(plhs[0]);
passed = 0;
out_jc[0] = 0;
for (col=0; col<n; col++){
starting_row_index = in_jc[col];
stopping_row_index = in_jc[col+1];
out_jc[col+1] = out_jc[col];
if (starting_row_index == stopping_row_index)
continue;
else {
for (current_row_index = starting_row_index;
current_row_index < stopping_row_index;
current_row_index++) {
if (fabs(in_pr[current_row_index])>thres){
out_pr[passed]=in_pr[current_row_index];
out_ir[passed]=in_ir[current_row_index];
out_jc[col+1] = out_jc[col+1]+1;
passed++;
}
}
}
}
}
else{
plhs[0] = mxCreateSparse(m,n,0,mxREAL);
}
}
else{ /* for full matrices */
/* read input */
in_pr = mxGetPr(prhs[0]);
n = mxGetN(prhs[0]);
m = mxGetM(prhs[0]);
thres = mxGetScalar(prhs[1]);
/* Count new nonzeros */
newnnz=0;
for(i=0; i<m*n; i++){
if ((fabs(in_pr[i]))>thres) {newnnz++;}
}
if (newnnz>0){
/* create output */
plhs[0] = mxCreateSparse(m,n,newnnz,mxREAL);
if (plhs[0]==NULL)
mexErrMsgTxt("Could not allocate enough memory!\n");
out_pr = mxGetPr(plhs[0]);
out_ir = mxGetIr(plhs[0]);
out_jc = mxGetJc(plhs[0]);
passed = 0;
out_jc[0] = 0;
for (col=0; col<n; col++){
out_jc[col+1] = out_jc[col];
for (current_row_index=0; current_row_index<m; current_row_index++){
if (fabs(in_pr[current_row_index+m*col])>thres){
out_pr[passed]=in_pr[current_row_index+m*col];
out_ir[passed]=current_row_index;
out_jc[col+1] = out_jc[col+1]+1;
passed++;
}
}
}
}
else{
plhs[0] = mxCreateSparse(m,n,0,mxREAL);
}
}
}
}
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
/* description:
* given a set of reference stimuli and responses (forming a joint distribution),
* this function will compute the log kernel density estimate over a grid of stimuli
* for a set of test responses. An epanechnikov kernel is used for all dimensions
* and the data is assumed to be normalized by the desired kernel width.
* Note that a radial-symmetric epanechnikov kernel is used, rather than a multiplicative kernel.
*
* syntax:
* out = kde_decode_epanechnikov( stimulus, stimulus_grid, response, test_response[, offset[, distance[, timestamp, bins]]] );
*
* arguments:
* stimulus = number of spikes x number of stimulus dimensions
* stimulus_grid = number of grid elements x number of stimulus dimensions
* response = number of spikes x number of response dimensions
* test_response = number of test spikes x number of response dimensions
* offset = number of grid elements
* distance = optional matrix of distances. If this argument is present (and not empty), the stimulus and stimulus_grid arguments should be given as a zero-based index into the distance matrix.
* timestamp = number of test spikes x 1
* bins = number of bins x 2
*
* out = number of test spikes x number of stimulus grid elements (if no timestamps and bins arguments are provided)
* out = number of bins x number of stimulus grid elements (if timestamps and bins arguments are provided)
*
*/
static const double pi = 3.141592653589793238462643383279502884197;
/* INPUTS */
double *stimulus; /* NxQ */
double *stimulus_grid; /* GxQ */
double *response; /* NxD or Nx0 or empty*/
double *test_response; /* MxD or Mx0 or empty*/
double *timestamp;
double *bins;
double *offset;
/* VARIABLES */
int N, D, M, Q, G, B;
int n, d, m, q, g, b;
int sizeM, loopM;
int next_idx, idx;
double acc_a, *acc_g, *skip_g, *z;
double tmp;
double *pout, *pout2;
mxArray *out, *out2;
int idx1, idx2;
mxArray *tmpmat;
int *use_distance_lookup;
int *NI;
double **distance;
double scaling_factor = 1;
double v;
double c1, c2;
mxArray *argout=NULL, *argin[1];
/* CHECK NUMBER OF INPUTS */
if (nrhs<2)
mexErrMsgTxt("This function requires at least two input arguments");
/* CHECK DIMENSIONS OF FIRST TWO ARGUMENTS */
if ( mxGetNumberOfDimensions(prhs[0])!=2 || mxGetNumberOfDimensions(prhs[1])!=2)
mexErrMsgTxt("stimulus and stimulus_grid input arguments need to be matrices");
/* GET POINTERS TO FIRST TWO ARGUMENTS */
stimulus = mxGetPr( prhs[0] );
stimulus_grid = mxGetPr( prhs[1] );
/* GET ARRAY SIZES */
N = mxGetM( prhs[0] ); /* number of source (encoding) spikes */
Q = mxGetN( prhs[0] ); /* number of stimulus dimensions */
G = mxGetM( prhs[1] ); /* number of points in stimulus grid */
/* CHECK NUMBER OF DIMENSIONS IN STIMULUS GRID */
if (mxGetN(prhs[1])!=Q)
mexErrMsgTxt("Incompatible size of stimulus_grid input arguments");
/* CHECK OPTIONAL INPUTS */
if (nrhs>2) {
/* CHECK DIMENSIONALITY OF SPIKE RESPONSE (ENCODING)*/
if ( mxGetNumberOfDimensions(prhs[2])!=2 )
mexErrMsgTxt("Response input argument needs to be a matrix");
response = mxGetPr( prhs[2] );
D = mxGetN( prhs[2] ); /* number of response dimensions */
/* CHECK NUMBER OF SPIKES IN RESPONSE */
if ( (D>0 && mxGetM(prhs[2])!=N ) )
mexErrMsgTxt("Incompatible size of response input argument");
} else {
D = 0;
}
if (nrhs>3) {
/* CHECK DIMENSIONALITY OF TEST RESPONSE (DECODING)*/
if ( mxGetNumberOfDimensions(prhs[3])!=2 )
mexErrMsgTxt("Test_response input argument needs to be a matrix");
test_response = mxGetPr( prhs[3] );
M = mxGetM( prhs[3] ); /* number of test (decoding) spikes */
/* CHECK NUMBER OF DIMENSIONS IN TEST RESPONSE */
if ( mxGetN(prhs[3])!=D )
mexErrMsgTxt("Incompatible size of test_response input argument");
} else {
M = 0; /* no test spikes specified */
}
if (D==0 && M==0 )
M = 1;
if (nrhs>4) {
/* CHECK SIZE OF OFFSET VECTOR */
if ( !mxIsDouble( prhs[4] ) || mxGetNumberOfElements( prhs[4] )!=G )
mexErrMsgTxt("Incompatible size of offset vector");
offset = mxGetPr( prhs[4] );
} else {
mexErrMsgTxt("Please provide offset vector");
}
/* ALLOCATE ARRAYS FOR DISTANCE LOOK-UP TABLES */
use_distance_lookup = (int*) mxCalloc( Q, sizeof(int) );
distance = (double**) mxCalloc( Q, sizeof(double*) );
NI = (int*) mxCalloc( Q, sizeof(int) ); /* size of distance LUTs */
/* INITIALIZE ARRAY */
for (q=0;q<Q;q++)
use_distance_lookup[q] = 0;
if (nrhs>5) {
/* CHECK CLASS AND SIZE OF DISTANCE LUTs */
if ( !mxIsCell( prhs[5] ) || mxGetNumberOfElements( prhs[5] )!=Q )
mexErrMsgTxt("Distance input argument needs to be a cell array with as many cells as stimulus dimensions");
for ( q=0 ; q<Q ; q++ ) {
tmpmat = mxGetCell( prhs[5], q );
if (tmpmat==NULL || mxIsEmpty(tmpmat)) {
use_distance_lookup[q] = 0;
} else {
/* CHECK SIZE OF DISTANCE LUT */
if ( mxGetNumberOfDimensions(tmpmat)!=2 || mxGetM( tmpmat )!=mxGetN( tmpmat ) )
mexErrMsgTxt("Distance arrays need to be square matrices");
distance[q] = (double*) mxGetPr( tmpmat );
NI[q] = mxGetM( tmpmat );
use_distance_lookup[q] = true;
/* when using distance LUT, the corresponding stimulus and stimulus grid should be indices into the LUT */
/* CHECK IF STIMULUS AND STIMULUS_GRID >=0 && <NI[q] */
for ( n=0; n<N; n++ ) {
if ( stimulus[n+q*N]<0 || stimulus[n+q*N]>=NI[q] )
mexErrMsgTxt("Invalid index");
}
for ( g=0; g<G; g++ ) {
if ( stimulus_grid[g+q*G]<0 || stimulus_grid[g+q*G]>=NI[q] )
mexErrMsgTxt("Invalid index");
}
}
}
}
B = 0; /* number of time bins */
if (nrhs>6) {
/* CHECK DIMENSIONALITY OF TEST (DECODING) SPIKE TIMESTAMPS */
if ( mxGetNumberOfDimensions(prhs[6])!=2 )
mexErrMsgTxt("Timestamp input argument needs to be a matrix");
timestamp = mxGetPr( prhs[6] );
/* CHECK SIZE OF TEST (DECODING) SPIKE TIMESTAMPS */
if ( mxGetM( prhs[6] )!=M || mxGetN( prhs[6] )!=1 )
mexErrMsgTxt("Incompatible size of timestamp input argument");
}
if (nrhs>7) {
/* CHECK DIMENSIONALITY AND SIZE OF TIME BINS ARGUMENT */
if ( mxGetNumberOfDimensions(prhs[7])!=2 || mxGetN(prhs[7])!=2)
mexErrMsgTxt("Bins input argument needs to be a Bx2 matrix");
B = mxGetM( prhs[7] ); /* number of time bins */
bins = mxGetPr( prhs[7] );
}
/* COMPUTE SCALING FACTOR */
argin[0] = mxCreateDoubleScalar( 1 + 0.5*(double)(D+Q) );
/* compute volume of hypersphere */
mexCallMATLAB(1, &argout, 1, argin, "gamma" );
v = pow(pi, 0.5*(double)(D+Q))/mxGetScalar(argout);
scaling_factor *= 0.5*(double)(D+Q+2)/v;
/* ALLOCATE TEMPORARY ARRAYS AND OUTPUT ARRAYS */
acc_g = (double*) mxCalloc( G, sizeof(double) );
skip_g = (double*) mxCalloc( G, sizeof(double) );
if (B==0) {
out = mxCreateDoubleMatrix( M, G, mxREAL );
z = mxGetPr( out );
} else {
if (D==0) {
z = (double*) mxCalloc( G, sizeof(double) );
} else {
z = (double*) mxCalloc( M*G, sizeof(double) );
}
out = mxCreateDoubleMatrix( B, G, mxREAL );
pout = mxGetPr( out );
out2 = mxCreateDoubleMatrix( B, 1, mxREAL );
pout2 = mxGetPr( out2 );
}
loopM = sizeM = M;
if (D==0) {
loopM = 1;
if (B>0)
sizeM = 1;
}
/* COMPUTE KDE */
/* LOOP THROUGH SOURCE (ENCODING) SPIKES */
for ( n=0; n<N; n++ ) {
/* LOOP THROUGH STIMULUS GRID */
for ( g=0; g<G; g++ ) {
/* INITIALIZE ACCUMULATORS */
acc_g[g] = 0;
skip_g[g] = 0;
/* INITIALIZE INDICES */
idx1 = g;
idx2 = n;
/* LOOP THROUGH STIMULUS DIMENSIONS */
for ( q=0; q<Q; q++ ) {
if (use_distance_lookup[q]) {
tmp = distance[q][ ((int) stimulus_grid[idx1])*NI[q] + (int)stimulus[idx2] ];
} else {
tmp = (stimulus_grid[idx1]-stimulus[idx2]);
}
tmp *= tmp;
acc_g[g] += tmp;
if (acc_g[g]>1) {
skip_g[g] = 1;
break;
}
/* UPDATE INDICES */
idx1 += G;
idx2 += N;
}
}
/* LOOP THROUGH TEST (DECODING) SPIKES */
for ( m=0; m<loopM; m++ ) {
/* INITIALIZE ACCUMULATORS */
acc_a = 0;
/* INTIALIZE INDICES */
idx1 = m;
idx2 = n;
/* LOOP THROUGH RESPONSE DIMENSIONS */
for ( d=0; d<D; d++ ) {
tmp = (test_response[idx1]-response[idx2]);
tmp *= tmp;
acc_a += tmp;
if (acc_a>1) {
goto nextm;
}
/* UPDATE INDICES */
idx1 += sizeM;
idx2 += N;
}
for ( g=0; g<G; g++ ) {
if (skip_g[g] || (acc_g[g]+acc_a)>1 )
continue;
z[m+g*sizeM] += (1-(acc_g[g]+acc_a));
}
nextm:
;
}
}
mxFree(acc_g);
mxFree(skip_g);
if (argout!=NULL)
mxDestroyArray(argout);
mxDestroyArray(argin[0]);
/* compute log */
c1 = log(scaling_factor) - log((double)N);
if (B==0 && D==0) {
for (g=0;g<G;g++)
z[g*sizeM]=log(z[g*sizeM] + offset[g]*(double)N/scaling_factor) + c1;
} else {
for (g=0; g<G; g++) {
idx = g*loopM;
c2 = offset[g]*(double)N/scaling_factor;
for (m=0; m<loopM ; m++ )
z[m+idx] = log(z[m+idx] + c2) + c1;
}
}
if (D==0 && B==0) { /* copy */
for (g=0; g<G; g++) {
idx = g*sizeM;
for (m=1; m<sizeM ; m++ )
z[m+idx] = z[idx];
}
}
if (B>0) {
next_idx = 0;
for (b=0; b<B; b++) {
while (next_idx<M && (timestamp[next_idx]<bins[b]))
next_idx++;
idx = next_idx;
while (idx<M && (timestamp[idx]<bins[b+B])) {
if (D>0) {
for (g=0; g<G; g++)
pout[b+g*B] += z[idx+g*M];
}
pout2[b]++;
idx++;
}
if (D==0) {
for (g=0; g<G; g++) {
pout[b+g*B] = pout2[b] * z[g*sizeM];
}
}
}
mxFree(z);
}
plhs[0] = out;
plhs[1] = out2;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
/* Input variables */
float *condQB = (float *) mxGetData(prhs[0]);
double *prediction = mxGetPr(prhs[1]);
float *mu_a_b = (float *) mxGetData(prhs[2]);
double *factorsPrec = mxGetPr(prhs[3]);
double *hashTable = mxGetPr(prhs[4]);
int numColumnsPred = (int) mxGetScalar(prhs[5]);
int *cmin = (int*) mxGetData(prhs[6]);
int *cmax = (int*) mxGetData(prhs[7]);
/* intern variables and pointers */
double* q_c = NULL;
double* boundaries = NULL;
int numRows = mxGetM(prhs[0]);
int numBounds = mxGetN(prhs[0])/numColumnsPred;
int alphaSize = numRows*numBounds*sizeof(double);
/* determines when to use the hash table */
/*int limit = 10;*/
int limit2 = -30;
/* int counter = 0;*/
/* switch from matlab indexing to C indexing */
int j;
plhs[0] = mxCreateDoubleMatrix(1,numRows*numBounds*numColumnsPred,mxREAL);
q_c = mxGetPr(plhs[0]);
plhs[1] = mxCreateDoubleMatrix(numBounds,numColumnsPred,mxREAL);
boundaries = mxGetPr(plhs[1]);
/* ****** start sum-product ******** */
#pragma omp parallel
{
int i,k,i1,i2;
double* alpha = malloc(alphaSize);
double* beta = malloc(alphaSize);
double* c = malloc(numBounds*sizeof(double));
double alphaTotal,q_c_total,tmp,val,factor,cInv;
double* preCalc = malloc(numRows*sizeof(double));
int idxQC,idx,idxA,idxB,idxC,idxNumRows,idxCond,idxBounds;
int* A = malloc(numBounds*sizeof(int));
int* B = malloc(numBounds*sizeof(int));
#pragma omp for
for (j=0; j < numColumnsPred; j++) {
/* for (j=0; j < 1; j++) {*/
memset(alpha, 0, alphaSize);
memset(beta, 0, alphaSize);
/* calculate limits of for-loops corresponding to transition matrices */
for (k=0; k < numBounds; k++) {
A[k] = cmin[j + k*numColumnsPred];
B[k] = cmax[j + k*numColumnsPred];
/*printf("%d, %d: %d, %d\n",j,k,A[k],B[k]);*/
}
alphaTotal = 0;
/* pred index for prediction */
idxC = j*numRows*numBounds;
for (i = A[0]; i <= B[0]; i++) {
alpha[i] = condQB[j*numRows + i]*prediction[idxC + i];
alphaTotal += alpha[i];
}
c[0] = alphaTotal; alphaTotal = 1/alphaTotal;
/* normalize alpha */
for (i=A[0]; i <= B[0]; i++) {
alpha[i] *= alphaTotal;
}
/* make forward message passing over all boundaries */
/* for boundaries 2 to numBounds */
for (k=1; k < numBounds; k++) {
/* for(k=1; k < 0; k++) { */
/* preCalc index for inner loop */
factor = -0.5*factorsPrec[(k-1)*numColumnsPred + j];
idxNumRows = ((k-1)*numColumnsPred + j)*numRows;
idxCond = (k*numColumnsPred + j)*numRows;
idx = numRows*k;
alphaTotal = 0;
/* iterates over the columns of each transition matrix; corresponds to idxNonZeroA in matlab; determines the non-zero entries of the current alpha */
for (i1 = A[k]; i1 <= B[k]; i1++) {
tmp = 0;
/* iterates over the rows of transition matrices; corresponds to idxNonZeroB in matlab */
/* upper triangular matrix --> ordering constraint on boundaries */
for (i2 = A[k-1]; i2 <= min(i1,B[k-1]); i2++) {
val = (i1 + 1 - mu_a_b[idxNumRows + i2]);
val = val*val*factor;
if (val > limit2) {tmp += alpha[idx - numRows + i2]*hashTable[(int)(-val*1000 + 0.5)];}
}
alpha[idx + i1] = prediction[idxC + idx + i1]*condQB[idxCond+i1]*tmp;
alphaTotal += alpha[idx + i1];
}
c[k] = alphaTotal; alphaTotal = 1/alphaTotal;
/* normalize alpha */
for (i = A[k]; i <= B[k]; i++) {
alpha[idx + i] *= alphaTotal;
}
} /* end for over bounds k */
/* init beta for the last node */
idxQC = j*numBounds*numRows;
idxBounds = (j+1)*numBounds - 1;
boundaries[idxBounds] = 0;
for (i=(numBounds-1)*numRows;i<numRows*numBounds;i++) {
beta[i] = 1;
q_c[idxQC + i] = alpha[i];
boundaries[idxBounds] += alpha[i]*((i+1)-(numBounds-1)*numRows);
}
/* message backward */
for (k=numBounds-2; k >= 0; k--) {
/* for (k = 0; k < 0; k++) {*/
idxCond = j*numRows + (k+1)*numColumnsPred*numRows;
idxB = numRows*(k+1);
idxA = j*numRows*numBounds + (k+1)*numRows;
/* precalculate entries for inner loop over z_{n+1}, that are independent of z_n */
for (i=A[k+1]; i <= B[k+1]; i++) {
preCalc[i] = beta[idxB + i]*prediction[idxA + i]*condQB[idxCond + i];
}
/* preCalc idx for inner loop */
factor = -0.5*factorsPrec[k*numColumnsPred + j];
idxNumRows = (k*numColumnsPred + j)*numRows; idx = numRows*k;
/* the outer loop (over z_n) is constrained by alpha (and therefor condQB), the inner loop over (z_{n+1}) by condQB */
q_c_total = 0; cInv = 1/c[k+1];
for (i1 = A[k]; i1 <= B[k]; i1++) {
tmp = 0;
/* idxFinal */
for (i2 = max(A[k+1],i1); i2 <= B[k+1]; i2++) {
val = (i2 + 1 - mu_a_b[idxNumRows + i1]);
val = factor*val*val;
if (val > limit2) {tmp += preCalc[i2]*hashTable[(int)(-val*1000 + 0.5)];}
}
beta[idx + i1] = tmp*cInv;
q_c[idxQC + idx + i1] = alpha[idx + i1]*beta[idx + i1];
q_c_total += q_c[idxQC + idx + i1];
}
idxBounds = j*numBounds + k;
boundaries[idxBounds] = 0;
/* convert to inverse */
q_c_total = 1/q_c_total;
/* normalize q_c distribution */
for (i1 = A[k]; i1 <= B[k]; i1++) {
q_c[idxQC + idx + i1] *= q_c_total;
boundaries[idxBounds] += q_c[idxQC + idx + i1]*(i1+1);
}
}
}
free(alpha); free(beta); free(c); free(preCalc); free(A); free(B);
}
}