/* Function CheckInputSampleTimes ==============================================
* Abstract:
* We know both input sample times, now verify that the make sense
* (see top of file).
*/
static void CheckInputSampleTimes(SimStruct *S)
{
real_T enableTs = ssGetInputPortSampleTime(S, ENABLE_IPORT);
real_T signalTs = ssGetInputPortSampleTime(S, SIGNAL_IPORT);
int enableTid = ssGetInputPortSampleTimeIndex(S,ENABLE_IPORT);
if (enableTs != CONTINUOUS_SAMPLE_TIME) {
/* signal must be a multiple of the enable */
real_T k = floor(signalTs/enableTs+0.5);
if (k < 1.0 ||
fabs(k - signalTs/enableTs) > mxGetEps()*128.0) {
ssSetErrorStatus(S, "Sample time of source signal (port 2) "
"must be a positive multiple of enable "
"(port 1) signal");
return;
}
}
if (enableTid == CONSTANT_TID) {
ssSetErrorStatus(S, "Constant sample time is not allowed "
"at enable port (port 1)");
return;
}
} /* end CheckInputSampleTimes */
// Function definitions for class L-BFGS-B Program.
// -----------------------------------------------------------------
lbfgsb_program::lbfgsb_program (user_function_data *fun, user_function_data *grad, iter_fun_data *iterF, size_t ndec,
double *in_lb, double *in_ub, double *in_x0, double *xfinal,
double *fval, double *iter, int printLevel, int maxIter, int maxFeval, double maxtime, double ftol, int m, double pgtol)
: Program((int)ndec,0,0,0,0,m,maxIter,maxFeval,maxtime,ftol/mxGetEps(),pgtol) //Program Constructor
{
//Create local copies of input args (we will change them)
x = new double[ndec];
lb = new double[ndec];
ub = new double[ndec];
for(int i = 0; i < ndec; i++) {
lb[i] = in_lb[i];
ub[i] = in_ub[i];
x[i] = in_x0[i];
}
//Solver bound types
btype = new int[ndec];
// Set the bound types.
for (int i = 0; i < ndec; i++)
btype[i] = getBoundType(lb[i],ub[i]);
//Save size
this->ndec = ndec;
//Save print stuff
this->printLevel = printLevel;
//Save Function Information
this->fun = fun;
this->grad = grad;
this->iterF = iterF;
//Assign Output Variables
this->xfinal = xfinal;
this->noFevals = 0;
}
/*************************************************************************
* main
*************************************************************************/
void dijkstra1( long int n, long int s, double *D1, double *P1, double *Gpr, int *Gir, int *Gjc) {
int finished;
long int i, startInd, endInd, whichNeigh, nDone, closest;
double closestD, arcLength, INF, SMALL, oldDist;
HeapNode *A, *hnMin, hnTmp; FibHeap *heap;
INF=mxGetInf(); SMALL=mxGetEps();
// setup heap
if ((heap = new FibHeap) == NULL || (A = new HeapNode[n+1]) == NULL )
mexErrMsgTxt( "Memory allocation failed-- ABORTING.\n" );
heap->ClearHeapOwnership();
// initialize
for (i=0; i<n; i++) {
if (i!=s) A[ i ] = (double) INF; else A[ i ] = (double) SMALL;
if (i!=s) D1[ i ] = (double) INF; else D1[ i ] = (double) SMALL;
P1[ i ] = -1;
heap->Insert( &A[i] );
A[ i ].SetIndexValue( (long int) i );
}
// Insert 0 then extract it, which will balance heap
heap->Insert(&hnTmp); heap->ExtractMin();
// loop over nonreached nodes
finished = nDone = 0;
while ((finished==0) && (nDone < n)) {
hnMin = (HeapNode *) heap->ExtractMin();
closest = hnMin->GetIndexValue();
closestD = hnMin->GetKeyValue();
if ((closest<0) || (closest>=n))
mexErrMsgTxt( "Minimum Index out of bound..." );
D1[ closest ] = closestD;
if (closestD == INF) finished=1; else {
// relax all nodes adjacent to closest
nDone++;
startInd = Gjc[ closest ];
endInd = Gjc[ closest+1 ] - 1;
if( startInd!=endInd+1 )
for( i=startInd; i<=endInd; i++ ) {
whichNeigh = Gir[ i ];
arcLength = Gpr[ i ];
oldDist = D1[ whichNeigh ];
if ( oldDist > ( closestD + arcLength )) {
D1[ whichNeigh ] = closestD + arcLength;
P1[ whichNeigh ] = closest + 1;
hnTmp = A[ whichNeigh ];
hnTmp.SetKeyValue( closestD + arcLength );
heap->DecreaseKey( &A[ whichNeigh ], hnTmp );
}
}
}
}
// cleanup
delete heap; delete[] A;
}
/**
* @brief Read in the option possible_words.
* Reads the option possible_words, converting string data into its numerical
* representation, and returning a flag to indicate the status.
*/
int ReadOptionPossibleWords(const mxArray *in,const char *field_name,double *member)
{
/* declare local variables */
mxArray *tmp;
double num;
int stringLength, flag, i;
char *str;
tmp = mxGetField(in,0,field_name);
if((tmp==NULL) || mxIsEmpty(tmp)) /* field is empty */
flag = 0;
else
{
if(mxIsChar(tmp)) /* field is string */
{
/* copy string and set to lowercase */
stringLength = mxGetNumberOfElements(tmp) + 1;
str = mxCalloc(stringLength,sizeof(char));
if(mxGetString(tmp,str,stringLength)!=0)
mexErrMsgIdAndTxt("STAToolkit:ReadOptionPossibleWords:invalidValue","Could not convert string data.");
for(i=0;str[i];i++)
str[i] = tolower(str[i]);
/* use string to set member value */
flag = 1;
if(strcmp(str,"recommended")==0)
*member = (double)(-1.0);
else if(strcmp(str,"unique")==0)
*member = (double)(-2.0);
else if(strcmp(str,"total")==0)
*member = (double)(-3.0);
else if(strcmp(str,"possible")==0)
*member = (double)(-4.0);
else if(strcmp(str,"min_tot_pos")==0)
*member = (double)(-5.0);
else if(strcmp(str,"min_lim_tot_pos")==0)
*member = (double)(-6.0);
else
{
mexWarnMsgIdAndTxt("STAToolkit:ReadOptionPossibleWords:invalidValue","Unrecognized option \"%s\" for possible_words. Using default \"recommended\".",str);
*member = (double)(-1.0);
}
}
else /* field is scalar */
{
flag = 2;
num = mxGetScalar(tmp);
if(num==mxGetInf())
*member = (double)(0.0);
else if((num<1.0) || (fmod(num,1.0)>mxGetEps()))
mexErrMsgIdAndTxt("STAToolkit:ReadOptionPossibleWords:invalidValue","possible_words must be a positive integer. Current value is %f.",num);
else
*member = num;
}
}
return flag;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
size_t NN, X_sum, P, D, Lp1;
mwSize K;
double *X_inds, *X_vals;
double *lPhi, *lTheta, *lPsi, *W;
mxArray *Km;
double *inds, *vals, *num_nz;
double eps;
if(nlhs != NOUTPUTS)
mexErrMsgTxt("Usage: see \"help sample_Xpn_kernel_meat\"");
if(nrhs != NINPUTS)
mexErrMsgTxt("Usage: see \"help sample_Xpn_kernel_meat\"");
// Grab inputs
X_inds = mxGetPr(prhs[0]);
X_vals = mxGetPr(prhs[1]);
lPhi = mxGetPr(prhs[2]);
lTheta = mxGetPr(prhs[3]);
lPsi = mxGetPr(prhs[4]);
W = mxGetPr(prhs[6]);
X_sum = mxGetScalar(prhs[7]);
Km = (mxArray*)prhs[5]; // Cell array
NN = mxGetM(prhs[0]);
P = mxGetM(prhs[2]);
K = mxGetN(prhs[2]);
D = mxGetN(prhs[4]);
Lp1 = mxGetM(prhs[6]);
eps = mxGetEps();
// Allocate output
plhs[0] = mxCreateDoubleMatrix(X_sum, 3, mxREAL);
plhs[1] = mxCreateDoubleMatrix(X_sum, 1, mxREAL);
plhs[2] = mxCreateDoubleMatrix(1, 1, mxREAL);
inds = mxGetPr(plhs[0]);
vals = mxGetPr(plhs[1]);
num_nz = mxGetPr(plhs[2]);
// Pre-allocate scratch arrays
unsigned int topic_sz = K*sizeof(double);
double *zeta = (double*)mxMalloc(topic_sz);
double *cump_scr = (double*)mxMalloc(topic_sz);
double *mnr = (double*)mxMalloc(topic_sz);
// log-thinning activation function, not Phi as in the topics
// Change this to just a double*
//mxArray *lphi = mxCreateDoubleMatrix(D, K, mxREAL);
//mxArray *res;
double *A, *lphi;
char *chn = "N"; // don't transpose for dgemv
double onef = 1.0, zerof = 0.0; // alpha, beta in dgemv
size_t onei = 1; // incx in dgemv
lphi = mxMalloc(D*K*sizeof(double));
//lphi_ptr = mxGetPr(lphi);
//printf("Computing lphi...\n");
//printf(" K: %d\n", K);
// Compute phi{k} = p(Znk = 1) for all k here (Each one is a matrix)
for (int k = 0; k < K; k++)
{
//printf("k: %d ", k);
A = mxGetPr(mxGetCell(Km, k));
//printf("Before dgemv..");
dgemv(chn, &D, &Lp1, &onef, A, &D, &W[k*Lp1], &onei, &zerof, &lphi[k*D], &onei);
//dgemv(chn, &D, &Lp1, &onef, A, &D, &W[k*Lp1], &onei, &zerof, &lphi_ptr[k*D], &onei);
//printf("after dgemv\n");
}
//printf("Sampling...\n");
// Probit each entry and take log
normcdf(lphi, D*K);
for (int ii = 0; ii < D*K; ii++)
lphi[ii] = log(lphi[ii] + eps);
// main loop
unsigned int cur_idx = 0;
unsigned int noff = X_sum;
unsigned int koff = 2*X_sum;
for(int ii = 0; ii < NN; ii++)
{
// Get indices and data (convert to 0-based)
unsigned int p = X_inds[ii] - 1;
unsigned int n = X_inds[ii + NN] - 1; // These arrays have NN rows
unsigned int xx = X_vals[ii];
// Construct probability
double zeta_max = DBL_MIN;
memset(zeta,0,topic_sz);
for(int k = 0; k < K; k++)
{
// When we say N we mean D
// lZnk is NxK and lPsi is KxN so we index them backwards
// lphi is NxK and contains the log-thinning probabilities
//zeta[k] = lPhi[p + k*P] + lTheta[k] + lZnk[n + k*D] + lPsi[k + n*K];
zeta[k] = lPhi[p + k*P] + lTheta[k] + lphi[n + k*D] + lPsi[k + n*K];
if(zeta[k] > zeta_max)
zeta_max = zeta[k];
}
for(int k = 0; k < K; k++)
{
zeta[k] -= zeta_max;
zeta[k] = exp(zeta[k]);
}
double zeta_sum = sum(zeta, K);
for(int k = 0; k < K; k++)
zeta[k] /= zeta_sum;
// for(int k = 0; k < K; k++)
// printf("%.2f,", zeta[k]);
// printf("\n");
// Split up observed counts into topics and add to output
if(xx > 1)
{
multirnd(mnr, xx, zeta, K, cump_scr, eps);
for(int k = 0; k < K; k++)
{
if(mnr[k] > 0)
{
inds[cur_idx] = p + 1;
inds[cur_idx + noff] = n + 1;
inds[cur_idx + koff] = k + 1; // +1 b/c k is index
vals[cur_idx] = mnr[k];
cur_idx++;
}
}
}
else
{
// Result of below is 1-based, so don't add 1 to kk below
double kk = discrnd(zeta, K, cump_scr, eps);
inds[cur_idx] = p + 1;
inds[cur_idx + noff] = n + 1;
inds[cur_idx + koff] = kk;
vals[cur_idx] = 1;
cur_idx++;
}
}
// Free scratch memory
mxFree(zeta);
mxFree(cump_scr);
mxFree(mnr);
mxFree(lphi);
*num_nz = cur_idx; // cur_idx has been incr. 1 beyond already
}
void dodijk_sparse(
long int M,
long int N,
long int S,
double *D,
double *sr,
int *irs,
int *jcs,
HeapNode *A,
FibHeap *theHeap )
{
int finished;
long int i,startind,endind,whichneighbor,ndone,index,switchwith,closest,closesti;
long int *INDICES;
double closestD,arclength;
double INF,SMALL,olddist;
HeapNode *Min;
HeapNode Temp;
INF = mxGetInf();
SMALL = mxGetEps();
/* initialize */
for (i=0; i<M; i++)
{
if (i!=S) A[ i ] = (double) INF; else A[ i ] = (double) SMALL;
if (i!=S) D[ i ] = (double) INF; else D[ i ] = (double) SMALL;
theHeap->Insert( &A[i] );
A[ i ].SetIndexValue( (long int) i );
}
// Insert 0 then extract it. This will cause the
// Fibonacci heap to get balanced.
theHeap->Insert(&Temp);
theHeap->ExtractMin();
/*theHeap->Print();
for (i=0; i<M; i++)
{
closest = A[ i ].GetIndexValue();
closestD = A[ i ].GetKeyValue();
mexPrintf( "Index at i=%d =%d value=%f\n" , i , closest , closestD );
}*/
/* loop over nonreached nodes */
finished = 0;
ndone = 0;
while ((finished==0) && (ndone < M))
{
//if ((ndone % 100) == 0) mexPrintf( "Done with node %d\n" , ndone );
Min = (HeapNode *) theHeap->ExtractMin();
closest = Min->GetIndexValue();
closestD = Min->GetKeyValue();
if ((closest<0) || (closest>=M)) mexErrMsgTxt( "Minimum Index out of bound..." );
//theHeap->Print();
//mexPrintf( "EXTRACTED MINIMUM NDone=%d S=%d closest=%d closestD=%f\n" , ndone , S , closest , closestD );
//mexErrMsgTxt( "Exiting..." );
D[ closest ] = closestD;
if (closestD == INF) finished=1; else
{
/* add the closest to the determined list */
ndone++;
/* relax all nodes adjacent to closest */
startind = jcs[ closest ];
endind = jcs[ closest+1 ] - 1;
if (startind!=endind+1)
for (i=startind; i<=endind; i++)
{
whichneighbor = irs[ i ];
arclength = sr[ i ];
olddist = D[ whichneighbor ];
//mexPrintf( "INSPECT NEIGHBOR #%d olddist=%f newdist=%f\n" , whichneighbor , olddist , closestD+arclength );
if ( olddist > ( closestD + arclength ))
{
D[ whichneighbor ] = closestD + arclength;
Temp = A[ whichneighbor ];
Temp.SetKeyValue( closestD + arclength );
theHeap->DecreaseKey( &A[ whichneighbor ], Temp );
//mexPrintf( "UPDATING NODE #%d olddist=%f newdist=%f\n" , whichneighbor , olddist , closestD+arclength );
}
}
}
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
/* Declare Variables */
char *ch1 = "V", *ch2 = "I", *ch3 = "L";
void *A, *W, *Z, *work;
double ABSTOLD = 0.0, VLD = 0.0, VUD = 0.0;
float ABSTOLF = 0.0, VLF = 0.0, VUF = 0.0;
#ifdef MWSIZE
mwSize N, *iwork, *ifail, info = 0, numElem, optional, IL, IU, M, num_eigs, isDouble = 1;
#else
int N, *iwork, *ifail, info = 0, numElem, optional, IL, IU, M, num_eigs, isDouble = 1;
#endif
/****** Do Error check *************/
if (nrhs != 4)
{
mexErrMsgTxt("icatb_eig_symm_sel function accepts four input arguments.\nThe usage of the function is [eigen_vectors, eigen_values] = icatb_eig_symm_sel(A, N, num_eigs, opt);\nwhere A is the lower triangular matrix without zeros entered as a vector, N is the original dimension, num_eigs is the number of eigen values and opt is parameter to operate on copy of the matrix or the original array.");
}
#ifdef MWSIZE
/* Inputs Matrix (A), order (N), number of eigen values (num_eigs), parameter (opt) to operate on copy or original matrix*/
numElem = (mwSize) mxGetNumberOfElements(prhs[0]);
N = (mwSize) mxGetScalar(prhs[1]);
num_eigs = (mwSize) mxGetScalar(prhs[2]);
/* Parameter for using the copy or original array to do operations*/
optional = (mwSize) mxGetScalar(prhs[3]);
#else
numElem = (int) mxGetNumberOfElements(prhs[0]);
N = (int) mxGetScalar(prhs[1]);
num_eigs = (int) mxGetScalar(prhs[2]);
optional = (int) mxGetScalar(prhs[3]);
#endif
if (numElem != (N*(N + 1)/2))
{
mexErrMsgTxt("Number of elements of input matrix (A) must be equal to N*(N+1)/2");
}
if (num_eigs > N)
{
mexErrMsgTxt("Number of eigen values desired is greater than the number of rows of matrix");
}
if (nlhs > 2)
{
mexErrMsgTxt("Maximum allowed output arguments is 2.");
}
/********* End for doing error check *********/
/* Eigen values desired */
IL = N - num_eigs + 1;
IU = N;
/* Check isdouble */
if (mxIsDouble(prhs[0]))
{
isDouble = 1;
if (optional == 1)
{
A = (void *) mxCalloc(numElem, sizeof(double));
memcpy(A, mxGetData(prhs[0]), numElem*sizeof(double));
}
else
{
A = mxGetData(prhs[0]);
}
A = (double *) A;
/* Pointer to work */
work = (double *) mxCalloc(8*N, sizeof(double));
/* Pointer to eigen vectors */
plhs[0] = mxCreateDoubleMatrix(N, num_eigs, mxREAL);
Z = mxGetPr(plhs[0]);
/* Pointer to eigen values */
plhs[1] = mxCreateDoubleMatrix(1, num_eigs, mxREAL);
W = mxGetPr(plhs[1]);
/* Tolerance */
ABSTOLD = mxGetEps();
}
else
{
isDouble = 0;
if (optional == 1)
{
A = (void *) mxCalloc(numElem, sizeof(float));
memcpy(A, mxGetData(prhs[0]), numElem*sizeof(float));
}
else
{
A = mxGetData(prhs[0]);
}
A = (float *) A;
/* Pointer to work */
work = (float *) mxCalloc(8*N, sizeof(float));
/* Pointer to eigen vectors */
plhs[0] = mxCreateNumericMatrix(N, num_eigs, mxSINGLE_CLASS, mxREAL);
Z = (float *) mxGetData(plhs[0]);
/* Pointer to eigen values */
plhs[1] = mxCreateNumericMatrix(1, num_eigs, mxSINGLE_CLASS, mxREAL);
W = (float *) mxGetData(plhs[1]);
ABSTOLF = (float) mxGetEps();
}
#ifdef MWSIZE
/* Pointer to iwork */
iwork = (mwSize *) mxCalloc(5*N, sizeof(mwSize));
/* Pointer to ifail */
ifail = (mwSize *) mxCalloc(N, sizeof(mwSize));
#else
/* Pointer to iwork */
iwork = (int *) mxCalloc(5*N, sizeof(int));
/* Pointer to ifail */
ifail = (int *) mxCalloc(N, sizeof(int));
#endif
/* Call subroutine to find eigen values and vectors */
#ifdef PC
/* Handle Windows */
if (isDouble == 1)
{
/* Double precision */
dspevx(ch1, ch2, ch3, &N, A, &VLD, &VUD, &IL, &IU, &ABSTOLD, &M, W, Z, &N, work, iwork, ifail, &info);
}
else
{
/* Use single precision routine */
#ifdef SINGLE_SUPPORT
sspevx(ch1, ch2, ch3, &N, A, &VLF, &VUF, &IL, &IU, &ABSTOLF, &M, W, Z, &N, work, iwork, ifail, &info);
#else
mexErrMsgTxt("Error executing sspevx function on this MATLAB version.\nUse double precision to solve eigen value problem");
#endif
}
/* End for handling Windows */
#else
/* Handle other OS */
if (isDouble == 1)
{
dspevx_(ch1, ch2, ch3, &N, A, &VLD, &VUD, &IL, &IU, &ABSTOLD, &M, W, Z, &N, work, iwork, ifail, &info);
}
else
{
/* Use single precision routine */
#ifdef SINGLE_SUPPORT
sspevx_(ch1, ch2, ch3, &N, A, &VLF, &VUF, &IL, &IU, &ABSTOLF, &M, W, Z, &N, work, iwork, ifail, &info);
#else
mexErrMsgTxt("Error executing sspevx_ function on this MATLAB version.\nUse double precision to solve eigen value problem");
#endif
}
/* End for handling other OS */
#endif
/* End for calling subroutine to find eigen values and vectors */
if (M != num_eigs)
{
mexPrintf("%s%d\t%s%d\n", "No. of eigen values estimated: ", M, "No. of eigen values desired: ", num_eigs);
mexErrMsgTxt("Error executing dspevx function\n");
}
if (info == 1)
{
mexErrMsgTxt("Error executing dspevx/sspevx function.");
}
/* Free memory */
if (optional == 1)
{
mxFree(A);
}
mxFree(work);
mxFree(iwork);
mxFree(ifail);
}
// Function definitions.
// -----------------------------------------------------------------
void mexFunction (int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
try {
// Check to see if we have the correct number of input and output
// arguments.
if (nrhs < minNumInputArgs)
throw MatlabException("Incorrect number of input arguments");
// Get the starting point for the variables. This is specified in
// the first input argument. The variables must be either a single
// matrix or a cell array of matrices.
int k = 0; // The index of the current input argument.
ArrayOfMatrices x0(prhs[k++]);
// Create the output, which stores the solution obtained from
// running IPOPT. There should be as many output arguments as cell
// entries in X.
if (nlhs != x0.length())
throw MatlabException("Incorrect number of output arguments");
ArrayOfMatrices x(plhs,x0);
// Load the lower and upper bounds on the variables as
// ArrayOfMatrices objects. They should have the same structure as
// the ArrayOfMatrices object "x".
ArrayOfMatrices lb(prhs[k++]);
ArrayOfMatrices ub(prhs[k++]);
// Check to make sure the bounds make sense.
if (lb != x || ub != x)
throw MatlabException("Input arguments LB and UB must have the same \
structure as X");
// Get the Matlab callback functions.
MatlabString objFunc(prhs[k++]);
MatlabString gradFunc(prhs[k++]);
// Get the auxiliary data.
const mxArray* auxData;
const mxArray* ptr = prhs[k++];
if (nrhs > 5) {
if (mxIsEmpty(ptr))
auxData = 0;
else
auxData = ptr;
}
else
auxData = 0;
// Get the intermediate callback function.
MatlabString* iterFunc;
ptr = prhs[k++];
if (nrhs > 6) {
if (mxIsEmpty(ptr))
iterFunc = 0;
else
iterFunc = new MatlabString(ptr);
}
else
iterFunc = 0;
// Set the options for the L-BFGS algorithm to their defaults.
int maxiter = defaultmaxiter;
int m = defaultm;
double factr = defaultfactr;
double pgtol = defaultpgtol;
// Process the remaining input arguments, which set options for
// the IPOPT algorithm.
while (k < nrhs) {
// Get the option label from the Matlab input argument.
MatlabString optionLabel(prhs[k++]);
if (k < nrhs) {
// Get the option value from the Matlab input argument.
MatlabScalar optionValue(prhs[k++]);
double value = optionValue;
if (!strcmp(optionLabel,"maxiter"))
maxiter = (int) value;
else if (!strcmp(optionLabel,"m"))
m = (int) value;
else if (!strcmp(optionLabel,"factr"))
factr = value / mxGetEps();
else if (!strcmp(optionLabel,"pgtol"))
pgtol = value;
else {
if (iterFunc) delete iterFunc;
throw MatlabException("Nonexistent option");
}
}
}
// Create a new instance of the optimization problem.
x = x0;
MatlabProgram program(x,lb,ub,&objFunc,&gradFunc,iterFunc,
(mxArray*) auxData,m,maxiter,factr,pgtol);
// Run the L-BFGS-B solver.
SolverExitStatus exitStatus = program.runSolver();
if (exitStatus == abnormalTermination) {
if (iterFunc) delete iterFunc;
throw MatlabException("Solver unable to satisfy convergence \
criteria due to abnormal termination");
}
else if (exitStatus == errorOnInput) {
void
mexFunction (int nlhs, mxArray * plhs[], int nrhs, const mxArray * prhs[])
{
mxArray *lf;
double tmp1, tmp2, tmp3, tmp4, tmp5, tmp6, tmp7, eps, alpha, beta, A, B, C;
double r[3], u[3], d[3];
double *lf_p, *rm_p, *um_p, *R;
int nchan, i;
char str[256];
if (nrhs<3)
mexErrMsgTxt("Not enough input arguments");
if (nrhs>3)
mexErrMsgTxt("Too many input arguments");
if (mxGetM(prhs[0])!=1 || mxGetN(prhs[0])!=3)
mexErrMsgTxt ("Invalid dimension for input argument 1");
if (mxGetN(prhs[1])!=3)
mexErrMsgTxt ("Invalid dimension for input argument 2");
if (mxGetN(prhs[2])!=3)
mexErrMsgTxt ("Invalid dimension for input argument 3");
nchan = mxGetM(prhs[1]);
if (mxGetM(prhs[2])!=nchan)
mexErrMsgTxt ("Number of channels does not match between argument 2 and 3");
lf = mxCreateDoubleMatrix(nchan, 3, mxREAL);
lf_p = mxGetData(lf);
R = mxGetData(prhs[0]);
rm_p = mxGetData(prhs[1]);
um_p = mxGetData(prhs[2]);
eps = mxGetEps();
tmp2 = sqrt(R[0]*R[0] + R[1]*R[1] + R[2]*R[2]); /* norm(R) */
for (i=0; i<nchan; i++)
{
/* get the position of this single channel */
r[0] = rm_p[i];
r[1] = rm_p[i+nchan];
r[2] = rm_p[i+nchan+nchan];
/* get the orientation of this single channel */
u[0] = um_p[i];
u[1] = um_p[i+nchan];
u[2] = um_p[i+nchan+nchan];
/* compute the difference between this channel and the dipole position */
d[0] = r[0] - R[0];
d[1] = r[1] - R[1];
d[2] = r[2] - R[2];
tmp1 = sqrt(r[0]*r[0] + r[1]*r[1] + r[2]*r[2]); /* norm(r) */
/* tmp2 = sqrt(R[0]*R[0] + R[1]*R[1] + R[2]*R[2]); */
tmp3 = sqrt(d[0]*d[0] + d[1]*d[1] + d[2]*d[2]); /* norm(d) */
tmp4 = r[0]*R[0] + r[1]*R[1] + r[2]*R[2]; /* dot(r, R) */
tmp5 = r[0]*d[0] + r[1]*d[1] + r[2]*d[2]; /* dot(r, d) */
tmp6 = R[0]*d[0] + R[1]*d[1] + R[2]*d[2]; /* dot(R, d) */
tmp7 = tmp1*tmp2*tmp1*tmp2 - tmp4*tmp4; /* norm(cross(r,R))^2 */
alpha = 1 / (-tmp3 * (tmp1*tmp3+tmp5));
A = 1/tmp3 - 2*alpha*tmp2*tmp2 - 1/tmp1;
B = 2*alpha*tmp4;
C = -tmp6/(tmp3*tmp3*tmp3);
if (tmp7<eps)
beta = 0;
else
beta = ((A*r[0] + B*R[0] + C*d[0])*u[0] +
(A*r[1] + B*R[1] + C*d[1])*u[1] +
(A*r[2] + B*R[2] + C*d[2])*u[2]) / tmp7;
/* re-use the temporary array d for something else */
d[0] = alpha*u[0] + beta*r[0];
d[1] = alpha*u[1] + beta*r[1];
d[2] = alpha*u[2] + beta*r[2];
/* lf(i,:) = 1e-7 * cross(alpha*u + beta*r, R) */
lf_p[i ] = (d[1]*R[2] - d[2]*R[1])*1e-7;
lf_p[i+nchan ] = (d[2]*R[0] - d[0]*R[2])*1e-7;
lf_p[i+nchan+nchan] = (d[0]*R[1] - d[1]*R[0])*1e-7;
}
/* assign the output parameter */
plhs[0] = lf;
return;
}
// Function definitions.
// -----------------------------------------------------------------
void mexFunction (int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
//Outputs Args
double *x, *fval, *exitflag, *iter;
//Internal Vars
double *mptr;
size_t ndec, ndat;
int i, havJac = 0;
//LMDIF + LMDER Vars
int m; //len data
int n; //len x
double *x0; //initial guess + sol
double *fvec; //final fvec
double ftol = 1e-7; //function tolerance
double xtol = 1e-7; //iterate tolerance
double gtol = 1e-7; //gradient tolerance
int maxfev = 1500; //max fevals
double epsfcn = mxGetEps(); //FD max error
double *diag; //scaling (ignored in mode 1)
int mode = 1; //internal scaling
double factor = 100; //used for initial step bound
int nprint = -1; //no printing
int info = 0; //output status
int nfev = 0; //number of fevals
int njev = 0; //number of jevals
double *fjac; //final jacobian
int ldfjac; //ld of jac
int *ipvt; //permutation
double *qtf; //qt * fvec
double *wa1, *wa2, *wa3, *wa4; //work vectors
if (nrhs < 1) {
if(nlhs < 1)
printSolverInfo();
else
plhs[0] = mxCreateString(""); //no version info?
return;
}
//Check user inputs
checkInputs(prhs,nrhs);
//Set Defaults
citer = 1;
printLevel = 0;
iterF.enabled = false;
maxtime = 1000;
//Get Sizes
ndec = mxGetNumberOfElements(prhs[2]);
ndat = mxGetNumberOfElements(prhs[3]);
//Get Objective Function Handle
if (mxIsChar(prhs[0])) {
CHECK(mxGetString(prhs[0], fun.f, FLEN) == 0,"error reading objective name string");
fun.nrhs = 1;
fun.xrhs = 0;
} else {
fun.prhs[0] = (mxArray*)prhs[0];
strcpy(fun.f, "feval");
fun.nrhs = 2;
fun.xrhs = 1;
}
fun.prhs[fun.xrhs] = mxCreateDoubleMatrix(ndec, 1, mxREAL); //x0
//Check and Get Gradient Function Handle
if(!mxIsEmpty(prhs[1])) {
havJac = 1;
if (mxIsChar(prhs[1])) {
CHECK(mxGetString(prhs[1], grad.f, FLEN) == 0,"error reading gradient name string");
grad.nrhs = 1;
grad.xrhs = 0;
} else {
grad.prhs[0] = (mxArray*)prhs[1];
strcpy(grad.f, "feval");
grad.nrhs = 2;
grad.xrhs = 1;
}
grad.prhs[grad.xrhs] = mxCreateDoubleMatrix(ndec, 1, mxREAL); //x0
}
//Get x0 + data
x0 = mxGetPr(prhs[2]);
ydata = mxGetPr(prhs[3]);
//Get Options if specified
if(nrhs > 4) {
if(mxGetField(prhs[4],0,"display"))
printLevel = (int)*mxGetPr(mxGetField(prhs[4],0,"display"));
if(mxGetField(prhs[4],0,"maxfeval"))
maxfev = (int)*mxGetPr(mxGetField(prhs[4],0,"maxfeval"));
if(mxGetField(prhs[4],0,"maxtime"))
maxtime = *mxGetPr(mxGetField(prhs[4],0,"maxtime"));
if(mxGetField(prhs[4],0,"tolrfun"))
ftol = *mxGetPr(mxGetField(prhs[4],0,"tolrfun"));
if(mxGetField(prhs[4],0,"iterfun") && !mxIsEmpty(mxGetField(prhs[4],0,"iterfun")))
{
iterF.prhs[0] = (mxArray*)mxGetField(prhs[4],0,"iterfun");
strcpy(iterF.f, "feval");
iterF.enabled = true;
iterF.prhs[1] = mxCreateNumericMatrix(1,1,mxINT32_CLASS,mxREAL);
iterF.prhs[2] = mxCreateDoubleMatrix(1,1,mxREAL);
iterF.prhs[3] = mxCreateDoubleMatrix(ndec,1,mxREAL);
}
}
//Create Outputs
plhs[0] = mxCreateDoubleMatrix(ndec,1, mxREAL);
plhs[1] = mxCreateDoubleMatrix(1,1, mxREAL);
plhs[2] = mxCreateDoubleMatrix(1,1, mxREAL);
plhs[3] = mxCreateDoubleMatrix(1,1, mxREAL);
x = mxGetPr(plhs[0]);
fval = mxGetPr(plhs[1]);
exitflag = mxGetPr(plhs[2]);
iter = mxGetPr(plhs[3]);
//Copy initial guess to x
memcpy(x,x0,ndec*sizeof(double));
//Print Header
if(printLevel) {
mexPrintf("\n------------------------------------------------------------------\n");
if(havJac)
mexPrintf(" This is LM_DER\n");
else
mexPrintf(" This is LM_DIF\n");
mexPrintf(" Authors: Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More\n MEX Interface J. Currie 2011\n\n");
mexPrintf(" Problem Properties:\n");
mexPrintf(" # Decision Variables: %4d\n",ndec);
mexPrintf(" # Data Points: %4d\n",ndat);
mexPrintf("------------------------------------------------------------------\n");
}
//Assign Arguments
m = (int)ndat; ldfjac = m;
n = (int)ndec;
diag = mxCalloc(ndec,sizeof(double));
fvec = mxCalloc(ndat,sizeof(double));
fjac = mxCalloc(ndec*ndat,sizeof(double));
ipvt = mxCalloc(ndec,sizeof(int));
qtf = mxCalloc(ndec,sizeof(double));
//Get Work Memory
mptr = mxCalloc(3*ndec+ndat,sizeof(double)); i = 0;
wa1 = &mptr[i]; i += (int)ndec;
wa2 = &mptr[i]; i += (int)ndec;
wa3 = &mptr[i]; i += (int)ndec;
wa4 = &mptr[i];
//Start Timer
start = clock();
//Call Algorithm based on Derivatives
if(havJac)
LMDER(jacfunc,&m,&n,x,fvec,fjac,&ldfjac,&ftol,&xtol,>ol,&maxfev,diag,&mode,&factor,&nprint,&info,&nfev,&njev,ipvt,qtf,wa1,wa2,wa3,wa4);
else
LMDIF(func,&m,&n,x,fvec,&ftol,&xtol,>ol,&maxfev,&epsfcn,diag,&mode,&factor,&nprint,&info,&nfev,fjac,&ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4);
//Calculate final Rnorm
*fval = 0;
for(i=0;i<m;i++)
*fval += fvec[i]*fvec[i];
//Check if maxtime exceeded
if(((double)(end-start))/CLOCKS_PER_SEC > maxtime)
info = 15;
//Save Status & Iterations
*exitflag = getStatus(info);
*iter = (double)nfev;
//Print Header
if(printLevel){
//Termination Detected
switch((int)info)
{
//Success
case 1:
mexPrintf("\n *** SUCCESSFUL TERMINATION ***\n *** Both actual and predicted relative reductions in the sum of squares are at most ftol ***\n"); break;
case 2:
mexPrintf("\n *** SUCCESSFUL TERMINATION ***\n *** Relative error between two consecutive iterates is at most xtol ***\n"); break;
case 3:
mexPrintf("\n *** SUCCESSFUL TERMINATION ***\n *** Reductions in the sum of squares are at most ftol AND relative error between two consecutive iterates is at most xtol ***\n"); break;
case 4:
mexPrintf("\n *** SUCCESSFUL TERMINATION ***\n *** The cosine of the angle between fvec and any column of the jacobian is at most gtol in absolute value ***\n"); break;
//Error
case 0:
mexPrintf("\n *** ERROR: IMPROPER INPUT PARAMETERS ***\n"); break;
case 5:
mexPrintf("\n *** MAXIMUM FUNCTION EVALUATIONS REACHED ***\n"); break;
case 15:
mexPrintf("\n *** MAXIMUM TIME REACHED ***\n"); break;
//Early Exit
case 6:
mexPrintf("\n *** TERMINATION: EARLY EXIT ***\n *** CAUSE: ftol is too small (OPTI setting tolfun). No further improvement in the sum of squares is possible ***\n"); break;
case 7:
mexPrintf("\n *** TERMINATION: EARLY EXIT ***\n *** CAUSE: xtol is too small (OPTI hard-codes 1e-7). No further improvement in the approximate solution x is possible ***\n"); break;
case 8:
mexPrintf("\n *** TERMINATION: EARLY EXIT ***\n *** CAUSE: gtol is too small (OPTI hard-codes 1e-7). Fvec is orthogonal to the columns of the jacobian to machine precision ***\n"); break;
case -1:
mexPrintf("\n *** TERMINATION: USER EXITED ***\n"); break;
}
if(*exitflag==1)
mexPrintf("\n Final SSE: %12.5g\n In %3.0f function evaluations\n",*fval,*iter);
mexPrintf("------------------------------------------------------------------\n\n");
}
//Free Memory
if(diag) mxFree(diag);
if(fvec) mxFree(fvec);
if(fjac) mxFree(fjac);
if(ipvt) mxFree(ipvt);
if(qtf) mxFree(qtf);
if(mptr) mxFree(mptr);
}
