C++ eigen::VectorXi类代码示例

void SliceStack::computeLaplace(const Eigen::MatrixXd &TV, const Eigen::MatrixXi &TT,
                                const Eigen::MatrixXi &TF, const Eigen::VectorXi &TO,
                                Eigen::VectorXd &Z, const set<int> &allowed) {
  bool laplace_DEBUG = false;
	Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
	Eigen::IOFormat LongFmt(10, 0, ", ", "\n", "[", "]");
	Eigen::IOFormat RFmt(4, 0, ", ", ", ", "", "", "(", ")");

	assert(TO.rows() == TV.rows());

	std::vector<int> known_v;
	std::vector<double> known_c_v;
  auto mx = TV.colwise().maxCoeff();
  auto mn = TV.colwise().minCoeff();

	for (int i = 0; i < TO.rows(); ++i) {
		if (allowed.size() == 0 && TO(i) != GLOBAL::nonoriginal_marker) {
			known_v.push_back(i);
			known_c_v.push_back(GLOBAL::inside_temp);
		}
    else if (allowed.find(TO(i)) != allowed.end()) {
			known_v.push_back(i);
			known_c_v.push_back(GLOBAL::inside_temp);
    }
    else if (TV(i,2) == mx(2) || TV(i,2) == mn(2)) {
			known_v.push_back(i);
			known_c_v.push_back(GLOBAL::outside_temp);
		}
	}

	if (laplace_DEBUG) 
    printf("done! Number of known values is %lu/%lu\n",
           known_v.size(), TV.rows());

	Eigen::VectorXi known(known_v.size());
	Eigen::VectorXd known_c(known_v.size());

	for (int i = 0; i < known_c.size(); ++i) {
		known(i) = known_v[i];
		known_c(i) = known_c_v[i];
	}

	Eigen::SparseMatrix<double> L(TV.rows(), TV.rows());
	// Set non-diag elements to 1 if connected, 0 otherwise
	// Use the tets instead of the faces
	for (int i = 0; i < TT.rows(); ++i) {
		L.coeffRef(TT(i,0), TT(i,1)) = -1; L.coeffRef(TT(i,1), TT(i,0)) = -1;
		L.coeffRef(TT(i,1), TT(i,2)) = -1; L.coeffRef(TT(i,2), TT(i,1)) = -1;
		L.coeffRef(TT(i,2), TT(i,3)) = -1; L.coeffRef(TT(i,3), TT(i,2)) = -1;
		L.coeffRef(TT(i,3), TT(i,0)) = -1; L.coeffRef(TT(i,0), TT(i,3)) = -1;
	}

	// Set diag elements to valence of entry
	for (int i = 0; i < TV.rows(); ++i) {
		L.coeffRef(i,i) = -L.row(i).sum();
	}

  if (laplace_DEBUG) {
    printf("done! Number non-zeros is %ld\n", L.nonZeros());
    printf("Solving energy constraints...");
  }

	// Solve energy constraints.
	igl::min_quad_with_fixed_data<double> mqwf;
	// Linear term is 0
	Eigen::VectorXd B = Eigen::VectorXd::Zero(TV.rows(), 1);
	// Empty Constraints
	Eigen::VectorXd Beq;
	Eigen::SparseMatrix<double> Aeq;

	if (!igl::min_quad_with_fixed_precompute(L, known, Aeq, false, mqwf))
		fprintf(stderr,"ERROR: fixed_precompute didn't work!\n");

	igl::min_quad_with_fixed_solve(mqwf,B,known_c,Beq, Z);

  if (laplace_DEBUG)
    printf("fixed_solve complete.\n");
}

int main(int argc, char * argv[])
{
  using namespace Eigen;
  using namespace std;
  using namespace igl;
  if(!readMESH("../shared/octopus-low.mesh",low.V,low.T,low.F))
  {
    cout<<"failed to load mesh"<<endl;
  }
  if(!readMESH("../shared/octopus-high.mesh",high.V,high.T,high.F))
  {
    cout<<"failed to load mesh"<<endl;
  }

  // Precomputation
  {
    Eigen::VectorXi b;
    {
      Eigen::VectorXi J = Eigen::VectorXi::LinSpaced(high.V.rows(),0,high.V.rows()-1);
      Eigen::VectorXd sqrD;
      Eigen::MatrixXd _2;
      cout<<"Finding closest points..."<<endl;
      igl::point_mesh_squared_distance(low.V,high.V,J,sqrD,b,_2);
      assert(sqrD.minCoeff() < 1e-7 && "low.V should exist in high.V");
    }
    // force perfect positioning, rather have popping in low-res than high-res.
    // The correct/elaborate thing to do is express original low.V in terms of
    // linear interpolation (or extrapolation) via elements in (high.V,high.F)
    igl::slice(high.V,b,1,low.V);
    // list of points --> list of singleton lists
    std::vector<std::vector<int> > S;
    igl::matrix_to_list(b,S);
    cout<<"Computing weights for "<<b.size()<<
      " handles at "<<high.V.rows()<<" vertices..."<<endl;
    // Technically k should equal 3 for smooth interpolation in 3d, but 2 is
    // faster and looks OK
    const int k = 2;
    igl::biharmonic_coordinates(high.V,high.T,S,k,W);
    cout<<"Reindexing..."<<endl;
    // Throw away interior tet-vertices, keep weights and indices of boundary
    VectorXi I,J;
    igl::remove_unreferenced(high.V.rows(),high.F,I,J);
    for_each(high.F.data(),high.F.data()+high.F.size(),[&I](int & a){a=I(a);});
    for_each(b.data(),b.data()+b.size(),[&I](int & a){a=I(a);});
    igl::slice(MatrixXd(high.V),J,1,high.V);
    igl::slice(MatrixXd(W),J,1,W);
  }

  // Resize low res (high res will also be resized by affine precision of W)
  low.V.rowwise() -= low.V.colwise().mean();
  low.V /= (low.V.maxCoeff()-low.V.minCoeff());
  low.V.rowwise() += RowVector3d(0,1,0);
  low.U = low.V;
  high.U = high.V;

  arap_data.with_dynamics = true;
  arap_data.max_iter = 10;
  arap_data.energy = ARAP_ENERGY_TYPE_DEFAULT;
  arap_data.h = 0.01;
  arap_data.ym = 0.001;
  if(!arap_precomputation(low.V,low.T,3,VectorXi(),arap_data))
  {
    cerr<<"arap_precomputation failed."<<endl;
    return EXIT_FAILURE;
  }
  // Constant gravitational force
  Eigen::SparseMatrix<double> M;
  igl::massmatrix(low.V,low.T,igl::MASSMATRIX_TYPE_DEFAULT,M);
  const size_t n = low.V.rows();
  arap_data.f_ext =  M * RowVector3d(0,-9.8,0).replicate(n,1);
  // Random initial velocities to wiggle things
  arap_data.vel = MatrixXd::Random(n,3);
  
  igl::viewer::Viewer viewer;
  // Create one huge mesh containing both meshes
  igl::cat(1,low.U,high.U,scene.U);
  igl::cat(1,low.F,MatrixXi(high.F.array()+low.V.rows()),scene.F);
  // Color each mesh
  viewer.data.set_mesh(scene.U,scene.F);
  MatrixXd C(scene.F.rows(),3);
  C<<
    RowVector3d(0.8,0.5,0.2).replicate(low.F.rows(),1),
    RowVector3d(0.3,0.4,1.0).replicate(high.F.rows(),1);
  viewer.data.set_colors(C);

  viewer.callback_key_pressed = 
    [&](igl::viewer::Viewer & viewer,unsigned int key,int mods)->bool
  {
    switch(key)
    {
      default: 
        return false;
      case ' ':
        viewer.core.is_animating = !viewer.core.is_animating;
        return true;
      case 'r':
        low.U = low.V;
        return true;
    }
  };
//.........这里部分代码省略.........

bool ExecControl::firing_rec2(int des_place, std::vector<int>& skip_transitions, std::vector<int>& visited_places)
{
	visited_places.push_back(des_place);

	//Find all possible transition activators for this place
	Eigen::VectorXi transitions = Dp.row(des_place);
	std::vector<int> activators;
	for (int i=0; i<transitions.size(); ++i)
	{
		if (transitions(i))
		{
			bool skipped = false;
			//Filter skipped transitions
			for (int j=0; j< skip_transitions.size(); ++j)
			{
				if ((skipped = (skip_transitions[j] == i))) break;
			}

			if (!skipped) activators.push_back(i);
		}
	}

	std::cout<<"Place "<<pname[des_place]<<" depends on transitions:";
	for (int i=0; i<activators.size(); ++i)
	{
		std::cout<<activators[i]<<", ";
	}
	std::cout<<std::endl;

	//If no activators this is a dead-end.
	if (activators.empty())
	{
		std::cout<<"Dead-end."<<std::endl;
		return false;
	}

	//For each activator find places
	bool add_seq = false;
	for (int i=0; i<activators.size(); ++i)
	{
		Eigen::VectorXi t_en = Dm.col(activators[i]);
		std::vector<int> places;
		for (int j=0; j<t_en.size(); ++j)
		{
			if (t_en(j))
			{

				bool skipped = false;
				//Filter skipped transitions
				for (int k=0; k< visited_places.size(); ++k)
				{
					if ((skipped = (visited_places[k] == j))) break;
				}

				if (!skipped) places.push_back(j);
			}
		}

		std::cout<<"Transition "<<activators[i]<<" depends on places:";
		for (int j=0; j<places.size(); ++j)
		{
			std::cout<<pname[places[j]]<<", ";
		}
		std::cout<<std::endl;

		bool add_cur_seq=true;
		for (int j=0; j<places.size(); ++j)
		{
			//If place is active add
			if (!marking(places[j]))
			{
				if (!firing_rec2(places[j], skip_transitions, visited_places))
				{
					add_cur_seq = false;
					break;
				}
			}
		}

		if (add_cur_seq && !places.empty())
		{
			bool add = true;
			for (int j=0; j<firing_seq.size(); ++j)
			{
				if (firing_seq[j] == activators[i])
				{
					add = false;
					break;
				}
			}
			if (add)
			{
				std::cout<<"Adding to firing sequence:"<<activators[i]<<std::endl;
				firing_seq.push_back(activators[i]);
				add_seq = true;
				if (activators[i]%2 == 0)
				{
					skip_transitions.push_back(activators[i]+1);
				}
				else
//.........这里部分代码省略.........

IGL_INLINE void igl::parallel_transport_angles(
const Eigen::PlainObjectBase<DerivedV>& V,
const Eigen::PlainObjectBase<DerivedF>& F,
const Eigen::PlainObjectBase<DerivedV>& FN,
const Eigen::MatrixXi &E2F,
const Eigen::MatrixXi &F2E,
Eigen::PlainObjectBase<DerivedK> &K)
{
  int numE = E2F.rows();

  Eigen::VectorXi isBorderEdge;
  isBorderEdge.setZero(numE,1);
  for(unsigned i=0; i<numE; ++i)
  {
    if ((E2F(i,0) == -1) || ((E2F(i,1) == -1)))
      isBorderEdge[i] = 1;
  }

  K.setZero(numE);
  // For every non-border edge
  for (unsigned eid=0; eid<numE; ++eid)
  {
    if (!isBorderEdge[eid])
    {
      int fid0 = E2F(eid,0);
      int fid1 = E2F(eid,1);

      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> N0 = FN.row(fid0);
//      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> N1 = FN.row(fid1);

      // find common edge on triangle 0 and 1
      int fid0_vc = -1;
      int fid1_vc = -1;
      for (unsigned i=0;i<3;++i)
      {
        if (F2E(fid0,i) == eid)
          fid0_vc = i;
        if (F2E(fid1,i) == eid)
          fid1_vc = i;
      }
      assert(fid0_vc != -1);
      assert(fid1_vc != -1);

      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> common_edge = V.row(F(fid0,(fid0_vc+1)%3)) - V.row(F(fid0,fid0_vc));
      common_edge.normalize();

      // Map the two triangles in a new space where the common edge is the x axis and the N0 the z axis
      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> P;
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> o = V.row(F(fid0,fid0_vc));
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> tmp = -N0.cross(common_edge);
      P << common_edge, tmp, N0;
      //      P.transposeInPlace();


      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> V0;
      V0.row(0) = V.row(F(fid0,0)) -o;
      V0.row(1) = V.row(F(fid0,1)) -o;
      V0.row(2) = V.row(F(fid0,2)) -o;

      V0 = (P*V0.transpose()).transpose();

      //      assert(V0(0,2) < 1e-10);
      //      assert(V0(1,2) < 1e-10);
      //      assert(V0(2,2) < 1e-10);

      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> V1;
      V1.row(0) = V.row(F(fid1,0)) -o;
      V1.row(1) = V.row(F(fid1,1)) -o;
      V1.row(2) = V.row(F(fid1,2)) -o;
      V1 = (P*V1.transpose()).transpose();

      //      assert(V1(fid1_vc,2) < 10e-10);
      //      assert(V1((fid1_vc+1)%3,2) < 10e-10);

      // compute rotation R such that R * N1 = N0
      // i.e. map both triangles to the same plane
      double alpha = -atan2(V1((fid1_vc+2)%3,2),V1((fid1_vc+2)%3,1));

      Eigen::Matrix<typename DerivedV::Scalar, 3, 3> R;
      R << 1,          0,            0,
      0, cos(alpha), -sin(alpha) ,
      0, sin(alpha),  cos(alpha);
      V1 = (R*V1.transpose()).transpose();

      //      assert(V1(0,2) < 1e-10);
      //      assert(V1(1,2) < 1e-10);
      //      assert(V1(2,2) < 1e-10);

      // measure the angle between the reference frames
      // k_ij is the angle between the triangle on the left and the one on the right
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> ref0 = V0.row(1) - V0.row(0);
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> ref1 = V1.row(1) - V1.row(0);

      ref0.normalize();
      ref1.normalize();

      double ktemp = atan2(ref1(1),ref1(0)) - atan2(ref0(1),ref0(0));

      // just to be sure, rotate ref0 using angle ktemp...
      Eigen::Matrix<typename DerivedV::Scalar,2,2> R2;
//.........这里部分代码省略.........

void
pcl::Permutohedral::init (const std::vector<float> &feature, const int feature_dimension, const int N)
{
  N_ = N;
  d_ = feature_dimension;
  
  // Create hash table
  std::vector<std::vector<short> > keys;
  keys.reserve ( (d_+1) * N_ );
  std::multimap<size_t, int> hash_table;

  // reserve class memory
  if (offset_.size () > 0) 
    offset_.clear ();
  offset_.resize ((d_ + 1) * N_);

  if (barycentric_.size () > 0) 
    barycentric_.clear ();
  barycentric_.resize ((d_ + 1) * N_);

  // create vectors and matrices
  Eigen::VectorXf scale_factor = Eigen::VectorXf::Zero (d_);
  Eigen::VectorXf elevated = Eigen::VectorXf::Zero (d_ + 1);
  Eigen::VectorXf rem0 = Eigen::VectorXf::Zero (d_+1);
  Eigen::VectorXf barycentric = Eigen::VectorXf::Zero (d_+2);
  Eigen::VectorXi rank = Eigen::VectorXi::Zero (d_+1);
  Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic> canonical;
  canonical = Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic>::Zero (d_+1, d_+1);
  //short * key = new short[d_+1];
  std::vector<short> key (d_+1);

  // Compute the canonical simple
  for (int i = 0; i <= d_; i++)
  {
    for (int j = 0; j <= (d_ - i); j++)
      canonical (j, i) = i;
    for (int j = (d_ - i + 1); j <= d_; j++)
      canonical (j, i) = i - (d_ + 1);
  }

  // Expected standard deviation of our filter (p.6 in [Adams etal 2010])
  float inv_std_dev = sqrt (2.0f / 3.0f) * static_cast<float> (d_ + 1);
  
  // Compute the diagonal part of E (p.5 in [Adams etal 2010])
  for( int i = 0; i < d_; i++ )
    scale_factor (i) = 1.0f / sqrt( static_cast<float> (i + 2) * static_cast<float> (i + 1) ) * inv_std_dev;

  // Compute the simplex each feature lies in
  for( int k = 0; k < N_; k++ )
    //for( int k = 0; k < 5; k++ )
  {

    // Elevate the feature ( y = Ep, see p.5 in [Adams etal 2010])
    int index = k * feature_dimension;
    // sm contains the sum of 1..n of our faeture vector
    float sm = 0;
    for( int j = d_; j > 0; j-- )
    {
      float cf = feature[index + j-1] * scale_factor (j-1);      
      elevated (j) = sm - static_cast<float> (j) * cf;
      sm += cf;
    }
    elevated (0) = sm;

    // Find the closest 0-colored simplex through rounding
    float down_factor = 1.0f / static_cast<float>(d_+1);
    float up_factor = static_cast<float>(d_+1);
    int sum = 0;
    for( int j = 0; j <= d_; j++ ){
      float rd = floorf ( 0.5f + ( down_factor * elevated (j) ) ) ;
      rem0 (j) = rd * up_factor;
      sum += static_cast<int> (rd);
    }
    
    // rank differential to find the permutation between this simplex and the canonical one.         
    // (See pg. 3-4 in paper.)    
    rank.setZero ();
    Eigen::VectorXf tmp = elevated - rem0;
    for( int i = 0; i < d_; i++ ){
      for( int j = i+1; j <= d_; j++ )
        if ( tmp (i) < tmp (j) )
          rank (i)++;
        else
          rank (j)++;
    }

    // If the point doesn't lie on the plane (sum != 0) bring it back
    for( int j = 0; j <= d_; j++ ){
      rank (j) += sum;
      if ( rank (j) < 0 ){
        rank (j) += d_+1;
        rem0 (j) += static_cast<float> (d_ + 1);
      }
      else if ( rank (j) > d_ ){
        rank (j) -= d_+1;
        rem0 (j) -= static_cast<float> (d_ + 1);
      }
    }

    // Compute the barycentric coordinates (p.10 in [Adams etal 2010])
//.........这里部分代码省略.........

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;
  igl::readOBJ(TUTORIAL_SHARED_PATH "/decimated-max.obj",V,F);
  U=V;
  // S(i) = j: j<0 (vertex i not in handle), j >= 0 (vertex i in handle j)
  VectorXi S;
  igl::readDMAT(TUTORIAL_SHARED_PATH "/decimated-max-selection.dmat",S);
  igl::colon<int>(0,V.rows()-1,b);
  b.conservativeResize(stable_partition( b.data(), b.data()+b.size(),
   [&S](int i)->bool{return S(i)>=0;})-b.data());

  // Boundary conditions directly on deformed positions
  U_bc.resize(b.size(),V.cols());
  V_bc.resize(b.size(),V.cols());
  for(int bi = 0;bi<b.size();bi++)
  {
    V_bc.row(bi) = V.row(b(bi));
    switch(S(b(bi)))
    {
      case 0:
        // Don't move handle 0
        U_bc.row(bi) = V.row(b(bi));
        break;
      case 1:
        // move handle 1 down
        U_bc.row(bi) = V.row(b(bi)) + RowVector3d(0,-50,0);
        break;
      case 2:
      default:
        // move other handles forward
        U_bc.row(bi) = V.row(b(bi)) + RowVector3d(0,0,-25);
        break;
    }
  }

  // Pseudo-color based on selection
  MatrixXd C(F.rows(),3);
  RowVector3d purple(80.0/255.0,64.0/255.0,255.0/255.0);
  RowVector3d gold(255.0/255.0,228.0/255.0,58.0/255.0);
  for(int f = 0;f<F.rows();f++)
  {
    if( S(F(f,0))>=0 && S(F(f,1))>=0 && S(F(f,2))>=0)
    {
      C.row(f) = purple;
    }else
    {
      C.row(f) = gold;
    }
  }

  // Plot the mesh with pseudocolors
  igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(U, F);
  viewer.data().show_lines = false;
  viewer.data().set_colors(C);
  viewer.core().trackball_angle = Eigen::Quaternionf(sqrt(2.0),0,sqrt(2.0),0);
  viewer.core().trackball_angle.normalize();
  viewer.callback_pre_draw = &pre_draw;
  viewer.callback_key_down = &key_down;
  //viewer.core().is_animating = true;
  viewer.core().animation_max_fps = 30.;
  cout<<
    "Press [space] to toggle deformation."<<endl<<
    "Press 'd' to toggle between biharmonic surface or displacements."<<endl;
  viewer.launch();
}

bool key_down(igl::viewer::Viewer& viewer, unsigned char key, int modifier)
{
  using namespace std;
  using namespace Eigen;

  if (key <'1' || key >'9')
    return false;

  viewer.data.lines.resize(0,9);

  int num = key  - '0';

  // Interpolate
  std::cerr << "Interpolating " << num  << "-PolyVector field" << std::endl;

  VectorXi b(3);
  b << 1511, 603, 506;

  int numConstraintsToGenerate;
  // if it's not a 2-PV or a 1-PV, include a line direction (2 opposite vectors)
  // in the field
  if (num>=5)
    numConstraintsToGenerate  = num-2;
  else
    if (num>=3)
      numConstraintsToGenerate  = num-1;
    else
      numConstraintsToGenerate  = num;


  MatrixXd bc(b.size(),numConstraintsToGenerate*3);
  for (unsigned i=0; i<b.size(); ++i)
  {
    VectorXd t = random_constraints(B1.row(b(i)),B2.row(b(i)),numConstraintsToGenerate);
    bc.row(i) = t;
  }
  VectorXi rootsIndex(num);
  for (int i =0; i<numConstraintsToGenerate; ++i)
    rootsIndex[i] = i+1;
  if (num>=5)
    rootsIndex[num-2] = -2;
    if (num>=3)
      rootsIndex[num-1] = -1;

  // Interpolated PolyVector field
  Eigen::MatrixXd pvf;
  igl::n_polyvector_general(V, F, b, bc, rootsIndex, pvf);

  ofstream ofs;
  ofs.open("pvf.txt", ofstream::out);
  ofs<<pvf;
  ofs.close();
  igl::writeOFF("pvf.off",V,F);
  
  // Highlight in red the constrained faces
  MatrixXd C = MatrixXd::Constant(F.rows(),3,1);
  for (unsigned i=0; i<b.size();++i)
    C.row(b(i)) << 1, 0, 0;
  viewer.data.set_colors(C);

  for (int n=0; n<num; ++n)
  {
//    const MatrixXd &VF = pvf.block(0,n*3,F.rows(),3);
    MatrixXd VF = MatrixXd::Zero(F.rows(),3);
    for (unsigned i=0; i<b.size(); ++i)
      VF.row(b[i]) = pvf.block(b[i],n*3,1,3);
    
    for (int i=0; i<samples.rows(); ++i)
      VF.row(samples[i]) = pvf.block(samples[i],n*3,1,3);
    
    VectorXd c = VF.rowwise().norm();
    MatrixXd C2;
    igl::jet(c,1,1+rand_factor,C2);
    // viewer.data.add_edges(B - global_scale*VF, B + global_scale*VF , C2);
    viewer.data.add_edges(B, B + global_scale*VF , C2);
  }


  return false;
}

/**
 * @function smoothPath
 */
void PathPlanner::smoothPath( int _robotId, 
                              const Eigen::VectorXi &_links, 
                              std::list<Eigen::VectorXd> &_path ) {
  
  // =========== YOUR CODE HERE ==================
  // HINT: Use whatever technique you like better, first try to shorten a path and then you can try to make it smoother
  	std::cout << "Starting path shortening with simple search..." << _links.size() << std::endl;
	std::cout << "start path length: " << _path.size() << std::endl;

	std::list<Eigen::VectorXd>::iterator start_point=_path.begin(),end_point=_path.end();
	end_point--;

	// loop while start has not reached the end of the path
	while (start_point != _path.end())
	{
		if (start_point == end_point)
		{
		//std::cout << "End iteration\n";
			++start_point;
			end_point = _path.end();
			end_point--;
		}
		else
		{
		  //Eigen::VectorXd start_node=*start_point, end_node=*end_point;
			std::list<Eigen::VectorXd> segment = createPath(_robotId,_links,*start_point, *end_point);
			double curDist =  countDist(start_point, end_point) * stepSize;
			double shortcutDist = segment.size() * stepSize;
			if (segment.size()>0 && shortcutDist < curDist)
			{
			  std::cout << "Shortcut length: " << shortcutDist << std::endl;
				std::cout << "Current distance: " << curDist << std::endl;
				std::cout << "Found shortcut!" << std::endl;
				// reconstruct path
				// first segment
				std::list<Eigen::VectorXd> new_path(_path.begin(), start_point);
				// middle segment
				new_path.insert(new_path.end(), segment.begin(), segment.end());
				// last segment
				new_path.insert(new_path.end(), end_point, _path.end());

        std::cout << "New path length: " << new_path.size() << std::endl;
				
				// replace optimized
				_path = new_path;

				start_point = _path.begin();
				end_point = _path.end();
				end_point--;
			}
			else
			{
			  --end_point;
			}
		}
	}
	std::cout << "Finished Optimizing!  Final path length: " << _path.size() << std::endl;  
  return;
  return;
  // ========================================	
}

IGL_INLINE bool igl::decimate(
  const Eigen::MatrixXd & OV,
  const Eigen::MatrixXi & OF,
  const std::function<void(
    const int,
    const Eigen::MatrixXd &,
    const Eigen::MatrixXi &,
    const Eigen::MatrixXi &,
    const Eigen::VectorXi &,
    const Eigen::MatrixXi &,
    const Eigen::MatrixXi &,
    double &,
    Eigen::RowVectorXd &)> & cost_and_placement,
  const std::function<bool(
      const Eigen::MatrixXd &,
      const Eigen::MatrixXi &,
      const Eigen::MatrixXi &,
      const Eigen::VectorXi &,
      const Eigen::MatrixXi &,
      const Eigen::MatrixXi &,
      const std::set<std::pair<double,int> > &,
      const std::vector<std::set<std::pair<double,int> >::iterator > &,
      const Eigen::MatrixXd &,
      const int,
      const int,
      const int,
      const int,
      const int)> & stopping_condition,
    const std::function<bool(
      const Eigen::MatrixXd &                                         ,/*V*/
      const Eigen::MatrixXi &                                         ,/*F*/
      const Eigen::MatrixXi &                                         ,/*E*/
      const Eigen::VectorXi &                                         ,/*EMAP*/
      const Eigen::MatrixXi &                                         ,/*EF*/
      const Eigen::MatrixXi &                                         ,/*EI*/
      const std::set<std::pair<double,int> > &                        ,/*Q*/
      const std::vector<std::set<std::pair<double,int> >::iterator > &,/*Qit*/
      const Eigen::MatrixXd &                                         ,/*C*/
      const int                                                        /*e*/
      )> & pre_collapse,
    const std::function<void(
      const Eigen::MatrixXd &                                         ,   /*V*/
      const Eigen::MatrixXi &                                         ,   /*F*/
      const Eigen::MatrixXi &                                         ,   /*E*/
      const Eigen::VectorXi &                                         ,/*EMAP*/
      const Eigen::MatrixXi &                                         ,  /*EF*/
      const Eigen::MatrixXi &                                         ,  /*EI*/
      const std::set<std::pair<double,int> > &                        ,   /*Q*/
      const std::vector<std::set<std::pair<double,int> >::iterator > &, /*Qit*/
      const Eigen::MatrixXd &                                         ,   /*C*/
      const int                                                       ,   /*e*/
      const int                                                       ,  /*e1*/
      const int                                                       ,  /*e2*/
      const int                                                       ,  /*f1*/
      const int                                                       ,  /*f2*/
      const bool                                                  /*collapsed*/
      )> & post_collapse,
  const Eigen::MatrixXi & OE,
  const Eigen::VectorXi & OEMAP,
  const Eigen::MatrixXi & OEF,
  const Eigen::MatrixXi & OEI,
  Eigen::MatrixXd & U,
  Eigen::MatrixXi & G,
  Eigen::VectorXi & J,
  Eigen::VectorXi & I
  )
{

  // Decimate 1
  using namespace Eigen;
  using namespace std;
  // Working copies
  Eigen::MatrixXd V = OV;
  Eigen::MatrixXi F = OF;
  Eigen::MatrixXi E = OE;
  Eigen::VectorXi EMAP = OEMAP;
  Eigen::MatrixXi EF = OEF;
  Eigen::MatrixXi EI = OEI;
  typedef std::set<std::pair<double,int> > PriorityQueue;
  PriorityQueue Q;
  std::vector<PriorityQueue::iterator > Qit;
  Qit.resize(E.rows());
  // If an edge were collapsed, we'd collapse it to these points:
  MatrixXd C(E.rows(),V.cols());
  for(int e = 0;e<E.rows();e++)
  {
    double cost = e;
    RowVectorXd p(1,3);
    cost_and_placement(e,V,F,E,EMAP,EF,EI,cost,p);
    C.row(e) = p;
    Qit[e] = Q.insert(std::pair<double,int>(cost,e)).first;
  }
  int prev_e = -1;
  bool clean_finish = false;

  while(true)
  {
    if(Q.empty())
    {
      break;
//.........这里部分代码省略.........

int main(int argc, char *argv[])
{
  using namespace std;
  // Load a mesh in OFF format
  igl::readOBJ(TUTORIAL_SHARED_PATH "/camel_b.obj", V, F);

  Eigen::MatrixXd bnd_uv, uv_init;

  Eigen::VectorXd M;
  igl::doublearea(V, F, M);
  std::vector<std::vector<int>> all_bnds;
  igl::boundary_loop(F, all_bnds);

  // Heuristic primary boundary choice: longest
  auto primary_bnd = std::max_element(all_bnds.begin(), all_bnds.end(), [](const std::vector<int> &a, const std::vector<int> &b) { return a.size()<b.size(); });

  Eigen::VectorXi bnd = Eigen::Map<Eigen::VectorXi>(primary_bnd->data(), primary_bnd->size());

  igl::map_vertices_to_circle(V, bnd, bnd_uv);
  bnd_uv *= sqrt(M.sum() / (2 * igl::PI));
  if (all_bnds.size() == 1)
  {
    if (bnd.rows() == V.rows()) // case: all vertex on boundary
    {
      uv_init.resize(V.rows(), 2);
      for (int i = 0; i < bnd.rows(); i++)
        uv_init.row(bnd(i)) = bnd_uv.row(i);
    }
    else
    {
      igl::harmonic(V, F, bnd, bnd_uv, 1, uv_init);
      if (igl::flipped_triangles(uv_init, F).size() != 0)
        igl::harmonic(F, bnd, bnd_uv, 1, uv_init); // fallback uniform laplacian
    }
  }
  else
  {
    // if there is a hole, fill it and erase additional vertices.
    all_bnds.erase(primary_bnd);
    Eigen::MatrixXi F_filled;
    igl::topological_hole_fill(F, bnd, all_bnds, F_filled);
    igl::harmonic(F_filled, bnd, bnd_uv ,1, uv_init);
    uv_init = uv_init.topRows(V.rows());
  }

  Eigen::VectorXi b; Eigen::MatrixXd bc;
  igl::scaf_precompute(V, F, uv_init, scaf_data, igl::MappingEnergyType::SYMMETRIC_DIRICHLET, b, bc, 0);

  // Plot the mesh
  igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(V, F);
  const auto& V_uv = uv_scale * scaf_data.w_uv.topRows(V.rows());
  viewer.data().set_uv(V_uv);
  viewer.callback_key_down = &key_down;

  // Enable wireframe
  viewer.data().show_lines = true;

  // Draw checkerboard texture
  viewer.data().show_texture = true;


  std::cerr << "Press space for running an iteration." << std::endl;
  std::cerr << "Press 1 for Mesh 2 for UV" << std::endl;

  // Launch the viewer
  viewer.launch();
}

IGL_INLINE bool igl::boundary_conditions(
  const Eigen::MatrixXd & V  ,
  const Eigen::MatrixXi & /*Ele*/,
  const Eigen::MatrixXd & C  ,
  const Eigen::VectorXi & P  ,
  const Eigen::MatrixXi & BE ,
  const Eigen::MatrixXi & CE ,
  Eigen::VectorXi &       b  ,
  Eigen::MatrixXd &       bc )
{
  using namespace Eigen;
  using namespace std;

  if(P.size()+BE.rows() == 0)
  {
    verbose("^%s: Error: no handles found\n",__FUNCTION__);
    return false;
  }

  vector<int> bci;
  vector<int> bcj;
  vector<double> bcv;

  // loop over points
  for(int p = 0;p<P.size();p++)
  {
    VectorXd pos = C.row(P(p));
    // loop over domain vertices
    for(int i = 0;i<V.rows();i++)
    {
      // Find samples just on pos
      //Vec3 vi(V(i,0),V(i,1),V(i,2));
      // EIGEN GOTCHA:
      // double sqrd = (V.row(i)-pos).array().pow(2).sum();
      // Must first store in temporary
      VectorXd vi = V.row(i);
      double sqrd = (vi-pos).squaredNorm();
      if(sqrd <= FLOAT_EPS)
      {
        //cout<<"sum((["<<
        //  V(i,0)<<" "<<
        //  V(i,1)<<" "<<
        //  V(i,2)<<"] - ["<<
        //  pos(0)<<" "<<
        //  pos(1)<<" "<<
        //  pos(2)<<"]).^2) = "<<sqrd<<endl;
        bci.push_back(i);
        bcj.push_back(p);
        bcv.push_back(1.0);
      }
    }
  }

  // loop over bone edges
  for(int e = 0;e<BE.rows();e++)
  {
    // loop over domain vertices
    for(int i = 0;i<V.rows();i++)
    {
      // Find samples from tip up to tail
      VectorXd tip = C.row(BE(e,0));
      VectorXd tail = C.row(BE(e,1));
      // Compute parameter along bone and squared distance
      double t,sqrd;
      project_to_line(
          V(i,0),V(i,1),V(i,2),
          tip(0),tip(1),tip(2),
          tail(0),tail(1),tail(2),
          t,sqrd);
      if(t>=-FLOAT_EPS && t<=(1.0f+FLOAT_EPS) && sqrd<=FLOAT_EPS)
      {
        bci.push_back(i);
        bcj.push_back(P.size()+e);
        bcv.push_back(1.0);
      }
    }
  }

  // loop over cage edges
  for(int e = 0;e<CE.rows();e++)
  {
    // loop over domain vertices
    for(int i = 0;i<V.rows();i++)
    {
      // Find samples from tip up to tail
      VectorXd tip = C.row(P(CE(e,0)));
      VectorXd tail = C.row(P(CE(e,1)));
      // Compute parameter along bone and squared distance
      double t,sqrd;
      project_to_line(
          V(i,0),V(i,1),V(i,2),
          tip(0),tip(1),tip(2),
          tail(0),tail(1),tail(2),
          t,sqrd);
      if(t>=-FLOAT_EPS && t<=(1.0f+FLOAT_EPS) && sqrd<=FLOAT_EPS)
      {
        bci.push_back(i);
        bcj.push_back(CE(e,0));
        bcv.push_back(1.0-t);
        bci.push_back(i);
//.........这里部分代码省略.........

bool key_down(igl::viewer::Viewer& viewer, unsigned char key, int modifier)
{
  using namespace std;
  using namespace Eigen;

  if (key <'1' || key >'6')
    return false;

  viewer.data.clear();
  viewer.core.show_lines = false;
  viewer.core.show_texture = false;

  if (key == '1')
  {
    // Frame field constraints
    viewer.data.set_mesh(V, F);

    MatrixXd F1_t = MatrixXd::Zero(FF1.rows(),FF1.cols());
    MatrixXd F2_t = MatrixXd::Zero(FF2.rows(),FF2.cols());
    // Highlight in red the constrained faces
    MatrixXd C = MatrixXd::Constant(F.rows(),3,1);
    for (unsigned i=0; i<b.size();++i)
    {
      C.row(b(i)) << 1, 0, 0;
      F1_t.row(b(i)) = bc1.row(i);
      F2_t.row(b(i)) = bc2.row(i);
    }

    viewer.data.set_colors(C);

    MatrixXd C1,C2;
    VectorXd K1 = F1_t.rowwise().norm();
    VectorXd K2 = F2_t.rowwise().norm();
    igl::jet(K1,true,C1);
    igl::jet(K2,true,C2);

    viewer.data.add_edges(B - global_scale*F1_t, B + global_scale*F1_t ,C1);
    viewer.data.add_edges(B - global_scale*F2_t, B + global_scale*F2_t ,C2);
  }

  if (key == '2')
  {
    // Frame field
    viewer.data.set_mesh(V, F);
    MatrixXd C1,C2;
    VectorXd K1 = FF1.rowwise().norm();
    VectorXd K2 = FF2.rowwise().norm();
    igl::jet(K1,true,C1);
    igl::jet(K2,true,C2);

    viewer.data.add_edges(B - global_scale*FF1, B + global_scale*FF1 ,C1);
    viewer.data.add_edges(B - global_scale*FF2, B + global_scale*FF2 ,C2);

    // Highlight in red the constrained faces
    MatrixXd C = MatrixXd::Constant(F.rows(),3,1);
    for (unsigned i=0; i<b.size();++i)
      C.row(b(i)) << 1, 0, 0;
    viewer.data.set_colors(C);

  }

  if (key == '3')
  {
    // Deformed with frame field
    viewer.data.set_mesh(V_deformed, F);
    viewer.data.add_edges(B_deformed - global_scale*FF1_deformed, B_deformed + global_scale*FF1_deformed ,Eigen::RowVector3d(1,0,0));
    viewer.data.add_edges(B_deformed - global_scale*FF2_deformed, B_deformed + global_scale*FF2_deformed ,Eigen::RowVector3d(0,0,1));
    viewer.data.set_colors(RowVector3d(1,1,1));
  }

  if (key == '4')
  {
    // Deformed with cross field
    viewer.data.set_mesh(V_deformed, F);
    viewer.data.add_edges(B_deformed - global_scale*X1_deformed, B_deformed + global_scale*X1_deformed ,Eigen::RowVector3d(0,0,1));
    viewer.data.add_edges(B_deformed - global_scale*X2_deformed, B_deformed + global_scale*X2_deformed ,Eigen::RowVector3d(0,0,1));
    viewer.data.set_colors(RowVector3d(1,1,1));
  }

  if (key == '5')
  {
    // Deformed with quad texture
    viewer.data.set_mesh(V_deformed, F);
    viewer.data.set_uv(V_uv,F_uv);
    viewer.data.set_colors(RowVector3d(1,1,1));
    viewer.core.show_texture = true;
  }

  if (key == '6')
  {
    // Deformed with quad texture
    viewer.data.set_mesh(V, F);
    viewer.data.set_uv(V_uv,F_uv);
    viewer.data.set_colors(RowVector3d(1,1,1));
    viewer.core.show_texture = true;
  }

  // Replace the standard texture with an integer shift invariant texture
  Eigen::Matrix<unsigned char,Eigen::Dynamic,Eigen::Dynamic> texture_R, texture_G, texture_B;
  line_texture(texture_R, texture_G, texture_B);
//.........这里部分代码省略.........

int main(int argc, char * argv[])
{
  using namespace std;
  using namespace Eigen;
  using namespace igl;
  string filename = "../shared/cheburashka.off";
  string skel_filename = "../shared/cheburashka.tgf";
  if(argc < 3)
  {
    cerr<<"Usage:"<<endl<<"    ./example input.obj"<<endl;
    cout<<endl<<"Opening default mesh..."<<endl;
  }else
  {
    // Read and prepare mesh
    filename = argv[1];
    skel_filename = argv[2];
  }

  // print key commands
  cout<<"[Click] and [drag]  Rotate model using trackball."<<endl;
  cout<<"[Z,z]               Snap rotation to canonical view."<<endl;
  cout<<"[⌘ Z]               Undo."<<endl;
  cout<<"[⇧ ⌘ Z]             Redo."<<endl;
  cout<<"[^C,ESC]            Exit."<<endl;

  // dirname, basename, extension and filename
  string d,b,ext,f;
  pathinfo(filename,d,b,ext,f);
  // Convert extension to lower case
  transform(ext.begin(), ext.end(), ext.begin(), ::tolower);
  vector<vector<double > > vV,vN,vTC;
  vector<vector<int > > vF,vFTC,vFN;
  if(ext == "obj")
  {
    // Convert extension to lower case
    if(!igl::readOBJ(filename,vV,vTC,vN,vF,vFTC,vFN))
    {
      return 1;
    }
  }else if(ext == "off")
  {
    // Convert extension to lower case
    if(!igl::readOFF(filename,vV,vF,vN))
    {
      return 1;
    }
  }else if(ext == "wrl")
  {
    // Convert extension to lower case
    if(!igl::readWRL(filename,vV,vF))
    {
      return 1;
    }
  //}else
  //{
  //  // Convert extension to lower case
  //  MatrixXi T;
  //  if(!igl::readMESH(filename,V,T,F))
  //  {
  //    return 1;
  //  }
  //  //if(F.size() > T.size() || F.size() == 0)
  //  {
  //    boundary_facets(T,F);
  //  }
  }
  if(vV.size() > 0)
  {
    if(!list_to_matrix(vV,V))
    {
      return 1;
    }
    polygon_mesh_to_triangle_mesh(vF,F);
  }

  readTGF(skel_filename,C,BE);
  // Recover parent indices because (C,BE) is crappy format for a tree.
  P.resize(BE.rows(),1);
  for(int e = 0;e<BE.rows();e++)
  {
    P(e) = -1;
    for(int f = 0;f<BE.rows();f++)
    {
      if(BE(e,0) == BE(f,1))
      {
        P(e) = f;
      }
    }
  }

  init_weights(V,F,C,BE,W);
  lbs_matrix(V,W,M);

  init_relative();

  // Init glut
  glutInit(&argc,argv);
  if( !TwInit(TW_OPENGL, NULL) )
  {
    // A fatal error occured
//.........这里部分代码省略.........

int main(int argc, char *argv[])
{

  // Load a mesh
  igl::readOBJ(TUTORIAL_SHARED_PATH "/inspired_mesh.obj", V, F);

  printf("--Initialization--\n");
  V_border = igl::is_border_vertex(V,F);
  igl::adjacency_list(F, VV);
  igl::vertex_triangle_adjacency(V,F,VF,VFi);
  igl::triangle_triangle_adjacency(V,F,TT,TTi);
  igl::edge_topology(V,F,E,F2E,E2F);

  // Generate "subdivided" mesh for visualization of curl terms
  igl::false_barycentric_subdivision(V, F, Vbs, Fbs);

  // Compute scale for visualizing fields
  global_scale =  .2*igl::avg_edge_length(V, F);

  //Compute scale for visualizing texture
  uv_scale = 0.6/igl::avg_edge_length(V, F);

  // Compute face barycenters
  igl::barycenter(V, F, B);

  // Compute local basis for faces
  igl::local_basis(V,F,B1,B2,B3);

  //Generate random vectors for constraints
  generate_constraints();

  // Interpolate a 2-PolyVector field to be used as the original field
  printf("--Initial solution--\n");
  igl::n_polyvector(V, F, b, bc, two_pv_ori);

  // Post process original field
  // Compute curl_minimizing matchings and curl
  Eigen::MatrixXi match_ab, match_ba;  // matchings across interior edges
  printf("--Matchings and curl--\n");
  double avgCurl = igl::polyvector_field_matchings(two_pv_ori, V, F, true, match_ab, match_ba, curl_ori);
  double maxCurl = curl_ori.maxCoeff();
  printf("original curl -- max: %.5g, avg: %.5g\n", maxCurl,  avgCurl);

  printf("--Singularities--\n");
  // Compute singularities
  igl::polyvector_field_singularities_from_matchings(V, F, V_border, VF, TT, E2F, F2E, match_ab, match_ba, singularities_ori);
  printf("original #singularities: %ld\n", singularities.rows());

  printf("--Cuts--\n");
 // Get mesh cuts based on singularities
  igl::polyvector_field_cut_mesh_with_singularities(V, F, VF, VV, TT, TTi, singularities_ori, cuts_ori);

  printf("--Combing--\n");
// Comb field
  Eigen::MatrixXd combed;
  igl::polyvector_field_comb_from_matchings_and_cuts(V, F, TT, E2F, F2E, two_pv_ori, match_ab, match_ba, cuts_ori, combed);

  printf("--Cut mesh--\n");
  // Reconstruct integrable vector fields from combed field
  igl::cut_mesh(V, F, VF, VFi, TT, TTi, V_border, cuts_ori, Vcut_ori, Fcut_ori);

  printf("--Poisson--\n");
  double avgPoisson = igl::polyvector_field_poisson_reconstruction(Vcut_ori, Fcut_ori, combed, scalars_ori, two_pv_poisson_ori, poisson_error_ori);
  double maxPoisson = poisson_error_ori.maxCoeff();
  printf("poisson error -- max: %.5g, avg: %.5g\n", maxPoisson, avgPoisson);


  // Set the curl-free 2-PolyVector to equal the original field
  two_pv = two_pv_ori;
  singularities = singularities_ori;
  curl = curl_ori;
  cuts = cuts_ori;
  two_pv_poisson = two_pv_poisson_ori;
  poisson_error = poisson_error_ori;
  Vcut = Vcut_ori;
  Fcut = Fcut_ori;
  scalars = scalars_ori;

  printf("--Integrable - Precomputation--\n");
  // Precompute stuff for solver
  igl::integrable_polyvector_fields_precompute(V, F, b, bc, blevel, two_pv_ori, ipfdata);

  cerr<<"Done. Press keys 1-0 for various visualizations, 'A' to improve integrability." <<endl;

  igl::viewer::Viewer viewer;
  viewer.callback_key_down = &key_down;
  viewer.core.show_lines = false;
  key_down(viewer,'2',0);

  // Replace the standard texture with an integer shift invariant texture
  line_texture(texture_R, texture_G, texture_B);

  viewer.launch();

  return 0;
}

void update_display(igl::viewer::Viewer& viewer)
{
  using namespace std;
  using namespace Eigen;

  viewer.data.clear();
  viewer.data.lines.resize(0,9);
  viewer.data.points.resize(0,6);
  viewer.core.show_texture = false;

  if (display_mode == 1)
  {
    cerr<< "Displaying original field, its singularities and its cuts"  <<endl;

    viewer.data.set_mesh(V, F);

    // Highlight in red the constrained faces
    MatrixXd C = MatrixXd::Constant(F.rows(),3,1);
    for (unsigned i=0; i<b.size();++i)
      C.row(b(i)) << 1, 0, 0;
    viewer.data.set_colors(C);

    //Draw constraints
    drawConstraints(viewer);

    // Draw Field
    Eigen::RowVector3d color; color<<0,0,1;
    drawField(viewer,two_pv_ori,color);

    // Draw Cuts
    drawCuts(viewer,cuts_ori);

    //Draw Singularities
    Eigen::MatrixXd singular_points = igl::slice(V, singularities_ori, 1);
    viewer.data.add_points(singular_points,Eigen::RowVector3d(239./255.,205./255.,57./255.));

  }

  if (display_mode == 2)
  {
    cerr<< "Displaying current field, its singularities and its cuts"  <<endl;

    viewer.data.set_mesh(V, F);

    // Highlight in red the constrained faces
    MatrixXd C = MatrixXd::Constant(F.rows(),3,1);
    for (unsigned i=0; i<b.size();++i)
      C.row(b(i)) << 1, 0, 0;
    viewer.data.set_colors(C);

    //Draw constraints
    drawConstraints(viewer);

    // Draw Field
    Eigen::RowVector3d color; color<<0,0,1;
    drawField(viewer,two_pv,color);

    // Draw Cuts
    drawCuts(viewer,cuts);

    //Draw Singularities
    Eigen::MatrixXd singular_points = igl::slice(V, singularities, 1);
    viewer.data.add_points(singular_points,Eigen::RowVector3d(239./255.,205./255.,57./255.));
  }

  if (display_mode == 3)
  {
    cerr<< "Displaying original field and its curl"  <<endl;

    viewer.data.set_mesh(Vbs, Fbs);
    Eigen::MatrixXd C;
    colorEdgeMeshFaces(curl_ori, 0, 0.2, C);
    viewer.data.set_colors(C);

    // Draw Field
    Eigen::RowVector3d color; color<<1,1,1;
    drawField(viewer,two_pv_ori,color);

  }

  if (display_mode == 4)
  {
    cerr<< "Displaying current field and its curl"  <<endl;

    viewer.data.set_mesh(Vbs, Fbs);
    Eigen::MatrixXd C;
    colorEdgeMeshFaces(curl, 0, 0.2, C);
    viewer.data.set_colors(C);

    // Draw Field
    Eigen::RowVector3d color; color<<1,1,1;
    drawField(viewer,two_pv,color);
  }

  if (display_mode == 5)
  {
    cerr<< "Displaying original poisson-integrated field and original poisson error"  <<endl;

    viewer.data.set_mesh(V, F);
    Eigen::MatrixXd C;
//.........这里部分代码省略.........