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);
//.........这里部分代码省略.........

void calculateOverlappingFOV(const cameras::CameraGeometryBase & camera1,
                             const cameras::CameraGeometryBase & camera2,
                             const sm::kinematics::Transformation& T_cam1_cam2,
                             cameras::ImageMask& outMask1,
                             cameras::ImageMask& outMask2,
                             double& outOverlappingRatio1,
                             double& outOverlappingRatio2,
                             const Eigen::VectorXi& sampleDistances,
                             double scale) {
  // get dimensions
  int width1 = (int) camera1.width() * scale;
  int height1 = (int) camera1.height() * scale;
  int width2 = (int) camera2.width() * scale;
  int height2 = (int) camera2.height() * scale;

  // clear incoming masks
  cv::Mat outMask1CV = outMask1.getMask();
  cv::Mat outMask2CV = outMask2.getMask();
  outMask1CV = cv::Mat::zeros(height1, width1, CV_8UC1);
  outMask2CV = cv::Mat::zeros(height2, width2, CV_8UC1);
  outMask1.setScale(scale);
  outMask2.setScale(scale);

  Eigen::VectorXd point;

  // build outMask1
  outOverlappingRatio1 = 0;
  for (int x = 0; x < width1; x++) {
    for (int y = 0; y < height1; y++) {
      // if visible
      // TODO: BB: what about rounding mistakes?
      Eigen::VectorXd kp(2);
      kp << (double) x / scale, (double) y / scale;
      if (camera1.vsKeypointToHomogeneous(kp, point)) {
        double norm = point.norm();
        int i = 0;
        // check points on the beam until one is found or no other points to check
        while (i < sampleDistances.size()
            && outMask1CV.at<unsigned char>(y, x) == 0) {
          // Send point to distance sampleDistances(i) on the beam
          Eigen::Vector4d tempPoint = point;
          tempPoint(3) = norm / (norm + sampleDistances(i));

          // Project point to other camera frame
          tempPoint = T_cam1_cam2.inverse() * tempPoint;

          Eigen::VectorXd keypointLocation;
          if (camera2.vsHomogeneousToKeypoint(tempPoint, keypointLocation)) {
            outMask1CV.at<unsigned char>(y, x) = 255;
            outOverlappingRatio1++;
          }  // if
          i++;
        }  // while
      }  // if
    }  // for
  }  // for
  outOverlappingRatio1 = outOverlappingRatio1 / (width1 * height1);

  // build outMask2
  outOverlappingRatio2 = 0;
  for (int x = 0; x < width2; x++) {
    for (int y = 0; y < height2; y++) {
      Eigen::VectorXd kp(2);
      kp << (double) x / scale, (double) y / scale;

      // if visible
      // TODO: BB: what about rounding mistakes?
      if (camera2.vsKeypointToHomogeneous(kp, point)) {

        double norm = point.norm();
        int i = 0;
        // check points on the beam until one is found or no other points to check
        while (i < sampleDistances.size()
            && outMask2CV.at<unsigned char>(y, x) == 0) {
          // Send point to distance sampleDistances(i) on the beam
          Eigen::Vector4d tempPoint = point;
          tempPoint(3) = norm / (norm + sampleDistances(i));

          // Project point to other camera frame
          tempPoint = T_cam1_cam2 * tempPoint;

          Eigen::VectorXd keypointLocation;
          if (camera1.vsHomogeneousToKeypoint(tempPoint, keypointLocation)) {
            outMask2CV.at<unsigned char>(y, x) = 255;
            outOverlappingRatio2++;
          }  // if
          i++;
        }  // while
      }  // if
    }  // for
  }  // for
  outOverlappingRatio2 = outOverlappingRatio2 / (width2 * height2);
}

int main(int argc, char *argv[])
{
  using namespace Eigen;

  // Load a mesh in OBJ format
  igl::readOBJ(TUTORIAL_SHARED_PATH "/bumpy-cube.obj", V, F);

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

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

  // Load constraints
  MatrixXd temp;
  igl::readDMAT(TUTORIAL_SHARED_PATH "/bumpy-cube.dmat",temp);

  b   = temp.block(0,0,temp.rows(),1).cast<int>();
  bc1 = temp.block(0,1,temp.rows(),3);
  bc2 = temp.block(0,4,temp.rows(),3);

  // Interpolate the frame field
  igl::comiso::frame_field(V, F, b, bc1, bc2, FF1, FF2);

  // Deform the mesh to transform the frame field in a cross field
  igl::frame_field_deformer(
    V,F,FF1,FF2,V_deformed,FF1_deformed,FF2_deformed);

  // Compute face barycenters deformed mesh
  igl::barycenter(V_deformed, F, B_deformed);

  // Find the closest crossfield to the deformed frame field
  igl::frame_to_cross_field(V_deformed,F,FF1_deformed,FF2_deformed,X1_deformed);

  // Find a smooth crossfield that interpolates the deformed constraints
  MatrixXd bc_x(b.size(),3);
  for (unsigned i=0; i<b.size();++i)
    bc_x.row(i) = X1_deformed.row(b(i));

  VectorXd S;
  igl::comiso::nrosy(
             V,
             F,
             b,
             bc_x,
             VectorXi(),
             VectorXd(),
             MatrixXd(),
             4,
             0.5,
             X1_deformed,
             S);

  // The other representative of the cross field is simply rotated by 90 degrees
  MatrixXd B1,B2,B3;
  igl::local_basis(V_deformed,F,B1,B2,B3);
  X2_deformed =
    igl::rotate_vectors(X1_deformed, VectorXd::Constant(1,M_PI/2), B1, B2);

  // Global seamless parametrization
  igl::comiso::miq(V_deformed,
           F,
           X1_deformed,
           X2_deformed,
           V_uv,
           F_uv,
           60.0,
           5.0,
           false,
           2);

  igl::viewer::Viewer viewer;
  // Plot the original mesh with a texture parametrization
  key_down(viewer,'6',0);

  // Launch the viewer
  viewer.callback_key_down = &key_down;
  viewer.launch();
}

bool init_arap()
{
  using namespace igl;
  using namespace Eigen;
  using namespace std;
  VectorXi b(num_in_selection(S));
  assert(S.rows() == V.rows());
  C.resize(S.rows(),3);
  MatrixXd bc = MatrixXd::Zero(b.size(),S.maxCoeff()+1);
  MatrixXi * Ele;
  if(T.rows()>0)
  {
    Ele = &T;
  }else
  {
    Ele = &F;
  }
  // get b from S
  {
    int bi = 0;
    for(int v = 0;v<S.rows(); v++)
    {
      if(S(v) >= 0)
      {
        b(bi) = v;
        bc(bi,S(v)) = 1;
        bi++;
        switch(S(v))
        {
          case 0:
            C.row(v) = RowVector3d(0.039,0.31,1);
            break;
          case 1:
            C.row(v) = RowVector3d(1,0.41,0.70);
            break;
          default:
            C.row(v) = RowVector3d(0.4,0.8,0.3);
            break;
        }
      }else
      {
        C.row(v) = RowVector3d(
          GOLD_DIFFUSE[0],
          GOLD_DIFFUSE[1],
          GOLD_DIFFUSE[2]);
      }
    }
  }
  // Store current mesh
  U = V;
  VectorXi _S;
  VectorXd _D;
  MatrixXd W;
  if(!harmonic(V,*Ele,b,bc,1,W))
  {
    return false;
  }
  arap_data.with_dynamics = true;
  arap_data.h = 0.05;
  //arap_data.max_iter = 100;
  //partition(W,100,arap_data.G,_S,_D);
  return arap_precomputation(V,*Ele,V.cols(),b,arap_data);
}

IGL_INLINE void igl::slice(
  const Eigen::SparseMatrix<TX>& X,
  const Eigen::Matrix<int,Eigen::Dynamic,1> & R,
  const Eigen::Matrix<int,Eigen::Dynamic,1> & C,
  Eigen::SparseMatrix<TY>& Y)
{
#if 1
  int xm = X.rows();
  int xn = X.cols();
  int ym = R.size();
  int yn = C.size();

  // special case when R or C is empty
  if(ym == 0 || yn == 0)
  {
    Y.resize(ym,yn);
    return;
  }

  assert(R.minCoeff() >= 0);
  assert(R.maxCoeff() < xm);
  assert(C.minCoeff() >= 0);
  assert(C.maxCoeff() < xn);

  // Build reindexing maps for columns and rows, -1 means not in map
  std::vector<std::vector<int> > RI;
  RI.resize(xm);
  for(int i = 0;i<ym;i++)
  {
    RI[R(i)].push_back(i);
  }
  std::vector<std::vector<int> > CI;
  CI.resize(xn);
  // initialize to -1
  for(int i = 0;i<yn;i++)
  {
    CI[C(i)].push_back(i);
  }
  // Resize output
  Eigen::DynamicSparseMatrix<TY, Eigen::RowMajor> dyn_Y(ym,yn);
  // Take a guess at the number of nonzeros (this assumes uniform distribution
  // not banded or heavily diagonal)
  dyn_Y.reserve((X.nonZeros()/(X.rows()*X.cols())) * (ym*yn));
  // Iterate over outside
  for(int k=0; k<X.outerSize(); ++k)
  {
    // Iterate over inside
    for(typename Eigen::SparseMatrix<TX>::InnerIterator it (X,k); it; ++it)
    {
      std::vector<int>::iterator rit, cit;
      for(rit = RI[it.row()].begin();rit != RI[it.row()].end(); rit++)
      {
        for(cit = CI[it.col()].begin();cit != CI[it.col()].end(); cit++)
        {
          dyn_Y.coeffRef(*rit,*cit) = it.value();
        }
      }
    }
  }
  Y = Eigen::SparseMatrix<TY>(dyn_Y);
#else

  // Alec: This is _not_ valid for arbitrary R,C since they don't necessary
  // representation a strict permutation of the rows and columns: rows or
  // columns could be removed or replicated. The removal of rows seems to be
  // handled here (although it's not clear if there is a performance gain when
  // the #removals >> #remains). If this is sufficiently faster than the
  // correct code above, one could test whether all entries in R and C are
  // unique and apply the permutation version if appropriate.
  //

  int xm = X.rows();
  int xn = X.cols();
  int ym = R.size();
  int yn = C.size();

  // special case when R or C is empty
  if(ym == 0 || yn == 0)
  {
    Y.resize(ym,yn);
    return;
  }

  assert(R.minCoeff() >= 0);
  assert(R.maxCoeff() < xm);
  assert(C.minCoeff() >= 0);
  assert(C.maxCoeff() < xn);

  // initialize row and col permutation vectors
  Eigen::VectorXi rowIndexVec = Eigen::VectorXi::LinSpaced(xm,0,xm-1);
  Eigen::VectorXi rowPermVec  = Eigen::VectorXi::LinSpaced(xm,0,xm-1);
  for(int i=0;i<ym;i++)
  {
    int pos = rowIndexVec.coeffRef(R(i));
    if(pos != i)
    {
      int& val = rowPermVec.coeffRef(i);
      std::swap(rowIndexVec.coeffRef(val),rowIndexVec.coeffRef(R(i)));
      std::swap(rowPermVec.coeffRef(i),rowPermVec.coeffRef(pos));
    }
//.........这里部分代码省略.........