void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[]) {
// Test number of parameters.
if (nrhs != 6 || nlhs != 4) {
mexWarnMsgTxt("Usage: [X,rval,Obj,iter]=solveInnerProblem(W,Y,rval,MAXITER,EPS,FourTimesMaxSumSquaredWeights)");
return;
}
// get important parameters
int rows = (int)mxGetM(prhs[0]); // number of rows of W
int cols = (int)mxGetN(prhs[0]); // number of columns of W (should be the same)
int len = (int)mxGetM(prhs[1]); // the desired output
int lenrval = (int)mxGetM(prhs[2]); // rval
if(!mxIsSparse(prhs[0])) { mexWarnMsgTxt("Matrix is not sparse");}
if(rows!=cols){
mexWarnMsgTxt("Sparse matrix is not square");
return;
}
if(rows!=len){
mexWarnMsgTxt("Length of the vector is not the same as the number of the rows of the sparse matrix");
return;
}
// Create output array and compute values
double* sr = mxGetPr(prhs[0]); // get values
mwIndex* irs = mxGetIr(prhs[0]); // get row
mwIndex* jcs = mxGetJc(prhs[0]); // get columns
double* Xobs = mxGetPr(prhs[1]);
double* rval = mxGetPr(prhs[2]); // get values
int MAXITER = (int) mxGetScalar(prhs[3]);
double EPS = mxGetScalar(prhs[4]);
double MaxSumSquaredWeights = mxGetScalar(prhs[5]);
//mexPrintf("Elements: %f\n",MaxSumSquaredWeights);
if(MaxSumSquaredWeights<=0){
mexWarnMsgTxt("Lipschitz constant has to be positive");
return;
}
double *X; /* output matrix */
plhs[0] = mxCreateDoubleMatrix(len,1,mxREAL); /* create the output vector */
plhs[1] = mxCreateDoubleMatrix(lenrval,1,mxREAL); /* create the output dual variable */
plhs[2] = mxCreateDoubleMatrix(1,1,mxREAL); /* create the output objective value */
plhs[3] = mxCreateDoubleMatrix(1,1,mxREAL); /* create the final iteration value */
//plhs[1]= (mxArray *)prhs[2];
//plhs[2]= (mxArray *)prhs[3];
X = mxGetPr(plhs[0]);
double* Z = mxGetPr(plhs[1]);
double* OutputObj = mxGetPr(plhs[2]);
double* FinalIter = mxGetPr(plhs[3]);
int counter=0,i,j,start,mid,end,iter=0;
double tnew=1; double told=1,alpha,beta,factor;
double dummy,normD,normDiff,relativeChange,Fval;
double* dummyPointer;
double* D =new double[len];
double* Dold =new double[len];
for(i=0; i<len; i++) { D[i]=0; Dold[i]=0;}
double* pval = new double[lenrval];
double* pvalold = new double[lenrval];
for(i=0; i<lenrval; i++) { pval[i]=0; }
for(i=0; i<lenrval; i++) { pvalold[i]=0; }
//MaxSumSquaredWeights=max(sum(W.^2,2));
/*double MaxSumSquaredWeights=0;
for(j=0; j<cols; j++)
{
dummy=0;
for(i=0; i<jcs[j+1]-jcs[j]; i++) { dummy+=sr[j]*sr[j]; }
if(dummy>MaxSumSquaredWeights) { MaxSumSquaredWeights=dummy; }
}
MaxSumSquaredWeights=4*MaxSumSquaredWeights;*/
Fval=EPS+1;
while(iter<MAXITER && Fval > EPS)
{
//if (iter % 100 == 0){ mexPrintf("iter=%d tnew = %.5g\n",iter,tnew);}
// exchange D and Dold
dummyPointer=D; D=Dold; Dold=dummyPointer;
//mexPrintf("Exchanged D \n");
// exchange pval and pvalold
dummyPointer=pval; pval=pvalold; pvalold=dummyPointer;
//mexPrintf("Exchanged Pointer \n");
// exchange tnew and told
told=tnew;
// initialize X with zeros
for(i=0; i<len; i++) { X[i]=0; }
//mexPrintf("Initialized X, %i \n",X[0]);
//sval = lambda*wval.*rval;
//X = -sparse(jx,1,sval);
dummy=0; counter=0;
for(j=0; j<cols; j++)
{
for(i=0; i<jcs[j+1]-jcs[j]; i++)
{
dummy = sr[counter]*rval[counter];
//mexPrintf("Computed dummy, %f\n",dummy);
X[j] -= dummy;
X[irs[counter]] += dummy;
counter++;
}
}
//mexPrintf("Computed X, %f %f %f %f %f \n",X[0],X[1],X[2],X[3],X[4]);
//D = Xobs-X;
normD = 0; normDiff=0;
for(i=0; i<len; i++) { D[i]=Xobs[i]-X[i]; normD+=D[i]*D[i]; normDiff+=(D[i]-Dold[i])*(D[i]-Dold[i]);}
//pval = rval + uval.*(D(ix)-D(jx));
// pval=pval./max(abs(pval),1);
counter=0;
tnew = (1 + sqrt(1+4*told*told))/2;
factor = (told-1)/tnew;
for(j=0; j<cols; j++)
{
alpha=D[j];
for(i=0; i<jcs[j+1]-jcs[j]; i++)
{
// update of pval
beta=rval[counter] + sr[counter]*( D[irs[counter]] - alpha)/MaxSumSquaredWeights;
// projection onto l_inf-cube
if(beta>1) beta=1;
else if(beta<-1) beta=-1;
pval[counter]=beta;
// update of rval
rval[counter] = beta + factor*(beta-pvalold[counter]);
counter++;
}
}
//tkp1=(1+sqrt(1+4*tk^2))/2;
//rval = pval + (tk-1)/(tkp1)*(pval-pvalold);
/*tnew = (1 + sqrt(1+4*told*told))/2;
for(j=0; j<jcs[len]; j++)
{
alpha=pval[j];
if(alpha>1) { pval[j]=1; alpha=1; }
else if(alpha<-1) { pval[j]=-1; alpha=-1;}
rval[j] = alpha + (told-1)/tnew*(alpha-pvalold[j]);
}*/
//mexPrintf("Comp pval: %f %f %f\n",pval[0],pval[1],pval[len-1]);
//mexPrintf("Comp rval: %f %f %f\n",rval[0],rval[1],rval[len-1]);
relativeChange = sqrt(normDiff/normD);
Fval = sqrt(normD);
//mexPrintf("Iteration: %i, Fval: %1.15f, RelativeChange %1.15f\n",iter,Fval,relativeChange);
//if(iter<10 || iter % 10==0)
// mexPrintf("Iteration: %i, Fval: %1.15f, RelativeChange %1.15f\n",iter,Fval,relativeChange);
iter++;
}
//mexPrintf("FINAL: Iterations %i, Fval: %1.15f, RelativeChange %1.15f\n",iter,Fval,relativeChange);
for(i=0; i<len; i++) { X[i]=D[i];}
for(i=0; i<lenrval; i++) { Z[i]=rval[i];}
OutputObj[0]=Fval;
FinalIter[0]=iter;
//mexPrintf("iter=%d tnew = %.5g\n",iter,tnew);
delete D; delete Dold; delete pvalold; delete pval;
// loop over columns
/*for(i=0; i<jcs[j+1]-jcs[j]; i++) { // loop over rows
if(irs[counter]==j)
{
sr[counter]+=vecvals[j];
}
counter++;
}*/
/*start=counter;
end=counter+jcs[j+1]-jcs[j];
mid = floor(0.5*(start+end));
while(irs[mid]!=j)
{
if(irs[mid]>j) { end=mid; mid = floor(0.5*(start+end)); }
else {start=mid; mid = floor(0.5*(start+end)); }
//mexPrintf("Start: %i, Mid: %i, End %i \n",start,mid,end);
}
sr[mid]+=vecvals[j];
counter=counter+jcs[j+1]-jcs[j];
}*/
}
int setupAndSolveQP(NewQPControllerData *pdata, std::shared_ptr<drake::lcmt_qp_controller_input> qp_input, double t, Map<VectorXd> &q, Map<VectorXd> &qd, const Ref<Matrix<bool, Dynamic, 1>> &b_contact_force, QPControllerOutput *qp_output, std::shared_ptr<QPControllerDebugData> debug) {
// The primary solve loop for our controller. This constructs and solves a Quadratic Program and produces the instantaneous desired torques, along with reference positions, velocities, and accelerations. It mirrors the Matlab implementation in atlasControllers.InstantaneousQPController.setupAndSolveQP(), and more documentation can be found there.
// Note: argument `debug` MAY be set to NULL, which signals that no debug information is requested.
// look up the param set by name
AtlasParams *params;
std::map<string,AtlasParams>::iterator it;
it = pdata->param_sets.find(qp_input->param_set_name);
if (it == pdata->param_sets.end()) {
mexWarnMsgTxt("Got a param set I don't recognize! Using standing params instead");
it = pdata->param_sets.find("standing");
if (it == pdata->param_sets.end()) {
mexErrMsgTxt("Could not fall back to standing parameters either. I have to give up here.");
}
}
// cout << "using params set: " + it->first + ", ";
params = &(it->second);
// mexPrintf("Kp_accel: %f, ", params->Kp_accel);
int nu = pdata->B.cols();
int nq = pdata->r->num_positions;
// zmp_data
Map<Matrix<double, 4, 4, RowMajor>> A_ls(&qp_input->zmp_data.A[0][0]);
Map<Matrix<double, 4, 2, RowMajor>> B_ls(&qp_input->zmp_data.B[0][0]);
Map<Matrix<double, 2, 4, RowMajor>> C_ls(&qp_input->zmp_data.C[0][0]);
Map<Matrix<double, 2, 2, RowMajor>> D_ls(&qp_input->zmp_data.D[0][0]);
Map<Matrix<double, 4, 1>> x0(&qp_input->zmp_data.x0[0][0]);
Map<Matrix<double, 2, 1>> y0(&qp_input->zmp_data.y0[0][0]);
Map<Matrix<double, 2, 1>> u0(&qp_input->zmp_data.u0[0][0]);
Map<Matrix<double, 2, 2, RowMajor>> R_ls(&qp_input->zmp_data.R[0][0]);
Map<Matrix<double, 2, 2, RowMajor>> Qy(&qp_input->zmp_data.Qy[0][0]);
Map<Matrix<double, 4, 4, RowMajor>> S(&qp_input->zmp_data.S[0][0]);
Map<Matrix<double, 4, 1>> s1(&qp_input->zmp_data.s1[0][0]);
Map<Matrix<double, 4, 1>> s1dot(&qp_input->zmp_data.s1dot[0][0]);
// // whole_body_data
if (qp_input->whole_body_data.num_positions != nq) mexErrMsgTxt("number of positions doesn't match num_dof for this robot");
Map<VectorXd> q_des(qp_input->whole_body_data.q_des.data(), nq);
Map<VectorXd> condof(qp_input->whole_body_data.constrained_dofs.data(), qp_input->whole_body_data.num_constrained_dofs);
PIDOutput pid_out = wholeBodyPID(pdata, t, q, qd, q_des, ¶ms->whole_body);
qp_output->q_ref = pid_out.q_ref;
// mu
// NOTE: we're using the same mu for all supports
double mu;
if (qp_input->num_support_data == 0) {
mu = 1.0;
} else {
mu = qp_input->support_data[0].mu;
for (int i=1; i < qp_input->num_support_data; i++) {
if (qp_input->support_data[i].mu != mu) {
mexWarnMsgTxt("Currently, we assume that all supports have the same value of mu");
}
}
}
const int dim = 3, // 3D
nd = 2*m_surface_tangents; // for friction cone approx, hard coded for now
assert(nu+6 == nq);
vector<DesiredBodyAcceleration> desired_body_accelerations;
desired_body_accelerations.resize(qp_input->num_tracked_bodies);
Vector6d body_pose_des, body_v_des, body_vdot_des;
Vector6d body_vdot;
for (int i=0; i < qp_input->num_tracked_bodies; i++) {
int body_id0 = qp_input->body_motion_data[i].body_id - 1;
double weight = params->body_motion[body_id0].weight;
desired_body_accelerations[i].body_id0 = body_id0;
Map<Matrix<double, 6, 4,RowMajor>>coefs_rowmaj(&qp_input->body_motion_data[i].coefs[0][0]);
Matrix<double, 6, 4> coefs = coefs_rowmaj;
evaluateCubicSplineSegment(t - qp_input->body_motion_data[i].ts[0], coefs, body_pose_des, body_v_des, body_vdot_des);
desired_body_accelerations[i].body_vdot = bodyMotionPD(pdata->r, q, qd, body_id0, body_pose_des, body_v_des, body_vdot_des, params->body_motion[body_id0].Kp, params->body_motion[body_id0].Kd);
desired_body_accelerations[i].weight = weight;
desired_body_accelerations[i].accel_bounds = params->body_motion[body_id0].accel_bounds;
// mexPrintf("body: %d, vdot: %f %f %f %f %f %f weight: %f\n", body_id0,
// desired_body_accelerations[i].body_vdot(0),
// desired_body_accelerations[i].body_vdot(1),
// desired_body_accelerations[i].body_vdot(2),
// desired_body_accelerations[i].body_vdot(3),
// desired_body_accelerations[i].body_vdot(4),
// desired_body_accelerations[i].body_vdot(5),
// weight);
// mexPrintf("tracking body: %d, coefs[:,0]: %f %f %f %f %f %f coefs(", body_id0,
}
int n_body_accel_eq_constraints = 0;
for (int i=0; i < desired_body_accelerations.size(); i++) {
if (desired_body_accelerations[i].weight < 0)
n_body_accel_eq_constraints++;
}
MatrixXd R_DQyD_ls = R_ls + D_ls.transpose()*Qy*D_ls;
pdata->r->doKinematics(q,false,qd);
//---------------------------------------------------------------------
vector<SupportStateElement> available_supports = loadAvailableSupports(qp_input);
vector<SupportStateElement> active_supports = getActiveSupports(pdata->r, pdata->map_ptr, q, qd, available_supports, b_contact_force, params->contact_threshold, pdata->default_terrain_height);
int num_active_contact_pts=0;
for (vector<SupportStateElement>::iterator iter = active_supports.begin(); iter!=active_supports.end(); iter++) {
num_active_contact_pts += iter->contact_pts.size();
}
pdata->r->HandC(q,qd,(MatrixXd*)nullptr,pdata->H,pdata->C,(MatrixXd*)nullptr,(MatrixXd*)nullptr,(MatrixXd*)nullptr);
pdata->H_float = pdata->H.topRows(6);
pdata->H_act = pdata->H.bottomRows(nu);
pdata->C_float = pdata->C.head(6);
pdata->C_act = pdata->C.tail(nu);
bool include_angular_momentum = (params->W_kdot.array().maxCoeff() > 1e-10);
if (include_angular_momentum) {
pdata->r->getCMM(q,qd,pdata->Ag,pdata->Agdot);
pdata->Ak = pdata->Ag.topRows(3);
pdata->Akdot = pdata->Agdot.topRows(3);
}
Vector3d xcom;
// consider making all J's into row-major
pdata->r->getCOM(xcom);
pdata->r->getCOMJac(pdata->J);
pdata->r->getCOMJacDot(pdata->Jdot);
pdata->J_xy = pdata->J.topRows(2);
pdata->Jdot_xy = pdata->Jdot.topRows(2);
MatrixXd Jcom,Jcomdot;
if (x0.size()==6) {
Jcom = pdata->J;
Jcomdot = pdata->Jdot;
}
else {
Jcom = pdata->J_xy;
Jcomdot = pdata->Jdot_xy;
}
MatrixXd B,JB,Jp,Jpdot,normals;
int nc = contactConstraintsBV(pdata->r,num_active_contact_pts,mu,active_supports,pdata->map_ptr,B,JB,Jp,Jpdot,normals,pdata->default_terrain_height);
int neps = nc*dim;
VectorXd x_bar,xlimp;
MatrixXd D_float(6,JB.cols()), D_act(nu,JB.cols());
if (nc>0) {
if (x0.size()==6) {
// x,y,z com
xlimp.resize(6);
xlimp.topRows(3) = xcom;
xlimp.bottomRows(3) = Jcom*qd;
}
else {
xlimp.resize(4);
xlimp.topRows(2) = xcom.topRows(2);
xlimp.bottomRows(2) = Jcom*qd;
}
x_bar = xlimp-x0;
D_float = JB.topRows(6);
D_act = JB.bottomRows(nu);
}
int nf = nc*nd; // number of contact force variables
int nparams = nq+nf+neps;
Vector3d kdot_des;
if (include_angular_momentum) {
VectorXd k = pdata->Ak*qd;
kdot_des = -params->Kp_ang*k; // TODO: parameterize
}
//----------------------------------------------------------------------
// QP cost function ----------------------------------------------------
//
// min: ybar*Qy*ybar + ubar*R*ubar + (2*S*xbar + s1)*(A*x + B*u) +
// w_qdd*quad(qddot_ref - qdd) + w_eps*quad(epsilon) +
// w_grf*quad(beta) + quad(kdot_des - (A*qdd + Adot*qd))
VectorXd f(nparams);
{
if (nc > 0) {
// NOTE: moved Hqp calcs below, because I compute the inverse directly for FastQP (and sparse Hqp for gurobi)
VectorXd tmp = C_ls*xlimp;
VectorXd tmp1 = Jcomdot*qd;
MatrixXd tmp2 = R_DQyD_ls*Jcom;
pdata->fqp = tmp.transpose()*Qy*D_ls*Jcom;
// mexPrintf("fqp head: %f %f %f\n", pdata->fqp(0), pdata->fqp(1), pdata->fqp(2));
pdata->fqp += tmp1.transpose()*tmp2;
pdata->fqp += (S*x_bar + 0.5*s1).transpose()*B_ls*Jcom;
pdata->fqp -= u0.transpose()*tmp2;
pdata->fqp -= y0.transpose()*Qy*D_ls*Jcom;
pdata->fqp -= (params->whole_body.w_qdd.array()*pid_out.qddot_des.array()).matrix().transpose();
if (include_angular_momentum) {
pdata->fqp += qd.transpose()*pdata->Akdot.transpose()*params->W_kdot*pdata->Ak;
pdata->fqp -= kdot_des.transpose()*params->W_kdot*pdata->Ak;
}
f.head(nq) = pdata->fqp.transpose();
} else {
f.head(nq) = -pid_out.qddot_des;
}
}
f.tail(nf+neps) = VectorXd::Zero(nf+neps);
int neq = 6+neps+6*n_body_accel_eq_constraints+qp_input->whole_body_data.num_constrained_dofs;
MatrixXd Aeq = MatrixXd::Zero(neq,nparams);
VectorXd beq = VectorXd::Zero(neq);
// constrained floating base dynamics
// H_float*qdd - J_float'*lambda - Dbar_float*beta = -C_float
Aeq.topLeftCorner(6,nq) = pdata->H_float;
beq.topRows(6) = -pdata->C_float;
if (nc>0) {
Aeq.block(0,nq,6,nc*nd) = -D_float;
}
if (nc > 0) {
// relative acceleration constraint
Aeq.block(6,0,neps,nq) = Jp;
Aeq.block(6,nq,neps,nf) = MatrixXd::Zero(neps,nf); // note: obvious sparsity here
Aeq.block(6,nq+nf,neps,neps) = MatrixXd::Identity(neps,neps); // note: obvious sparsity here
beq.segment(6,neps) = (-Jpdot -params->Kp_accel*Jp)*qd;
}
// add in body spatial equality constraints
// VectorXd body_vdot;
MatrixXd orig = MatrixXd::Zero(4,1);
orig(3,0) = 1;
int equality_ind = 6+neps;
MatrixXd Jb(6,nq);
MatrixXd Jbdot(6,nq);
for (int i=0; i<desired_body_accelerations.size(); i++) {
if (desired_body_accelerations[i].weight < 0) { // negative implies constraint
if (!inSupport(active_supports,desired_body_accelerations[i].body_id0)) {
pdata->r->forwardJac(desired_body_accelerations[i].body_id0,orig,1,Jb);
pdata->r->forwardJacDot(desired_body_accelerations[i].body_id0,orig,1,Jbdot);
for (int j=0; j<6; j++) {
if (!std::isnan(desired_body_accelerations[i].body_vdot(j))) {
Aeq.block(equality_ind,0,1,nq) = Jb.row(j);
beq[equality_ind++] = -Jbdot.row(j)*qd + desired_body_accelerations[i].body_vdot(j);
}
}
}
}
}
if (qp_input->whole_body_data.num_constrained_dofs>0) {
// add joint acceleration constraints
for (int i=0; i<qp_input->whole_body_data.num_constrained_dofs; i++) {
Aeq(equality_ind,(int)condof[i]-1) = 1;
beq[equality_ind++] = pid_out.qddot_des[(int)condof[i]-1];
}
}
int n_ineq = 2*nu+2*6*desired_body_accelerations.size();
MatrixXd Ain = MatrixXd::Zero(n_ineq,nparams); // note: obvious sparsity here
VectorXd bin = VectorXd::Zero(n_ineq);
// linear input saturation constraints
// u=B_act'*(H_act*qdd + C_act - Jz_act'*z - Dbar_act*beta)
// using transpose instead of inverse because B is orthogonal
Ain.topLeftCorner(nu,nq) = pdata->B_act.transpose()*pdata->H_act;
Ain.block(0,nq,nu,nc*nd) = -pdata->B_act.transpose()*D_act;
bin.head(nu) = -pdata->B_act.transpose()*pdata->C_act + pdata->umax;
Ain.block(nu,0,nu,nparams) = -1*Ain.block(0,0,nu,nparams);
bin.segment(nu,nu) = pdata->B_act.transpose()*pdata->C_act - pdata->umin;
int constraint_start_index = 2*nu;
for (int i=0; i<desired_body_accelerations.size(); i++) {
pdata->r->forwardJac(desired_body_accelerations[i].body_id0,orig,1,Jb);
pdata->r->forwardJacDot(desired_body_accelerations[i].body_id0,orig,1,Jbdot);
Ain.block(constraint_start_index,0,6,pdata->r->num_positions) = Jb;
bin.segment(constraint_start_index,6) = -Jbdot*qd + desired_body_accelerations[i].accel_bounds.max;
constraint_start_index += 6;
Ain.block(constraint_start_index,0,6,pdata->r->num_positions) = -Jb;
bin.segment(constraint_start_index,6) = Jbdot*qd - desired_body_accelerations[i].accel_bounds.min;
constraint_start_index += 6;
}
for (int i=0; i<n_ineq; i++) {
// remove inf constraints---needed by gurobi
if (std::isinf(double(bin(i)))) {
Ain.row(i) = 0*Ain.row(i);
bin(i)=0;
}
}
GRBmodel * model = nullptr;
int info=-1;
// set obj,lb,up
VectorXd lb(nparams), ub(nparams);
lb.head(nq) = pdata->qdd_lb;
ub.head(nq) = pdata->qdd_ub;
lb.segment(nq,nf) = VectorXd::Zero(nf);
ub.segment(nq,nf) = 1e3*VectorXd::Ones(nf);
lb.tail(neps) = -params->slack_limit*VectorXd::Ones(neps);
ub.tail(neps) = params->slack_limit*VectorXd::Ones(neps);
VectorXd alpha(nparams);
MatrixXd Qnfdiag(nf,1), Qneps(neps,1);
vector<MatrixXd*> QBlkDiag( nc>0 ? 3 : 1 ); // nq, nf, neps // this one is for gurobi
VectorXd w = (params->whole_body.w_qdd.array() + REG).matrix();
#ifdef USE_MATRIX_INVERSION_LEMMA
double max_body_accel_weight = -numeric_limits<double>::infinity();
for (int i=0; i < desired_body_accelerations.size(); i++) {
max_body_accel_weight = max(max_body_accel_weight, desired_body_accelerations[i].weight);
}
bool include_body_accel_cost_terms = desired_body_accelerations.size() > 0 && max_body_accel_weight > 1e-10;
if (pdata->use_fast_qp > 0 && !include_angular_momentum && !include_body_accel_cost_terms)
{
// TODO: update to include angular momentum, body accel objectives.
// We want Hqp inverse, which I can compute efficiently using the
// matrix inversion lemma (see wikipedia):
// inv(A + U'CV) = inv(A) - inv(A)*U* inv([ inv(C)+ V*inv(A)*U ]) V inv(A)
if (nc>0) {
MatrixXd Wi = ((1/(params->whole_body.w_qdd.array() + REG)).matrix()).asDiagonal();
if (R_DQyD_ls.trace()>1e-15) { // R_DQyD_ls is not zero
pdata->Hqp = Wi - Wi*Jcom.transpose()*(R_DQyD_ls.inverse() + Jcom*Wi*Jcom.transpose()).inverse()*Jcom*Wi;
}
}
else {
pdata->Hqp = MatrixXd::Constant(nq,1,1/(1+REG));
}
#ifdef TEST_FAST_QP
if (nc>0) {
MatrixXd Hqp_test(nq,nq);
MatrixXd W = w.asDiagonal();
Hqp_test = (Jcom.transpose()*R_DQyD_ls*Jcom + W).inverse();
if (((Hqp_test-pdata->Hqp).array().abs()).maxCoeff() > 1e-6) {
mexErrMsgTxt("Q submatrix inverse from matrix inversion lemma does not match direct Q inverse.");
}
}
#endif
Qnfdiag = MatrixXd::Constant(nf,1,1/REG);
Qneps = MatrixXd::Constant(neps,1,1/(.001+REG));
QBlkDiag[0] = &pdata->Hqp;
if (nc>0) {
QBlkDiag[1] = &Qnfdiag;
QBlkDiag[2] = &Qneps; // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps)
}
MatrixXd Ain_lb_ub(n_ineq+2*nparams,nparams);
VectorXd bin_lb_ub(n_ineq+2*nparams);
Ain_lb_ub << Ain, // note: obvious sparsity here
-MatrixXd::Identity(nparams,nparams),
MatrixXd::Identity(nparams,nparams);
bin_lb_ub << bin, -lb, ub;
info = fastQPThatTakesQinv(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->state.active, alpha);
//if (info<0) mexPrintf("fastQP info = %d. Calling gurobi.\n", info);
}
else {
#endif
if (nc>0) {
pdata->Hqp = Jcom.transpose()*R_DQyD_ls*Jcom;
if (include_angular_momentum) {
pdata->Hqp += pdata->Ak.transpose()*params->W_kdot*pdata->Ak;
}
pdata->Hqp += params->whole_body.w_qdd.asDiagonal();
pdata->Hqp += REG*MatrixXd::Identity(nq,nq);
} else {
pdata->Hqp = (1+REG)*MatrixXd::Identity(nq,nq);
}
// add in body spatial acceleration cost terms
for (int i=0; i<desired_body_accelerations.size(); i++) {
if (desired_body_accelerations[i].weight > 0) {
if (!inSupport(active_supports,desired_body_accelerations[i].body_id0)) {
pdata->r->forwardJac(desired_body_accelerations[i].body_id0,orig,1,Jb);
pdata->r->forwardJacDot(desired_body_accelerations[i].body_id0,orig,1,Jbdot);
for (int j=0; j<6; j++) {
if (!std::isnan(desired_body_accelerations[i].body_vdot[j])) {
pdata->Hqp += desired_body_accelerations[i].weight*(Jb.row(j)).transpose()*Jb.row(j);
f.head(nq) += desired_body_accelerations[i].weight*(qd.transpose()*Jbdot.row(j).transpose() - desired_body_accelerations[i].body_vdot[j])*Jb.row(j).transpose();
}
}
}
}
}
Qnfdiag = MatrixXd::Constant(nf,1,params->w_grf+REG);
Qneps = MatrixXd::Constant(neps,1,params->w_slack+REG);
QBlkDiag[0] = &pdata->Hqp;
if (nc>0) {
QBlkDiag[1] = &Qnfdiag;
QBlkDiag[2] = &Qneps; // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps)
}
MatrixXd Ain_lb_ub(n_ineq+2*nparams,nparams);
VectorXd bin_lb_ub(n_ineq+2*nparams);
Ain_lb_ub << Ain, // note: obvious sparsity here
-MatrixXd::Identity(nparams,nparams),
MatrixXd::Identity(nparams,nparams);
bin_lb_ub << bin, -lb, ub;
if (pdata->use_fast_qp > 0)
{ // set up and call fastqp
info = fastQP(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->state.active, alpha);
//if (info<0) mexPrintf("fastQP info=%d... calling Gurobi.\n", info);
}
else {
// use gurobi active set
model = gurobiActiveSetQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->state.vbasis,pdata->state.vbasis_len,pdata->state.cbasis,pdata->state.cbasis_len,alpha);
CGE(GRBgetintattr(model,"NumVars",&(pdata->state.vbasis_len)), pdata->env);
CGE(GRBgetintattr(model,"NumConstrs",&(pdata->state.cbasis_len)), pdata->env);
info=66;
//info = -1;
}
if (info<0) {
model = gurobiQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->state.active,alpha);
int status; CGE(GRBgetintattr(model, "Status", &status), pdata->env);
//if (status!=2) mexPrintf("Gurobi reports non-optimal status = %d\n", status);
}
#ifdef USE_MATRIX_INVERSION_LEMMA
}
#endif
//----------------------------------------------------------------------
// Solve for inputs ----------------------------------------------------
qp_output->qdd = alpha.head(nq);
VectorXd beta = alpha.segment(nq,nc*nd);
// use transpose because B_act is orthogonal
qp_output->u = pdata->B_act.transpose()*(pdata->H_act*qp_output->qdd + pdata->C_act - D_act*beta);
//y = pdata->B_act.jacobiSvd(ComputeThinU|ComputeThinV).solve(pdata->H_act*qdd + pdata->C_act - Jz_act.transpose()*lambda - D_act*beta);
bool foot_contact[2];
foot_contact[0] = b_contact_force(pdata->rpc.body_ids.r_foot) == 1;
foot_contact[1] = b_contact_force(pdata->rpc.body_ids.l_foot) == 1;
qp_output->qd_ref = velocityReference(pdata, t, q, qd, qp_output->qdd, foot_contact, &(params->vref_integrator), &(pdata->rpc));
// Remember t for next time around
pdata->state.t_prev = t;
// If a debug pointer was passed in, fill it with useful data
if (debug) {
debug->active_supports.resize(active_supports.size());
for (int i=0; i < active_supports.size(); i++) {
debug->active_supports[i] = active_supports[i];
}
debug->nc = nc;
debug->normals = normals;
debug->B = B;
debug->alpha = alpha;
debug->f = f;
debug->Aeq = Aeq;
debug->beq = beq;
debug->Ain_lb_ub = Ain_lb_ub;
debug->bin_lb_ub = bin_lb_ub;
debug->Qnfdiag = Qnfdiag;
debug->Qneps = Qneps;
debug->x_bar = x_bar;
debug->S = S;
debug->s1 = s1;
debug->s1dot = s1dot;
debug->s2dot = qp_input->zmp_data.s2dot;
debug->A_ls = A_ls;
debug->B_ls = B_ls;
debug->Jcom = Jcom;
debug->Jcomdot = Jcomdot;
debug->beta = beta;
}
// if we used gurobi, clean up
if (model) {
GRBfreemodel(model);
}
// GRBfreeenv(env);
return info;
}
/*
* s e t u p A u x i l i a r y I n p u t s
*/
returnValue setupAuxiliaryInputs( const mxArray* auxInput, unsigned int nV, unsigned int nC,
HessianType* hessianType, double** x0, double** guessedBounds, double** guessedConstraints, double** R
)
{
mxArray* curField = 0;
double* hessianTypeTmp;
int hessianTypeInt;
/* hessianType */
curField = mxGetField( auxInput,0,"hessianType" );
if ( curField == NULL )
mexWarnMsgTxt( "auxInput struct does not contain entry 'hessianType'!\n Type 'help qpOASES_auxInput' for further information." );
else
{
if ( mxIsEmpty(curField) == true )
{
*hessianType = HST_UNKNOWN;
}
else
{
if ( mxIsScalar(curField) == false )
return RET_INVALID_ARGUMENTS;
hessianTypeTmp = mxGetPr(curField);
hessianTypeInt = (int)*hessianTypeTmp;
if ( hessianTypeInt < 0 )
hessianTypeInt = 6; /* == HST_UNKNOWN */
if ( hessianTypeInt > 5 )
hessianTypeInt = 6; /* == HST_UNKNOWN */
*hessianType = (REFER_NAMESPACE_QPOASES HessianType)hessianTypeInt;
}
}
/* x0 */
curField = mxGetField( auxInput,0,"x0" );
if ( curField == NULL )
mexWarnMsgTxt( "auxInput struct does not contain entry 'x0'!\n Type 'help qpOASES_auxInput' for further information." );
else
{
*x0 = mxGetPr(curField);
if ( smartDimensionCheck( x0,nV,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
return RET_INVALID_ARGUMENTS;
}
/* guessedWorkingSetB */
curField = mxGetField( auxInput,0,"guessedWorkingSetB" );
if ( curField == NULL )
mexWarnMsgTxt( "auxInput struct does not contain entry 'guessedWorkingSetB'!\n Type 'help qpOASES_auxInput' for further information." );
else
{
*guessedBounds = mxGetPr(curField);
if ( smartDimensionCheck( guessedBounds,nV,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
return RET_INVALID_ARGUMENTS;
}
/* guessedWorkingSetC */
curField = mxGetField( auxInput,0,"guessedWorkingSetC" );
if ( curField == NULL )
mexWarnMsgTxt( "auxInput struct does not contain entry 'guessedWorkingSetC'!\n Type 'help qpOASES_auxInput' for further information." );
else
{
*guessedConstraints = mxGetPr(curField);
if ( smartDimensionCheck( guessedConstraints,nC,1, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
return RET_INVALID_ARGUMENTS;
}
/* R */
curField = mxGetField( auxInput,0,"R" );
if ( curField == NULL )
mexWarnMsgTxt( "auxInput struct does not contain entry 'R'!\n Type 'help qpOASES_auxInput' for further information." );
else
{
*R = mxGetPr(curField);
if ( smartDimensionCheck( R,nV,nV, BT_TRUE,((const mxArray**)&curField),0 ) != SUCCESSFUL_RETURN )
return RET_INVALID_ARGUMENTS;
}
return SUCCESSFUL_RETURN;
}
//--------------------------------------------------------------
// function: mexFunction - Entry point from Matlab environment
// INPUTS:
// nlhs - number of left hand side arguments (outputs)
// plhs[] - pointer to table where created matrix pointers are
// to be placed
// nrhs - number of right hand side arguments (inputs)
// prhs[] - pointer to table of input matrices
//--------------------------------------------------------------
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
ELEMENT_ATTRIBUTES Attributes;
int ByteCount; // Total number of bytes required for C structure
int Alignment = 8;
int SwapFlag; // Byte Swapping Flag
int LittleEndian = 0; // 0/1=local machine is big/litle Endian
int ii;
// Determine local byte ordering
{
unsigned char pattern[2] = {0x12, 0x34};
unsigned short word1 = 0x1234;
unsigned short word2 = *(unsigned short *)&pattern;
LittleEndian = (word1 == word2) ? 0 : 1;
}
SwapFlag = LittleEndian; // Default = assume data is big endian
if (nlhs == 0)
MatlabCStruct_DebugFlag = 1;
else
MatlabCStruct_DebugFlag = 0;
if (nrhs < 1)
mexErrMsgTxt("Not enough input arguments.");
if (nrhs >= 3)
{
for (ii=2; ii<nrhs; ii++)
{
// Double = alignment specifier
if (mxIsDouble(prhs[ii]))
{
if (mxGetNumberOfElements(prhs[ii]) != 1)
mexErrMsgTxt("Alignment specifier must be single element (1, 2, 4, or 8)");
Alignment = (int)*mxGetPr(prhs[ii]);
if (Alignment != 1 && Alignment != 2 && Alignment != 4 && Alignment != 8)
mexWarnMsgTxt("Alignment specifier normally has of value of 1, 2, 4, or 8");
}
else if (mxIsChar(prhs[ii]))
{
char ch;
if (mxGetNumberOfElements(prhs[ii]) != 1)
mexErrMsgTxt("Byte-ordering specifier must be single element ('n','l','b' or 'r')");
ch = *(char *)mxGetData(prhs[ii]);
switch(ch)
{
case 'n': SwapFlag = 0; break; // Native byte ordering requested
case 'b': SwapFlag = LittleEndian; break; // Big Endian data
case 'l': SwapFlag = !LittleEndian; break; // Big Endian data
case 'r': SwapFlag = 1; break; // Reverse byte ordering
default: mexErrMsgTxt("Byte-ordering specifier must be one if 'n','l','b' or 'r'");
}
}
else
mexErrMsgTxt("Optional Arguments must be either Aligmnent Specifier (1,2,4,8), or Byte-orderin Specifier ('n','l','b','r')");
}
}
Attributes = MatlabCStruct(prhs[0], Alignment, 0, (unsigned char *)-1, NULL, SwapFlag);
ByteCount = Attributes.ElementCount*Attributes.StorageSize;
if (nrhs >= 2 && !mxIsEmpty(prhs[1]))
{
mxClassID ClassID;
ClassID = mxGetClassID(prhs[1]);
if (ClassID != mxUINT8_CLASS && ClassID != mxINT8_CLASS)
mexErrMsgTxt("Byte data vector must be of type INT8 or UINT8");
if (ByteCount != mxGetNumberOfElements(prhs[1]))
{
char msg[100];
sprintf(msg, "Number of elements in Byte data vector (%d) does not match structure size (%d)",
mxGetNumberOfElements(prhs[1]), ByteCount);
mexErrMsgTxt(msg);
}
plhs[0] = mxDuplicateArray(prhs[0]);
MatlabCStruct(plhs[0], Alignment, 0, NULL, mxGetData(prhs[1]), SwapFlag);
}
else
// Create output structure for Attributes
{
char *FieldNames[] = {"size","align"};
plhs[0] = mxCreateStructMatrix(1, 1, 2, FieldNames);
mxSetField(plhs[0], 0, "size", mxCreateScalarDouble(ByteCount));
mxSetField(plhs[0], 0, "align", mxCreateScalarDouble(Attributes.Alignment));
if (nlhs >= 2)
{
// Extract C Data from array
plhs[1] = mxCreateNumericMatrix(1, ByteCount, mxUINT8_CLASS, mxREAL);
MatlabCStruct(prhs[0], Alignment, 0, mxGetData(plhs[1]), NULL, SwapFlag);
}
}
} // end mexFunction()
void
read_input_parameters (int argc,char *argv[],
long *verbosity,long *struct_verbosity,
STRUCT_LEARN_PARM *struct_parm,
LEARN_PARM *learn_parm, KERNEL_PARM *kernel_parm,
int *alg_type)
{
long i ;
(*alg_type)=DEFAULT_ALG_TYPE;
/* SVM struct options */
(*struct_verbosity)=1;
struct_parm->C=-0.01;
struct_parm->slack_norm=1;
struct_parm->epsilon=DEFAULT_EPS;
struct_parm->custom_argc=0;
struct_parm->loss_function=DEFAULT_LOSS_FCT;
struct_parm->loss_type=DEFAULT_RESCALING;
struct_parm->newconstretrain=100;
struct_parm->ccache_size=5;
struct_parm->batch_size=100;
/* SVM light options */
(*verbosity)=0;
strcpy (learn_parm->predfile, "trans_predictions");
strcpy (learn_parm->alphafile, "");
learn_parm->biased_hyperplane=1;
learn_parm->remove_inconsistent=0;
learn_parm->skip_final_opt_check=0;
learn_parm->svm_maxqpsize=10;
learn_parm->svm_newvarsinqp=0;
learn_parm->svm_iter_to_shrink=-9999;
learn_parm->maxiter=100000;
learn_parm->kernel_cache_size=40;
learn_parm->svm_c=99999999; /* overridden by struct_parm->C */
learn_parm->eps=0.001; /* overridden by struct_parm->epsilon */
learn_parm->transduction_posratio=-1.0;
learn_parm->svm_costratio=1.0;
learn_parm->svm_costratio_unlab=1.0;
learn_parm->svm_unlabbound=1E-5;
learn_parm->epsilon_crit=0.001;
learn_parm->epsilon_a=1E-10; /* changed from 1e-15 */
learn_parm->compute_loo=0;
learn_parm->rho=1.0;
learn_parm->xa_depth=0;
kernel_parm->kernel_type=0;
kernel_parm->poly_degree=3;
kernel_parm->rbf_gamma=1.0;
kernel_parm->coef_lin=1;
kernel_parm->coef_const=1;
strcpy (kernel_parm->custom,"empty");
/* Parse -x options, delegat --x ones */
for(i=1;(i<argc) && ((argv[i])[0] == '-');i++) {
switch ((argv[i])[1])
{
case 'a': i++; strcpy(learn_parm->alphafile,argv[i]); break;
case 'c': i++; struct_parm->C=atof(argv[i]); break;
case 'p': i++; struct_parm->slack_norm=atol(argv[i]); break;
case 'e': i++; struct_parm->epsilon=atof(argv[i]); break;
case 'k': i++; struct_parm->newconstretrain=atol(argv[i]); break;
case 'h': i++; learn_parm->svm_iter_to_shrink=atol(argv[i]); break;
case '#': i++; learn_parm->maxiter=atol(argv[i]); break;
case 'm': i++; learn_parm->kernel_cache_size=atol(argv[i]); break;
case 'w': i++; (*alg_type)=atol(argv[i]); break;
case 'o': i++; struct_parm->loss_type=atol(argv[i]); break;
case 'n': i++; learn_parm->svm_newvarsinqp=atol(argv[i]); break;
case 'q': i++; learn_parm->svm_maxqpsize=atol(argv[i]); break;
case 'l': i++; struct_parm->loss_function=atol(argv[i]); break;
case 'f': i++; struct_parm->ccache_size=atol(argv[i]); break;
case 'b': i++; struct_parm->batch_size=atof(argv[i]); break;
case 't': i++; kernel_parm->kernel_type=atol(argv[i]); break;
case 'd': i++; kernel_parm->poly_degree=atol(argv[i]); break;
case 'g': i++; kernel_parm->rbf_gamma=atof(argv[i]); break;
case 's': i++; kernel_parm->coef_lin=atof(argv[i]); break;
case 'r': i++; kernel_parm->coef_const=atof(argv[i]); break;
case 'u': i++; strcpy(kernel_parm->custom,argv[i]); break;
case 'v': i++; (*struct_verbosity)=atol(argv[i]); break;
case 'y': i++; (*verbosity)=atol(argv[i]); break;
case '-':
strcpy(struct_parm->custom_argv[struct_parm->custom_argc++],argv[i]);
i++;
strcpy(struct_parm->custom_argv[struct_parm->custom_argc++],argv[i]);
break;
default:
{
char msg [1024+1] ;
#ifndef WIN
snprintf(msg, sizeof(msg)/sizeof(char),
"Unrecognized option '%s'",argv[i]) ;
#else
sprintf(msg, sizeof(msg)/sizeof(char),
"Unrecognized option '%s'",argv[i]) ;
#endif
mexErrMsgTxt(msg) ;
}
}
}
/* whatever is left is an error */
if (i < argc) {
char msg [1024+1] ;
#ifndef WIN
snprintf(msg, sizeof(msg)/sizeof(char),
"Unrecognized argument '%s'", argv[i]) ;
#else
sprintf(msg, sizeof(msg)/sizeof(char),
"Unrecognized argument '%s'", argv[i]) ;
#endif
mexErrMsgTxt(msg) ;
}
/* Check parameter validity */
if(learn_parm->svm_iter_to_shrink == -9999) {
learn_parm->svm_iter_to_shrink=100;
}
if((learn_parm->skip_final_opt_check)
&& (kernel_parm->kernel_type == LINEAR)) {
mexWarnMsgTxt("It does not make sense to skip the final optimality check for linear kernels.");
learn_parm->skip_final_opt_check=0;
}
if((learn_parm->skip_final_opt_check)
&& (learn_parm->remove_inconsistent)) {
mexErrMsgTxt("It is necessary to do the final optimality check when removing inconsistent examples.");
}
if((learn_parm->svm_maxqpsize<2)) {
char msg [1025] ;
#ifndef WIN
snprintf(msg, sizeof(msg)/sizeof(char),
"Maximum size of QP-subproblems not in valid range: %ld [2..]",learn_parm->svm_maxqpsize) ;
#else
sprintf(msg, sizeof(msg)/sizeof(char),
"Maximum size of QP-subproblems not in valid range: %ld [2..]",learn_parm->svm_maxqpsize) ;
#endif
mexErrMsgTxt(msg) ;
}
if((learn_parm->svm_maxqpsize<learn_parm->svm_newvarsinqp)) {
char msg [1025] ;
#ifndef WIN
snprintf(msg, sizeof(msg)/sizeof(char),
"Maximum size of QP-subproblems [%ld] must be larger than the number of"
" new variables [%ld] entering the working set in each iteration.",
learn_parm->svm_maxqpsize, learn_parm->svm_newvarsinqp) ;
#else
sprintf(msg, sizeof(msg)/sizeof(char),
"Maximum size of QP-subproblems [%ld] must be larger than the number of"
" new variables [%ld] entering the working set in each iteration.",
learn_parm->svm_maxqpsize, learn_parm->svm_newvarsinqp) ;
#endif
mexErrMsgTxt(msg) ;
}
if(learn_parm->svm_iter_to_shrink<1) {
char msg [1025] ;
#ifndef WIN
snprintf(msg, sizeof(msg)/sizeof(char),
"Maximum number of iterations for shrinking not in valid range: %ld [1,..]",
learn_parm->svm_iter_to_shrink);
#else
sprintf(msg, sizeof(msg)/sizeof(char),
"Maximum number of iterations for shrinking not in valid range: %ld [1,..]",
learn_parm->svm_iter_to_shrink);
#endif
mexErrMsgTxt(msg) ;
}
if(struct_parm->C<0) {
mexErrMsgTxt("You have to specify a value for the parameter '-c' (C>0)!");
}
if(((*alg_type) < 0) || (((*alg_type) > 5) && ((*alg_type) != 9))) {
mexErrMsgTxt("Algorithm type must be either '0', '1', '2', '3', '4', or '9'!");
}
if(learn_parm->transduction_posratio>1) {
mexErrMsgTxt("The fraction of unlabeled examples to classify as positives must "
"be less than 1.0 !!!");
}
if(learn_parm->svm_costratio<=0) {
mexErrMsgTxt("The COSTRATIO parameter must be greater than zero!");
}
if(struct_parm->epsilon<=0) {
mexErrMsgTxt("The epsilon parameter must be greater than zero!");
}
if((struct_parm->ccache_size<=0) && ((*alg_type) == 4)) {
mexErrMsgTxt("The cache size must be at least 1!");
}
if(((struct_parm->batch_size<=0) || (struct_parm->batch_size>100))
&& ((*alg_type) == 4)) {
mexErrMsgTxt("The batch size must be in the interval ]0,100]!");
}
if((struct_parm->slack_norm<1) || (struct_parm->slack_norm>2)) {
mexErrMsgTxt("The norm of the slacks must be either 1 (L1-norm) or 2 (L2-norm)!");
}
if((struct_parm->loss_type != SLACK_RESCALING)
&& (struct_parm->loss_type != MARGIN_RESCALING)) {
mexErrMsgTxt("The loss type must be either 1 (slack rescaling) or 2 (margin rescaling)!");
}
if(learn_parm->rho<0) {
mexErrMsgTxt("The parameter rho for xi/alpha-estimates and leave-one-out pruning must"
" be greater than zero (typically 1.0 or 2.0, see T. Joachims, Estimating the"
" Generalization Performance of an SVM Efficiently, ICML, 2000.)!");
}
if((learn_parm->xa_depth<0) || (learn_parm->xa_depth>100)) {
mexErrMsgTxt("The parameter depth for ext. xi/alpha-estimates must be in [0..100] (zero"
"for switching to the conventional xa/estimates described in T. Joachims,"
"Estimating the Generalization Performance of an SVM Efficiently, ICML, 2000.)") ;
}
parse_struct_parameters (struct_parm) ;
}
void mcx_set_field(const mxArray *root,const mxArray *item,int idx, Config *cfg){
const char *name=mxGetFieldNameByNumber(root,idx);
const int *arraydim;
char *jsonshapes=NULL;
int i,j;
cfg->flog=stderr;
GET_1ST_FIELD(cfg,nphoton)
GET_ONE_FIELD(cfg,nblocksize)
GET_ONE_FIELD(cfg,nthread)
GET_ONE_FIELD(cfg,seed)
GET_ONE_FIELD(cfg,tstart)
GET_ONE_FIELD(cfg,tstep)
GET_ONE_FIELD(cfg,tend)
GET_ONE_FIELD(cfg,maxdetphoton)
GET_ONE_FIELD(cfg,sradius)
GET_ONE_FIELD(cfg,maxgate)
GET_ONE_FIELD(cfg,respin)
GET_ONE_FIELD(cfg,gpuid)
GET_ONE_FIELD(cfg,isreflect)
GET_ONE_FIELD(cfg,isref3)
GET_ONE_FIELD(cfg,isrefint)
GET_ONE_FIELD(cfg,isnormalized)
GET_ONE_FIELD(cfg,issavedet)
GET_ONE_FIELD(cfg,issave2pt)
GET_ONE_FIELD(cfg,isgpuinfo)
GET_ONE_FIELD(cfg,issrcfrom0)
GET_ONE_FIELD(cfg,autopilot)
GET_ONE_FIELD(cfg,minenergy)
GET_ONE_FIELD(cfg,unitinmm)
GET_VEC3_FIELD(cfg,srcpos)
GET_VEC3_FIELD(cfg,srcdir)
GET_VEC3_FIELD(cfg,steps)
GET_VEC3_FIELD(cfg,crop0)
GET_VEC3_FIELD(cfg,crop1)
else if(strcmp(name,"vol")==0){
if(!mxIsUint8(item) || mxGetNumberOfDimensions(item)!=3 )
mexErrMsgTxt("the 'vol' field must be a 3D uint8 array");
arraydim=mxGetDimensions(item);
for(i=0;i<3;i++) ((unsigned int *)(&cfg->dim))[i]=arraydim[i];
if(cfg->vol) free(cfg->vol);
cfg->vol=(unsigned char *)malloc(cfg->dim.x*cfg->dim.y*cfg->dim.z);
memcpy(cfg->vol,mxGetData(item),cfg->dim.x*cfg->dim.y*cfg->dim.z);
printf("mcx.dim=[%d %d %d];\n",cfg->dim.x,cfg->dim.y,cfg->dim.z);
}else if(strcmp(name,"detpos")==0){
arraydim=mxGetDimensions(item);
if(arraydim[0]>0 && arraydim[1]!=4)
mexErrMsgTxt("the 'detpos' field must have 4 columns (x,y,z,radius)");
double *val=mxGetPr(item);
cfg->detnum=arraydim[0];
if(cfg->detpos) free(cfg->detpos);
cfg->detpos=(float4 *)malloc(cfg->detnum*sizeof(float4));
for(j=0;j<4;j++)
for(i=0;i<cfg->detnum;i++)
((float *)(&cfg->detpos[i]))[j]=val[j*cfg->detnum+i];
printf("mcx.detnum=%d;\n",cfg->detnum);
}else if(strcmp(name,"prop")==0){
arraydim=mxGetDimensions(item);
if(arraydim[0]>0 && arraydim[1]!=4)
mexErrMsgTxt("the 'prop' field must have 4 columns (mua,mus,g,n)");
double *val=mxGetPr(item);
cfg->medianum=arraydim[0];
if(cfg->prop) free(cfg->prop);
cfg->prop=(Medium *)malloc(cfg->medianum*sizeof(Medium));
for(j=0;j<4;j++)
for(i=0;i<cfg->medianum;i++)
((float *)(&cfg->prop[i]))[j]=val[j*cfg->medianum+i];
printf("mcx.medianum=%d;\n",cfg->medianum);
}else if(strcmp(name,"session")==0){
int len=mxGetNumberOfElements(item);
if(!mxIsChar(item) || len==0)
mexErrMsgTxt("the 'session' field must be a non-empty string");
if(len>MAX_SESSION_LENGTH)
mexErrMsgTxt("the 'session' field is too long");
int status = mxGetString(item, cfg->session, MAX_SESSION_LENGTH);
if (status != 0)
mexWarnMsgTxt("not enough space. string is truncated.");
printf("mcx.session='%s';\n",cfg->session);
}else if(strcmp(name,"shapes")==0){
int len=mxGetNumberOfElements(item);
if(!mxIsChar(item) || len==0)
mexErrMsgTxt("the 'shapes' field must be a non-empty string");
jsonshapes=new char[len+1];
mxGetString(item, jsonshapes, len+1);
jsonshapes[len]='\0';
}else{
printf("WARNING: redundant field '%s'\n",name);
}
if(jsonshapes){
Grid3D grid={&(cfg->vol),&(cfg->dim),{1.f,1.f,1.f},0};
if(cfg->issrcfrom0) memset(&(grid.orig.x),0,sizeof(float3));
int status=mcx_parse_shapestring(&grid,jsonshapes);
delete [] jsonshapes;
if(status){
mexErrMsgTxt(mcx_last_shapeerror());
}
}
}
int buffer_puthdr(int server, mxArray * plhs[], const mxArray * prhs[])
{
int fieldnumber;
mxArray *field;
int result;
message_t request;
messagedef_t request_def;
message_t *response = NULL;
headerdef_t header_def;
ft_chunkdef_t chunk_def;
/* allocate the request message */
request.def = &request_def;
request.buf = NULL;
request_def.version = VERSION;
request_def.command = PUT_HDR;
request_def.bufsize = 0;
/* define the header, it has the fields "nchans", "nsamples", "nevents", "fsample", "data_type" */
if (mxGetNumberOfElements(prhs[0])!=1)
mexErrMsgTxt("Only one header can be put into the buffer at a time.");
fieldnumber = mxGetFieldNumber(prhs[0], "nchans");
if (fieldnumber<0)
mexErrMsgTxt("field 'nchans' is missing");
field = mxGetFieldByNumber(prhs[0], 0, fieldnumber);
if (!mxIsNumeric(field) || mxIsEmpty(field))
mexErrMsgTxt("invalid data type for 'nchans'");
header_def.nchans = (UINT32_T)mxGetScalar(field) ;
fieldnumber = mxGetFieldNumber(prhs[0], "nsamples");
if (fieldnumber<0)
mexErrMsgTxt("field 'nsamples' is missing");
field = mxGetFieldByNumber(prhs[0], 0, fieldnumber);
if (!mxIsNumeric(field) || mxIsEmpty(field))
mexErrMsgTxt("invalid data type for 'nsamples'");
header_def.nsamples = (UINT32_T)mxGetScalar(field) ;
fieldnumber = mxGetFieldNumber(prhs[0], "nevents");
if (fieldnumber<0)
mexErrMsgTxt("field is missing 'nevents'");
field = mxGetFieldByNumber(prhs[0], 0, fieldnumber);
if (!mxIsNumeric(field) || mxIsEmpty(field))
mexErrMsgTxt("invalid data type for 'nevents'");
header_def.nevents = (UINT32_T)mxGetScalar(field) ;
fieldnumber = mxGetFieldNumber(prhs[0], "fsample");
if (fieldnumber<0)
mexErrMsgTxt("field is missing 'fsample'");
field = mxGetFieldByNumber(prhs[0], 0, fieldnumber);
if (!mxIsNumeric(field) || mxIsEmpty(field))
mexErrMsgTxt("invalid data type for 'fsample'");
header_def.fsample = (float)mxGetScalar(field) ;
fieldnumber = mxGetFieldNumber(prhs[0], "data_type");
if (fieldnumber<0)
mexErrMsgTxt("field 'data_type' is missing");
field = mxGetFieldByNumber(prhs[0], 0, fieldnumber);
if (!mxIsNumeric(field) || mxIsEmpty(field))
mexErrMsgTxt("invalid data type for 'data_type'");
header_def.data_type = (UINT32_T)mxGetScalar(field) ;
/* construct a PUT_HDR request */
request_def.bufsize = ft_mx_append(&request.buf, request_def.bufsize, &header_def, sizeof(headerdef_t));
/* append existing chunks to request.buf, set correct header_def.bufsize at the end */
fieldnumber = mxGetFieldNumber(prhs[0], "nifti_1");
if (fieldnumber>=0) {
field = mxGetFieldByNumber(prhs[0], 0, fieldnumber);
if (!mxIsUint8(field) || mxGetNumberOfElements(field)!=SIZE_NIFTI_1) {
mexWarnMsgTxt("invalid data type for field 'nifti_1' -- ignoring");
} else {
chunk_def.size = SIZE_NIFTI_1;
chunk_def.type = FT_CHUNK_NIFTI1;
request_def.bufsize = ft_mx_append(&request.buf, request_def.bufsize, &chunk_def, sizeof(chunk_def));
request_def.bufsize = ft_mx_append(&request.buf, request_def.bufsize, mxGetData(field), chunk_def.size);
}
}
fieldnumber = mxGetFieldNumber(prhs[0], "siemensap");
if (fieldnumber>=0) {
field = mxGetFieldByNumber(prhs[0], 0, fieldnumber);
if (!mxIsUint8(field)) {
mexWarnMsgTxt("invalid data type for field 'siemensap' -- ignoring");
} else {
chunk_def.size = mxGetNumberOfElements(field);
chunk_def.type = FT_CHUNK_SIEMENS_AP;
request_def.bufsize = ft_mx_append(&request.buf, request_def.bufsize, &chunk_def, sizeof(chunk_def));
request_def.bufsize = ft_mx_append(&request.buf, request_def.bufsize, mxGetData(field), chunk_def.size);
}
}
fieldnumber = mxGetFieldNumber(prhs[0], "ctf_res4");
if (fieldnumber>=0) {
field = mxGetFieldByNumber(prhs[0], 0, fieldnumber);
if (!mxIsUint8(field)) {
mexWarnMsgTxt("invalid data type for field 'ctf_res4' -- ignoring");
} else {
chunk_def.size = mxGetNumberOfElements(field);
chunk_def.type = FT_CHUNK_CTF_RES4;
request_def.bufsize = ft_mx_append(&request.buf, request_def.bufsize, &chunk_def, sizeof(chunk_def));
request_def.bufsize = ft_mx_append(&request.buf, request_def.bufsize, mxGetData(field), chunk_def.size);
}
}
fieldnumber = mxGetFieldNumber(prhs[0], "channel_names");
if (fieldnumber>=0) {
ft_chunk_t *chunk = NULL;
field = mxGetFieldByNumber(prhs[0], 0, fieldnumber);
chunk = encodeChannelNames(field, header_def.nchans);
if (chunk == NULL) {
mexWarnMsgTxt("invalid data type for field 'channel_names' -- ignoring.");
} else {
request_def.bufsize = ft_mx_append(&request.buf, request_def.bufsize, chunk, sizeof(ft_chunkdef_t) + chunk->def.size);
mxFree(chunk);
}
}
fieldnumber = mxGetFieldNumber(prhs[0], "resolutions");
if (fieldnumber>=0) {
ft_chunk_t *chunk = NULL;
field = mxGetFieldByNumber(prhs[0], 0, fieldnumber);
chunk = encodeResolutions(field, header_def.nchans);
if (chunk == NULL) {
mexWarnMsgTxt("invalid data type for field 'resolutions' -- ignoring.");
} else {
request_def.bufsize = ft_mx_append(&request.buf, request_def.bufsize, chunk, sizeof(ft_chunkdef_t) + chunk->def.size);
mxFree(chunk);
}
}
/* header->def->bufsize is the request->def->bufsize - sizeof(header->def) */
((headerdef_t *) request.buf)->bufsize = request_def.bufsize - sizeof(headerdef_t);
/* write the request, read the response */
result = clientrequest(server, &request, &response);
/* the request structure is not needed any more, but only .buf needs to be free'd */
if (request.buf != NULL) mxFree(request.buf);
if (result == 0) {
/* check that the response is PUT_OK */
if (!response)
mexErrMsgTxt("unknown error in response\n");
if (!response->def) {
FREE(response->buf);
FREE(response);
mexErrMsgTxt("unknown error in response\n");
}
if (response->def->command!=PUT_OK) {
result = response->def->command;
}
}
/* the response structure is not needed any more */
if (response) {
FREE(response->def);
FREE(response->buf);
FREE(response);
}
return result;
}
void addChunksToMatrix(mxArray *S, const char *buf, int bufsize, int numChannels) {
int bufpos = 0;
int numBlobs = 0;
mxArray *blobs[MAX_NUM_BLOBS];
mxArray *keyval = NULL;
mxArray *A;
int field;
while (bufpos + sizeof(ft_chunkdef_t) <= bufsize) {
ft_chunk_t *chunk = (ft_chunk_t *) (buf + bufpos);
/* "chunk" now points to the right location, make sure it has a valid size definition */
if (bufpos + sizeof(ft_chunkdef_t) + chunk->def.size > bufsize) {
printf("Invalid chunk size (%i) in Fieldtrip header detected. Stopping to parse.\n", chunk->def.size);
break;
}
switch (chunk->def.type) {
case FT_CHUNK_CHANNEL_NAMES:
field = addIfNew(S, "channel_names");
if (field < 0) break;
A = channelNames2Cell(chunk->data, chunk->def.size, numChannels);
mxSetFieldByNumber(S, 0, field, A);
break;
case FT_CHUNK_NIFTI1:
if (chunk->def.size != SIZE_NIFTI_1) {
mexWarnMsgTxt("Invalid NIFTI-1 chunk detected. Skipping.");
break;
}
field = addIfNew(S, "nifti_1");
if (field < 0) break;
/* pass on as 348 bytes (uint8), should be decoded on Matlab level (?) */
A = mxCreateNumericMatrix(1, SIZE_NIFTI_1, mxUINT8_CLASS, mxREAL);
memcpy(mxGetData(A), chunk->data, SIZE_NIFTI_1);
mxSetFieldByNumber(S, 0, field, A);
break;
case FT_CHUNK_SIEMENS_AP:
field = addIfNew(S, "siemensap");
if (field < 0) break;
/* pass on as uint8, should be decoded on Matlab level (?) */
A = mxCreateNumericMatrix(1, chunk->def.size, mxUINT8_CLASS, mxREAL);
memcpy(mxGetData(A), chunk->data, chunk->def.size);
mxSetFieldByNumber(S, 0, field, A);
break;
case FT_CHUNK_CTF_RES4:
field = addIfNew(S, "ctf_res4");
if (field < 0) break;
/* pass on as uint8, should be decoded on Matlab level (?) */
A = mxCreateNumericMatrix(1, chunk->def.size, mxUINT8_CLASS, mxREAL);
memcpy(mxGetData(A), chunk->data, chunk->def.size);
mxSetFieldByNumber(S, 0, field, A);
break;
case FT_CHUNK_RESOLUTIONS:
field = addIfNew(S, "resolutions");
if (field >=0) {
int nc = chunk->def.size / sizeof(double);
/* If the chunk is buggy and there are less resolution values present,
we only fill in those we have. If there are more, we only fill in numChannels.
So the returned 'resolutions' field will always match the number of channels in the buffer.
*/
if (nc>numChannels) nc = numChannels;
A = mxCreateDoubleMatrix(numChannels, 1, mxREAL);
memcpy(mxGetPr(A), chunk->data, nc*sizeof(double));
}
break;
case FT_CHUNK_UNSPECIFIED:
default:
if (numBlobs < MAX_NUM_BLOBS) {
/* pass on the binary(?) blob as an uint8 matrix */
A = mxCreateNumericMatrix(chunk->def.size, (chunk->def.size>0)?1:0, mxUINT8_CLASS, mxREAL);
memcpy(mxGetData(A), chunk->data, chunk->def.size);
blobs[numBlobs++] = A;
} else {
mexWarnMsgTxt("Encountered too many unspecified chunks in header. Skipping this one.");
}
}
/* jump to next chunk */
bufpos += chunk->def.size + sizeof(ft_chunkdef_t);
}
if (numBlobs > 0) {
int i;
field = addIfNew(S, "unspecified_blob");
if (field < 0) return;
A = mxCreateCellMatrix(numBlobs,1);
for (i=0;i<numBlobs;i++) {
mxSetCell(A, i, blobs[i]);
}
mxSetFieldByNumber(S, 0, field, A);
}
}
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]){
int i;
if(nrhs!=1){
mexPrintf("\nwriteFileNifti(niftiStruct)\n\n");
mexPrintf("Writes a NIFTI image based on fields of a structure that resembles\n");
mexPrintf("the NIFTI 1 standard (see http://nifti.nimh.nih.gov/nifti-1/ ).\n");
mexPrintf("See readFileNifti for details about the expected niftiStruct.\n\n");
return;
}else if(nlhs>0) { mexPrintf("Too many output arguments"); return; }
/* The first arg must be a nifti struct. */
if(!mxIsStruct(prhs[0])) mexErrMsgTxt("First arg must be a nifti struct.");
const mxArray *mxnim = prhs[0];
// Sanity check that this is a complete NIFTI struct
if(mxGetField(mxnim,0,"fname")==NULL || mxGetField(mxnim,0,"data")==NULL)
myErrMsg("First argument must be a proper NIFTI struct (see readFileNifti).\n\n");
/* Create an empty NIFTI C struct */
nifti_image *nim = (nifti_image *)mxCalloc(1, sizeof(nifti_image));
if(!nim) myErrMsg("failed to allocate nifti image");
nim->nifti_type = 1; // We only support single-file NIFTI format
/* Load the C-struct with fields from the matlab struct */
mxArray *fname = mxGetField(mxnim,0,"fname");
mxArray *data = mxGetField(mxnim,0,"data");
// fname field needs to be allocated
int buflen = (mxGetN(fname)) + 1;
nim->fname = (char *)mxCalloc(buflen, sizeof(char));
if(mxGetString(fname, nim->fname, buflen))
mexWarnMsgTxt("Not enough space- fname string is truncated.");
nim->iname = NULL;
nim->data = mxGetData(data);
nim->ndim = mxGetNumberOfDimensions(data);
nim->dim[0] = nim->ndim;
const int *dims = mxGetDimensions(data);
for(i=0; i<nim->ndim; i++) nim->dim[i+1] = dims[i];
for(i=nim->ndim+1; i<8; i++) nim->dim[i] = 1;
// Why do I have to assign these explicitly?
nim->nx = nim->dim[1];
nim->ny = nim->dim[2];
nim->nz = nim->dim[3];
nim->nt = nim->dim[4];
nim->nu = nim->dim[5];
nim->nv = nim->dim[6];
nim->nw = nim->dim[7];
//for(i=0; i<8; i++) mexPrintf("%d ",nim->dim[i]); mexPrintf("\n\n");
nim->nvox = mxGetNumberOfElements(data);
// *** TO DO: we should support DT_RGB24 type (triplet of uint8)
if(mxIsComplex(data)){
switch(mxGetClassID(data)){
case mxSINGLE_CLASS: nim->datatype=DT_COMPLEX64; nim->nbyper=8; break;
case mxDOUBLE_CLASS: nim->datatype=DT_COMPLEX128; nim->nbyper=16; break;
default: myErrMsg("Unknown data type!");
}
}else{
switch(mxGetClassID(data)){
case mxUINT8_CLASS: nim->datatype=DT_UINT8; nim->nbyper=1; break;
case mxINT8_CLASS: nim->datatype=DT_INT8; nim->nbyper=1; break;
case mxUINT16_CLASS: nim->datatype=DT_UINT16; nim->nbyper=2; break;
case mxINT16_CLASS: nim->datatype=DT_INT16; nim->nbyper=2; break;
case mxUINT32_CLASS: nim->datatype=DT_UINT32; nim->nbyper=4; break;
case mxINT32_CLASS: nim->datatype=DT_INT32; nim->nbyper=4; break;
case mxUINT64_CLASS: nim->datatype=DT_UINT64; nim->nbyper=8; break;
case mxINT64_CLASS: nim->datatype=DT_INT64; nim->nbyper=8; break;
case mxSINGLE_CLASS: nim->datatype=DT_FLOAT32; nim->nbyper=4; break;
case mxDOUBLE_CLASS: nim->datatype=DT_FLOAT64; nim->nbyper=8; break;
default: mexErrMsgTxt("Unknown data type!");
}
}
double *pdPtr = (double *)mxGetData(mxGetField(mxnim,0,"pixdim"));
int nPixDim = mxGetM(mxGetField(mxnim,0,"pixdim"))*mxGetN(mxGetField(mxnim,0,"pixdim"));
if(nPixDim>8) nPixDim=8;
for(i=0; i<nPixDim; i++) nim->pixdim[i+1] = (float)pdPtr[i];
// xxx dla fixed bug below (i was not being assigned).
// for(nPixDim+1; i<8; i++) nim->pixdim[i] = (float)1.0;
for(i = nPixDim+1; i<8; i++) nim->pixdim[i] = (float)1.0;
nim->dx = nim->pixdim[1];
nim->dy = nim->pixdim[2];
nim->dz = nim->pixdim[3];
nim->dt = nim->pixdim[4];
nim->du = nim->pixdim[5];
nim->dv = nim->pixdim[6];
nim->dw = nim->pixdim[7];
nim->scl_slope = mxGetScalar(mxGetField(mxnim,0,"scl_slope"));
nim->scl_inter = mxGetScalar(mxGetField(mxnim,0,"scl_inter"));
nim->cal_min = mxGetScalar(mxGetField(mxnim,0,"cal_min"));
nim->cal_max = mxGetScalar(mxGetField(mxnim,0,"cal_max"));
nim->qform_code = (int)mxGetScalar(mxGetField(mxnim,0,"qform_code"));
nim->sform_code = (int)mxGetScalar(mxGetField(mxnim,0,"sform_code"));
nim->freq_dim = (int)mxGetScalar(mxGetField(mxnim,0,"freq_dim"));
nim->phase_dim = (int)mxGetScalar(mxGetField(mxnim,0,"phase_dim"));
nim->slice_dim = (int)mxGetScalar(mxGetField(mxnim,0,"slice_dim"));
nim->slice_code = (int)mxGetScalar(mxGetField(mxnim,0,"slice_code"));
nim->slice_start = (int)mxGetScalar(mxGetField(mxnim,0,"slice_start"));
nim->slice_end = (int)mxGetScalar(mxGetField(mxnim,0,"slice_end"));
nim->slice_duration = mxGetScalar(mxGetField(mxnim,0,"slice_duration"));
/* if qform_code > 0, the quatern_*, qoffset_*, and qfac fields determine
* the qform output, NOT the qto_xyz matrix; if you want to compute these
* fields from the qto_xyz matrix, you can use the utility function
* nifti_mat44_to_quatern() */
nim->quatern_b = mxGetScalar(mxGetField(mxnim,0,"quatern_b"));
nim->quatern_c = mxGetScalar(mxGetField(mxnim,0,"quatern_c"));
nim->quatern_d = mxGetScalar(mxGetField(mxnim,0,"quatern_d"));
nim->qoffset_x = mxGetScalar(mxGetField(mxnim,0,"qoffset_x"));
nim->qoffset_y = mxGetScalar(mxGetField(mxnim,0,"qoffset_y"));
nim->qoffset_z = mxGetScalar(mxGetField(mxnim,0,"qoffset_z"));
nim->qfac = mxGetScalar(mxGetField(mxnim,0,"qfac"));
nim->pixdim[0] = nim->qfac; // pixdim[0] is the same as qfac (again- why duplicate the field?)
double *sxPtr = (double *)mxGetData(mxGetField(mxnim,0,"sto_xyz"));
for(i=0; i<16; i++) nim->sto_xyz.m[i%4][i/4] = (float)sxPtr[i];
nim->toffset = mxGetScalar(mxGetField(mxnim,0,"toffset"));
// Allow units to be specified as a string
char str[16];
mxGetString(mxGetField(mxnim,0,"xyz_units"),str,16);
nim->xyz_units = getNiftiUnitCode(str);
mxGetString(mxGetField(mxnim,0,"time_units"),str,16);
nim->time_units = getNiftiUnitCode(str);
nim->intent_code = (int)mxGetScalar(mxGetField(mxnim,0,"intent_code"));
nim->intent_p1 = mxGetScalar(mxGetField(mxnim,0,"intent_p1"));
nim->intent_p2 = mxGetScalar(mxGetField(mxnim,0,"intent_p2"));
nim->intent_p3 = mxGetScalar(mxGetField(mxnim,0,"intent_p3"));
mxGetString(mxGetField(mxnim,0,"intent_name"), nim->intent_name, 16);
mxGetString(mxGetField(mxnim,0,"descrip"), nim->descrip, 80);
mxGetString(mxGetField(mxnim,0,"aux_file"), nim->aux_file, 24);
// *** TO DO: support extended header fileds!
nim->num_ext = 0;
/* I assume that we can rely on the nifti routine to byte-swap for us? */
nifti_image_write(nim);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *Prhs[])
{
register int i;
register double *pdbl;
mxArray **prhs=(mxArray **)&Prhs[0], *At, *Ct;
struct mexdata mdata;
int len, status;
double *p, *p0, *ret, *x;
int m, n, havejac, Arows, Crows, itmax, nopts, mintype, nextra;
double opts[LM_OPTS_SZ]={LM_INIT_MU, LM_STOP_THRESH, LM_STOP_THRESH, LM_STOP_THRESH, LM_DIFF_DELTA};
double info[LM_INFO_SZ];
double *lb=NULL, *ub=NULL, *A=NULL, *b=NULL, *wghts=NULL, *C=NULL, *d=NULL, *covar=NULL;
/* parse input args; start by checking their number */
if((nrhs<5))
matlabFmtdErrMsgTxt("levmar: at least 5 input arguments required (got %d).", nrhs);
if(nlhs>4)
matlabFmtdErrMsgTxt("levmar: too many output arguments (max. 4, got %d).", nlhs);
else if(nlhs<2)
matlabFmtdErrMsgTxt("levmar: too few output arguments (min. 2, got %d).", nlhs);
/* note that in order to accommodate optional args, prhs & nrhs are adjusted accordingly below */
/** func **/
/* first argument must be a string , i.e. a char row vector */
if(mxIsChar(prhs[0])!=1)
mexErrMsgTxt("levmar: first argument must be a string.");
if(mxGetM(prhs[0])!=1)
mexErrMsgTxt("levmar: first argument must be a string (i.e. char row vector).");
/* store supplied name */
len=mxGetN(prhs[0])+1;
mdata.fname=mxCalloc(len, sizeof(char));
status=mxGetString(prhs[0], mdata.fname, len);
if(status!=0)
mexErrMsgTxt("levmar: not enough space. String is truncated.");
/** jac (optional) **/
/* check whether second argument is a string */
if(mxIsChar(prhs[1])==1){
if(mxGetM(prhs[1])!=1)
mexErrMsgTxt("levmar: second argument must be a string (i.e. row vector).");
/* store supplied name */
len=mxGetN(prhs[1])+1;
mdata.jacname=mxCalloc(len, sizeof(char));
status=mxGetString(prhs[1], mdata.jacname, len);
if(status!=0)
mexErrMsgTxt("levmar: not enough space. String is truncated.");
havejac=1;
++prhs;
--nrhs;
}
else{
mdata.jacname=NULL;
havejac=0;
}
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: %s analytic Jacobian\n", havejac? "with" : "no");
#endif /* DEBUG */
/* CHECK
if(!mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) || !(mxGetM(prhs[1])==1 && mxGetN(prhs[1])==1))
*/
/** p0 **/
/* the second required argument must be a real row or column vector */
if(!mxIsDouble(prhs[1]) || mxIsComplex(prhs[1]) || !(mxGetM(prhs[1])==1 || mxGetN(prhs[1])==1))
mexErrMsgTxt("levmar: p0 must be a real vector.");
p0=mxGetPr(prhs[1]);
/* determine if we have a row or column vector and retrieve its
* size, i.e. the number of parameters
*/
if(mxGetM(prhs[1])==1){
m=mxGetN(prhs[1]);
mdata.isrow_p0=1;
}
else{
m=mxGetM(prhs[1]);
mdata.isrow_p0=0;
}
/* copy input parameter vector to avoid destroying it */
p=mxMalloc(m*sizeof(double));
for(i=0; i<m; ++i)
p[i]=p0[i];
/** x **/
/* the third required argument must be a real row or column vector */
if(!mxIsDouble(prhs[2]) || mxIsComplex(prhs[2]) || !(mxGetM(prhs[2])==1 || mxGetN(prhs[2])==1))
mexErrMsgTxt("levmar: x must be a real vector.");
x=mxGetPr(prhs[2]);
n=__MAX__(mxGetM(prhs[2]), mxGetN(prhs[2]));
/** itmax **/
/* the fourth required argument must be a scalar */
if(!mxIsDouble(prhs[3]) || mxIsComplex(prhs[3]) || mxGetM(prhs[3])!=1 || mxGetN(prhs[3])!=1)
mexErrMsgTxt("levmar: itmax must be a scalar.");
itmax=(int)mxGetScalar(prhs[3]);
/** opts **/
/* the fifth required argument must be a real row or column vector */
if(!mxIsDouble(prhs[4]) || mxIsComplex(prhs[4]) || (!(mxGetM(prhs[4])==1 || mxGetN(prhs[4])==1) &&
!(mxGetM(prhs[4])==0 && mxGetN(prhs[4])==0)))
mexErrMsgTxt("levmar: opts must be a real vector.");
pdbl=mxGetPr(prhs[4]);
nopts=__MAX__(mxGetM(prhs[4]), mxGetN(prhs[4]));
if(nopts!=0){ /* if opts==[], nothing needs to be done and the defaults are used */
if(nopts>LM_OPTS_SZ)
matlabFmtdErrMsgTxt("levmar: opts must have at most %d elements, got %d.", LM_OPTS_SZ, nopts);
else if(nopts<((havejac)? LM_OPTS_SZ-1 : LM_OPTS_SZ))
matlabFmtdWarnMsgTxt("levmar: only the %d first elements of opts specified, remaining set to defaults.", nopts);
for(i=0; i<nopts; ++i)
opts[i]=pdbl[i];
}
#ifdef DEBUG
else{
fflush(stderr);
fprintf(stderr, "LEVMAR: empty options vector, using defaults\n");
}
#endif /* DEBUG */
/** mintype (optional) **/
/* check whether sixth argument is a string */
if(nrhs>=6 && mxIsChar(prhs[5])==1 && mxGetM(prhs[5])==1){
char *minhowto;
/* examine supplied name */
len=mxGetN(prhs[5])+1;
minhowto=mxCalloc(len, sizeof(char));
status=mxGetString(prhs[5], minhowto, len);
if(status!=0)
mexErrMsgTxt("levmar: not enough space. String is truncated.");
for(i=0; minhowto[i]; ++i)
minhowto[i]=tolower(minhowto[i]);
if(!strncmp(minhowto, "unc", 3)) mintype=MIN_UNCONSTRAINED;
else if(!strncmp(minhowto, "bc", 2)) mintype=MIN_CONSTRAINED_BC;
else if(!strncmp(minhowto, "lec", 3)) mintype=MIN_CONSTRAINED_LEC;
else if(!strncmp(minhowto, "blec", 4)) mintype=MIN_CONSTRAINED_BLEC;
else if(!strncmp(minhowto, "bleic", 5)) mintype=MIN_CONSTRAINED_BLEIC;
else if(!strncmp(minhowto, "blic", 4)) mintype=MIN_CONSTRAINED_BLIC;
else if(!strncmp(minhowto, "leic", 4)) mintype=MIN_CONSTRAINED_LEIC;
else if(!strncmp(minhowto, "lic", 3)) mintype=MIN_CONSTRAINED_BLIC;
else matlabFmtdErrMsgTxt("levmar: unknown minimization type '%s'.", minhowto);
mxFree(minhowto);
++prhs;
--nrhs;
}
else
mintype=MIN_UNCONSTRAINED;
if(mintype==MIN_UNCONSTRAINED) goto extraargs;
/* arguments below this point are optional and their presence depends
* upon the minimization type determined above
*/
/** lb, ub **/
if(nrhs>=7 && (mintype==MIN_CONSTRAINED_BC || mintype==MIN_CONSTRAINED_BLEC || mintype==MIN_CONSTRAINED_BLIC || mintype==MIN_CONSTRAINED_BLEIC)){
/* check if the next two arguments are real row or column vectors */
if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && (mxGetM(prhs[5])==1 || mxGetN(prhs[5])==1)){
if(mxIsDouble(prhs[6]) && !mxIsComplex(prhs[6]) && (mxGetM(prhs[6])==1 || mxGetN(prhs[6])==1)){
if((i=__MAX__(mxGetM(prhs[5]), mxGetN(prhs[5])))!=m)
matlabFmtdErrMsgTxt("levmar: lb must have %d elements, got %d.", m, i);
if((i=__MAX__(mxGetM(prhs[6]), mxGetN(prhs[6])))!=m)
matlabFmtdErrMsgTxt("levmar: ub must have %d elements, got %d.", m, i);
lb=mxGetPr(prhs[5]);
ub=mxGetPr(prhs[6]);
prhs+=2;
nrhs-=2;
}
}
}
/** A, b **/
if(nrhs>=7 && (mintype==MIN_CONSTRAINED_LEC || mintype==MIN_CONSTRAINED_BLEC || mintype==MIN_CONSTRAINED_LEIC || mintype==MIN_CONSTRAINED_BLEIC)){
/* check if the next two arguments are a real matrix and a real row or column vector */
if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && mxGetM(prhs[5])>=1 && mxGetN(prhs[5])>=1){
if(mxIsDouble(prhs[6]) && !mxIsComplex(prhs[6]) && (mxGetM(prhs[6])==1 || mxGetN(prhs[6])==1)){
if((i=mxGetN(prhs[5]))!=m)
matlabFmtdErrMsgTxt("levmar: A must have %d columns, got %d.", m, i);
if((i=__MAX__(mxGetM(prhs[6]), mxGetN(prhs[6])))!=(Arows=mxGetM(prhs[5])))
matlabFmtdErrMsgTxt("levmar: b must have %d elements, got %d.", Arows, i);
At=prhs[5];
b=mxGetPr(prhs[6]);
A=getTranspose(At);
prhs+=2;
nrhs-=2;
}
}
}
/* wghts */
/* check if we have a weights vector */
if(nrhs>=6 && mintype==MIN_CONSTRAINED_BLEC){ /* only check if we have seen both box & linear constraints */
if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && (mxGetM(prhs[5])==1 || mxGetN(prhs[5])==1)){
if(__MAX__(mxGetM(prhs[5]), mxGetN(prhs[5]))==m){
wghts=mxGetPr(prhs[5]);
++prhs;
--nrhs;
}
}
}
/** C, d **/
if(nrhs>=7 && (mintype==MIN_CONSTRAINED_BLEIC || mintype==MIN_CONSTRAINED_BLIC || mintype==MIN_CONSTRAINED_LEIC || mintype==MIN_CONSTRAINED_LIC)){
/* check if the next two arguments are a real matrix and a real row or column vector */
if(mxIsDouble(prhs[5]) && !mxIsComplex(prhs[5]) && mxGetM(prhs[5])>=1 && mxGetN(prhs[5])>=1){
if(mxIsDouble(prhs[6]) && !mxIsComplex(prhs[6]) && (mxGetM(prhs[6])==1 || mxGetN(prhs[6])==1)){
if((i=mxGetN(prhs[5]))!=m)
matlabFmtdErrMsgTxt("levmar: C must have %d columns, got %d.", m, i);
if((i=__MAX__(mxGetM(prhs[6]), mxGetN(prhs[6])))!=(Crows=mxGetM(prhs[5])))
matlabFmtdErrMsgTxt("levmar: d must have %d elements, got %d.", Crows, i);
Ct=prhs[5];
d=mxGetPr(prhs[6]);
C=getTranspose(Ct);
prhs+=2;
nrhs-=2;
}
}
}
/* arguments below this point are assumed to be extra arguments passed
* to every invocation of the fitting function and its Jacobian
*/
extraargs:
/* handle any extra args and allocate memory for
* passing the current parameter estimate to matlab
*/
nextra=nrhs-5;
mdata.nrhs=nextra+1;
mdata.rhs=(mxArray **)mxMalloc(mdata.nrhs*sizeof(mxArray *));
for(i=0; i<nextra; ++i)
mdata.rhs[i+1]=(mxArray *)prhs[nrhs-nextra+i]; /* discard 'const' modifier */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: %d extra args\n", nextra);
#endif /* DEBUG */
if(mdata.isrow_p0){ /* row vector */
mdata.rhs[0]=mxCreateDoubleMatrix(1, m, mxREAL);
/*
mxSetM(mdata.rhs[0], 1);
mxSetN(mdata.rhs[0], m);
*/
}
else{ /* column vector */
mdata.rhs[0]=mxCreateDoubleMatrix(m, 1, mxREAL);
/*
mxSetM(mdata.rhs[0], m);
mxSetN(mdata.rhs[0], 1);
*/
}
/* ensure that the supplied function & Jacobian are as expected */
if(checkFuncAndJacobian(p, m, n, havejac, &mdata)){
status=LM_ERROR;
goto cleanup;
}
if(nlhs>3) /* covariance output required */
covar=mxMalloc(m*m*sizeof(double));
/* invoke levmar */
switch(mintype){
case MIN_UNCONSTRAINED: /* no constraints */
if(havejac)
status=dlevmar_der(func, jacfunc, p, x, m, n, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_dif(func, p, x, m, n, itmax, opts, info, NULL, covar, (void *)&mdata);
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_der()/dlevmar_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_BC: /* box constraints */
if(havejac)
status=dlevmar_bc_der(func, jacfunc, p, x, m, n, lb, ub, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_bc_dif(func, p, x, m, n, lb, ub, itmax, opts, info, NULL, covar, (void *)&mdata);
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_bc_der()/dlevmar_bc_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_LEC: /* linear equation constraints */
#ifdef HAVE_LAPACK
if(havejac)
status=dlevmar_lec_der(func, jacfunc, p, x, m, n, A, b, Arows, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_lec_dif(func, p, x, m, n, A, b, Arows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
mexErrMsgTxt("levmar: no linear constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_lec_der()/dlevmar_lec_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_BLEC: /* box & linear equation constraints */
#ifdef HAVE_LAPACK
if(havejac)
status=dlevmar_blec_der(func, jacfunc, p, x, m, n, lb, ub, A, b, Arows, wghts, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_blec_dif(func, p, x, m, n, lb, ub, A, b, Arows, wghts, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
mexErrMsgTxt("levmar: no box & linear constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_blec_der()/dlevmar_blec_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_BLEIC: /* box, linear equation & inequalities constraints */
#ifdef HAVE_LAPACK
if(havejac)
status=dlevmar_bleic_der(func, jacfunc, p, x, m, n, lb, ub, A, b, Arows, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_bleic_dif(func, p, x, m, n, lb, ub, A, b, Arows, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
mexErrMsgTxt("levmar: no box, linear equation & inequality constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_bleic_der()/dlevmar_bleic_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_BLIC: /* box, linear inequalities constraints */
#ifdef HAVE_LAPACK
if(havejac)
status=dlevmar_bleic_der(func, jacfunc, p, x, m, n, lb, ub, NULL, NULL, 0, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_bleic_dif(func, p, x, m, n, lb, ub, NULL, NULL, 0, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
mexErrMsgTxt("levmar: no box & linear inequality constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_blic_der()/dlevmar_blic_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_LEIC: /* linear equation & inequalities constraints */
#ifdef HAVE_LAPACK
if(havejac)
status=dlevmar_bleic_der(func, jacfunc, p, x, m, n, NULL, NULL, A, b, Arows, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_bleic_dif(func, p, x, m, n, NULL, NULL, A, b, Arows, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
mexErrMsgTxt("levmar: no linear equation & inequality constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_leic_der()/dlevmar_leic_dif()\n");
#endif /* DEBUG */
break;
case MIN_CONSTRAINED_LIC: /* linear inequalities constraints */
#ifdef HAVE_LAPACK
if(havejac)
status=dlevmar_bleic_der(func, jacfunc, p, x, m, n, NULL, NULL, NULL, NULL, 0, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
else
status=dlevmar_bleic_dif(func, p, x, m, n, NULL, NULL, NULL, NULL, 0, C, d, Crows, itmax, opts, info, NULL, covar, (void *)&mdata);
#else
mexErrMsgTxt("levmar: no linear equation & inequality constraints support, HAVE_LAPACK was not defined during MEX-file compilation.");
#endif /* HAVE_LAPACK */
#ifdef DEBUG
fflush(stderr);
fprintf(stderr, "LEVMAR: calling dlevmar_lic_der()/dlevmar_lic_dif()\n");
#endif /* DEBUG */
break;
default:
mexErrMsgTxt("levmar: unexpected internal error.");
}
#ifdef DEBUG
fflush(stderr);
printf("LEVMAR: minimization returned %d in %g iter, reason %g\n\tSolution: ", status, info[5], info[6]);
for(i=0; i<m; ++i)
printf("%.7g ", p[i]);
printf("\n\n\tMinimization info:\n\t");
for(i=0; i<LM_INFO_SZ; ++i)
printf("%g ", info[i]);
printf("\n");
#endif /* DEBUG */
/* copy back return results */
/** ret **/
plhs[0]=mxCreateDoubleMatrix(1, 1, mxREAL);
ret=mxGetPr(plhs[0]);
ret[0]=(double)status;
/** popt **/
plhs[1]=(mdata.isrow_p0==1)? mxCreateDoubleMatrix(1, m, mxREAL) : mxCreateDoubleMatrix(m, 1, mxREAL);
pdbl=mxGetPr(plhs[1]);
for(i=0; i<m; ++i)
pdbl[i]=p[i];
/** info **/
if(nlhs>2){
plhs[2]=mxCreateDoubleMatrix(1, LM_INFO_SZ, mxREAL);
pdbl=mxGetPr(plhs[2]);
for(i=0; i<LM_INFO_SZ; ++i)
pdbl[i]=info[i];
}
/** covar **/
if(nlhs>3){
plhs[3]=mxCreateDoubleMatrix(m, m, mxREAL);
pdbl=mxGetPr(plhs[3]);
for(i=0; i<m*m; ++i) /* covariance matrices are symmetric, thus no need to transpose! */
pdbl[i]=covar[i];
}
cleanup:
/* cleanup */
mxDestroyArray(mdata.rhs[0]);
if(A) mxFree(A);
if(C) mxFree(C);
mxFree(mdata.fname);
if(havejac) mxFree(mdata.jacname);
mxFree(p);
mxFree(mdata.rhs);
if(covar) mxFree(covar);
if(status==LM_ERROR)
mexWarnMsgTxt("levmar: optimization returned with an error!");
}
input_t get_input(int nlhs,int nrhs,const mxArray *prhs[]) {
input_t input;
int n,i;
double *x,*ry_temp,*iy_temp,*third,fourth,fifth;
COMPLEX_T *y;
input.stop_params.threshold = DEFAULT_THRESHOLD;
input.stop_params.tolerance = DEFAULT_TOLERANCE;
input.allocated_x=0;
#ifdef _ALT_MEXERRMSGTXT_
input.error_flag=0;
#endif
input.max_imfs=0;
input.nbphases=DEFAULT_NBPHASES;
/* argument checking*/
if (nrhs>5)
mexErrMsgTxt("Too many arguments");
if (nrhs<2)
mexErrMsgTxt("Not enough arguments");
if (nlhs>2)
mexErrMsgTxt("Too many output arguments");
if (!mxIsEmpty(prhs[0]))
if (!mxIsNumeric(prhs[0]) || mxIsComplex(prhs[0]) ||
mxIsSparse(prhs[0]) || !mxIsDouble(prhs[0]) ||
(mxGetNumberOfDimensions(prhs[0]) > 2))
mexErrMsgTxt("X must be either empty or a double precision real vector.");
if (!mxIsNumeric(prhs[1]) || !mxIsComplex(prhs[1]) ||
mxIsSparse(prhs[1]) || !mxIsDouble(prhs[1]) ||/* length of vector x */
(mxGetNumberOfDimensions(prhs[1]) > 2))
mexErrMsgTxt("Y must be a double precision complex vector.");
/* input reading: x and y */
n=GREATER(mxGetN(prhs[1]),mxGetM(prhs[1])); /* length of vector x */
if (mxIsEmpty(prhs[0])) {
input.allocated_x = 1;
x = (double *)malloc(n*sizeof(double));
for(i=0;i<n;i++) x[i] = i;
}
else
x=mxGetPr(prhs[0]);
ry_temp=mxGetPr(prhs[1]);
iy_temp=mxGetPi(prhs[1]);
/* third argument */
if (nrhs>=3) {
if(!mxIsEmpty(prhs[2])) {
if (!mxIsNumeric(prhs[2]) || mxIsComplex(prhs[2]) || mxIsSparse(prhs[2])
|| !mxIsDouble(prhs[2]) || (mxGetN(prhs[2])!=1 && mxGetM(prhs[2])!=1))
mexPrintf("STOP must be a real vector of 1 or 2 elements");
i = GREATER(mxGetN(prhs[2]),mxGetM(prhs[2]));
if (i>2)
mexErrMsgTxt("STOP must be a vector of 1 or 2 elements");
third=mxGetPr(prhs[2]);
switch (i) {
case 1 : {
input.stop_params.threshold=*third;
break;
}
case 2 : {
input.stop_params.threshold=third[0];
input.stop_params.tolerance=third[1];
}
}
/* input checking */
if (input.stop_params.threshold <= 0)
mexErrMsgTxt("threshold must be a positive number");
if (input.stop_params.threshold >= 1)
mexWarnMsgTxt("threshold should be lower than 1");
if (input.stop_params.tolerance < 0 || input.stop_params.tolerance >= 1)
mexErrMsgTxt("tolerance must be a real number in [O,1]");
}
}
/* fourth argument */
if (nrhs>=4) {
if (!mxIsEmpty(prhs[3])) { /* if empty -> do nothing */
if (!mxIsNumeric(prhs[3]) || mxIsComplex(prhs[3]) || mxIsSparse(prhs[3])
|| !mxIsDouble(prhs[3]) || mxGetN(prhs[3])!=1 || mxGetM(prhs[3])!=1)
mexErrMsgTxt("NB_IMFS must be a positive integer");
fourth=*mxGetPr(prhs[3]);
if ((unsigned int)fourth != fourth)
mexErrMsgTxt("NB_IMFS must be a positive integer");
input.max_imfs=(int)fourth;
}
}
/* fifth argument */
if (nrhs==5) {
if(!mxIsNumeric(prhs[4]) || mxIsComplex(prhs[4]) || mxIsSparse(prhs[4])
|| mxGetN(prhs[4])!=1 || mxGetM(prhs[4])!=1)
mexErrMsgTxt("NBPHASES must be a positive integer");
fifth=*mxGetPr(prhs[4]);
if ((int)fifth != fifth)
mexErrMsgTxt("NBPHASES must be a positive integer");
input.nbphases = (int)fifth;
}
/* more input checking */
if (!input.allocated_x && SMALLER(mxGetN(prhs[0]),mxGetM(prhs[0]))!=1 ||
SMALLER(mxGetN(prhs[1]),mxGetM(prhs[1]))!=1)
mexErrMsgTxt("X and Y must be vectors");
if (GREATER(mxGetN(prhs[1]),mxGetM(prhs[1]))!=n)
mexErrMsgTxt("X and Y must have the same length");
i=1;
while (i<n && x[i]>x[i-1]) i++;
if (i<n) mexErrMsgTxt("Values in X must be non decreasing");
/* copy vector y to avoid erasing input data */
y=(COMPLEX_T *)malloc(n*sizeof(COMPLEX_T));
#ifdef C99_OK
for (i=0;i<n;i++) y[i]=ry_temp[i]+I*iy_temp[i];
#else
for (i=0;i<n;i++) {
y[i].r=ry_temp[i];
y[i].i=iy_temp[i];
}
#endif
input.n=n;
input.x=x;
input.y=y;
return input;
}
maxpool3d* factory_mp3d_homebrew::parse_and_create(int no, mxArray *vo[], int ni, mxArray const *vi[])
{
if (ni < 1)
throw mp3d_ex("Too few input arguments.");
// fprop or bprop?
maxpool3d holder;
int opt_beg = -1;
xpuMxArrayTW::DEV_TYPE dt;
if (no == 2) { // fprop
holder.X.setMxArray( (mxArray*) vi[0] ); // we won't change it
dt = holder.X.getDevice();
if ( ni < 1 || (holder.X.getElemType() != mxSINGLE_CLASS) )
throw mp3d_ex("For fprop(), there should be at least one input, X, of SINGLE type,"
"be all gpuArray or be all mxArray.\n");
holder.ct = maxpool3d::FPROP;
opt_beg = 1;
}
else if (no == 1) { // bprop
holder.dY.setMxArray( (mxArray*) vi[0]);
holder.ind.setMxArray( (mxArray*) vi[1]);
dt = holder.dY.getDevice();
if ( ni < 2 ||
holder.dY.getElemType() != mxSINGLE_CLASS ||
holder.ind.getElemType() != mxINT32_CLASS)
throw mp3d_ex("For bprop(): there should be at least 3 arguments, dzdY, ind.\n"
"The dzdY must be SINGLE, the max index ind must be int32,"
"they should be both gpuArray or be both mxArray.\n");
holder.ct = maxpool3d::BPROP;
opt_beg = 2;
}
else {
throw mp3d_ex("Unrecognized arguments/way of calling. "
"The output should be either [Y, ind] (fprop) or ind (bprop). ");
}
// if bprop: create dX if szX is provided args:(dzdY, ind, szX)
if (holder.ct == maxpool3d::BPROP) {
if (ni >= 3 && !mxIsChar(vi[2]) ) {
// szX provided, check it
if (!mxIsDouble(vi[2])) mexErrMsgTxt("setCArray: pa must be double matrix\n");
double *ptr = (double*)mxGetData(vi[2]);
mwSize nelem = mxGetNumberOfElements(vi[2]);
if (nelem > 5 || nelem < 3)
throw mp3d_ex("The third argument must be: 3 <= numel(szX) <= 5.\n");
// get szX
mwSize szX[5];
szX[3] = szX[4] = 1;
for (int i = 0; i < nelem; ++i) szX[i] = (mwSize) ptr[i];
// create the dX
holder.dX.setMxArray( createVol5dZeros(szX, holder.dY.dt) );
// reset the option beginning
opt_beg = 3;
}
else // issue the warning
mexWarnMsgTxt("For bprop(), the calling method with 2 args:\n"
"[...] = mex_maxpool3d(dzdY, ind, ...)\n"
"is deprecated, because this could cause ambiguity when inferring input X size.\n"
"Use the new one to specify the size for input X (or dzdX) explicitly:\n"
"[...] = mex_maxpool3d(dzdY, ind, szX,...)\n");
}
// set options
set_options(holder, opt_beg, ni, vi);
// check validity
check_padpool(holder);
// create the desired worker and set the parameters
#ifdef WITH_GPUARRAY
if (dt == xpuMxArrayTW::GPU)
return new maxpool3d_gpu(holder);
else
return new maxpool3d_cpu(holder);
#else
return new maxpool3d_cpu(holder);
#endif // WITH_GPUARRAY
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){
Config cfg;
GPUInfo *gpuinfo=NULL;
mxArray *tmp;
int ifield, jstruct;
int ncfg, nfields;
int fielddim[4];
int activedev=0;
int errorflag=0;
int threadid=0;
const char *outputtag[]={"data"};
const char *datastruct[]={"data","stat"};
const char *statstruct[]={"runtime","nphoton","energytot","energyabs","normalizer","workload"};
const char *gpuinfotag[]={"name","id","devcount","major","minor","globalmem",
"constmem","sharedmem","regcount","clock","sm","core",
"autoblock","autothread","maxgate"};
if (nrhs==0){
mcxlab_usage();
return;
}
if(nrhs==1 && mxIsChar(prhs[0])){
char shortcmd[MAX_SESSION_LENGTH];
mxGetString(prhs[0], shortcmd, MAX_SESSION_LENGTH);
shortcmd[MAX_SESSION_LENGTH-1]='\0';
if(strcmp(shortcmd,"gpuinfo")==0){
mcx_initcfg(&cfg);
cfg.isgpuinfo=3;
if(!(activedev=mcx_list_gpu(&cfg,&gpuinfo))){
mexWarnMsgTxt("no active GPU device found");
}
plhs[0] = mxCreateStructMatrix(gpuinfo[0].devcount,1,15,gpuinfotag);
for(int i=0;i<gpuinfo[0].devcount;i++){
mxSetField(plhs[0],i,"name",mxCreateString(gpuinfo[i].name));
SET_GPU_INFO(plhs[0],i,id);
SET_GPU_INFO(plhs[0],i,devcount);
SET_GPU_INFO(plhs[0],i,major);
SET_GPU_INFO(plhs[0],i,minor);
SET_GPU_INFO(plhs[0],i,globalmem);
SET_GPU_INFO(plhs[0],i,constmem);
SET_GPU_INFO(plhs[0],i,sharedmem);
SET_GPU_INFO(plhs[0],i,regcount);
SET_GPU_INFO(plhs[0],i,clock);
SET_GPU_INFO(plhs[0],i,sm);
SET_GPU_INFO(plhs[0],i,core);
SET_GPU_INFO(plhs[0],i,autoblock);
SET_GPU_INFO(plhs[0],i,autothread);
SET_GPU_INFO(plhs[0],i,maxgate);
}
mcx_cleargpuinfo(&gpuinfo);
mcx_clearcfg(&cfg);
}
return;
}
printf("Launching MCXLAB - Monte Carlo eXtreme for MATLAB & GNU Octave ...\n");
if (!mxIsStruct(prhs[0]))
mexErrMsgTxt("Input must be a structure.");
nfields = mxGetNumberOfFields(prhs[0]);
ncfg = mxGetNumberOfElements(prhs[0]);
if(nlhs>=1)
plhs[0] = mxCreateStructMatrix(ncfg,1,2,datastruct);
if(nlhs>=2)
plhs[1] = mxCreateStructMatrix(ncfg,1,1,outputtag);
if(nlhs>=3)
plhs[2] = mxCreateStructMatrix(ncfg,1,1,outputtag);
if(nlhs>=4)
plhs[3] = mxCreateStructMatrix(ncfg,1,1,outputtag);
if(nlhs>=5)
plhs[4] = mxCreateStructMatrix(ncfg,1,1,outputtag);
for (jstruct = 0; jstruct < ncfg; jstruct++) { /* how many configs */
try{
printf("Running simulations for configuration #%d ...\n", jstruct+1);
mcx_initcfg(&cfg);
for (ifield = 0; ifield < nfields; ifield++) { /* how many input struct fields */
tmp = mxGetFieldByNumber(prhs[0], jstruct, ifield);
if (tmp == NULL) {
continue;
}
mcx_set_field(prhs[0],tmp,ifield,&cfg);
}
#ifndef MATLAB_MEX_FILE
mexEvalString("fflush(stdout);");
#else
mexEvalString("drawnow;");
#endif
cfg.issave2pt=(nlhs>=1);
cfg.issavedet=(nlhs>=2);
cfg.issaveseed=(nlhs>=4);
#if defined(USE_MT_RAND)
cfg.issaveseed=0;
#endif
if(cfg.vol==NULL || cfg.medianum==0){
mexErrMsgTxt("You must define 'vol' and 'prop' field.");
}
if(!(activedev=mcx_list_gpu(&cfg,&gpuinfo))){
mexErrMsgTxt("No active GPU device found");
}
if(nlhs>=1){
int fieldlen=cfg.dim.x*cfg.dim.y*cfg.dim.z*(int)((cfg.tend-cfg.tstart)/cfg.tstep+0.5);
cfg.exportfield = (float*)calloc(fieldlen,sizeof(float));
}
if(nlhs>=2){
cfg.exportdetected=(float*)malloc((cfg.medianum+1)*cfg.maxdetphoton*sizeof(float));
}
if(nlhs>=4){
cfg.seeddata=malloc(cfg.maxdetphoton*sizeof(float)*RAND_BUF_LEN);
}
if(nlhs>=5){
cfg.exportdebugdata=(float*)malloc(cfg.maxjumpdebug*sizeof(float)*MCX_DEBUG_REC_LEN);
}
mcx_validate_config(&cfg);
#ifdef _OPENMP
omp_set_num_threads(activedev);
#pragma omp parallel shared(errorflag)
{
threadid=omp_get_thread_num();
#endif
try{
mcx_run_simulation(&cfg,gpuinfo);
}catch(const char *err){
mexPrintf("Error from thread (%d): %s\n",threadid,err);
errorflag++;
}catch(const std::exception &err){
mexPrintf("C++ Error from thread (%d): %s\n",threadid,err.what());
errorflag++;
}catch(...){
mexPrintf("Unknown Exception from thread (%d)",threadid);
errorflag++;
}
#ifdef _OPENMP
}
#endif
if(errorflag)
mexErrMsgTxt("MCXLAB Terminated due to an exception!");
if(nlhs>=5){
fielddim[0]=MCX_DEBUG_REC_LEN; fielddim[1]=cfg.debugdatalen; // his.savedphoton is for one repetition, should correct
fielddim[2]=0; fielddim[3]=0;
mxSetFieldByNumber(plhs[4],jstruct,0, mxCreateNumericArray(2,fielddim,mxSINGLE_CLASS,mxREAL));
if(cfg.debuglevel & MCX_DEBUG_MOVE)
memcpy((float*)mxGetPr(mxGetFieldByNumber(plhs[4],jstruct,0)),cfg.exportdebugdata,fielddim[0]*fielddim[1]*sizeof(float));
if(cfg.exportdebugdata)
free(cfg.exportdebugdata);
cfg.exportdebugdata=NULL;
}
if(nlhs>=4){
fielddim[0]=(cfg.issaveseed>0)*RAND_BUF_LEN*sizeof(float); fielddim[1]=cfg.detectedcount; // his.savedphoton is for one repetition, should correct
fielddim[2]=0; fielddim[3]=0;
mxSetFieldByNumber(plhs[3],jstruct,0, mxCreateNumericArray(2,fielddim,mxUINT8_CLASS,mxREAL));
memcpy((unsigned char*)mxGetPr(mxGetFieldByNumber(plhs[3],jstruct,0)),cfg.seeddata,fielddim[0]*fielddim[1]);
free(cfg.seeddata);
cfg.seeddata=NULL;
}
if(nlhs>=3){
fielddim[0]=cfg.dim.x; fielddim[1]=cfg.dim.y;
fielddim[2]=cfg.dim.z; fielddim[3]=0;
if(cfg.vol){
mxSetFieldByNumber(plhs[2],jstruct,0, mxCreateNumericArray(3,fielddim,mxUINT8_CLASS,mxREAL));
memcpy((unsigned char*)mxGetPr(mxGetFieldByNumber(plhs[2],jstruct,0)),cfg.vol,
fielddim[0]*fielddim[1]*fielddim[2]*sizeof(unsigned char));
}
}
if(nlhs>=2){
fielddim[0]=(cfg.medianum+1); fielddim[1]=cfg.detectedcount;
fielddim[2]=0; fielddim[3]=0;
if(cfg.detectedcount>0){
mxSetFieldByNumber(plhs[1],jstruct,0, mxCreateNumericArray(2,fielddim,mxSINGLE_CLASS,mxREAL));
memcpy((float*)mxGetPr(mxGetFieldByNumber(plhs[1],jstruct,0)),cfg.exportdetected,
fielddim[0]*fielddim[1]*sizeof(float));
}
free(cfg.exportdetected);
cfg.exportdetected=NULL;
}
if(nlhs>=1){
fielddim[0]=cfg.dim.x; fielddim[1]=cfg.dim.y;
fielddim[2]=cfg.dim.z; fielddim[3]=(int)((cfg.tend-cfg.tstart)/cfg.tstep+0.5);
mxSetFieldByNumber(plhs[0],jstruct,0, mxCreateNumericArray(4,fielddim,mxSINGLE_CLASS,mxREAL));
memcpy((float*)mxGetPr(mxGetFieldByNumber(plhs[0],jstruct,0)),cfg.exportfield,
fielddim[0]*fielddim[1]*fielddim[2]*fielddim[3]*sizeof(float));
free(cfg.exportfield);
cfg.exportfield=NULL;
mxArray *stat=mxCreateStructMatrix(1,1,6,statstruct);
mxArray *val = mxCreateDoubleMatrix(1,1,mxREAL);
*mxGetPr(val) = cfg.runtime;
mxSetFieldByNumber(stat,0,0, val);
val = mxCreateDoubleMatrix(1,1,mxREAL);
*mxGetPr(val) = cfg.nphoton;
mxSetFieldByNumber(stat,0,1, val);
val = mxCreateDoubleMatrix(1,1,mxREAL);
*mxGetPr(val) = cfg.energytot;
mxSetFieldByNumber(stat,0,2, val);
val = mxCreateDoubleMatrix(1,1,mxREAL);
*mxGetPr(val) = cfg.energyabs;
mxSetFieldByNumber(stat,0,3, val);
val = mxCreateDoubleMatrix(1,1,mxREAL);
*mxGetPr(val) = cfg.normalizer;
mxSetFieldByNumber(stat,0,4, val);
val = mxCreateDoubleMatrix(1,activedev,mxREAL);
for(int i=0;i<activedev;i++)
*(mxGetPr(val)+i) = cfg.workload[i];
mxSetFieldByNumber(stat,0,5, val);
mxSetFieldByNumber(plhs[0],jstruct,1, stat);
}
}catch(const char *err){
mexPrintf("Error: %s\n",err);
}catch(const std::exception &err){
mexPrintf("C++ Error: %s\n",err.what());
}catch(...){
mexPrintf("Unknown Exception");
}
if(detps)
free(detps);
mcx_cleargpuinfo(&gpuinfo);
mcx_clearcfg(&cfg);
}
return;
}
void mcx_set_field(const mxArray *root,const mxArray *item,int idx, Config *cfg){
const char *name=mxGetFieldNameByNumber(root,idx);
const int *arraydim;
char *jsonshapes=NULL;
int i,j;
if(strcmp(name,"nphoton")==0 && cfg->replay.seed!=NULL)
return;
cfg->flog=stderr;
GET_1ST_FIELD(cfg,nphoton)
GET_ONE_FIELD(cfg,nblocksize)
GET_ONE_FIELD(cfg,nthread)
GET_ONE_FIELD(cfg,tstart)
GET_ONE_FIELD(cfg,tstep)
GET_ONE_FIELD(cfg,tend)
GET_ONE_FIELD(cfg,maxdetphoton)
GET_ONE_FIELD(cfg,sradius)
GET_ONE_FIELD(cfg,maxgate)
GET_ONE_FIELD(cfg,respin)
GET_ONE_FIELD(cfg,isreflect)
GET_ONE_FIELD(cfg,isref3)
GET_ONE_FIELD(cfg,isrefint)
GET_ONE_FIELD(cfg,isnormalized)
GET_ONE_FIELD(cfg,isgpuinfo)
GET_ONE_FIELD(cfg,issrcfrom0)
GET_ONE_FIELD(cfg,autopilot)
GET_ONE_FIELD(cfg,minenergy)
GET_ONE_FIELD(cfg,unitinmm)
GET_ONE_FIELD(cfg,reseedlimit)
GET_ONE_FIELD(cfg,printnum)
GET_ONE_FIELD(cfg,voidtime)
GET_ONE_FIELD(cfg,issaveseed)
GET_ONE_FIELD(cfg,replaydet)
GET_ONE_FIELD(cfg,faststep)
GET_ONE_FIELD(cfg,maxvoidstep)
GET_ONE_FIELD(cfg,maxjumpdebug)
GET_VEC3_FIELD(cfg,srcpos)
GET_VEC3_FIELD(cfg,srcdir)
GET_VEC3_FIELD(cfg,steps)
GET_VEC3_FIELD(cfg,crop0)
GET_VEC3_FIELD(cfg,crop1)
GET_VEC4_FIELD(cfg,srcparam1)
GET_VEC4_FIELD(cfg,srcparam2)
else if(strcmp(name,"vol")==0){
if(!mxIsUint8(item) || mxGetNumberOfDimensions(item)!=3 )
mexErrMsgTxt("the 'vol' field must be a 3D uint8 array");
arraydim=mxGetDimensions(item);
for(i=0;i<3;i++) ((unsigned int *)(&cfg->dim))[i]=arraydim[i];
if(cfg->vol) free(cfg->vol);
cfg->vol=(unsigned char *)malloc(cfg->dim.x*cfg->dim.y*cfg->dim.z);
memcpy(cfg->vol,mxGetData(item),cfg->dim.x*cfg->dim.y*cfg->dim.z);
printf("mcx.dim=[%d %d %d];\n",cfg->dim.x,cfg->dim.y,cfg->dim.z);
}else if(strcmp(name,"detpos")==0){
arraydim=mxGetDimensions(item);
if(arraydim[0]>0 && arraydim[1]!=4)
mexErrMsgTxt("the 'detpos' field must have 4 columns (x,y,z,radius)");
double *val=mxGetPr(item);
cfg->detnum=arraydim[0];
if(cfg->detpos) free(cfg->detpos);
cfg->detpos=(float4 *)malloc(cfg->detnum*sizeof(float4));
for(j=0;j<4;j++)
for(i=0;i<cfg->detnum;i++)
((float *)(&cfg->detpos[i]))[j]=val[j*cfg->detnum+i];
printf("mcx.detnum=%d;\n",cfg->detnum);
}else if(strcmp(name,"prop")==0){
arraydim=mxGetDimensions(item);
if(arraydim[0]>0 && arraydim[1]!=4)
mexErrMsgTxt("the 'prop' field must have 4 columns (mua,mus,g,n)");
double *val=mxGetPr(item);
cfg->medianum=arraydim[0];
if(cfg->prop) free(cfg->prop);
cfg->prop=(Medium *)malloc(cfg->medianum*sizeof(Medium));
for(j=0;j<4;j++)
for(i=0;i<cfg->medianum;i++)
((float *)(&cfg->prop[i]))[j]=val[j*cfg->medianum+i];
printf("mcx.medianum=%d;\n",cfg->medianum);
}else if(strcmp(name,"session")==0){
int len=mxGetNumberOfElements(item);
if(!mxIsChar(item) || len==0)
mexErrMsgTxt("the 'session' field must be a non-empty string");
if(len>MAX_SESSION_LENGTH)
mexErrMsgTxt("the 'session' field is too long");
int status = mxGetString(item, cfg->session, MAX_SESSION_LENGTH);
if (status != 0)
mexWarnMsgTxt("not enough space. string is truncated.");
printf("mcx.session='%s';\n",cfg->session);
}else if(strcmp(name,"srctype")==0){
int len=mxGetNumberOfElements(item);
const char *srctypeid[]={"pencil","isotropic","cone","gaussian","planar","pattern","fourier","arcsine","disk","fourierx","fourierx2d","zgaussian","line","slit", "rectangular",""};
char strtypestr[MAX_SESSION_LENGTH]={'\0'};
if(!mxIsChar(item) || len==0)
mexErrMsgTxt("the 'srctype' field must be a non-empty string");
if(len>MAX_SESSION_LENGTH)
mexErrMsgTxt("the 'srctype' field is too long");
int status = mxGetString(item, strtypestr, MAX_SESSION_LENGTH);
if (status != 0)
mexWarnMsgTxt("not enough space. string is truncated.");
cfg->srctype=mcx_keylookup(strtypestr,srctypeid);
if(cfg->srctype==-1)
mexErrMsgTxt("the specified source type is not supported");
printf("mcx.srctype='%s';\n",strtypestr);
}else if(strcmp(name,"outputtype")==0){
int len=mxGetNumberOfElements(item);
const char *outputtype[]={"flux","fluence","energy","jacobian",""};
char outputstr[MAX_SESSION_LENGTH]={'\0'};
if(!mxIsChar(item) || len==0)
mexErrMsgTxt("the 'outputtype' field must be a non-empty string");
if(len>MAX_SESSION_LENGTH)
mexErrMsgTxt("the 'outputtype' field is too long");
int status = mxGetString(item, outputstr, MAX_SESSION_LENGTH);
if (status != 0)
mexWarnMsgTxt("not enough space. string is truncated.");
cfg->outputtype=mcx_keylookup(outputstr,outputtype);
if(cfg->outputtype==-1)
mexErrMsgTxt("the specified output type is not supported");
printf("mcx.outputtype='%s';\n",outputstr);
}else if(strcmp(name,"debuglevel")==0){
int len=mxGetNumberOfElements(item);
const char debugflag[]={'R','M','P','\0'};
char debuglevel[MAX_SESSION_LENGTH]={'\0'};
if(!mxIsChar(item) || len==0)
mexErrMsgTxt("the 'debuglevel' field must be a non-empty string");
if(len>MAX_SESSION_LENGTH)
mexErrMsgTxt("the 'debuglevel' field is too long");
int status = mxGetString(item, debuglevel, MAX_SESSION_LENGTH);
if (status != 0)
mexWarnMsgTxt("not enough space. string is truncated.");
cfg->debuglevel=mcx_parsedebugopt(debuglevel,debugflag);
if(cfg->debuglevel==0)
mexWarnMsgTxt("the specified debuglevel is not supported");
printf("mcx.debuglevel='%d';\n",cfg->debuglevel);
}else if(strcmp(name,"srcpattern")==0){
arraydim=mxGetDimensions(item);
double *val=mxGetPr(item);
if(cfg->srcpattern) free(cfg->srcpattern);
cfg->srcpattern=(float*)malloc(arraydim[0]*arraydim[1]*sizeof(float));
for(i=0;i<arraydim[0]*arraydim[1];i++)
cfg->srcpattern[i]=val[i];
printf("mcx.srcpattern=[%d %d];\n",arraydim[0],arraydim[1]);
}else if(strcmp(name,"shapes")==0){
int len=mxGetNumberOfElements(item);
if(!mxIsChar(item) || len==0)
mexErrMsgTxt("the 'shapes' field must be a non-empty string");
jsonshapes=new char[len+1];
mxGetString(item, jsonshapes, len+1);
jsonshapes[len]='\0';
}else if(strcmp(name,"detphotons")==0){
arraydim=mxGetDimensions(item);
dimdetps[0]=arraydim[0];
dimdetps[1]=arraydim[1];
detps=(float *)malloc(arraydim[0]*arraydim[1]*sizeof(float));
memcpy(detps,mxGetData(item),arraydim[0]*arraydim[1]*sizeof(float));
printf("mcx.detphotons=[%d %d];\n",arraydim[0],arraydim[1]);
}else if(strcmp(name,"seed")==0){
arraydim=mxGetDimensions(item);
if(MAX(arraydim[0],arraydim[1])==0)
mexErrMsgTxt("the 'seed' field can not be empty");
if(!mxIsUint8(item)){
double *val=mxGetPr(item);
cfg->seed=val[0];
printf("mcx.seed=%d;\n",cfg->seed);
}else{
seedbyte=arraydim[0];
cfg->replay.seed=malloc(arraydim[0]*arraydim[1]);
if(arraydim[0]!=sizeof(float)*RAND_BUF_LEN)
mexErrMsgTxt("the row number of cfg.seed does not match RNG seed byte-length");
memcpy(cfg->replay.seed,mxGetData(item),arraydim[0]*arraydim[1]);
cfg->seed=SEED_FROM_FILE;
cfg->nphoton=arraydim[1];
printf("mcx.nphoton=%d;\n",cfg->nphoton);
}
}else if(strcmp(name,"gpuid")==0){
int len=mxGetNumberOfElements(item);
if(mxIsChar(item)){
if(len==0)
mexErrMsgTxt("the 'gpuid' field must be an integer or non-empty string");
if(len>MAX_DEVICE)
mexErrMsgTxt("the 'gpuid' field is too long");
int status = mxGetString(item, cfg->deviceid, MAX_DEVICE);
if (status != 0)
mexWarnMsgTxt("not enough space. string is truncated.");
printf("mcx.gpuid='%s';\n",cfg->deviceid);
}else{
double *val=mxGetPr(item);
cfg->gpuid=val[0];
memset(cfg->deviceid,0,MAX_DEVICE);
if(cfg->gpuid<MAX_DEVICE){
memset(cfg->deviceid,'0',cfg->gpuid-1);
cfg->deviceid[cfg->gpuid-1]='1';
}else
mexErrMsgTxt("GPU id can not be more than 256");
printf("mcx.gpuid=%d;\n",cfg->gpuid);
}
for(int i=0;i<MAX_DEVICE;i++)
if(cfg->deviceid[i]=='0')
cfg->deviceid[i]='\0';
}else if(strcmp(name,"workload")==0){
double *val=mxGetPr(item);
arraydim=mxGetDimensions(item);
if(arraydim[0]*arraydim[1]>MAX_DEVICE)
mexErrMsgTxt("the workload list can not be longer than 256");
for(i=0;i<arraydim[0]*arraydim[1];i++)
cfg->workload[i]=val[i];
printf("mcx.workload=<<%d>>;\n",arraydim[0]*arraydim[1]);
}else{
printf("WARNING: redundant field '%s'\n",name);
}
if(jsonshapes){
Grid3D grid={&(cfg->vol),&(cfg->dim),{1.f,1.f,1.f},0};
if(cfg->issrcfrom0) memset(&(grid.orig.x),0,sizeof(float3));
int status=mcx_parse_shapestring(&grid,jsonshapes);
delete [] jsonshapes;
if(status){
mexErrMsgTxt(mcx_last_shapeerror());
}
}
}
int pfput_mxArray( Pf *pf, char *name, const mxArray *array )
{
Pf *sub_pf;
double number;
char *string;
mxArray *in[2], *out[1];
char warning[STRSZ];
char *fieldname;
mxArray *mxfield;
mxArray *mxcell;
int M,N;
int rc;
int i;
if( mxIsClass( array, "dbpf" ) )
{
if( ! get_pf( array, &sub_pf ) )
{
return PFINVALID;
}
if( sub_pf->type == PFFILE)
{
/* Don't embed a PFFILE in something else */
sub_pf->type = PFARR;
}
switch (pf->type) {
case PFFILE:
case PFARR:
setarr ( pf->value.arr, name, sub_pf ) ;
break ;
case PFTBL:
settbl ( pf->value.tbl, (int) name, sub_pf ) ;
break ;
default :
return PFINVALID;
}
antelope_mex_clear_register( 1 );
}
else if( mxIsDouble( array ) )
{
if( ! get_scalar( array, &number ) )
{
return PFINVALID;
}
in[0] = (mxArray *) array; /* Input scalar */
mexCallMATLAB( 1, out, 1, in, "floor" );
in[1] = out[0]; /* floor( Input scalar ) */
mexCallMATLAB( 1, out, 2, in, "eq" );
mxDestroyArray( in[1] );
if( mxIsLogicalScalarTrue( out[0] ) )
{
pfput_int( pf, name, (int) number );
}
else
{
pfput_double( pf, name, number );
}
antelope_mex_clear_register( 1 );
mxDestroyArray( out[0] );
}
else if( mxIsChar( array ) )
{
if( ! mtlb_get_string( array, &string ) )
{
sprintf( warning,
"failed to extract string for parameter %s\n",
name );
mexWarnMsgTxt( warning );
return PFINVALID;
}
pfput_string( pf, name, string );
antelope_mex_clear_register( 1 );
mxFree( string );
}
else if( mxIsStruct( array ) )
{
if( mxGetNumberOfDimensions( array ) > 2 )
{
sprintf( warning,
"structure has too many dimensions for parameter %s\n",
name );
mexWarnMsgTxt( warning );
return PFINVALID;
}
else if( mxGetM( array ) != 1 || mxGetN( array ) != 1 )
{
sprintf( warning,
"structure has too many elements for parameter %s\n",
name );
mexWarnMsgTxt( warning );
return PFINVALID;
}
N = mxGetNumberOfFields( array );
sub_pf = pfnew( PFARR );
for( i = 0; i < N; i++ )
{
fieldname = (char *) mxGetFieldNameByNumber( array, i );
mxfield = mxGetFieldByNumber( array, 0, i );
rc = pfput_mxArray( sub_pf, fieldname, mxfield );
if( rc == PFINVALID )
{
pffree( sub_pf );
return PFINVALID;
}
}
switch (pf->type) {
case PFFILE:
case PFARR:
setarr ( pf->value.arr, name, sub_pf ) ;
break ;
case PFTBL:
settbl ( pf->value.tbl, (int) name, sub_pf ) ;
break ;
default :
pffree( sub_pf );
return PFINVALID;
}
antelope_mex_clear_register( 1 );
}
else if( mxIsCell( array ) )
{
if( mxGetNumberOfDimensions( array ) > 2 )
{
sprintf( warning,
"cell array has too many dimensions for parameter %s\n",
name );
mexWarnMsgTxt( warning );
return PFINVALID;
}
M = mxGetM( array );
N = mxGetN( array );
sub_pf = pfnew( PFTBL );
for( i = 0; i < M * N; i++ )
{
mxcell = mxGetCell( array, i );
rc = pfput_mxArray( sub_pf, (char *) i, mxcell );
if( rc == PFINVALID )
{
pffree( sub_pf );
return PFINVALID;
}
}
switch (pf->type) {
case PFFILE:
case PFARR:
setarr ( pf->value.arr, name, sub_pf ) ;
break ;
case PFTBL:
settbl ( pf->value.tbl, (int) name, sub_pf ) ;
break ;
default :
pffree( sub_pf );
return PFINVALID;
}
antelope_mex_clear_register( 1 );
}
else
{
return PFINVALID;
}
return 0;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
int code = 'L';
PmError err1;
PtError err2;
init(); /* returns immediately if already done */
if (nrhs > 0) {
char arg[2];
if (!mxIsChar(prhs[0]) || mxGetNumberOfElements(prhs[0]) > 1) {
mexErrMsgTxt("Bad call\n");
}
mxGetString(prhs[0], arg, 2);
code = arg[0];
}
switch(code) {
case 'V':
if (inStream == NULL) {
mexErrMsgTxt("No MIDI input device is opened");
} else {
if (nrhs < 2) mexErrMsgTxt("Usage for 'verbose': midiIn('V', value)");
verbose = mxGetScalar(prhs[1]);
}
break;
case 'G': /* Return the buffered events as array */
if (inStream == NULL) {
mexErrMsgTxt("No MIDI input device is opened");
} else {
plhs[0] = getEvent();
/* reset the counter to the beginning of the buffer */
numReceived = 0;
}
break;
case 'F': /* Flush all buffered events */
if (inStream == NULL) {
mexErrMsgTxt("No MIDI input device is opened");
} else {
/* reset the counter to the beginning of the buffer */
numReceived = 0;
}
break;
case 'C': /* Close input stream */
if (inStream == NULL) {
mexWarnMsgTxt("No MIDI input device is opened - ignoring 'close' command");
} else {
err1 = Pm_Close(inStream);
inStream = NULL;
if (err1 != pmNoError) reportPmError(err1);
}
break;
case 'O': /* Open input stream */
{
int device = 0;
if (nrhs<2 || !mxIsNumeric(prhs[1])) mexErrMsgTxt("Bad call\n");
device = (int) mxGetScalar(prhs[1]);
if (device < 1 || device > Pm_CountDevices()) mexErrMsgTxt("Device index out of range");
--device;
if (inStream != NULL) {
if (deviceOpen == device) {
mexWarnMsgTxt("MIDI input device is already open - ignoring request");
return;
}
mexWarnMsgTxt("Another MIDI input device is open - closing that one");
err1 = Pm_Close(inStream);
inStream = NULL;
if (err1 != pmNoError) reportPmError(err1);
}
err1 = Pm_OpenInput(&inStream, device, NULL, INPUT_BUFFER_SIZE, NULL, NULL);
if (err1 != pmNoError) {
inStream = NULL;
reportPmError(err1);
}
/* remember which device we just opened */
deviceOpen = device;
}
break;
case 'L': /* List devices */
plhs[0] = getDevices();
break;
default:
mexErrMsgTxt("Bad call\n");
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
size_t numRow,numVec;
mxArray *retMat;
double *xVec, *retData;
double TT1, TT2, UT11, UT12;
//The if-statements below should properly initialize all of the EOP.
//The following initializations to zero are to suppress warnings when
//compiling with -Wconditional-uninitialized.
double xp=0;
double yp=0;
double deltaT=0;
double LOD=0;
double ITRS2TEME[3][3];
double PEF2TEME[3][3];
double WInv[3][3];//The inverse polar motion matrix to go from ITRS to PEF.
double Omega[3];//The angular velocity vector for the Earth's rotation.
if(nrhs<3||nrhs>6){
mexErrMsgTxt("Wrong number of inputs");
}
if(nlhs>2) {
mexErrMsgTxt("Wrong number of outputs.");
return;
}
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<6||mxIsEmpty(prhs[3])||mxIsEmpty(prhs[4])||mxIsEmpty(prhs[5])) {
mxArray *retVals[5];
double *xpyp;
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(5,retVals,2,JulUTCMATLAB,"getEOP");
mxDestroyArray(JulUTCMATLAB[0]);
mxDestroyArray(JulUTCMATLAB[1]);
checkRealDoubleArray(retVals[0]);
checkRealDoubleArray(retVals[1]);
if(mxGetM(retVals[0])!=2||mxGetN(retVals[0])!=1||mxGetM(retVals[1])!=2||mxGetN(retVals[1])!=1) {
mxDestroyArray(retVals[0]);
mxDestroyArray(retVals[1]);
mxDestroyArray(retVals[2]);
mxDestroyArray(retVals[3]);
mxDestroyArray(retVals[4]);
mexErrMsgTxt("Error using the getEOP function.");
return;
}
xpyp=(double*)mxGetData(retVals[0]);
xp=xpyp[0];
yp=xpyp[1];
//The celestial pole offsets are not used.
//This is TT-UT1
deltaT=getDoubleFromMatlab(retVals[3]);
LOD=getDoubleFromMatlab(retVals[4]);
//Free the returned arrays.
mxDestroyArray(retVals[0]);
mxDestroyArray(retVals[1]);
mxDestroyArray(retVals[2]);
mxDestroyArray(retVals[3]);
mxDestroyArray(retVals[4]);
}
//If deltaT=TT-UT1 is given
if(nrhs>3&&!mxIsEmpty(prhs[3])) {
deltaT=getDoubleFromMatlab(prhs[3]);
}
//Obtain UT1 from terestrial time and deltaT.
iauTtut1(TT1, TT2, deltaT, &UT11, &UT12);
//Get polar motion values, if given.
if(nrhs>4&&!mxIsEmpty(prhs[4])) {
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 *xpyp=(double*)mxGetData(prhs[4]);
xp=xpyp[0];
yp=xpyp[1];
} else {
mexErrMsgTxt("The celestial pole offsets have the wrong dimensionality.");
return;
}
}
//If LOD is given
if(nrhs>5&&!mxIsEmpty(prhs[5])) {
LOD=getDoubleFromMatlab(prhs[5]);
}
{
double GMST1982=iauGmst82(UT11, UT12);
double TEME2PEF[3][3];
double TEME2ITRS[3][3];
double W[3][3];
double omega;
//Get Greenwhich mean sidereal time under the IAU's 1982 model. This
//is given in radians and will be used to build a rotation matrix to
//rotate into the PEF system.
GMST1982=iauGmst82(UT11, UT12);
{
double cosGMST,sinGMST;
cosGMST=cos(GMST1982);
sinGMST=sin(GMST1982);
//Build the rotation matrix to rotate by GMST about the z-axis. This
//will put the position vector in the PEF system.
TEME2PEF[0][0]=cosGMST;
TEME2PEF[0][1]=sinGMST;
TEME2PEF[0][2]=0;
TEME2PEF[1][0]=-sinGMST;
TEME2PEF[1][1]=cosGMST;
TEME2PEF[1][2]=0;
TEME2PEF[2][0]=0;
TEME2PEF[2][1]=0;
TEME2PEF[2][2]=1.0;
}
//The inverse rotation is just the transpose
iauTr(TEME2PEF, PEF2TEME);
//To go from PEF to ITRS, we need to build the polar motion matrix
//using the IAU's 1980 conventions.
{
double cosXp,sinXp,cosYp,sinYp;
cosXp=cos(xp);
sinXp=sin(xp);
cosYp=cos(yp);
sinYp=sin(yp);
W[0][0]=cosXp;
W[0][1]=sinXp*sinYp;
W[0][2]=sinXp*cosYp;
W[1][0]=0;
W[1][1]=cosYp;
W[1][2]=-sinYp;
W[2][0]=-sinXp;
W[2][1]=cosXp*sinXp;
W[2][2]=cosXp*cosYp;
}
//The inverse rotation is just the transpose
iauTr(W, WInv);
//The total rotation matrix is thus the product of the two rotations.
//TEME2ITRS=W*TEME2PEF;
iauRxr(W, TEME2PEF, TEME2ITRS);
//We want the inverse rotation
iauTr(TEME2ITRS, ITRS2TEME);
//The angular velocity vector of the Earth in the TIRS in radians.
omega=getScalarMatlabClassConst("Constants","IERSMeanEarthRotationRate");
//Adjust for LOD
omega=omega*(1-LOD/86400.0);//86400.0 is the number of seconds in a TT day.
Omega[0]=0;
Omega[1]=0;
Omega[2]=omega;
}
//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(ITRS2TEME, xVec+numRow*curVec, retData+numRow*curVec);
//If a velocity vector was given.
if(numRow>3) {
double *posITRS=xVec+numRow*curVec;
double *velITRS=xVec+numRow*curVec+3;//Velocity in TEME
double posPEF[3];
double velPEF[3];
double *retDataVel=retData+numRow*curVec+3;
double rotVel[3];
//If a velocity was provided with the position, first
//convert to PEF coordinates, then account for the rotation
//of the Earth, then rotate into TEME coordinates.
//Convert velocity from ITRS to PEF.
iauRxp(WInv, velITRS, velPEF);
//Convert position from ITRS to PEF
iauRxp(WInv, posITRS, posPEF);
//Evaluate the cross product for the angular velocity due
//to the Earth's rotation.
iauPxp(Omega, posPEF, rotVel);
//Add the instantaneous velocity due to rotation.
iauPpp(velPEF, rotVel, retDataVel);
//Rotate from the PEF into the TEME
iauRxp(PEF2TEME, retDataVel, retDataVel);
}
}
}
plhs[0]=retMat;
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]=ITRS2TEME[i][j];
}
}
}
}
void phasemethods( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
double vv;
int iok=0, k;
int ph = getInt(prhs[1]);
int job = getInt(prhs[2]);
bool ok = true;
char* input_buf;
double* ptr = 0;
size_t nsp, n, m;
int mjob, show_thermo;
// methods to set attributes
if (job < 0) {
mjob = -job;
if (mxIsChar(prhs[3]) != 1) {
ptr = mxGetPr(prhs[3]);
}
m = mxGetM(prhs[3]);
n = mxGetN(prhs[3]);
nsp = phase_nSpecies(ph);
// set scalar attributes
bool ok = true;
if (mjob < 10) {
if (m != 1 || n != 1)
mexErrMsgTxt("value must be scalar.");
switch (mjob) {
case 1:
iok = phase_setTemperature(ph,*ptr); break;
case 2:
iok = phase_setDensity(ph,*ptr); break;
default:
ok = false;
}
}
// set array attributes
else if (mjob < 30) {
if ((m == nsp && n == 1) || (m == 1 && n == nsp)) {
int norm = 1;
switch (mjob) {
case 20:
iok = phase_setMoleFractions(ph, nsp, ptr, norm);
break;
case 21:
iok = phase_setMassFractions(ph, nsp, ptr, norm);
break;
case 22:
norm = 0;
iok = phase_setMoleFractions(ph, nsp, ptr, norm);
break;
case 23:
norm = 0;
iok = phase_setMassFractions(ph, nsp, ptr, norm);
break;
default:
ok = false;
}
}
else {
mexErrMsgTxt("wrong array size");
}
}
// set attributes from a string
else {
int status;
mwSize buflen;
char* input_buf;
if (mxIsChar(prhs[3]) == 1) {
if(mxGetM(prhs[3]) != 1)
mexErrMsgTxt("Input must be a row vector.");
buflen = (mwSize) (mxGetM(prhs[3]) * mxGetN(prhs[3])) + 1;
input_buf = (char*)mxCalloc(buflen, sizeof(char));
status = mxGetString(prhs[3], input_buf, buflen);
if (status != 0)
mexWarnMsgTxt("Not enough space. "
"String is truncated.");
switch (mjob) {
case 30:
iok = phase_setMoleFractionsByName(ph, input_buf);
break;
case 31:
iok = phase_setMassFractionsByName(ph, input_buf);
break;
case 32:
iok = phase_setName(ph, input_buf);
break;
default:
mexErrMsgTxt("what?");
}
}
else {
mexErrMsgTxt("expected a string.");
}
}
}
else if (job < 20) {
switch (job) {
case 0:
vv = (double) newThermoFromXML(ph); break;
// floating-point attributes
case 1:
vv = phase_temperature(ph); break;
case 2:
vv = phase_density(ph); break;
case 3:
vv = phase_molarDensity(ph); break;
case 4:
vv = phase_meanMolecularWeight(ph); break;
case 8:
vv = 1.0/phase_density(ph); break;
case 10:
vv = (double) phase_nElements(ph); break;
case 11:
vv = (double) phase_nSpecies(ph); break;
case 12:
input_buf = getString(prhs[3]);
vv = (double) phase_speciesIndex(ph, input_buf) + 1;
break;
case 13:
input_buf = getString(prhs[3]);
vv = (double) phase_elementIndex(ph, input_buf) + 1;
break;
case 14:
k = getInt(prhs[3]);
m = getInt(prhs[4]);
vv = phase_nAtoms(ph,k-1,m-1); break;
case 15:
show_thermo = getInt(prhs[3]);
vv = write_phase(ph,show_thermo);
break;
default:
ok = false;
}
if (ok) {
if (vv == DERR) reportError();
plhs[0] = mxCreateNumericMatrix(1,1,mxDOUBLE_CLASS,mxREAL);
double *h = mxGetPr(plhs[0]);
*h = vv;
return;
}
}
//ok = true;
else if (job < 30) {
iok = 0;
size_t nsp = phase_nSpecies(ph);
double* x = new double[nsp];
switch (job) {
case 20:
iok = phase_getMoleFractions(ph,nsp,x);
break;
case 21:
iok = phase_getMassFractions(ph,nsp,x);
break;
case 22:
iok = phase_getMolecularWeights(ph,nsp,x);
break;
default:
;
}
plhs[0] = mxCreateNumericMatrix((mwSize) nsp,1,
mxDOUBLE_CLASS,mxREAL);
double *h = mxGetPr(plhs[0]);
if (iok >= 0) {
for (size_t i = 0; i < nsp; i++) h[i] = x[i];
delete x;
return;
}
else {
for (size_t i = 0; i < nsp; i++) h[i] = -999.99;
delete x;
mexErrMsgTxt("unknown attribute");
return;
}
}
else if (job < 40) {
iok = 0;
size_t nel = phase_nElements(ph);
double* x = new double[nel];
switch (job) {
case 30:
iok = phase_getAtomicWeights(ph,nel,x);
break;
default:
;
}
plhs[0] = mxCreateNumericMatrix((mwSize) nel,1,
mxDOUBLE_CLASS,mxREAL);
double *h = mxGetPr(plhs[0]);
if (iok >= 0) {
for (size_t i = 0; i < nel; i++) h[i] = x[i];
delete x;
return;
}
else {
for (size_t i = 0; i < nel; i++) h[i] = -999.99;
delete x;
mexErrMsgTxt("unknown attribute");
return;
}
}
else if (job < 50) {
iok = -1;
int ksp, mel;
int buflen;
char* output_buf;
switch (job) {
case 40:
ksp = getInt(prhs[3]);
buflen = 40;
output_buf = (char*)mxCalloc(buflen, sizeof(char));
iok = phase_getSpeciesName(ph, ksp-1, buflen, output_buf);
break;
case 41:
mel = getInt(prhs[3]);
buflen = 40;
output_buf = (char*)mxCalloc(buflen, sizeof(char));
iok = phase_getElementName(ph, mel-1, buflen, output_buf);
break;
case 42:
buflen = 40;
output_buf = (char*)mxCalloc(buflen, sizeof(char));
iok = phase_getName(ph, buflen, output_buf);
break;
default:
iok = -1;
}
if (iok >= 0) {
plhs[0] = mxCreateString(output_buf);
return;
}
else {
mexErrMsgTxt("error or unknown method.");
reportError();
return;
}
}
else {
mexErrMsgTxt("unimplemented method.");
return;
}
if (iok < 0) reportError();
}
void mexFunction(int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[]) {
mwSize sizeWm,sizeWn;
mxArray * v;
mwSize nzmax;
mwIndex *ir, *jc, *irs, *jcs;
double *weights,*sr;
double * p;
mwIndex currentEntryIndex,currentColumnIndex,numColumnEntries;
mwIndex i;
// Test number of parameters.
if (nrhs != 2 || nlhs != 1) {
mexWarnMsgTxt("Usage: K = computePower(W,p)");
return;
}
// Parse parameters
if (!mxIsSparse(prhs[0])) {
mexWarnMsgTxt("Error: Expects sparse matrix W.");
return;
}
sizeWm=mxGetM(prhs[0]);
sizeWn=mxGetN(prhs[0]);
nzmax = mxGetNzmax(prhs[0]); // number of nonzero elements of sparse matrix
ir = mxGetIr(prhs[0]);
jc = mxGetJc(prhs[0]);
weights = mxGetPr(prhs[0]);
p = mxGetPr(prhs[1]);
// Allocate memory for output (sparse real matrix)
v = mxCreateSparse(sizeWm, sizeWn, nzmax, mxREAL);
sr = mxGetPr(v);
irs = mxGetIr(v);
jcs = mxGetJc(v);
currentEntryIndex=0;
currentColumnIndex=0;
numColumnEntries=0;
jcs[0]=0;
#pragma omp parallel for schedule(static)
for (i=0;i<nzmax;i++)
{
if (i<sizeWn+1)
jcs[i]=jc[i];
irs[i]=ir[i];
sr[i]=pow(fabs(weights[i]),*p);
}
// if we have less than sizeWn entries
if (nzmax < sizeWn+1)
{
for (i=nzmax;i<sizeWn+1;i++)
jcs[i]=jc[i];
}
plhs[0]=v;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
//Possible Inputs
char fpath[FLEN];
char msg[FLEN];
char cmd[FLEN]; //user commmand
int sp = 0;
//Outputs
const char *fnames[15] = {"H","f","lb","ub","A","cl","cu","Q","l","qcind","x0","v0","sense","objbias","conlin"};
double *sizes;
//Internal Vars
int ii; size_t i,j,k; //indexing vars
char *what, **whatp; //error message vars
static FILE *nl; //file handle
ASL *asl = cur_ASL; //Current ASL instance
int icmd = ASLCMD_ERROR; //Command Integer
double *sense; //Objective sense
double *objbias; //Objective bias
int obj_lin; //linearity of the objectiuve (see ASL_DEGREE_ defines)
double *con_lin; //linearity of the constraints (see ASL_DEGREE_ defines)
double *isopen; //Is ASL open
bool nlcon = false; //indicates whether any constraint is nonlinear
double *x; //Evaluation point
double *f, *g, *c = NULL; //Return pointers
int nerror; //eval errors
//Sparse Indexing
mwIndex *Ir, *Jc;
double *Pr;
//QP Checking Vars
int nqpz = 0; //number of nzs in quadratic objective
int nqc_con = 0; //number of quadratic constraints
int *QP_ir, *QP_jc; //Pointers used when calling nqpcheck
double *QP_pr;
double *pqi; //pointer to quadratic index vector
ograd *og; //objective gradient structure
//Jacobian Vars
static double *J = NULL; //Memory to store intermediate Jacobian Values when using Dense Mode
static double *J1 = NULL; //Memory to store Jacobian Values
cgrad *cg, **cgp, **cgpe; //constraint gradient structures
int *cs; //Column starts
//Hessian Vars
static double *Hsp = NULL; //Memory to store Hessian Values
static int nhnz; //Number of Hessian nz
double *s, *v; //Sigma, Lambda
int *hcs, *hr; //Hessian column starts, row indexs
double *H, *He, *W;
//Error catching
Jmp_buf err_jmp0;
//If no inputs, just return info
if(nrhs < 1)
{
if (nlhs >= 1)
{
sprintf(msgbuf,"%s %s",__TIME__,__DATE__);
plhs[0] = mxCreateString(msgbuf);
plhs[1] = mxCreateDoubleScalar(OPTI_VER);
}
else
{
printUtilityInfo();
}
return;
}
//Get User Command
icmd = getCommand(prhs[0]);
//Switch Yard for Command
switch(icmd)
{
case ASLCMD_ISOPEN:
isopen = mxGetPr(plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL));
if(asl)
*isopen = 1;
else
*isopen = 0;
break;
case ASLCMD_OPEN:
//Check for Errors
if(nrhs < 2)
mexErrMsgTxt("Expected two arguments to open a file! [x0,v0,lb,ub,cl,cu,sense,sizes] = asl('open','file path')\n");
if(!mxIsChar(prhs[1]))
mexErrMsgTxt("File path must be a char array!");
//Get String
CHECK(mxGetString(prhs[1], fpath, FLEN) == 0,"error reading file path!");
//Clear any existing objects
if (cur_ASL)
ASL_free(&cur_ASL);
//Set MEX exit function
mexAtExit(mexExit);
//Open file for LP/QP/QCQP checking
asl = ASL_alloc(ASL_read_fg); //allocate for qp read
return_nofile = 1; //return 0 if stub doesn't exist
nl = jac0dim(fpath,(ftnlen)strlen(fpath)); //read in passed file
//Check we got the file
if(!nl) {
sprintf(msgbuf, "Can't open (or error opening) %s\n", fpath);
mexErrMsgTxt(msgbuf);
}
//Allocate Vector Memory
pPROB = mxCreateStructMatrix(1,1,15,fnames);
mxSetField(pPROB,0,fnames[eX0],mxCreateDoubleMatrix(n_var,1, mxREAL));
mxSetField(pPROB,0,fnames[eV0],mxCreateDoubleMatrix(n_con, 1, mxREAL));
mxSetField(pPROB,0,fnames[eLB],mxCreateDoubleMatrix(n_var, 1, mxREAL));
mxSetField(pPROB,0,fnames[eUB],mxCreateDoubleMatrix(n_var, 1, mxREAL));
mxSetField(pPROB,0,fnames[eCL],mxCreateDoubleMatrix(n_con, 1, mxREAL));
mxSetField(pPROB,0,fnames[eCU],mxCreateDoubleMatrix(n_con, 1, mxREAL));
mxSetField(pPROB,0,fnames[eSENSE],mxCreateDoubleMatrix(1, 1, mxREAL));
mxSetField(pPROB,0,fnames[eOBJBIAS],mxCreateDoubleMatrix(1, 1, mxREAL));
mxSetField(pPROB,0,fnames[eCONLIN],mxCreateDoubleMatrix(n_con, 1, mxREAL));
//Get Fields (ASL will fill)
X0 = mxGetPr(mxGetField(pPROB,0,fnames[eX0]));
pi0 = mxGetPr(mxGetField(pPROB,0,fnames[eV0]));
LUv = mxGetPr(mxGetField(pPROB,0,fnames[eLB]));
Uvx = mxGetPr(mxGetField(pPROB,0,fnames[eUB]));
LUrhs = mxGetPr(mxGetField(pPROB,0,fnames[eCL]));
Urhsx = mxGetPr(mxGetField(pPROB,0,fnames[eCU]));
sense = mxGetPr(mxGetField(pPROB,0,fnames[eSENSE]));
objbias = mxGetPr(mxGetField(pPROB,0,fnames[eOBJBIAS]));
con_lin = mxGetPr(mxGetField(pPROB,0,fnames[eCONLIN]));
//Other Output Args
sizes = mxGetPr(pSIZE = mxCreateDoubleMatrix(16, 1, mxREAL));
//Check for complementarity problems
if(n_cc)
mexWarnMsgTxt("Ignoring Complementarity Constraints!");
//Assign asl problem sizes
sizes[0] = (double)n_var; sizes[1] = (double)n_con; sizes[2] = (double)nzc;
sizes[3] = (double)lnc; sizes[4] = (double)nbv; sizes[5] = (double)niv;
sizes[6] = (double)nlc; sizes[7] = (double)nlnc; sizes[8] = (double)nlo;
sizes[9] = (double)nlvb; sizes[10] = (double)nlvc; sizes[11] = (double)nlvo;
sizes[12] = (double)nlvbi; sizes[13] = (double)nlvci; sizes[14] = (double)nlvoi;
sizes[15] = (double)nwv;
//Read In For QP Checking
qp_read(nl,0);
//Assign sense
if(objtype[0] == 1)
*sense = -1; //max
else
*sense = 1; //min
//Determine Objective Linearity
obj_lin = linCheck(asl, 0);
//Determine Constraints Linearity
for(ii = 0; ii < n_con; ii++) {
con_lin[ii] = linCheck(asl, -(ii+1));
//Check if nonlinear or quadratic
if(con_lin[ii] >= ASL_DEGREE_NLIN)
nlcon = true;
else if(con_lin[ii] == ASL_DEGREE_QUAD)
{
//con_lin indicates quadratic constraint, ensure is inequality
if(LUrhs[ii] != Urhsx[ii])
nqc_con++;
else
nlcon = true; //quadratic equalities not currently handled by any explicit QCQP solver (I know of), make nl
}
}
//Check to force to read as nonlinear problem
if(nrhs > 2 && *mxGetPr(prhs[2])==1)
nlcon = true;
//If objective or any constraint is nonlinear, then we have to process as an NLP
if(obj_lin == ASL_DEGREE_NLIN || nlcon) {
//Free the QP read memory
ASL_free(&asl);
//Re-open for full NLP read
asl = ASL_alloc(ASL_read_pfgh); //allocate memory for pfgh read
nl = jac0dim(fpath,(ftnlen)strlen(fpath)); //read passed file (full nl read)
//Allocate Jacobian Memory [note use M1alloc to let ASL clean it up if multiple instances opened]
J = (double*)M1alloc(nzc*sizeof(double)); //Memory to store Jacobian nzs
//Assign memory for saving obj + con x
objx = (double*)M1alloc(n_var*sizeof(double));
conx = (double*)M1alloc(n_var*sizeof(double));
//Read File (f + g + H)
pfgh_read(nl, ASL_findgroups);
//Assign Hessian Memory
nhnz = sphsetup(1, 1, n_con > 0, 0); //one obj, use sigma, optionally use lambda, full hessian
Hsp = (double*)M1alloc(nhnz*sizeof(double)); //memory to store hessian nzs
}
//Otherwise we can process as a LP, QP or QCQP
else {
//Assign objective bias
*objbias = objconst(0);
//Check for quadratic objective
if(obj_lin == ASL_DEGREE_QUAD) {
//Capture Pointers
nqpz = nqpcheck(0, &QP_ir, &QP_jc, &QP_pr); //check objective for qp
//Create QP H
mxSetField(pPROB,0,fnames[eH],mxCreateSparse(n_var,n_var,nqpz,mxREAL));
//Copy in Objective Quadratic Elements (copy-cast where appropriate)
memcpy(mxGetPr(mxGetField(pPROB,0,fnames[eH])),QP_pr,nqpz*sizeof(double));
Jc = mxGetJc(mxGetField(pPROB,0,fnames[eH]));
Ir = mxGetIr(mxGetField(pPROB,0,fnames[eH]));
for(i = 0; i <= n_var; i++)
Jc[i] = (mwIndex)QP_jc[i];
for(i = 0; i < nqpz; i++)
Ir[i] = (mwIndex)QP_ir[i];
}
else //create an empty sparse matrix
mxSetField(pPROB,0,fnames[eH],mxCreateSparse(n_var,n_var,0,mxREAL));
//Create QP f
mxSetField(pPROB,0,fnames[eF],mxCreateDoubleMatrix(n_var,1,mxREAL));
Pr = mxGetPr(mxGetField(pPROB,0,fnames[eF]));
//Copy in Objective Linear Elements
for( og = Ograd[0]; og; og = og->next )
Pr[og->varno] = og->coef;
//Create A (linear constraints)
mxSetField(pPROB,0,fnames[eA],mxCreateSparse(n_con, n_var, nzc, mxREAL));
if(n_con) {
Pr = mxGetPr(mxGetField(pPROB,0,fnames[eA]));
Ir = mxGetIr(mxGetField(pPROB,0,fnames[eA]));;
//Fill in A (will double on quadratic linear sections, but easier to remove once in MATLAB)
for(Jc = mxGetJc(mxGetField(pPROB,0,fnames[eA])), cs = A_colstarts, i = 0; i <= n_var; ++i)
Jc[i] = (mwIndex)cs[i];
cgp = Cgrad;
for(i = 0; i < n_con; i++)
for(cg = *cgp++; cg; cg = cg->next) {
Ir[cg->goff] = (mwIndex)i;
Pr[cg->goff] = cg->coef;
}
}
//Add quadratic constraints if present
if(nqc_con) {
//Allocate a Cell Array to store the quadratic constraint Qs, and vector to store indices
mxSetField(pPROB,0,fnames[eQ],mxCreateCellMatrix(nqc_con,1)); //Q
mxSetField(pPROB,0,fnames[eL],mxCreateDoubleMatrix(n_var, nqc_con,mxREAL)); //l
mxSetField(pPROB,0,fnames[eQCIND],mxCreateDoubleMatrix(nqc_con,1,mxREAL)); //ind
pqi = mxGetPr(mxGetField(pPROB,0,fnames[eQCIND]));
//Fill In Constraints
for(ii=0,j=0;ii<n_con;ii++) {
//Quadratic Constraints
if(con_lin[ii] == ASL_DEGREE_QUAD) {
//Create index
pqi[j] = ii+1; //increment for matlab index
//Capture Pointers
nqpz = nqpcheck(-(ii+1), &QP_ir, &QP_jc, &QP_pr); //check constraint for qp;
if(nqpz <= 0)
mexErrMsgTxt("Error reading quadratic constraints. Assumed constraint was quadratic based on prescan, now appears not?");
//Create QC Q
mxSetCell(mxGetField(pPROB,0,fnames[eQ]),j,mxCreateSparse(n_var,n_var,nqpz,mxREAL));
//Copy in Constraint Quadratic Elements (copy-cast where appropriate)
Pr = mxGetPr(mxGetCell(mxGetField(pPROB,0,fnames[eQ]),j));
Jc = mxGetJc(mxGetCell(mxGetField(pPROB,0,fnames[eQ]),j));
Ir = mxGetIr(mxGetCell(mxGetField(pPROB,0,fnames[eQ]),j));
for(k = 0; k <= n_var; k++)
Jc[k] = (mwIndex)QP_jc[k];
for(k = 0; k < nqpz; k++) {
Ir[k] = (mwIndex)QP_ir[k];
Pr[k] = 0.5*QP_pr[k]; //to QP form
}
//Create QC l (not sure why we can't extract this from Jacobian, values are wrong)
Pr = mxGetPr(mxGetField(pPROB,0,fnames[eL]));
for( cg = Cgrad[ii]; cg; cg = cg->next )
Pr[j*n_var + cg->varno] = cg->coef;
//Increment for next cell / col
j++;
}
}
}
//Put back into function eval mode (just in case)
qp_opify();
}
break;
case ASLCMD_CLOSE:
//Check for Errors
CHECKASL(asl);
//Call Exit Function
mexExit();
break;
case ASLCMD_FUN:
//Check for Errors
CHECKASL(asl);
CHECKNRHS(nrhs,2);
//Get x and check dimensions
x = sizechk(prhs[1],"x",n_var);
//Save x
if(objx) memcpy(objx,x,n_var*sizeof(double));
//Create objective val memory and get it from ASL
SETERRJMP(); what = "objective";
f = mxGetPr(plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL));
*f = objval(0, x, &nerror);
break;
case ASLCMD_GRAD:
//Check for Errors
CHECKASL(asl);
CHECKNRHS(nrhs,2);
//Get x and check dimensions
x = sizechk(prhs[1],"x",n_var);
//Save x
if(objx) memcpy(objx,x,n_var*sizeof(double));
//Create objective grad memory and get it from ASL
SETERRJMP(); what = "gradient";
g = mxGetPr(plhs[0] = mxCreateDoubleMatrix(1, n_var, mxREAL));
objgrd(0, x, g, &nerror);
break;
case ASLCMD_CON:
//Check for Errors
CHECKASL(asl);
CHECKNRHS(nrhs,2);
//Get x and check dimensions
x = sizechk(prhs[1],"x",n_var);
//Save x
if(conx) memcpy(conx,x,n_var*sizeof(double));
//Create constraint memory and get it from ASL
SETERRJMP(); what = "constraints";
c = mxGetPr(plhs[0] = mxCreateDoubleMatrix(n_con, 1, mxREAL));
if(n_con)
conval(x, c, &nerror);
break;
case ASLCMD_JAC:
//Check for Errors
CHECKASL(asl);
CHECKNRHS(nrhs,2);
//Get x and check dimensions
x = sizechk(prhs[1],"x",n_var);
//Save x
if(conx) memcpy(conx,x,n_var*sizeof(double));
//Create constraint jac memory and get it from ASL
SETERRJMP(); what = "Jacobian";
//Check for sparsity
if(nrhs > 2 && *mxGetPr(prhs[2])) {
sp = 1;
J1 = mxGetPr(plhs[0] = mxCreateSparse(n_con, n_var, nzc, mxREAL));
}
else {
sp = 0;
J1 = mxGetPr(plhs[0] = mxCreateDoubleMatrix(n_con, n_var, mxREAL));
}
//Evaluate if we have constraints
if (n_con) {
//Sparse
if(sp) {
jacval(x, J1, &nerror);
Ir = mxGetIr(plhs[0]);
for(Jc = mxGetJc(plhs[0]), cs = A_colstarts, i = 0; i <= n_var; ++i)
Jc[i] = (mwIndex)cs[i];
cgp = Cgrad;
for(i = 0; i < n_con; i++)
for(cg = *cgp++; cg; cg = cg->next)
Ir[cg->goff] = (mwIndex)i;
}
//Dense
else {
jacval(x, J, &nerror);
cgp = Cgrad;
for(cgpe = cgp + n_con; cgp < cgpe; J1++)
for(cg = *cgp++; cg; cg = cg->next)
J1[n_con*cg->varno] = J[cg->goff];
}
}
break;
case ASLCMD_JACSTR:
//Check for Errors
CHECKASL(asl);
CHECKNRHS(nrhs,1);
//Create constraint jacstr memory and get it from ASL
SETERRJMP(); what = "Jacobian Structure)";
J1 = mxGetPr(plhs[0] = mxCreateSparse(n_con, n_var, nzc, mxREAL));
//Fill In Structure
for(i=0;i<nzc;i++)
J1[i] = 1.0;
for(Jc = mxGetJc(plhs[0]), cs = A_colstarts, i = 0; i <= n_var; ++i)
Jc[i] = (mwIndex)cs[i];
cgp = Cgrad;
Ir = mxGetIr(plhs[0]);
for(i = 0; i < n_con; i++)
for(cg = *cgp++; cg; cg = cg->next)
Ir[cg->goff] = (mwIndex)i;
break;
case ASLCMD_HES:
//Check for Errors
CHECKASL(asl);
CHECKNRHS(nrhs,4); //assume hess(x,sigma,lambda) and optionally sparse
//Check dimensions & get args
x = sizechk(prhs[1],"x",n_var);
s = sizechk(prhs[2],"sigma",1);
v = sizechk(prhs[3],"lambda",n_con);
//Check for sparsity
if(nrhs > 4 && *mxGetPr(prhs[4])) {
sp = 1;
W = mxGetPr(plhs[0] = mxCreateSparse(n_var, n_var, nhnz, mxREAL));
}
else {
sp = 0;
W = mxGetPr(plhs[0] = mxCreateDoubleMatrix(n_var, n_var, mxREAL));
}
//Check if we need to recalculate objective / constraints
if(!comp_x(objx,x,n_var)) {
//Setup Error Catching
SETERRJMP(); what = "Objective for Hessian";
//Re-evaluate Objective
objval(0, x, &nerror);
}
if(!comp_x(conx,x,n_var)){
if(!c)
c = mxGetPr(mxCreateDoubleMatrix(n_con, 1, mxREAL));
//Setup Error Catching
SETERRJMP(); what = "Constraints for Hessian";
//Re-evaluate Constraints
conval(x, c, &nerror);
}
//Setup Error Catching
SETERRJMP(); what = "Hessian";
//Sparse
if(sp) {
//This function returns the full (symmetric) Hessian as setup above
sphes(H = Hsp, 1, s, v);
Ir = mxGetIr(plhs[0]);
Jc = mxGetJc(plhs[0]);
hcs = sputinfo->hcolstarts;
hr = sputinfo->hrownos;
for(i = 0; i <= n_var; i++)
Jc[i] = (mwIndex)hcs[i];
He = H + hcs[n_var];
while(H < He) {
*W++ = *H++;
*Ir++ = (mwIndex)*hr++;
}
}
//Dense
else
fullhes(W, n_var, 1, s, v);
break;
case ASLCMD_HESSTR:
//mexPrintf("CMD: Get Hessian Structure\n");
//Check for Errors
CHECKASL(asl);
CHECKNRHS(nrhs,1);
//Create hessianstr memory and get it from ASL
SETERRJMP(); what = "Hessian Structure";
W = mxGetPr(plhs[0] = mxCreateSparse(n_var, n_var, nhnz, mxREAL));
Ir = mxGetIr(plhs[0]);
Jc = mxGetJc(plhs[0]);
//Get Sparse Info
hcs = sputinfo->hcolstarts;
hr = sputinfo->hrownos;
//Assign col starts
for(i = 0; i <= n_var; i++)
Jc[i] = (mwIndex)hcs[i];
//Assign rows + 1.0 for nz positions
H = Hsp; //Start of nz Hsp elements
He = H + hcs[n_var]; //End of nz Hsp elements
while(H < He) {
*W++ = 1.0;
*Ir++ = (mwIndex)*hr++;
*H++; //increment nz element position
}
break;
case ASLCMD_WRITESOL:
//Check for Errors
CHECKASL(asl);
CHECKNRHS(nrhs,2); //asl('writesol',msg,x)
//Get Input Args
CHECK(mxGetString(prhs[1], msg, FLEN) == 0,"error reading message!");
x = sizechk(prhs[2],"x",n_var);
//Write to solution stub file
write_sol(msg,x,NULL,NULL);
break;
default:
mexExit(); //clean up
mxGetString(prhs[0], cmd, FLEN);
sprintf(msgbuf, "ASL Command Error! Unknown Command: '%s'\n", cmd);
mexErrMsgTxt(msgbuf);
break;
}
}
static void func(double *p, double *hx, int m, int n, void *adata)
{
bool havrnorm = false, stop = false;
int stat, i;
double *fval, rnorm;
user_function_data *fun = (user_function_data *) adata;
fun->plhs[0] = NULL;
memcpy(mxGetPr(fun->prhs[fun->xrhs]), p, m * sizeof(double));
stat = mexCallMATLAB(1, fun->plhs, fun->nrhs, fun->prhs, fun->f);
if(stat)
mexErrMsgTxt("Error calling Objective Function!");
//Get Objective
fval = mxGetPr(fun->plhs[0]);
memcpy(hx,fval,n*sizeof(double));
// Clean up Ptr
mxDestroyArray(fun->plhs[0]);
//Iteration Printing
if(fun->print > 1) {
if(citer == 1 || !(citer%10))
mexPrintf(" feval sse\n");
//Calculate Residual Norm
rnorm = 0; havrnorm = true;
for(i=0;i<n;i++)
rnorm += (fval[i]-fun->ydata[i])*(fval[i]-fun->ydata[i]);
mexPrintf("%5d %12.5g\n",citer,rnorm);
mexEvalString("drawnow;"); //flush draw buffer
}
//Iteration Callback
if(iterF.enabled)
{
//Calculate sse if we don't have it
if(!havrnorm)
for(i=0;i<n;i++)
rnorm += (fval[i]-fun->ydata[i])*(fval[i]-fun->ydata[i]);
iterF.plhs[0] = NULL;
memcpy(mxGetData(iterF.prhs[1]), &citer, sizeof(int));
memcpy(mxGetPr(iterF.prhs[2]), &rnorm, sizeof(double));
memcpy(mxGetPr(iterF.prhs[3]), p, m * sizeof(double));
stat = mexCallMATLAB(1, iterF.plhs, 4, iterF.prhs, iterF.f);
if(stat)
mexErrMsgTxt("Error calling Callback Function!");
//Collect return argument
stop = *(bool*)mxGetData(iterF.plhs[0]);
// Clean up Ptr
mxDestroyArray(iterF.plhs[0]);
}
citer++;
//Warn user stop not implemented
if(stop)
mexWarnMsgTxt("LEVMAR does not implement the stop feature of iterfun");
}
void Disloc(double *pOutput, double *pModel, double *pCoords, double nu, int NumStat, int NumDisl,
int RefStat)
{
int i,j, sIndex, dIndex, kIndex;
double alp, sd, cd, Angle, cosAngle, sinAngle, HalfLength, x,y, X, Y,SS[3],DS[3],TS[3], S[3];
alp = 1 - 2 * nu;
/*Loop through dislocations*/
for (i=0; i< NumDisl; i=i++)
{
dIndex=i*10;
if (GoodModel(&pModel[dIndex]))
{
if (pModel[dIndex+3] == 90.0)
{
cd = 0.0;
sd = 1.0;
}
else if (pModel[dIndex+3] == -90.0)
{
cd = 0.0;
sd = -1.0;
}
else
{
cd = cos(pModel[dIndex+3] * DEG2RAD);
sd = sin(pModel[dIndex+3] * DEG2RAD);
}
Angle = -(90 - pModel[dIndex+4]) * DEG2RAD;
cosAngle = cos(Angle);
sinAngle = sin(Angle);
HalfLength = 0.5 * pModel[dIndex];
/*Loop through stations*/
for(j=0; j < NumStat; j++)
{
SS[0] = SS[1] = SS[2] = 0;
DS[0] = DS[1] = DS[2] = 0;
TS[0] = TS[1] = TS[2] = 0;
sIndex = j*2;
kIndex = j*3;
x = pCoords[sIndex] - pModel[dIndex+5];
y = pCoords[sIndex + 1] - pModel[dIndex+6];
X = cosAngle * x - sinAngle * y + HalfLength;
Y = sinAngle * x + cosAngle * y;
Okada(&SS[0],&DS[0],&TS[0],alp,sd,cd,pModel[dIndex],pModel[dIndex+1],pModel[dIndex+2],X,Y,pModel[dIndex+7], pModel[dIndex+8], pModel[dIndex+9]);
if (pModel[dIndex+7])
{
x=SS[0];
y=SS[1];
SS[0] = cosAngle * x + sinAngle * y;
SS[1] = -sinAngle * x + cosAngle * y;
pOutput[kIndex]+=SS[0];
pOutput[kIndex+1]+=SS[1];
pOutput[kIndex+2]+=SS[2];
}
if (pModel[dIndex+8])
{
x=DS[0];
y=DS[1];
DS[0] = cosAngle * x + sinAngle * y;
DS[1] = -sinAngle * x + cosAngle * y;
pOutput[kIndex]+=DS[0];
pOutput[kIndex+1]+=DS[1];
pOutput[kIndex+2]+=DS[2];
}
if (pModel[dIndex+9])
{
x=TS[0];
y=TS[1];
TS[0] = cosAngle * x + sinAngle * y;
TS[1] = -sinAngle * x + cosAngle * y;
pOutput[kIndex]+=TS[0];
pOutput[kIndex+1]+=TS[1];
pOutput[kIndex+2]+=TS[2];
}
}
}
else {
mexWarnMsgTxt("Unphysical model.");
}
}
if (RefStat)
{
kIndex=(RefStat-1)*3;
S[0]=pOutput[kIndex];
S[1]=pOutput[kIndex+1];
S[2]=pOutput[kIndex+2];
memmove(&pOutput[kIndex],&pOutput[kIndex+3],(NumStat-RefStat)*24);
for (i=0; i<NumStat-1; i++)
{
kIndex=i*3;
pOutput[kIndex]-=S[0];
pOutput[kIndex+1]-=S[1];
pOutput[kIndex+2]-=S[2];
}
}
}
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
double *x; /* input data, size N*n */
double *y; /* output data, size N*1 */
double C; /* upper bound for Lagrange multipliers */
double e; /* insensitivity zone epsilon */
long n; /* dimension of input space */
long N; /* number of input data */
long k; /* counter */
int d0,d1; /* true number of dimensions of arguments #0 and #1 */
int label1,label2; /* different class labels */
kernel ker; /* kernel function */
options opt; /* optimizer settings */
const mxArray *mlopt; /* mxArray for getOptionsStruct and getOptions */
struct svm_problem prob; /* LIBSVM problem (i.e. data) */
struct svm_parameter param; /* LIBSVM parameters */
struct svm_model *model; /* calculated LIBSVM model */
if (nrhs<4)
mexErrMsgTxt("Invalid number of input arguments.");
/* --- get input and output data --- */
if ( mxIsEmpty(prhs[0]) ||
!mxIsDouble(prhs[0]) ||
mxIsComplex(prhs[0]) )
mexErrMsgTxt("Invalid first argument (input data).");
d0 = mxGetNumberOfDimensions(prhs[0]); /* compute true number of dims #0 */
while ( mxGetDimensions(prhs[0])[d0-1] == 1 )
d0--;
if ( mxIsEmpty(prhs[1]) ) { /* one-class SVM */
n = mxGetDimensions(prhs[0])[d0-1];
N = mxGetNumberOfElements(prhs[0])/n;
}
else if ( !mxIsDouble(prhs[1]) ||
mxIsComplex(prhs[1]) ) {
mexErrMsgTxt("Invalid second argument (output data).");
}
else {
d1 = mxGetNumberOfDimensions(prhs[1]); /* compute true number of dims #1 */
while ( mxGetDimensions(prhs[1])[d1-1] == 1 )
d1--;
if ( d1 == d0 ) /* exatly 1 regressor */
n = 1;
else if ( d1 == d0-1 ) /* more than 1 regressor */
n = mxGetDimensions(prhs[0])[d1];
else
mexErrMsgTxt("Sizes of input and output data do not match.");
N = 1; /* get number of data */
for (k=0; k<d1; k++) {
if ( mxGetDimensions(prhs[1])[k] != mxGetDimensions(prhs[0])[k] )
mexErrMsgTxt("Sizes of first and second argument do not match.");
else
N *= mxGetDimensions(prhs[1])[k];
}
}
if ( mxGetNumberOfElements(prhs[2]) != 1 ||
!mxIsDouble(prhs[2]) ||
mxIsComplex(prhs[2]) ||
mxGetScalar(prhs[2]) <= 0 )
mexErrMsgTxt("Invalid third argument (upper bound).");
x = mxGetPr(prhs[0]); /* get the input arguments */
y = mxGetPr(prhs[1]);
C = mxGetScalar(prhs[2]);
/* set default values */
opt.tol = 1e-3; /* tolerance for KKT check */
opt.shrink = 1; /* enable shrinking */
opt.cache = 40.0; /* cache size in MB */
opt.weight = 1.0; /* weight for class wlabel */
opt.wlabel = -1; /* class to be weighted */
opt.verbose = 0; /* no verbosity */
opt.style = 0; /* C/eps style */
opt.prob = 0; /* no probability information */
/* --- SVM classification --- */
if ( mxGetNumberOfElements(prhs[3]) == 1 &&
mxIsStruct(prhs[3]) ) {
/* check lables for struct output (1 argument) */
if ( !mxIsEmpty(prhs[1]) && nlhs == 1 ) { /* allow different class labels */
label1 = (int)y[0];
for (k=0; k<N; k++) {
label2 = (int)y[k];
if (label1 != label2)
break;
}
for (k=0; k<N; k++)
if ( y[k]!=label1 && y[k]!=label2 )
mexErrMsgTxt("Only two different class labels are allowed.");
}
/* check lables for vector output (2 arguments) */
if ( !mxIsEmpty(prhs[1]) && nlhs>1 ) { /* class labels must be -1 or +1 */
for (k=0; k<N; k++)
if ( y[k]!=1 && y[k]!=-1 )
mexErrMsgTxt("Only class labels +1 and -1 are allowed.");
label1 = +1;
label2 = -1;
}
/* get kernel and options */
if ( getKernel(&ker,prhs[3],n) ||
ker.type == KERNEL_GAUSS )
mexErrMsgTxt("Invalid fourth argument (kernel function).");
if ( nrhs>4 ) {
getOptionsStruct(&mlopt,prhs+4,nrhs-4);
getOptions(&opt,mlopt,N);
}
/* compute probability information only if it can be returned */
if ( nlhs>1 )
opt.prob = 0;
/* check if weighting makes sense */
if ( !mxIsEmpty(prhs[1]) &&
opt.wlabel!=label1 && opt.wlabel!=label2 ) {
opt.weight = 1;
mexWarnMsgTxt("Class to be weighted does not exist.");
}
/* check nu value (C is treated as nu) */
if ( opt.style == 1 && C > 1 )
mexErrMsgTxt("nu must be in the range (0,1]");
/* solve the classification problem */
if ( mxIsEmpty(prhs[1]) ) {
if ( C > 1 )
mexErrMsgTxt("nu must be in the range (0,1]");
convOptions(¶m,C,0,&ker,&opt,2);
}
else {
convOptions(¶m,C,0,&ker,&opt,1);
}
convData(&prob,x,y,n,N);
/* dump information about options and settings into MATLAB window */
e = -1;
if (opt.verbose > 0) {
if ( mxIsEmpty(prhs[1]) )
dumpOptions(&ker,&opt,C,e,2);
else
dumpOptions(&ker,&opt,C,e,1);
}
/* compute the solution */
model = svm_train(&prob,¶m);
if ( mxIsEmpty(prhs[1]) )
buildSolution(plhs,prhs[3],model,&prob,&opt,nlhs,2,N);
else
buildSolution(plhs,prhs[3],model,&prob,&opt,nlhs,1,N);
/* dump information about solution into MATLAB window */
if (opt.verbose > 0) {
if ( mxIsEmpty(prhs[1]) )
dumpSolution(&opt,model,&prob,y,C,2,N,(nlhs>1));
else
dumpSolution(&opt,model,&prob,y,C,1,N,(nlhs>1));
}
/* the model is no longer needed */
svm_destroy_model(model);
}
/* --- SVM regression --- */
else if ( !mxIsEmpty(prhs[1]) && /* one-class SVM not allowed */
mxGetNumberOfElements(prhs[3]) == 1 &&
mxIsDouble(prhs[3]) &&
!mxIsComplex(prhs[3]) &&
mxGetScalar(prhs[3]) >= 0 ) {
if (nrhs<5)
mexErrMsgTxt("Invalid number of input arguments.");
if ( mxGetNumberOfElements(prhs[4]) != 1 ||
!mxIsStruct(prhs[4]) )
mexErrMsgTxt("Invalid fifth argument (kernel function).");
/* get epsilon, kernel and options */
e = mxGetScalar(prhs[3]);
if ( getKernel(&ker,prhs[4],n) ||
ker.type == KERNEL_GAUSS )
mexErrMsgTxt("Invalid fourth argument (kernel function).");
if ( nrhs>5 ) {
getOptionsStruct(&mlopt,prhs+5,nrhs-5);
getOptions(&opt,mlopt,N);
}
/* check nu value (e is treated as nu) */
if ( opt.style == 1 && e > 1 )
mexErrMsgTxt("Invalid fourth argument (nu).");
/* solve the regression problem */
convOptions(¶m,C,e,&ker,&opt,0);
convData(&prob,x,y,n,N);
/* dump information about options and settings into MATLAB window */
if (opt.verbose > 0)
dumpOptions(&ker,&opt,C,e,0);
model = svm_train(&prob,¶m);
buildSolution(plhs,prhs[4],model,&prob,&opt,nlhs,0,N);
/* dump information about solution into MATLAB window */
if (opt.verbose > 0)
dumpSolution(&opt,model,&prob,y,C,0,N,(nlhs>1));
/* the model is no longer needed */
svm_destroy_model(model);
}
else
mexErrMsgTxt("Invalid fourth argument (kernel function).");
}
/*
* bar_convolution: Converts the BW image into a line detected image.
* Inputs:
* (image *) Org Black and White image struct.
* (image *) The line detected image struct.
*
* Returns:
* int: 0 is successful run. -1 is a bad malloc.
*
* How this algorithm works:
* http://www.pages.drexel.edu/~weg22/edge.html
*/
int bar_convolution( image *bw_im, int bar_Length, double *templates_ptr, double *pixel_count_ptr, image *ed_im )
{
int X, Y, I, J, elements, im_offset, mask_offset, r;
double SUM, MAXIMUM;
elements = bw_im->dims[0] * bw_im->dims[1];
ed_im->dims[0] = bw_im->dims[0];
ed_im->dims[1] = bw_im->dims[1];
if ( (ed_im->im = malloc(sizeof(double) * elements)) == NULL ){
mexWarnMsgTxt("Edge im malloc failed...\n");
return MALLOCFAIL;
}
/* Convolution starts here*/
for(Y=0; Y<ed_im->dims[1]; Y++) {
for(X=0; X<ed_im->dims[0]; X++) {
/* image boundaries */
if( Y<bar_Length-1 || Y==ed_im->dims[1]){ /*rows*/
MAXIMUM = 0;
} else if( X<bar_Length-1 || X==ed_im->dims[0]) { /*cols*/
MAXIMUM = 0;
/* Convolution starts here*/
} else {
MAXIMUM = 0;
for(r=0; r<32; r++){
SUM = 0;
for(I=-(bar_Length-1); I<=0; I++) {
for(J=-(bar_Length-1); J<=0; J++) {
im_offset = (Y+J)*ed_im->dims[0] + (X+I);
/*mask_offset = (I+1)+((J+1)*bar_Length);*/
mask_offset = (I+(bar_Length-1)) + ((J+(bar_Length-1))*bar_Length) + (r*bar_Length*bar_Length);
/*printf("image: %f\n", (*(bw_im->im+im_offset)));*/
/*printf("template: %f\n", *(templates_ptr + mask_offset));*/
SUM +=
(*(bw_im->im+im_offset)) *
(*(templates_ptr + mask_offset));
}
}
SUM = fabs(SUM) / *(pixel_count_ptr + r);
/*printf("SUM: %f\n", SUM);*/
if(SUM == 0){
MAXIMUM = SUM;
}else{
if(SUM > MAXIMUM){
MAXIMUM = SUM;
}
}
}
}
if(MAXIMUM > 255) MAXIMUM = 255;
else if(MAXIMUM < 0) MAXIMUM = 0;
*(ed_im->im+Y*ed_im->dims[0]+X) = 255 - MAXIMUM;
}
}
return SUCCESS;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
int code = 'L';
PmError err;
init(); /* returns immediately if already done */
if (nrhs > 0) {
char arg[2];
if (mxIsUint8(prhs[0])) {
if (outStream == NULL) {
mexErrMsgTxt("No MIDI output device is opened");
} else {
playRaw(prhs[0]);
}
return;
}
if (!mxIsChar(prhs[0]) || mxGetNumberOfElements(prhs[0]) > 1) {
mexErrMsgTxt("Bad call\n");
}
mxGetString(prhs[0], arg, 2);
code = arg[0];
}
switch(code) {
case 'P':
if (outStream == NULL) {
mexErrMsgTxt("No MIDI output device is opened");
} else {
if (nrhs < 3) mexErrMsgTxt("Usage for 'program change': midiOut('P', channel, program)");
sendProgChange(prhs);
}
break;
case '+': /* Note on */
if (outStream == NULL) {
mexErrMsgTxt("No MIDI output device is opened");
} else {
if (nrhs < 4) mexErrMsgTxt("Usage for 'note on': midiOut('+', channel, notes, velocities)");
sendNoteOnOff(0x90, prhs);
}
break;
case '-': /* Note off */
if (outStream == NULL) {
mexErrMsgTxt("No MIDI output device is opened");
} else {
if (nrhs < 4) mexErrMsgTxt("Usage for 'note on': midiOut('+', channel, notes, velocities)");
sendNoteOnOff(0x80, prhs);
}
break;
case '.': /* All notes off */
if (outStream == NULL) {
mexErrMsgTxt("No MIDI output device is opened");
} else {
int channel;
if (nrhs < 2) mexErrMsgTxt("Usage for 'all notes off': midiOut('.', channel)");
channel = (int) mxGetScalar(prhs[1]);
if (channel < 1 || channel > 16) mexErrMsgTxt("Channel (2nd arg.) must be 1..16");
--channel;
err = Pm_WriteShort(outStream, 0, Pm_Message(0xB0 | channel, 123, 0));
if (err!=pmNoError) reportPmError(err);
}
break;
case 'C': /* Close output stream */
if (outStream == NULL) {
mexWarnMsgTxt("No MIDI output device is opened - ignoring 'close' command");
} else {
err = Pm_Close(outStream);
outStream = NULL;
}
break;
case 'O': /* Open output stream */
{
int device = 0;
if (nrhs<2 || !mxIsNumeric(prhs[1])) mexErrMsgTxt("Bad call\n");
device = (int) mxGetScalar(prhs[1]);
if (device < 1 || device > Pm_CountDevices()) mexErrMsgTxt("Device index out of range");
--device;
if (outStream != NULL) {
if (openID == device) {
mexWarnMsgTxt("MIDI output device is already open - ignoring request");
return;
}
mexWarnMsgTxt("Another MIDI output device is open - closing that one");
err = Pm_Close(outStream);
outStream = NULL;
if (err != pmNoError) reportPmError(err);
}
/* last parameter = latency = 0 means that timestamps are ignored */
err = Pm_OpenOutput(&outStream, device, NULL, OUTPUT_BUFFER_SIZE, NULL, NULL, 0);
if (err != pmNoError) {
outStream = NULL;
reportPmError(err);
}
openID = device;
}
break;
case 'L': /* List devices */
plhs[0] = getDevices();
break;
default:
mexErrMsgTxt("Bad call\n");
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
// Get the command string
char cmd[64];
if (nrhs < 1 || mxGetString(prhs[0], cmd, sizeof(cmd)))
mexErrMsgTxt("First input should be a command string less than 64 characters long.");
// New
if (!strcmp("new", cmd)) {
// Check parameters
if (nlhs != 1)
mexErrMsgTxt("New: One output expected.");
// Return a handle to a new C++ instance
plhs[0] = convertPtr2Mat<urg>(new urg);
return;
}
// Check there is a second input, which should be the class instance handle
if (nrhs < 2)
mexErrMsgTxt("Second input should be a class instance handle.");
// Delete
if (!strcmp("delete", cmd)) {
// Destroy the C++ object
destroyObject<urg>(prhs[1]);
// Warn if other commands were ignored
if (nlhs != 0 || nrhs != 2)
mexWarnMsgTxt("Delete: Unexpected arguments ignored.");
return;
}
// Get the class instance pointer from the second input
urg *lid = convertMat2Ptr<urg>(prhs[1]);
// Call the various class methods
//Connect
if (!strcmp("connect", cmd))
{
// Check parameters
if (nlhs != 1 || nrhs != 3)
mexErrMsgTxt("Connect: Unexpected arguments.");
int port = (int)mxGetScalar(prhs[2]);
plhs[0]= mxCreateLogicalScalar(lid->connect(port));
return;
}
//Disconnect
if (!strcmp("disconnect", cmd))
{
// Check parameters
if (nlhs != 1 || nrhs != 2)
mexErrMsgTxt("Disconnect: Unexpected arguments.");
plhs[0]= mxCreateLogicalScalar(lid->disconnect());
return;
}
// getScan
if (!strcmp("getScan", cmd))
{
// Check parameters
if (nlhs != 1 || nrhs != 2)
mexErrMsgTxt("Test: Unexpected arguments.");
// Allocate output arrays
plhs[0] = mxCreateDoubleMatrix(1,682,mxREAL);
double *outRange = mxGetPr(plhs[0]);
lid->getScan(outRange);
return;
}
// Got here, so command not recognized
mexErrMsgTxt("Command not recognized.");
}
/*
* s e t u p O p t i o n s
*/
returnValue setupOptions( Options* options, const mxArray* optionsPtr, int* nWSRin, real_t* maxCpuTime )
{
double* optionValue;
int optionValueInt;
/* Check for correct number of option entries;
* may occur, e.g., if user types options.<misspelledName> = <someValue>; */
if ( mxGetNumberOfFields(optionsPtr) != 31 )
mexWarnMsgTxt( "Options might be set incorrectly as struct has wrong number of entries!\n Type 'help qpOASES_options' for further information." );
if ( hasOptionsValue( optionsPtr,"maxIter",&optionValue ) == BT_TRUE )
if ( *optionValue >= 0.0 )
*nWSRin = (int)*optionValue;
if ( hasOptionsValue( optionsPtr,"maxCpuTime",&optionValue ) == BT_TRUE )
if ( *optionValue >= 0.0 )
*maxCpuTime = *optionValue;
if ( hasOptionsValue( optionsPtr,"printLevel",&optionValue ) == BT_TRUE )
{
#ifdef __SUPPRESSANYOUTPUT__
options->printLevel = PL_NONE;
#else
optionValueInt = (int)*optionValue;
options->printLevel = (REFER_NAMESPACE_QPOASES PrintLevel)optionValueInt;
if ( options->printLevel < PL_DEBUG_ITER )
options->printLevel = PL_DEBUG_ITER;
if ( options->printLevel > PL_HIGH )
options->printLevel = PL_HIGH;
#endif
}
if ( hasOptionsValue( optionsPtr,"enableRamping",&optionValue ) == BT_TRUE )
{
optionValueInt = (int)*optionValue;
options->enableRamping = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
}
if ( hasOptionsValue( optionsPtr,"enableFarBounds",&optionValue ) == BT_TRUE )
{
optionValueInt = (int)*optionValue;
options->enableFarBounds = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
}
if ( hasOptionsValue( optionsPtr,"enableFlippingBounds",&optionValue ) == BT_TRUE )
{
optionValueInt = (int)*optionValue;
options->enableFlippingBounds = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
}
if ( hasOptionsValue( optionsPtr,"enableRegularisation",&optionValue ) == BT_TRUE )
{
optionValueInt = (int)*optionValue;
options->enableRegularisation = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
}
if ( hasOptionsValue( optionsPtr,"enableFullLITests",&optionValue ) == BT_TRUE )
{
optionValueInt = (int)*optionValue;
options->enableFullLITests = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
}
if ( hasOptionsValue( optionsPtr,"enableNZCTests",&optionValue ) == BT_TRUE )
{
optionValueInt = (int)*optionValue;
options->enableNZCTests = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
}
if ( hasOptionsValue( optionsPtr,"enableDriftCorrection",&optionValue ) == BT_TRUE )
options->enableDriftCorrection = (int)*optionValue;
if ( hasOptionsValue( optionsPtr,"enableCholeskyRefactorisation",&optionValue ) == BT_TRUE )
options->enableCholeskyRefactorisation = (int)*optionValue;
if ( hasOptionsValue( optionsPtr,"enableEqualities",&optionValue ) == BT_TRUE )
{
optionValueInt = (int)*optionValue;
options->enableEqualities = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt;
}
if ( hasOptionsValue( optionsPtr,"terminationTolerance",&optionValue ) == BT_TRUE )
options->terminationTolerance = *optionValue;
if ( hasOptionsValue( optionsPtr,"boundTolerance",&optionValue ) == BT_TRUE )
options->boundTolerance = *optionValue;
if ( hasOptionsValue( optionsPtr,"boundRelaxation",&optionValue ) == BT_TRUE )
options->boundRelaxation = *optionValue;
if ( hasOptionsValue( optionsPtr,"epsNum",&optionValue ) == BT_TRUE )
options->epsNum = *optionValue;
if ( hasOptionsValue( optionsPtr,"epsDen",&optionValue ) == BT_TRUE )
options->epsDen = *optionValue;
if ( hasOptionsValue( optionsPtr,"maxPrimalJump",&optionValue ) == BT_TRUE )
options->maxPrimalJump = *optionValue;
if ( hasOptionsValue( optionsPtr,"maxDualJump",&optionValue ) == BT_TRUE )
options->maxDualJump = *optionValue;
if ( hasOptionsValue( optionsPtr,"initialRamping",&optionValue ) == BT_TRUE )
options->initialRamping = *optionValue;
if ( hasOptionsValue( optionsPtr,"finalRamping",&optionValue ) == BT_TRUE )
options->finalRamping = *optionValue;
if ( hasOptionsValue( optionsPtr,"initialFarBounds",&optionValue ) == BT_TRUE )
options->initialFarBounds = *optionValue;
if ( hasOptionsValue( optionsPtr,"growFarBounds",&optionValue ) == BT_TRUE )
options->growFarBounds = *optionValue;
if ( hasOptionsValue( optionsPtr,"initialStatusBounds",&optionValue ) == BT_TRUE )
{
optionValueInt = (int)*optionValue;
if ( optionValueInt < -1 )
optionValueInt = -1;
if ( optionValueInt > 1 )
optionValueInt = 1;
options->initialStatusBounds = (REFER_NAMESPACE_QPOASES SubjectToStatus)optionValueInt;
}
if ( hasOptionsValue( optionsPtr,"epsFlipping",&optionValue ) == BT_TRUE )
options->epsFlipping = *optionValue;
if ( hasOptionsValue( optionsPtr,"numRegularisationSteps",&optionValue ) == BT_TRUE )
options->numRegularisationSteps = (int)*optionValue;
if ( hasOptionsValue( optionsPtr,"epsRegularisation",&optionValue ) == BT_TRUE )
options->epsRegularisation = *optionValue;
if ( hasOptionsValue( optionsPtr,"numRefinementSteps",&optionValue ) == BT_TRUE )
options->numRefinementSteps = (int)*optionValue;
if ( hasOptionsValue( optionsPtr,"epsIterRef",&optionValue ) == BT_TRUE )
options->epsIterRef = *optionValue;
if ( hasOptionsValue( optionsPtr,"epsLITests",&optionValue ) == BT_TRUE )
options->epsLITests = *optionValue;
if ( hasOptionsValue( optionsPtr,"epsNZCTests",&optionValue ) == BT_TRUE )
options->epsNZCTests = *optionValue;
return SUCCESSFUL_RETURN;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
double *year,*month,*day,*hour,*minute,*second,*Jul1,*Jul2;
mxArray *Jul1MATLAB,*Jul2MATLAB;
size_t numRow,numCol,numElements,i;
if(nrhs!=6){
mexErrMsgTxt("Wrong number of inputs");
}
if(nlhs>2){
mexErrMsgTxt("Wrong number of outputs.");
return;
}
numRow=mxGetM(prhs[0]);
numCol=mxGetN(prhs[0]);
numElements=numRow*numCol;
if(numElements==0) {
mexErrMsgTxt("The dimensionalities of the inputs are incorrect.");
return;
}
for(i=0;i<6;i++) {
if(numRow==mxGetM(prhs[i])&&numCol==mxGetN(prhs[i])) {
checkRealDoubleArray(prhs[i]);
} else {
mexErrMsgTxt("The dimensionalities of the inputs are incorrect.");
return;
}
}
/*Extract the input arguments.*/
year=(double*)mxGetData(prhs[0]);
month=(double*)mxGetData(prhs[1]);
day=(double*)mxGetData(prhs[2]);
hour=(double*)mxGetData(prhs[3]);
minute=(double*)mxGetData(prhs[4]);
second=(double*)mxGetData(prhs[5]);
//Allocate space for the return variables.
Jul1MATLAB=mxCreateDoubleMatrix(numRow,numCol,mxREAL);
Jul1=(double*)mxGetData(Jul1MATLAB);
Jul2MATLAB=mxCreateDoubleMatrix(numRow,numCol,mxREAL);
Jul2=(double*)mxGetData(Jul2MATLAB);
/*Call the function in the SOFA library.*/
for(i=0;i<numElements;i++) {
int retVal;
retVal=iauDtf2d("UTC",(int)year[i],(int)month[i],(int)day[i],(int)hour[i],(int)minute[i],second[i], &Jul1[i], &Jul2[i]);
switch(retVal){
case 3:
mexWarnMsgTxt("The time is after the end of the day and the year is dubious.");
break;
case 2:
mexWarnMsgTxt("The time is after the end of the day.");
break;
case 1:
mexWarnMsgTxt("The year is dubious.");
break;
case -1:
mxDestroyArray(Jul1MATLAB);
mxDestroyArray(Jul2MATLAB);
mexErrMsgTxt("Bad year given.");
return;
case -2:
mxDestroyArray(Jul1MATLAB);
mxDestroyArray(Jul2MATLAB);
mexErrMsgTxt("Bad month given.");
return;
case -3:
mxDestroyArray(Jul1MATLAB);
mxDestroyArray(Jul2MATLAB);
mexErrMsgTxt("Bad day given.");
return;
case -4:
mxDestroyArray(Jul1MATLAB);
mxDestroyArray(Jul2MATLAB);
mexErrMsgTxt("Bad hour given.");
return;
case -5:
mxDestroyArray(Jul1MATLAB);
mxDestroyArray(Jul2MATLAB);
mexErrMsgTxt("Bad minute given.");
return;
case-6:
mxDestroyArray(Jul1MATLAB);
mxDestroyArray(Jul2MATLAB);
mexErrMsgTxt("Bad second given.");
return;
default:
break;
}
}
plhs[0]=Jul1MATLAB;
if(nlhs>1) {
plhs[1]=Jul2MATLAB;
} else {
mxDestroyArray(Jul2MATLAB);
}
}
void CallMethod( vtkOBJ_TYPE *O , char *met , char *v ) {
// Call_1( SetFileName , v );
mexWarnMsgTxt("Invalid Method: ");
mexPrintf(" %s\n", met );
myFlush();
}
void perform_dijkstra_propagation(T_callback_insert_node callback_insert_node = NULL)
{
// create the Fibonacci heap
struct fibheap* open_heap = fh_makeheap();
fh_setcmp(open_heap, compare_points);
// initialize points
for( int i=0; i<n; ++i )
{
D[i] = GW_INFINITE;
S[i] = kFar;
}
// record all the points
heap_pool = new fibheap_el*[n];
memset( heap_pool, NULL, n*sizeof(fibheap_el*) );
// inialize open list
for( int k=0; k<nb_start_points; ++k )
{
int i = (int) start_points[k];
if( D[i]==0 )
mexErrMsgTxt("start_points should not contain duplicates.");
heap_pool[i] = fh_insert( open_heap, (void*) i ); // add to heap
D[i] = 0;
S[i] = kOpen;
}
// perform the front propagation
int num_iter = 0;
bool stop_iteration = GW_False;
while( !fh_isempty(open_heap) && num_iter<nb_iter_max && !stop_iteration )
{
num_iter++;
// current point
int i = (int) fh_extractmin( open_heap );
heap_pool[i] = NULL;
S[i] = kDead;
stop_iteration = end_points_reached(i);
CHECK_HEAP;
// index in irs[jcs[k]]...irs[jcs[k+1]-1] are the node connected to node k
// values are W[jcs[k]]...W[jcs[k]-1]
// recurse on each neighbor of i
for( int k=jcs[i]; k<jcs[i+1]; ++k )
{
int ii = irs[k];
double P = W[k]; // graph weight
bool bInsert = true;
if( callback_insert_node!=NULL )
bInsert = callback_insert_node(ii,ii);
if( ii>=0 && ii<n && bInsert )
{
// compute its neighboring values
double a1 = D[i] + P;
if( ((int) S[ii]) == kDead )
{
// check if action has change. Should not happen for Dijkstra
if( a1<D[ii] )
mexWarnMsgTxt("The update is not monotone");
#if 1
if( a1<D[ii] ) // should not happen for FM
D[ii] = a1;
#endif
}
else if( ((int) S[ii]) == kOpen )
{
// check if action has change.
if( a1<D[ii] )
{
D[ii] = a1;
// Modify the value in the heap
fibheap_el* cur_el = heap_pool[ii];
if( cur_el!=NULL )
fh_replacedata( open_heap, cur_el, cur_el->fhe_data ); // use same data for update
else
mexErrMsgTxt("Error in heap pool allocation.");
}
}
else if( ((int) S[ii]) == kFar )
{
if( D[ii]!=GW_INFINITE )
mexErrMsgTxt("Distance must be initialized to Inf");
S[ii] = kOpen;
// distance must have change.
D[ii] = a1;
// add to open list
heap_pool[ii] = fh_insert( open_heap, (void*) ii ); // add to heap
}
else
mexErrMsgTxt("Unkwnown state.");
} // end switch
} // end for
} // end while
// free heap
fh_deleteheap(open_heap);
// free fibheap pool
GW_DELETEARRAY(heap_pool);
}
