C++ VectorXd::data方法代码示例

void approximateIK(RigidBodyTree* model, const MatrixBase<DerivedA>& q_seed,
                   const MatrixBase<DerivedB>& q_nom, const int num_constraints,
                   RigidBodyConstraint* *const constraint_array,
                   const IKoptions& ikoptions,
                   MatrixBase<DerivedC>* q_sol, int* info) {
  int num_kc = 0;
  int nq = model->number_of_positions();
  SingleTimeKinematicConstraint** kc_array =
      new SingleTimeKinematicConstraint* [num_constraints];
  double* joint_lb = new double[nq];
  double* joint_ub = new double[nq];
  for (int j = 0; j < nq; j++) {
    joint_lb[j] = model->joint_limit_min[j];
    joint_ub[j] = model->joint_limit_max[j];
  }
  for (int i = 0; i < num_constraints; i++) {
    int constraint_category = constraint_array[i]->getCategory();
    if (constraint_category ==
        RigidBodyConstraint::SingleTimeKinematicConstraintCategory) {
      kc_array[num_kc] =
          static_cast<SingleTimeKinematicConstraint*>(constraint_array[i]);
      num_kc++;
    } else if (constraint_category ==
               RigidBodyConstraint::PostureConstraintCategory) {
      VectorXd joint_min, joint_max;
      PostureConstraint* pc =
          static_cast<PostureConstraint*>(constraint_array[i]);
      pc->bounds(nullptr, joint_min, joint_max);
      for (int j = 0; j < nq; j++) {
        joint_lb[j] = (joint_lb[j] < joint_min[j] ? joint_min[j] : joint_lb[j]);
        joint_ub[j] = (joint_ub[j] > joint_max[j] ? joint_max[j] : joint_ub[j]);
        if (joint_lb[j] > joint_ub[j]) {
          cerr << "Drake:approximateIK:posture constraint has lower bound "
                  "larger than upper bound" << endl;
        }
      }
    } else {
      cerr
          << "Drake:approximateIK: The constraint category is not supported yet"
          << endl;
    }
  }
  MatrixXd Q;
  ikoptions.getQ(Q);
  int error;
  GRBenv* grb_env = nullptr;
  GRBmodel* grb_model = nullptr;

  VectorXd qtmp = -2 * Q * q_nom;

  // create gurobi environment
  error = GRBloadenv(&grb_env, nullptr);
  if (error) {
    printf("Load Gurobi environment error %s\n", GRBgeterrormsg(grb_env));
  }

  // set solver params
  // (http://www.gurobi.com/documentation/5.5/reference-manual/node798#sec:Parameters)
  error = GRBsetintparam(grb_env, "outputflag", 0);
  if (error) {
    printf("Set Gurobi outputflag error %s\n", GRBgeterrormsg(grb_env));
  }
  /*error = GRBsetintparam(grb_env,"method", 2);
  error = GRBsetintparam(grb_env,"presolve", 0);
  error = GRBsetintparam(grb_env,"bariterlimit", 20);
  error = GRBsetintparam(grb_env,"barhomogenous", 0);
  error = GRBsetdblparam(grb_env,"barconvtol", 1e-4);*/
  error = GRBnewmodel(grb_env, &grb_model, "ApproximateIK", nq, qtmp.data(),
                      joint_lb, joint_ub, nullptr, nullptr);
  if (error) {
    printf("Create Gurobi model error %s\n", GRBgeterrormsg(grb_env));
  }

  // set up cost function
  // cost: (q-q_nom)'*Q*(q-q_nom)
  error = GRBsetdblattrarray(grb_model, "Obj", 0, nq, qtmp.data());
  if (error) {
    printf("Set Gurobi obj error %s\n", GRBgeterrormsg(grb_env));
  }
  for (int i = 0; i < nq; i++) {
    for (int j = 0; j < nq; j++) {
      if (abs(Q(i, j)) > 1e-10) {
        error = GRBaddqpterms(grb_model, 1, &i, &j, Q.data() + i + j * nq);
        if (error) {
          printf("Gurobi error %s\n", GRBgeterrormsg(grb_env));
        }
      }
    }
  }
  int* allIndsData = new int[nq];
  for (int j = 0; j < nq; j++) {
    allIndsData[j] = j;
  }
  // TODO(tkoolen): pass this into the function?
  KinematicsCache<double> cache = model->doKinematics(q_seed);
  int kc_idx, c_idx;
  for (kc_idx = 0; kc_idx < num_kc; kc_idx++) {
    int nc = kc_array[kc_idx]->getNumConstraint(nullptr);
    VectorXd lb(nc);
    VectorXd ub(nc);
//.........这里部分代码省略.........

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  int error;
  if (nrhs<1) mexErrMsgTxt("usage: ptr = QPControllermex(0,control_obj,robot_obj,...); alpha=QPControllermex(ptr,...,...)");
  if (nlhs<1) mexErrMsgTxt("take at least one output... please.");
  
  struct QPControllerData* pdata;
  mxArray* pm;
  double* pr;
  int i,j;

  if (mxGetScalar(prhs[0])==0) { // then construct the data object and return
    pdata = new struct QPControllerData;
    
    // get control object properties
    const mxArray* pobj = prhs[1];
    
    pm = myGetProperty(pobj,"slack_limit");
    pdata->slack_limit = mxGetScalar(pm);

    pm = myGetProperty(pobj,"W_kdot");
    assert(mxGetM(pm)==3); assert(mxGetN(pm)==3);
    pdata->W_kdot.resize(mxGetM(pm),mxGetN(pm));
    memcpy(pdata->W_kdot.data(),mxGetPr(pm),sizeof(double)*mxGetM(pm)*mxGetN(pm));

    pm= myGetProperty(pobj,"w_grf");
    pdata->w_grf = mxGetScalar(pm);    

    pm= myGetProperty(pobj,"w_slack");
    pdata->w_slack = mxGetScalar(pm);    

    pm = myGetProperty(pobj,"Kp_ang");
    pdata->Kp_ang = mxGetScalar(pm);

    pm= myGetProperty(pobj,"n_body_accel_inputs");
    pdata->n_body_accel_inputs = mxGetScalar(pm); 

    pm = myGetProperty(pobj,"body_accel_input_weights");
    pdata->body_accel_input_weights.resize(pdata->n_body_accel_inputs);
    memcpy(pdata->body_accel_input_weights.data(),mxGetPr(pm),sizeof(double)*pdata->n_body_accel_inputs);

    pdata->n_body_accel_constraints = 0;
    for (int i=0; i<pdata->n_body_accel_inputs; i++) {
      if (pdata->body_accel_input_weights(i) < 0)
        pdata->n_body_accel_constraints++;
    }

    // get robot mex model ptr
    if (!mxIsNumeric(prhs[2]) || mxGetNumberOfElements(prhs[2])!=1)
      mexErrMsgIdAndTxt("DRC:QPControllermex:BadInputs","the third argument should be the robot mex ptr");
    memcpy(&(pdata->r),mxGetData(prhs[2]),sizeof(pdata->r));
    
    pdata->B.resize(mxGetM(prhs[3]),mxGetN(prhs[3]));
    memcpy(pdata->B.data(),mxGetPr(prhs[3]),sizeof(double)*mxGetM(prhs[3])*mxGetN(prhs[3]));

    int nq = pdata->r->num_dof, nu = pdata->B.cols();
    
    pm = myGetProperty(pobj,"w_qdd");
    pdata->w_qdd.resize(nq);
    memcpy(pdata->w_qdd.data(),mxGetPr(pm),sizeof(double)*nq);

    pdata->umin.resize(nu);
    pdata->umax.resize(nu);
    memcpy(pdata->umin.data(),mxGetPr(prhs[4]),sizeof(double)*nu);
    memcpy(pdata->umax.data(),mxGetPr(prhs[5]),sizeof(double)*nu);

    pdata->B_act.resize(nu,nu);
    pdata->B_act = pdata->B.bottomRows(nu);

     // get the map ptr back from matlab
     if (!mxIsNumeric(prhs[6]) || mxGetNumberOfElements(prhs[6])!=1)
     mexErrMsgIdAndTxt("DRC:QPControllermex:BadInputs","the seventh argument should be the map ptr");
     memcpy(&pdata->map_ptr,mxGetPr(prhs[6]),sizeof(pdata->map_ptr));
    
//    pdata->map_ptr = NULL;
    if (!pdata->map_ptr)
      mexWarnMsgTxt("Map ptr is NULL.  Assuming flat terrain at z=0");
    
    // create gurobi environment
    error = GRBloadenv(&(pdata->env),NULL);

    // set solver params (http://www.gurobi.com/documentation/5.5/reference-manual/node798#sec:Parameters)
    mxArray* psolveropts = myGetProperty(pobj,"gurobi_options");
    int method = (int) mxGetScalar(myGetField(psolveropts,"method"));
    CGE ( GRBsetintparam(pdata->env,"outputflag",0), pdata->env );
    CGE ( GRBsetintparam(pdata->env,"method",method), pdata->env );
    // CGE ( GRBsetintparam(pdata->env,"method",method), pdata->env );
    CGE ( GRBsetintparam(pdata->env,"presolve",0), pdata->env );
    if (method==2) {
      CGE ( GRBsetintparam(pdata->env,"bariterlimit",20), pdata->env );
      CGE ( GRBsetintparam(pdata->env,"barhomogeneous",0), pdata->env );
      CGE ( GRBsetdblparam(pdata->env,"barconvtol",0.0005), pdata->env );
    }

    mxClassID cid;
    if (sizeof(pdata)==4) cid = mxUINT32_CLASS;
    else if (sizeof(pdata)==8) cid = mxUINT64_CLASS;
    else mexErrMsgIdAndTxt("Drake:constructModelmex:PointerSize","Are you on a 32-bit machine or 64-bit machine??");
    
    plhs[0] = mxCreateNumericMatrix(1,1,cid,mxREAL);
//.........这里部分代码省略.........

int main()
{
  URDFRigidBodyManipulator* model = loadURDFfromFile("examples/Atlas/urdf/atlas_minimal_contact.urdf");
  if(!model)
  {
    cerr<<"ERROR: Failed to load model"<<endl;
  }
  Vector2d tspan;
  tspan<<0,1;
  int l_hand;
  int r_hand;
  //int l_foot;
  //int r_foot;
  for(int i = 0;i<model->num_bodies;i++)
  {
    if(model->bodies[i].linkname.compare(string("l_hand")))
    {
      l_hand = i;
    }
    else if(model->bodies[i].linkname.compare(string("r_hand")))
    {
      r_hand = i;
    }
    //else if(model->bodies[i].linkname.compare(string("l_foot")))
    //{
    //  l_foot = i;
    //}
    //else if(model->bodies[i].linkname.compare(string("r_foot")))
    //{
    //  r_foot = i;
    //}
  }
  int nq = model->num_dof;
  VectorXd qstar = VectorXd::Zero(nq);
  qstar(3) = 0.8;
  model->doKinematics(qstar.data());
  Vector3d com0;
  model->getCOM(com0);
  Vector4d l_hand_pt;
  l_hand_pt<<0.0,0.0,0.0,1.0;
  Vector4d r_hand_pt;
  r_hand_pt<<0.0,0.0,0.0,1.0;
  Vector3d lhand_pos0;
  model->forwardKin(l_hand,l_hand_pt,0,lhand_pos0);
  Vector3d rhand_pos0;
  model->forwardKin(r_hand,r_hand_pt,0,rhand_pos0);
  int nT = 4;
  double* t = new double[nT];
  double dt = 1.0/(nT-1);
  for(int i = 0;i<nT;i++)
  {
    t[i] = dt*i;
  }
  MatrixXd q0 = qstar.replicate(1,nT);
  VectorXd qdot0 = VectorXd::Zero(model->num_dof);
  Vector3d com_lb = com0;
  com_lb(0) = nan("");;
  com_lb(1) = nan("");
  Vector3d com_ub = com0;
  com_ub(0) = nan("");
  com_ub(1) = nan("");
  com_ub(2) = com0(2)+0.5;
  WorldCoMConstraint* com_kc = new WorldCoMConstraint(model,com_lb,com_ub);
  Vector3d rhand_pos_lb = rhand_pos0;
  rhand_pos_lb(0) +=0.1;
  rhand_pos_lb(1) +=0.05;
  rhand_pos_lb(2) +=0.25;
  Vector3d rhand_pos_ub = rhand_pos_lb;
  rhand_pos_ub(2) += 0.25;
  Vector2d tspan_end;
  tspan_end<<t[nT-1],t[nT-1];
  WorldPositionConstraint* kc_rhand = new WorldPositionConstraint(model,r_hand,r_hand_pt,rhand_pos_lb,rhand_pos_ub,tspan_end);
  int num_constraints = 2;
  RigidBodyConstraint** constraint_array = new RigidBodyConstraint*[num_constraints];
  constraint_array[0] = com_kc;
  constraint_array[1] = kc_rhand;
  IKoptions ikoptions(model);
  MatrixXd q_sol(model->num_dof,nT);
  MatrixXd qdot_sol(model->num_dof,nT);
  MatrixXd qddot_sol(model->num_dof,nT);
  int info = 0;
  vector<string> infeasible_constraint;
  inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
  printf("INFO = %d\n",info);
  if(info != 1)
  {
    cerr<<"Failure"<<endl;
    return 1;
  }
  ikoptions.setFixInitialState(false);
  ikoptions.setMajorIterationsLimit(500);
  inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
  printf("INFO = %d\n",info);
  if(info != 1)
  {
    cerr<<"Failure"<<endl;
    return 1;
  }
  RowVectorXd t_inbetween(5);
  t_inbetween << 0.1,0.15,0.3,0.4,0.6;
//.........这里部分代码省略.........

double HamSolver::lanczos(const MatrixX_t& mat, rmMatrixX_t& seed,
                          double lancTolerance)
{
    int matSize = mat.rows();
    if(matSize == 1)
        return re(mat(0, 0));
    const int minIters = std::min(matSize, globalMinLancIters),
              maxIters = std::min(matSize, globalMaxLancIters);
    std::vector<double> a,
                        b;
    a.reserve(minIters);
    b.reserve(minIters);
    MatrixX_t basisVecs = seed;
    VectorX_t x = mat * basisVecs;
    a.push_back(re(seed.col(0).dot(x)));
    b.push_back(0.);
    VectorX_t oldGS;
    int i = 0;                                             // iteration counter
    char JOBZ = 'V',                                 // define dstemr arguments
         RANGE = 'I';
    int N = 1;
    std::vector<double> D,
                        E;
    D.reserve(minIters);
    E.reserve(minIters);
    double VL,
           VU;
    int IL = 1,
        IU = 1,
        M;
    std::vector<double> W;
    W.reserve(minIters);
    VectorXd Z;
    int LDZ,
        NZC = 1,
        ISUPPZ[2];
    bool TRYRAC = true;
    double optLWORK;
    std::vector<double> WORK;
    int LWORK,
        optLIWORK;
    std::vector<int> IWORK;
    int LIWORK,
        INFO;
    double gStateDiff;
          // change in ground state vector across subsequent Lanczos iterations
    do
    {
        i++;
        oldGS = seed;
        
        // Lanczos stage 1: Lanczos iteration
        x -= a[i - 1] * basisVecs.col(i - 1);
        b.push_back(x.norm());
        basisVecs.conservativeResize(NoChange, i + 1);
        basisVecs.col(i) = x / b[i];
        x.noalias() = mat * basisVecs.col(i) - b[i] * basisVecs.col(i - 1);
        a.push_back(re(basisVecs.col(i).dot(x)));
        
        // Lanczos stage 2: diagonalize tridiagonal matrix
        N++;
        D = a;
        E.assign(b.begin() + 1, b.end());
        E.resize(N);
        W.resize(N);
        Z.resize(N);
        LDZ = N;
        LWORK = -1;
        LIWORK = -1;
        dstemr_(&JOBZ, &RANGE, &N, D.data(), E.data(), &VL, &VU, &IL, &IU, &M,
                W.data(), Z.data(), &LDZ, &NZC, ISUPPZ, &TRYRAC, &optLWORK,
                &LWORK, &optLIWORK, &LIWORK, &INFO);
                                     // query for optimal workspace allocations
        LWORK = int(optLWORK);
        WORK.resize(LWORK);
        LIWORK = optLIWORK;
        IWORK.resize(LIWORK);
        dstemr_(&JOBZ, &RANGE, &N, D.data(), E.data(), &VL, &VU, &IL, &IU, &M,
                W.data(), Z.data(), &LDZ, &NZC, ISUPPZ, &TRYRAC, WORK.data(),
                &LWORK, IWORK.data(), &LIWORK, &INFO); // calculate ground state
        seed = (basisVecs * Z).normalized();
        gStateDiff = std::abs(1 - std::abs(seed.col(0).dot(oldGS)));
    } while(N < minIters || (N < maxIters && gStateDiff > lancTolerance));
    if(N == maxIters && gStateDiff > lancTolerance)
                          // check if last iteration converges to an eigenstate
    {
        double gStateError = std::abs(1 - std::abs(seed.col(0)
                                                   .dot((mat * seed).col(0)
                                                        .normalized())));
        std::cout << "Warning: final Lanczos iteration reached. The inner "
                  << "product of the final approximate ground state and its "
                  << "normalized image differs from 1 by " << gStateError
                  << std::endl;
        if(gStateError > fallbackLancTolerance)
        {
            std::cerr << "Lanczos algorithm failed to converge after "
                      << maxIters << " iterations." << std::endl;
            exit(EXIT_FAILURE);
        };
    };
//.........这里部分代码省略.........

 VectorXd operator()(const VectorXd& x) const {
   py::object outarr = np_mod.attr("array")(m_pyfunc(toNdarray1<double>(x.data(), x.size())), "float64");
   VectorXd out = Map<const VectorXd>(getPointer<double>(outarr), py::extract<int>(outarr.attr("size")));
   return out;
 }

void shearsteps(double deltaboxdx,
               int NumberStepsRight,
               int NumberStepsLeft,
               vector<spring> &springlist,
               vector<vector<int>> &springpairs,
               VectorXd &XY,
                int bendingOn,
               double kappa,
               int Nit,
               double tolGradE,
               ofstream &shearcoordinates,
               ofstream &shearenergy,
               ofstream &Strestens,
               ofstream &EN)
              {
    double g11,g12,g22; //The components of the metric tensor
    double boxdx=0.0;
    double EBEND,ESTRETCH,ETOT;
    VectorXd gradE(XY.size());
    VectorXd s0(XY.size());
    int conjsteps;
    double lenGrad;
    int iter;
    double fret;
    
    networkinfo info(springlist,springpairs);
    info.g11=1.0;
    info.g12=0.0;
    info.g22=1.0;
//     info.springlist=springlist;
//     info.springpairs=springpairs;
    info.size=XY.size();
    info.bendingon=bendingOn;
    info.sheardeformation=boxdx;
    info.kappa=kappa;

    //XY is in BOX coordinates 
   frprmn(XY.data(),XY.size(),1e-20,&iter,&fret,Efunc2use,grad2use,info,EN); //Do one calibration before shearing
//     frprmn(XY.data(),XY.size(),1e-12,&iter,&fret,NULL,NULL,info); //Do one calibration before shearing
//     CGAGONY(XY,springlist,springpairs,bendingOn,kappa,1.0,0.0,1.0,0.0);
    stresstensor S=StressTensor(springlist,XY,0.0);  
    Strestens<<boxdx<<"\t"<<S.sxx<<"\t"<<S.sxy<<"\t"<<S.syy<<"\t"<<0.5*(S.sxx+S.syy)<<endl;

    for(int k=0;k<(NumberStepsLeft+NumberStepsRight);k++){
        g11=1.0;
        g12=boxdx;
        g22=1.0+boxdx*boxdx;
        info.g11=1.0;
        info.g12=boxdx;
        info.g22=1.0+boxdx*boxdx;
        info.sheardeformation=boxdx;
	
//         CGAGONY(XY,springlist,springpairs,bendingOn,kappa,g11,g12,g22,boxdx); //fast
        frprmn(XY.data(),XY.size(),1e-15,&iter,&fret,Efunc2use,grad2use,info,EN); //slow but better THIS ONE

        S=StressTensor(springlist,XY,boxdx);
        Write_ShearCoordinates_2txt(shearcoordinates,XY);
	//ESTRETCH=EnergyNetworkn(XY.data(),info);
	ESTRETCH=StretchEnergy(springlist,XY,boxdx);
//         cout << "Now EBEND: " << XY(600) << "\t" << kappa << "\t" << boxdx << "\t" << springpairs[80][0] << "\t" << springlist[80].one <<  endl;
        EBEND=BendEnergy(springpairs,springlist,XY,kappa,boxdx);
//         cout << "RESULT: " << EBEND << endl;

        Write_ShearEnergy_2txt(shearenergy,boxdx,ESTRETCH+EBEND,ESTRETCH,EBEND,lenGrad,iter);

        if(k<NumberStepsRight){
            boxdx=boxdx+deltaboxdx;
        } else {
            boxdx=boxdx-deltaboxdx;
        }
        cout<<"This was a shearstep     "<<k<<" with    "<<iter<<endl;
    }
}

void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
  // check number of arguments
  if (nrhs < 4) {
    mexErrMsgIdAndTxt("Drake:collisionDetectmex:NotEnoughInputs",
                      "Usage: [xA,xB,normal,distance,idxA,idxB] = "
                      "collisionDetectmex(mex_model_ptr, cache_ptr, "
                      "allow_multiple_contacts, active_collision_options)");
  }

  // check argument types
  if (!mxIsClass(prhs[0], "DrakeMexPointer")) {
    mexErrMsgIdAndTxt(
        "Drake:collisionDetectmex:InvalidInputType",
        "Expected a DrakeMexPointer for mex_model_ptr but got something else.");
  }

  if (!mxIsClass(prhs[1], "DrakeMexPointer")) {
    mexErrMsgIdAndTxt(
        "Drake:collisionDetectmex:InvalidInputType",
        "Expected a DrakeMexPointer for cache_ptr but got something else.");
  }

  if (!mxIsLogical(prhs[2])) {
    mexErrMsgIdAndTxt("Drake:collisionDetectmex:InvalidInputType",
                      "Expected a boolean logic type for "
                      "allow_multiple_collisions but got something else.");
  }

  if (!mxIsStruct(prhs[3])) {
    mexErrMsgIdAndTxt("Drake:collisionDetectmex:InvalidInputType",
                      "Expected a struct type for active_collision_options but "
                      "got something else.");
  }

  int arg_num = 0;
  RigidBodyTree* model =
      static_cast<RigidBodyTree*>(getDrakeMexPointer(prhs[arg_num++]));

  KinematicsCache<double>& cache =
      fromMex(prhs[arg_num++], static_cast<KinematicsCache<double>*>(nullptr));

  // Parse `active_collision_options`
  vector<int> active_bodies_idx;
  set<string> active_group_names;

  const mxArray* allow_multiple_contacts = prhs[arg_num++];
  const mxArray* active_collision_options = prhs[arg_num++];

  const mxArray* body_idx = mxGetField(active_collision_options, 0, "body_idx");
  if (body_idx != NULL) {
    size_t n_active_bodies =
        static_cast<size_t>(mxGetNumberOfElements(body_idx));

    active_bodies_idx.resize(n_active_bodies);
    memcpy(active_bodies_idx.data(), (int*)mxGetData(body_idx),
           sizeof(int) * n_active_bodies);
    transform(active_bodies_idx.begin(), active_bodies_idx.end(),
              active_bodies_idx.begin(), [](int i) { return --i; });
  }

  const mxArray* collision_groups =
      mxGetField(active_collision_options, 0, "collision_groups");

  if (collision_groups != NULL) {
    size_t num_collision_groups =
        static_cast<size_t>(mxGetNumberOfElements(collision_groups));
    for (size_t i = 0; i < num_collision_groups; i++) {
      const mxArray* ptr = mxGetCell(collision_groups, i);
      size_t buflen = static_cast<size_t>(mxGetN(ptr) * sizeof(mxChar)) + 1;
      char* str = (char*)mxMalloc(buflen);
      mxGetString(ptr, str, buflen);
      active_group_names.insert(str);
      mxFree(str);
    }
  }

  vector<int> bodyA_idx, bodyB_idx;
  Matrix3Xd ptsA, ptsB, normals;
  MatrixXd JA, JB, Jd;
  VectorXd dist;

  if (active_bodies_idx.size() > 0) {
    if (active_group_names.size() > 0) {
      model->collisionDetect(cache, dist, normals, ptsA, ptsB, bodyA_idx,
                             bodyB_idx, active_bodies_idx, active_group_names);
    } else {
      model->collisionDetect(cache, dist, normals, ptsA, ptsB, bodyA_idx,
                             bodyB_idx, active_bodies_idx);
    }
  } else {
    const bool multiple_contacts =
        mxIsLogicalScalarTrue(allow_multiple_contacts);
    if (multiple_contacts) {
      model->potentialCollisions(cache, dist, normals, ptsA, ptsB, bodyA_idx,
                                 bodyB_idx);
    } else if (active_group_names.size() > 0) {
      model->collisionDetect(cache, dist, normals, ptsA, ptsB, bodyA_idx,
                             bodyB_idx, active_group_names);
    } else {
      model->collisionDetect(cache, dist, normals, ptsA, ptsB, bodyA_idx,
//.........这里部分代码省略.........

 void merPredD::setBeta0(const VectorXd& nBeta) {
     if (nBeta.size() != d_p) throw invalid_argument("setBeta0: dimension mismatch");
     std::copy(nBeta.data(), nBeta.data() + d_p, d_beta0.data());
 }

 void merPredD::setDelu(const VectorXd& newDelu) {
     if (newDelu.size() != d_q)
         throw invalid_argument("setDelu: dimension mismatch");
     std::copy(newDelu.data(), newDelu.data() + d_q, d_delu.data());
 }

drwnCompressionBuffer drwnCompressVector(const VectorXd& x)
{
    drwnCompressionBuffer buffer;
    buffer.compress((unsigned char *)x.data(), x.rows() * sizeof(double));
    return buffer;
}

void mexFunction(int nlhs, mxArray *plhs[], 
    int nrhs, const mxArray *prhs[])
{
  // This is useful for debugging whether Matlab is caching the mex binary
  //mexPrintf("%s %s\n",__TIME__,__DATE__);
  igl::matlab::MexStream mout;
  std::streambuf *outbuf = std::cout.rdbuf(&mout);

  using namespace std;
  using namespace Eigen;
  using namespace igl;
  using namespace igl::matlab;

  MatrixXd P,V,C;
  VectorXi I;
  VectorXd sqrD;
  MatrixXi F;
  if(nrhs < 3)
  {
    mexErrMsgTxt("nrhs < 3");
  }
  parse_rhs_double(prhs,P);
  parse_rhs_double(prhs+1,V);
  parse_rhs_index(prhs+2,F);
  mexErrMsgTxt(P.cols()==3 || P.cols()==2,"P must be #P by (3|2)");
  mexErrMsgTxt(V.cols()==3 || V.cols()==2,"V must be #V by (3|2)");
  mexErrMsgTxt(V.cols()==P.cols(),"dim(V) must be dim(P)");
  mexErrMsgTxt(F.cols()==3 || F.cols()==2 || F.cols()==1,"F must be #F by (3|2|1)");

  point_mesh_squared_distance(P,V,F,sqrD,I,C);
  // Prepare left-hand side
  switch(nlhs)
  {
    case 3:
    {
      // Treat indices as reals
      plhs[2] = mxCreateDoubleMatrix(C.rows(),C.cols(), mxREAL);
      double * Cp = mxGetPr(plhs[2]);
      copy(&C.data()[0],&C.data()[0]+C.size(),Cp);
      // Fallthrough
    }
    case 2:
    {
      // Treat indices as reals
      plhs[1] = mxCreateDoubleMatrix(I.rows(),I.cols(), mxREAL);
      double * Ip = mxGetPr(plhs[1]);
      VectorXd Id = (I.cast<double>().array()+1).matrix();
      copy(&Id.data()[0],&Id.data()[0]+Id.size(),Ip);
      // Fallthrough
    }
    case 1:
    {
      plhs[0] = mxCreateDoubleMatrix(sqrD.rows(),sqrD.cols(), mxREAL);
      double * sqrDp = mxGetPr(plhs[0]);
      copy(&sqrD.data()[0],&sqrD.data()[0]+sqrD.size(),sqrDp);
      break;
    }
    default:break;
  }

  // Restore the std stream buffer Important!
  std::cout.rdbuf(outbuf);
}

mfloat_t CKroneckerLMM::nLLeval(mfloat_t ldelta, const MatrixXdVec& A,const MatrixXdVec& X, const MatrixXd& Y, const VectorXd& S_C1, const VectorXd& S_R1, const VectorXd& S_C2, const VectorXd& S_R2)
{
//#define debugll
	muint_t R = (muint_t)Y.rows();
	muint_t C = (muint_t)Y.cols();
	assert(A.size() == X.size());
	assert(R == (muint_t)S_R1.rows());
	assert(C == (muint_t)S_C1.rows());
	assert(R == (muint_t)S_R2.rows());
	assert(C == (muint_t)S_C2.rows());
	muint_t nWeights = 0;
	for(muint_t term = 0; term < A.size();++term)
	{
		assert((muint_t)(X[term].rows())==R);
		assert((muint_t)(A[term].cols())==C);
		nWeights+=(muint_t)(A[term].rows()) * (muint_t)(X[term].cols());
	}
	mfloat_t delta = exp(ldelta);
	mfloat_t ldet = 0.0;//R * C * ldelta;

	//build D and compute the logDet of D
	MatrixXd D = MatrixXd(R,C);
	for (muint_t r=0; r<R;++r)
	{
		if(S_R2(r)>1e-10)
		{
			ldet += (mfloat_t)C * log(S_R2(r));//ldet
		}
		else
		{
			std::cout << "S_R2(" << r << ")="<< S_R2(r)<<"\n";
		}
	}
#ifdef debugll
	std::cout << ldet;
	std::cout << "\n";
#endif
	for (muint_t c=0; c<C;++c)
	{
		if(S_C2(c)>1e-10)
		{
			ldet += (mfloat_t)R * log(S_C2(c));//ldet
		}
		else
		{
			std::cout << "S_C2(" << c << ")="<< S_C2(c)<<"\n";
		}
	}
#ifdef debugll
	std::cout << ldet;
	std::cout << "\n";
#endif
	for (muint_t r=0; r<R;++r)
	{
		for (muint_t c=0; c<C;++c)
		{
			mfloat_t SSd = S_R1.data()[r]*S_C1.data()[c] + delta;
			ldet+=log(SSd);
			D(r,c) = 1.0/SSd;
		}
	}
#ifdef debugll
	std::cout << ldet;
	std::cout << "\n";
#endif
	MatrixXd DY = Y.array() * D.array();

	VectorXd XYA = VectorXd(nWeights);

	muint_t cumSumR = 0;

	MatrixXd covW = MatrixXd(nWeights,nWeights);
	for(muint_t termR = 0; termR < A.size();++termR){
		muint_t nW_AR = A[termR].rows();
		muint_t nW_XR = X[termR].cols();
		muint_t rowsBlock = nW_AR * nW_XR;
		MatrixXd XYAblock = X[termR].transpose() * DY * A[termR].transpose();
		XYAblock.resize(rowsBlock,1);
		XYA.block(cumSumR,0,rowsBlock,1) = XYAblock;

		muint_t cumSumC = 0;

		for(muint_t termC = 0; termC < A.size(); ++termC){
			muint_t nW_AC = A[termC].rows();
			muint_t nW_XC = X[termC].cols();
			muint_t colsBlock = nW_AC * nW_XC;
			MatrixXd block = MatrixXd::Zero(rowsBlock,colsBlock);
			if (R<C)
			{
				for(muint_t r=0; r<R; ++r)
				{
					MatrixXd AD = A[termR];
					AD.array().rowwise() *= D.row(r).array();
					MatrixXd AA = AD * A[termC].transpose();
					//sum up col matrices
					MatrixXd XX = X[termR].row(r).transpose() * X[termC].row(r);
					akron(block,AA,XX,true);
				}
			}
			else
//.........这里部分代码省略.........

void eigs_gen_F77(MatrixXd &M, VectorXd &init_resid, int k, int m,
                  double &time_used, double &prec_err, int &nops)
{
    double start, end;
    prec_err = -1.0;
    start = get_wall_time();

    // Begin ARPACK
    //
    // Initial value of ido
    int ido = 0;
    // 'I' means standard eigen value problem, A * x = lambda * x
    char bmat = 'I';
    // dimension of A (n by n)
    int n = M.rows();
    // Specify selection criteria
    // "LM": largest magnitude
    char which[3] = {'L', 'M', '\0'};
    // Number of eigenvalues requested
    int nev = k;
    // Precision
    double tol = 1e-10;
    // Residual vector
    double *resid = new double[n]();
    std::copy(init_resid.data(), init_resid.data() + n, resid);
    // Number of Ritz values used
    int ncv = m;
    // Vector of eigenvalues
    VectorXd evals_re(nev + 1);
    VectorXd evals_im(nev + 1);
    // Matrix of eigenvectors
    MatrixXd evecs(n, ncv);

    // Store final results of eigenvectors
    // double *V = new double[n * ncv]();
    double *V = evecs.data();
    // Leading dimension of V, required by FORTRAN
    int ldv = n;
    // Control parameters
    int *iparam = new int[11]();
    iparam[1 - 1] = 1; // ishfts
    iparam[3 - 1] = 1000; // maxitr
    iparam[7 - 1] = 1; // mode
    // Some pointers
    int *ipntr = new int[14]();
    /* workd has 3 columns.
     * ipntr[2] - 1 ==> first column to store B * X,
     * ipntr[1] - 1 ==> second to store Y,
     * ipntr[0] - 1 ==> third to store X. */
    double *workd = new double[3 * n]();
    int lworkl = 3 * ncv * ncv + 6 * ncv;
    double *workl = new double[lworkl]();
    // Error flag. 0 means random initialization,
    // otherwise using resid as initial value
    int info = 1;

    naupd(ido, bmat, n, which,
          nev, tol, resid,
          ncv, V, ldv,
          iparam, ipntr, workd,
          workl, lworkl, info);
    // ido == -1 or ido == 1 means more iterations needed
    while (ido == -1 || ido == 1)
    {
        MapVec vec_in(&workd[ipntr[0] - 1], n);
        MapVec vec_out(&workd[ipntr[1] - 1], n);
        vec_out.noalias() = M * vec_in;

        naupd(ido, bmat, n, which,
              nev, tol, resid,
              ncv, V, ldv,
              iparam, ipntr, workd,
              workl, lworkl, info);
    }

    // info > 0 means warning, < 0 means error
    if(info > 0)
        std::cout << "warnings occured" << std::endl;
    if(info < 0)
    {
        delete [] workl;
        delete [] workd;
        delete [] ipntr;
        delete [] iparam;
        delete [] resid;

        std::cout << "errors occured" << std::endl;
        end = get_wall_time();
        time_used = (end - start) * 1000;

        return;
    }

    // Retrieve results
    //
    // Whether to calculate eigenvectors or not.
    bool rvec = true;
    // 'A' means to calculate Ritz vectors
    // 'P' to calculate Schur vectors
    char howmny = 'A';
//.........这里部分代码省略.........

void randVec(VectorXd& M)
{
    for (int nn = 0; nn < M.size(); nn++)
        M.data()[nn] = -1.0 + 2.0 * Uniform(myrng);
}

Vector<double> as_vector (const VectorXd & x)
{
  return Vector<double>(x.size(), const_cast<double *>(x.data()));
}