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); }