aslam::backend::EuclideanExpression EuclideanBSplineDesignVariable::toEuclideanExpression(double time, int order)
 {
     Eigen::VectorXi dvidxs = _bspline.localVvCoefficientVectorIndices(time);
     std::vector<aslam::backend::DesignVariable *> dvs;
     for(int i = 0; i < dvidxs.size(); ++i)
     {
         dvs.push_back(&_designVariables[dvidxs[i]]);
     }
     boost::shared_ptr<aslam::splines::BSplineEuclideanExpressionNode > root( new aslam::splines::BSplineEuclideanExpressionNode(&_bspline, dvs, time, order) );
     
     return aslam::backend::EuclideanExpression(root);
 }
void ExecControl::reachability()
{
	rgraph.clear();
	Eigen::VectorXi trans = marking.transpose()*Dm;

	std::cout<<"Transition matrix:"<<trans<<std::endl;

	int last_m = boost::add_vertex(rgraph);
	rgraph[last_m].marking = marking;

	Eigen::VectorXi fire = Eigen::VectorXi::Zero(trans.size());
	int tnum = -1;
	for (int i=0; i<trans.size(); ++i)
	{
		if (trans(i)>0)
		{
			std::cout<<"Transition "<<i<<" can fire."<<std::endl;
			fire(i) = 1;
			tnum = i;
			break;
		}
	}

	//marking = marking + (Dp-Dm)*fire;

	int new_m = boost::add_vertex(rgraph);
	rgraph[new_m].marking = marking;
	REdgeProperty prop;
	prop.t = tnum;
	prop.weight = 1;
	//Enable edge
	boost::add_edge(last_m, new_m, prop, rgraph);
	prop.t+=1;
	//Disable edge added so we can skip it in next iteration.
	boost::add_edge(new_m, last_m, prop, rgraph);


	std::cout<<"Transition fired, new marking:"<<marking<<std::endl;
}
Exemple #3
0
IGL_INLINE bool igl::copyleft::boolean::mesh_boolean(
  const Eigen::PlainObjectBase<DerivedVA > & VA,
  const Eigen::PlainObjectBase<DerivedFA > & FA,
  const Eigen::PlainObjectBase<DerivedVB > & VB,
  const Eigen::PlainObjectBase<DerivedFB > & FB,
  const MeshBooleanType & type,
  Eigen::PlainObjectBase<DerivedVC > & VC,
  Eigen::PlainObjectBase<DerivedFC > & FC,
  Eigen::PlainObjectBase<DerivedJ > & J)
{
  typedef CGAL::Epeck Kernel;
  typedef Kernel::FT ExactScalar;
  typedef Eigen::Matrix<
    ExactScalar,
    Eigen::Dynamic,
    Eigen::Dynamic,
    DerivedVC::IsRowMajor> MatrixXES;

  std::function<void(
      const Eigen::PlainObjectBase<DerivedVA>&,
      const Eigen::PlainObjectBase<DerivedFA>&,
      Eigen::PlainObjectBase<MatrixXES>&,
      Eigen::PlainObjectBase<DerivedFC>&,
      Eigen::PlainObjectBase<DerivedJ>&)> resolve_func =
    [](const Eigen::PlainObjectBase<DerivedVA>& V,
        const Eigen::PlainObjectBase<DerivedFA>& F,
        Eigen::PlainObjectBase<MatrixXES>& Vo,
        Eigen::PlainObjectBase<DerivedFC>& Fo,
        Eigen::PlainObjectBase<DerivedJ>& J) {
      Eigen::VectorXi I;
      igl::copyleft::cgal::RemeshSelfIntersectionsParam params;

      MatrixXES Vr;
      DerivedFC Fr;
      Eigen::MatrixXi IF;
      igl::copyleft::cgal::remesh_self_intersections(
          V, F, params, Vr, Fr, IF, J, I);
      assert(I.size() == Vr.rows());

      // Merge coinciding vertices into non-manifold vertices.
      std::for_each(Fr.data(), Fr.data()+Fr.size(),
          [&I](typename DerivedFC::Scalar& a) { a=I[a]; });

      // Remove unreferenced vertices.
      Eigen::VectorXi UIM;
      igl::remove_unreferenced(Vr, Fr, Vo, Fo, UIM);
    };

  return mesh_boolean(VA,FA,VB,FB,type,resolve_func,VC,FC,J);
}
int ComputationGraph::matrix(Eigen::VectorXi vector) {
    std::vector<int> output;
    for (int i = 0; i < vector.size(); i++) {
        output.push_back(vector(i));
    }

    nodes.push_back({
        -1,
        {},
        output
    });

    return nodes.size() - 1;
}
Exemple #5
0
IGL_INLINE void igl::n_polyvector(const Eigen::MatrixXd &V,
                             const Eigen::MatrixXi &F,
                             const Eigen::VectorXi& b,
                             const Eigen::MatrixXd& bc,
                             Eigen::MatrixXd &output)
{
  Eigen::VectorXi isConstrained = Eigen::VectorXi::Constant(F.rows(),0);
  Eigen::MatrixXd cfW = Eigen::MatrixXd::Constant(F.rows(),bc.cols(),0);

  for(unsigned i=0; i<b.size();++i)
  {
    isConstrained(b(i)) = 1;
    cfW.row(b(i)) << bc.row(i);
  }
  if (b.size() == F.rows())
  {
    output = cfW;
    return;
  }

  int n = cfW.cols()/3;
  igl::PolyVectorFieldFinder<Eigen::MatrixXd, Eigen::MatrixXi> pvff(V,F,n);
  pvff.solve(isConstrained, cfW, output);
}
Exemple #6
0
void cDrawKinTree::DrawTree(const Eigen::MatrixXd& joint_desc, const Eigen::VectorXd& pose, int joint_id, double link_width,
						const tVector& fill_col, const tVector& line_col)
{
	const double node_radius = 0.02;

	if (joint_id != cKinTree::gInvalidJointID)
	{
		bool has_parent = cKinTree::HasParent(joint_desc, joint_id);
		if (has_parent)
		{
			tVector attach_pt = cKinTree::GetScaledAttachPt(joint_desc, joint_id);
			attach_pt[2] = 0; // hack ignore z
			double len = attach_pt.norm();

			glPushMatrix();
			tVector pos = tVector(len / 2, 0, 0, 0);
			tVector size = tVector(len, link_width, 0, 0);

			double theta = std::atan2(attach_pt[1], attach_pt[0]);
			cDrawUtil::Rotate(theta, tVector(0, 0, 1, 0));

			cDrawUtil::SetColor(tVector(fill_col[0], fill_col[1], fill_col[2], fill_col[3]));
			cDrawUtil::DrawRect(pos, size, cDrawUtil::eDrawSolid);
			cDrawUtil::SetColor(tVector(line_col[0], line_col[1], line_col[2], line_col[3]));
			cDrawUtil::DrawRect(pos, size, cDrawUtil::eDrawWire);
			
			// draw node
			cDrawUtil::SetColor(tVector(fill_col[0] * 0.25, fill_col[1] * 0.25, fill_col[2] * 0.25, fill_col[3]));
			cDrawUtil::DrawDisk(node_radius, 16);
			
			glPopMatrix();
		}

		glPushMatrix();
		tMatrix m = cKinTree::ChildParentTrans(joint_desc, pose, joint_id);
		cDrawUtil::GLMultMatrix(m);

		Eigen::VectorXi children;
		cKinTree::FindChildren(joint_desc, joint_id, children);
		for (int i = 0; i < children.size(); ++i)
		{
			int child_id = children[i];
			DrawTree(joint_desc, pose, child_id, link_width, fill_col, line_col);
		}

		glPopMatrix();
	}
}
Exemple #7
0
int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;
  igl::readOFF("../shared/decimated-knight.off",V,F);
  U=V;
  igl::readDMAT("../shared/decimated-knight-selection.dmat",S);

  // vertices in selection
  igl::colon<int>(0,V.rows()-1,b);
  b.conservativeResize(stable_partition( b.data(), b.data()+b.size(), 
   [](int i)->bool{return S(i)>=0;})-b.data());
  // Centroid
  mid = 0.5*(V.colwise().maxCoeff() + V.colwise().minCoeff());
  // Precomputation
  arap_data.max_iter = 100;
  igl::arap_precomputation(V,F,V.cols(),b,arap_data);

  // Set 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::viewer::Viewer viewer;
  viewer.data.set_mesh(U, F);
  viewer.data.set_colors(C);
  viewer.callback_pre_draw = &pre_draw;
  viewer.callback_key_down = &key_down;
  viewer.core.is_animating = false;
  viewer.core.animation_max_fps = 30.;
  cout<<
    "Press [space] to toggle animation"<<endl;
  viewer.launch();
}
Exemple #8
0
// Plots the mesh with an N-RoSy field and its singularities on top
// The constrained faces (b) are colored in red.
void plot_mesh_nrosy(
    igl::viewer::Viewer& viewer,
    Eigen::MatrixXd& V,
    Eigen::MatrixXi& F,
    int N,
    Eigen::MatrixXd& PD1,
    Eigen::VectorXd& S,
    Eigen::VectorXi& b)
{
    using namespace Eigen;
    using namespace std;
    // Clear the mesh
    viewer.data.clear();
    viewer.data.set_mesh(V,F);

    // Expand the representative vectors in the full vector set and plot them as lines
    double avg = igl::avg_edge_length(V, F);
    MatrixXd Y;
    representative_to_nrosy(V, F, PD1, N, Y);

    MatrixXd B;
    igl::barycenter(V,F,B);

    MatrixXd Be(B.rows()*N,3);
    for(unsigned i=0; i<B.rows(); ++i)
        for(unsigned j=0; j<N; ++j)
            Be.row(i*N+j) = B.row(i);

    viewer.data.add_edges(Be,Be+Y*(avg/2),RowVector3d(0,0,1));

    // Plot the singularities as colored dots (red for negative, blue for positive)
    for (unsigned i=0; i<S.size(); ++i)
    {
        if (S(i) < -0.001)
            viewer.data.add_points(V.row(i),RowVector3d(1,0,0));
        else if (S(i) > 0.001)
            viewer.data.add_points(V.row(i),RowVector3d(0,1,0));
    }

    // 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);
}
IGL_INLINE void igl::polyvector_field_singularities_from_matchings(
                                                                   const Eigen::PlainObjectBase<DerivedV> &V,
                                                                   const Eigen::PlainObjectBase<DerivedF> &F,
                                                                   const std::vector<bool> &V_border,
                                                                   const std::vector<std::vector<VFType> > &VF,
                                                                   const Eigen::MatrixXi &TT,
                                                                   const Eigen::MatrixXi &E2F,
                                                                   const Eigen::MatrixXi &F2E,
                                                                   const Eigen::PlainObjectBase<DerivedM> &match_ab,
                                                                   const Eigen::PlainObjectBase<DerivedM> &match_ba,
                                                                   Eigen::PlainObjectBase<DerivedS> &singularities)
{

  int numV = V.rows();

  std::vector<int> singularities_v;
  int half_degree = match_ab.cols();
  for (int vi =0; vi<numV; ++vi)
  {
    ///check that is on border..
    if (V_border[vi])
      continue;
    Eigen::VectorXi fi;
    Eigen::MatrixXi mvi;
    igl::polyvector_field_one_ring_matchings(V, F, VF, E2F, F2E, TT, match_ab, match_ba, vi, mvi, fi);

    int num = fi.size();
    //pick one of the vectors to check for singularities
    for (int vector_to_match = 0; vector_to_match < half_degree; ++vector_to_match)
    {
      if(mvi(num-1,vector_to_match) != mvi(0,vector_to_match))
      {
        singularities_v.push_back(vi);
        break;
      }
    }
  }
  std::sort(singularities_v.begin(), singularities_v.end());
  auto last = std::unique(singularities_v.begin(), singularities_v.end());
  singularities_v.erase(last, singularities_v.end());

  igl::list_to_matrix(singularities_v, singularities);
}
void place::removeMinimumConnectedComponents(cv::Mat &image) {
  std::list<std::pair<int, int>> toLabel;
  Eigen::RowMatrixXi labeledImage =
      Eigen::RowMatrixXi::Zero(image.rows, image.cols);
  int currentLabel = 1;
  for (int j = 0; j < image.rows; ++j) {
    const uchar *src = image.ptr<uchar>(j);
    for (int i = 0; i < image.cols; ++i) {
      if (src[i] != 255 && labeledImage(j, i) == 0) {
        labeledImage(j, i) = currentLabel;
        toLabel.emplace_front(j, i);

        labelNeighbours(image, currentLabel, labeledImage, toLabel);

        ++currentLabel;
      }
    }
  }

  Eigen::VectorXi countPerLabel = Eigen::VectorXi::Zero(currentLabel);
  const int *labeledImagePtr = labeledImage.data();
  for (int i = 0; i < labeledImage.size(); ++i)
    ++countPerLabel[*(labeledImagePtr + i)];

  double average = 0.0, sigma = 0.0;
  const int *countPerLabelPtr = countPerLabel.data();
  std::tie(average, sigma) = place::aveAndStdev(
      countPerLabelPtr + 1, countPerLabelPtr + countPerLabel.size());

  double threshHold = average;
  for (int j = 0; j < image.rows; ++j) {
    uchar *src = image.ptr<uchar>(j);
    for (int i = 0; i < image.cols; ++i) {
      if (src[i] != 255) {
        const int label = labeledImage(j, i);
        const int count = countPerLabel[label];
        if (count < threshHold)
          src[i] = 255;
      }
    }
  }
}
void addMotionErrorTerms(OptimizationProblemBase& problem, SplineDv& splineDv, const Eigen::MatrixXd& W, unsigned int errorTermOrder) {
  // Add one error term for each segment
  auto spline = splineDv.spline();

  for(int i = 0; i < spline.numValidTimeSegments(); ++i) {
    Eigen::MatrixXd R = spline.segmentIntegral(i, W, errorTermOrder);
    Eigen::VectorXd c = spline.segmentCoefficientVector(i);
    // These indices tell us the order of R and c.
    Eigen::VectorXi idxs = spline.segmentVvCoefficientVectorIndices(i);

    std::vector< DesignVariable * > dvs;
    for(unsigned i = 0; i < idxs.size(); ++i) {
      dvs.push_back(splineDv.designVariable(idxs[i]));
    }
    MarginalizationPriorErrorTerm::Ptr err( 
        new MarginalizationPriorErrorTerm( dvs, R*c, R ));
    problem.addErrorTerm(err);
            
  }
}
Exemple #12
0
/**
 * @function getLeftArmIds
 * @brief Get DOF's IDs for ROBINA's left arm
 */
Eigen::VectorXi NSTab::GetLeftArmIds() {

  mEENode = mWorld->mRobots[mRobotId]->getNode( mEEName.c_str() );
  mEEId = mEENode->getSkelIndex();
  
  string LINK_NAMES[7] = {"LJ0", "LJ1", "LJ2", "LJ3", "LJ4", "LJ5", "LJ6" };
  
  Eigen::VectorXi linksAll = mWorld->mRobots[mRobotId]->getQuickDofsIndices(); 

  Eigen::VectorXi linksLeftArm(7);
  for( unsigned int i = 0; i < 7; i++ ) {
    for( unsigned int j = 0; j < linksAll.size(); j++ ) {      
      if( mWorld->mRobots[mRobotId]->getDof( linksAll[j] )->getJoint()->getChildNode()->getName() == LINK_NAMES[i] ) {
	linksLeftArm[i] = linksAll[j]; 
	break;   
      }
    }
  }
  
  return linksLeftArm;
}
Exemple #13
0
/**
 * @function GetLinksId
 * @brief Get DOF's IDs for Manipulator
 */
Eigen::VectorXi ConfigTab::GetLinksId() {

    string LINK_NAMES[sNumLinks];
    if( gRobotName.compare("Robina") == 0 ) {
        LINK_NAMES[0] = "LJ0";
        LINK_NAMES[1] = "LJ1";
        LINK_NAMES[2] = "LJ2";
        LINK_NAMES[3] ="LJ3";
        LINK_NAMES[4] = "LJ4";
        LINK_NAMES[5] = "LJ5";
        LINK_NAMES[6] = "LJ6";
    }
    else if( gRobotName.compare("LWA3") == 0 ) {
        LINK_NAMES[0] = "L1";
        LINK_NAMES[1] = "L2";
        LINK_NAMES[2] = "L3";
        LINK_NAMES[3] = "L4";
        LINK_NAMES[4] = "L5";
        LINK_NAMES[5] = "L6";
        LINK_NAMES[6] = "FT";
    }

    Eigen::VectorXi linksAll = mWorld->getRobot(gRobotId)->getQuickDofsIndices();

    Eigen::VectorXi links( sNumLinks );
    for( unsigned int i = 0; i < sNumLinks; i++ ) {
        for( unsigned int j = 0; j < linksAll.size(); j++ ) {
            if( mWorld->getRobot(gRobotId)->getDof( linksAll[j] )->getJoint()->getChildNode()->getName() == LINK_NAMES[i] ) {
                links[i] = linksAll[j];
                break;
            }
        }
    }

    return links;
}
IGL_INLINE void igl::map_vertices_to_circle(
  const Eigen::MatrixXd& V,
  const Eigen::VectorXi& bnd,
  Eigen::MatrixXd& UV)
{
  // Get sorted list of boundary vertices
  std::vector<int> interior,map_ij;
  map_ij.resize(V.rows());

  std::vector<bool> isOnBnd(V.rows(),false);
  for (int i = 0; i < bnd.size(); i++)
  {
    isOnBnd[bnd[i]] = true;
    map_ij[bnd[i]] = i;
  }

  for (int i = 0; i < (int)isOnBnd.size(); i++)
  {
    if (!isOnBnd[i])
    {
      map_ij[i] = interior.size();
      interior.push_back(i);
    }
  }

  // Map boundary to unit circle
  std::vector<double> len(bnd.size());
  len[0] = 0.;

  for (int i = 1; i < bnd.size(); i++)
  {
    len[i] = len[i-1] + (V.row(bnd[i-1]) - V.row(bnd[i])).norm();
  }
  double total_len = len[len.size()-1] + (V.row(bnd[0]) - V.row(bnd[bnd.size()-1])).norm();

  UV.resize(bnd.size(),2);
  for (int i = 0; i < bnd.size(); i++)
  {
    double frac = len[i] * 2. * M_PI / total_len;
    UV.row(map_ij[bnd[i]]) << cos(frac), sin(frac);
  }

}
Exemple #15
0
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();
}
Exemple #16
0
bool pre_draw(igl::Viewer & viewer)
{
  using namespace Eigen;
  using namespace std;
  if(resolve)
  {
    MatrixXd bc(b.size(),V.cols());
    VectorXd Beq(3*b.size());
    for(int i = 0;i<b.size();i++)
    {
      bc.row(i) = V.row(b(i));
      switch(i%4)
      {
        case 2:
          bc(i,0) += 0.15*bbd*sin(0.5*anim_t);
          bc(i,1) += 0.15*bbd*(1.-cos(0.5*anim_t));
          break;
        case 1:
          bc(i,1) += 0.10*bbd*sin(1.*anim_t*(i+1));
          bc(i,2) += 0.10*bbd*(1.-cos(1.*anim_t*(i+1)));
          break;
        case 0:
          bc(i,0) += 0.20*bbd*sin(2.*anim_t*(i+1));
          break;
      }
      Beq(3*i+0) = bc(i,0);
      Beq(3*i+1) = bc(i,1);
      Beq(3*i+2) = bc(i,2);
    }
    switch(mode)
    {
      case MODE_TYPE_ARAP:
        igl::arap_solve(bc,arap_data,U);
        break;
      case MODE_TYPE_ARAP_GROUPED:
        igl::arap_solve(bc,arap_grouped_data,U);
        break;
      case MODE_TYPE_ARAP_DOF:
      {
        VectorXd L0 = L;
        arap_dof_update(arap_dof_data,Beq,L0,30,0,L);
        const auto & Ucol = M*L;
        U.col(0) = Ucol.block(0*U.rows(),0,U.rows(),1);
        U.col(1) = Ucol.block(1*U.rows(),0,U.rows(),1);
        U.col(2) = Ucol.block(2*U.rows(),0,U.rows(),1);
        break;
      }
    }
    viewer.data.set_vertices(U);
    viewer.data.set_points(bc,sea_green);
    viewer.data.compute_normals();
    if(viewer.core.is_animating)
    {
      anim_t += anim_t_dir;
    }else
    {
      resolve = false;
    }
  }
  return false;
}
Exemple #17
0
IGL_INLINE void igl::sort_new(
  const Eigen::PlainObjectBase<DerivedX>& X,
  const int dim,
  const bool ascending,
  Eigen::PlainObjectBase<DerivedY>& Y,
  Eigen::PlainObjectBase<DerivedIX>& IX)
{
  // get number of rows (or columns)
  int num_inner = (dim == 1 ? X.rows() : X.cols() );
  // Special case for swapping
  if(num_inner == 2)
  {
    return igl::sort2(X,dim,ascending,Y,IX);
  }
  using namespace Eigen;
  // get number of columns (or rows)
  int num_outer = (dim == 1 ? X.cols() : X.rows() );
  // dim must be 2 or 1
  assert(dim == 1 || dim == 2);
  // Resize output
  Y.resize(X.rows(),X.cols());
  IX.resize(X.rows(),X.cols());
  // idea is to process each column (or row) as a std vector
  // loop over columns (or rows)
  for(int i = 0; i<num_outer;i++)
  {
    Eigen::VectorXi ix;
    colon(0,num_inner-1,ix);
    // Sort the index map, using unsorted for comparison
    if(dim == 1)
    {
      std::sort(
        ix.data(),
        ix.data()+ix.size(),
        igl::IndexVectorLessThan<const typename DerivedX::ConstColXpr >(X.col(i)));
    }else
    {
      std::sort(
        ix.data(),
        ix.data()+ix.size(),
        igl::IndexVectorLessThan<const typename DerivedX::ConstRowXpr >(X.row(i)));
    }
    // if not ascending then reverse
    if(!ascending)
    {
      std::reverse(ix.data(),ix.data()+ix.size());
    }
    for(int j = 0;j<num_inner;j++)
    {
      if(dim == 1)
      {
        Y(j,i) = X(ix[j],i);
        IX(j,i) = ix[j];
      }else
      {
        Y(i,j) = X(i,ix[j]);
        IX(i,j) = ix[j];
      }
    }
  }
}
Exemple #18
0
#include <test_common.h>
#include <igl/unique_rows.h>
#include <igl/matrix_to_list.h>

TEST_CASE("unique: matrix", "[igl]")
{
  Eigen::VectorXi A(12);
  A = (Eigen::VectorXd::Random(A.size(),1).array().abs()*9).cast<int>();
  Eigen::VectorXi C,IA,IC;
  igl::unique_rows(A,C,IA,IC);
  std::vector<bool> inA(A.maxCoeff()+1,false);
  for(int i = 0;i<A.size();i++)
  {
    inA[A(i)] = true;
    REQUIRE (C(IC(i)) == A(i));
  }
  std::vector<bool> inC(inA.size(),false);
  // Expect a column vector
  REQUIRE (C.cols() == 1);
  for(int i = 0;i<C.size();i++)
  {
    // Should be the first time finding this
    REQUIRE (!inC[C(i)]);
    // Mark as found
    inC[C(i)] = true;
    // Should be something also found in A
    REQUIRE (inA[C(i)]);
    REQUIRE (A(IA(i)) == C(i));
  }
  for(int i = 0;i<inC.size();i++)
  {
Exemple #19
0
IGL_INLINE void igl::copyleft::cgal::outer_hull(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedF> & F,
  Eigen::PlainObjectBase<DerivedG> & G,
  Eigen::PlainObjectBase<DerivedJ> & J,
  Eigen::PlainObjectBase<Derivedflip> & flip)
{
#ifdef IGL_OUTER_HULL_DEBUG
  std::cerr << "Extracting outer hull" << std::endl;
#endif
  using namespace Eigen;
  using namespace std;
  typedef typename DerivedF::Index Index;
  Matrix<Index,DerivedF::RowsAtCompileTime,1> C;
  typedef Matrix<typename DerivedV::Scalar,Dynamic,DerivedV::ColsAtCompileTime> MatrixXV;
  //typedef Matrix<typename DerivedF::Scalar,Dynamic,DerivedF::ColsAtCompileTime> MatrixXF;
  typedef Matrix<typename DerivedG::Scalar,Dynamic,DerivedG::ColsAtCompileTime> MatrixXG;
  typedef Matrix<typename DerivedJ::Scalar,Dynamic,DerivedJ::ColsAtCompileTime> MatrixXJ;
  const Index m = F.rows();

  // UNUSED:
  //const auto & duplicate_simplex = [&F](const int f, const int g)->bool
  //{
  //  return
  //    (F(f,0) == F(g,0) && F(f,1) == F(g,1) && F(f,2) == F(g,2)) ||
  //    (F(f,1) == F(g,0) && F(f,2) == F(g,1) && F(f,0) == F(g,2)) ||
  //    (F(f,2) == F(g,0) && F(f,0) == F(g,1) && F(f,1) == F(g,2)) ||
  //    (F(f,0) == F(g,2) && F(f,1) == F(g,1) && F(f,2) == F(g,0)) ||
  //    (F(f,1) == F(g,2) && F(f,2) == F(g,1) && F(f,0) == F(g,0)) ||
  //    (F(f,2) == F(g,2) && F(f,0) == F(g,1) && F(f,1) == F(g,0));
  //};

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"outer hull..."<<endl;
#endif

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"edge map..."<<endl;
#endif
  typedef Matrix<typename DerivedF::Scalar,Dynamic,2> MatrixX2I;
  typedef Matrix<typename DerivedF::Index,Dynamic,1> VectorXI;
  //typedef Matrix<typename DerivedV::Scalar, 3, 1> Vector3F;
  MatrixX2I E,uE;
  VectorXI EMAP;
  vector<vector<typename DerivedF::Index> > uE2E;
  unique_edge_map(F,E,uE,EMAP,uE2E);
#ifdef IGL_OUTER_HULL_DEBUG
  for (size_t ui=0; ui<uE.rows(); ui++) {
      std::cout << ui << ": " << uE2E[ui].size() << " -- (";
      for (size_t i=0; i<uE2E[ui].size(); i++) {
          std::cout << uE2E[ui][i] << ", ";
      }
      std::cout << ")" << std::endl;
  }
#endif

  std::vector<std::vector<typename DerivedF::Index> > uE2oE;
  std::vector<std::vector<bool> > uE2C;
  order_facets_around_edges(V, F, uE, uE2E, uE2oE, uE2C);
  uE2E = uE2oE;
  VectorXI diIM(3*m);
  for (auto ue : uE2E) {
      for (size_t i=0; i<ue.size(); i++) {
          auto fe = ue[i];
          diIM[fe] = i;
      }
  }

  vector<vector<vector<Index > > > TT,_1;
  triangle_triangle_adjacency(E,EMAP,uE2E,false,TT,_1);
  VectorXI counts;
#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"facet components..."<<endl;
#endif
  facet_components(TT,C,counts);
  assert(C.maxCoeff()+1 == counts.rows());
  const size_t ncc = counts.rows();
  G.resize(0,F.cols());
  J.resize(0,1);
  flip.setConstant(m,1,false);

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"reindex..."<<endl;
#endif
  // H contains list of faces on outer hull;
  vector<bool> FH(m,false);
  vector<bool> EH(3*m,false);
  vector<MatrixXG> vG(ncc);
  vector<MatrixXJ> vJ(ncc);
  vector<MatrixXJ> vIM(ncc);
  //size_t face_count = 0;
  for(size_t id = 0;id<ncc;id++)
  {
    vIM[id].resize(counts[id],1);
  }
  // current index into each IM
  vector<size_t> g(ncc,0);
  // place order of each face in its respective component
  for(Index f = 0;f<m;f++)
  {
    vIM[C(f)](g[C(f)]++) = f;
  }

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"barycenters..."<<endl;
#endif
  // assumes that "resolve" has handled any coplanar cases correctly and nearly
  // coplanar cases can be sorted based on barycenter.
  MatrixXV BC;
  barycenter(V,F,BC);

#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"loop over CCs (="<<ncc<<")..."<<endl;
#endif
  for(Index id = 0;id<(Index)ncc;id++)
  {
    auto & IM = vIM[id];
    // starting face that's guaranteed to be on the outer hull and in this
    // component
    int f;
    bool f_flip;
#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"outer facet..."<<endl;
#endif
  igl::copyleft::cgal::outer_facet(V,F,IM,f,f_flip);
#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"outer facet: "<<f<<endl;
  //cout << V.row(F(f, 0)) << std::endl;
  //cout << V.row(F(f, 1)) << std::endl;
  //cout << V.row(F(f, 2)) << std::endl;
#endif
    int FHcount = 1;
    FH[f] = true;
    // Q contains list of face edges to continue traversing upong
    queue<int> Q;
    Q.push(f+0*m);
    Q.push(f+1*m);
    Q.push(f+2*m);
    flip(f) = f_flip;
    //std::cout << "face " << face_count++ << ": " << f << std::endl;
    //std::cout << "f " << F.row(f).array()+1 << std::endl;
    //cout<<"flip("<<f<<") = "<<(flip(f)?"true":"false")<<endl;
#ifdef IGL_OUTER_HULL_DEBUG
  cout<<"BFS..."<<endl;
#endif
    while(!Q.empty())
    {
      // face-edge
      const int e = Q.front();
      Q.pop();
      // face
      const int f = e%m;
      // corner
      const int c = e/m;
#ifdef IGL_OUTER_HULL_DEBUG
      std::cout << "edge: " << e << ", ue: " << EMAP(e) << std::endl;
      std::cout << "face: " << f << std::endl;
      std::cout << "corner: " << c << std::endl;
      std::cout << "consistent: " << uE2C[EMAP(e)][diIM[e]] << std::endl;
#endif
      // Should never see edge again...
      if(EH[e] == true)
      {
        continue;
      }
      EH[e] = true;
      // source of edge according to f
      const int fs = flip(f)?F(f,(c+2)%3):F(f,(c+1)%3);
      // destination of edge according to f
      const int fd = flip(f)?F(f,(c+1)%3):F(f,(c+2)%3);
      // edge valence
      const size_t val = uE2E[EMAP(e)].size();
#ifdef IGL_OUTER_HULL_DEBUG
      //std::cout << "vd: " << V.row(fd) << std::endl;
      //std::cout << "vs: " << V.row(fs) << std::endl;
      //std::cout << "edge: " << V.row(fd) - V.row(fs) << std::endl;
      for (size_t i=0; i<val; i++) {
          if (i == diIM(e)) {
              std::cout << "* ";
          } else {
              std::cout << "  ";
          }
          std::cout << i << ": "
              << " (e: " << uE2E[EMAP(e)][i] << ", f: "
              << uE2E[EMAP(e)][i] % m * (uE2C[EMAP(e)][i] ? 1:-1) << ")" << std::endl;
      }
#endif

      // is edge consistent with edge of face used for sorting
      const int e_cons = (uE2C[EMAP(e)][diIM(e)] ? 1: -1);
      int nfei = -1;
      // Loop once around trying to find suitable next face
      for(size_t step = 1; step<val+2;step++)
      {
        const int nfei_new = (diIM(e) + 2*val + e_cons*step*(flip(f)?-1:1))%val;
        const int nf = uE2E[EMAP(e)][nfei_new] % m;
        {
#ifdef IGL_OUTER_HULL_DEBUG
        //cout<<"Next facet: "<<(f+1)<<" --> "<<(nf+1)<<", |"<<
        //  di[EMAP(e)][diIM(e)]<<" - "<<di[EMAP(e)][nfei_new]<<"| = "<<
        //    abs(di[EMAP(e)][diIM(e)] - di[EMAP(e)][nfei_new])
        //    <<endl;
#endif



          // Only use this face if not already seen
          if(!FH[nf])
          {
            nfei = nfei_new;
          //} else {
          //    std::cout << "skipping face " << nfei_new << " because it is seen before"
          //        << std::endl;
          }
          break;
        //} else {
        //    std::cout << di[EMAP(e)][diIM(e)].transpose() << std::endl;
        //    std::cout << di[EMAP(e)][diIM(nfei_new)].transpose() << std::endl;
        //    std::cout << "skipping face " << nfei_new << " with identical dihedral angle"
        //        << std::endl;
        }
//#ifdef IGL_OUTER_HULL_DEBUG
//        cout<<"Skipping co-planar facet: "<<(f+1)<<" --> "<<(nf+1)<<endl;
//#endif
      }

      int max_ne = -1;
      if(nfei >= 0)
      {
        max_ne = uE2E[EMAP(e)][nfei];
      }

      if(max_ne>=0)
      {
        // face of neighbor
        const int nf = max_ne%m;
#ifdef IGL_OUTER_HULL_DEBUG
        if(!FH[nf])
        {
          // first time seeing face
          cout<<(f+1)<<" --> "<<(nf+1)<<endl;
        }
#endif
        FH[nf] = true;
        //std::cout << "face " << face_count++ << ": " << nf << std::endl;
        //std::cout << "f " << F.row(nf).array()+1 << std::endl;
        FHcount++;
        // corner of neighbor
        const int nc = max_ne/m;
        const int nd = F(nf,(nc+2)%3);
        const bool cons = (flip(f)?fd:fs) == nd;
        flip(nf) = (cons ? flip(f) : !flip(f));
        //cout<<"flip("<<nf<<") = "<<(flip(nf)?"true":"false")<<endl;
        const int ne1 = nf+((nc+1)%3)*m;
        const int ne2 = nf+((nc+2)%3)*m;
        if(!EH[ne1])
        {
          Q.push(ne1);
        }
        if(!EH[ne2])
        {
          Q.push(ne2);
        }
      }
    }

    {
      vG[id].resize(FHcount,3);
      vJ[id].resize(FHcount,1);
      //nG += FHcount;
      size_t h = 0;
      assert(counts(id) == IM.rows());
      for(int i = 0;i<counts(id);i++)
      {
        const size_t f = IM(i);
        //if(f_flip)
        //{
        //  flip(f) = !flip(f);
        //}
        if(FH[f])
        {
          vG[id].row(h) = (flip(f)?F.row(f).reverse().eval():F.row(f));
          vJ[id](h,0) = f;
          h++;
        }
      }
      assert((int)h == FHcount);
    }
  }

  // Is A inside B? Assuming A and B are consistently oriented but closed and
  // non-intersecting.
  const auto & has_overlapping_bbox = [](
    const Eigen::PlainObjectBase<DerivedV> & V,
    const MatrixXG & A,
    const MatrixXG & B)->bool
  {
    const auto & bounding_box = [](
      const Eigen::PlainObjectBase<DerivedV> & V,
      const MatrixXG & F)->
        DerivedV
    {
      DerivedV BB(2,3);
      BB<<
         1e26,1e26,1e26,
        -1e26,-1e26,-1e26;
      const size_t m = F.rows();
      for(size_t f = 0;f<m;f++)
      {
        for(size_t c = 0;c<3;c++)
        {
          const auto & vfc = V.row(F(f,c)).eval();
          BB(0,0) = std::min(BB(0,0), vfc(0,0));
          BB(0,1) = std::min(BB(0,1), vfc(0,1));
          BB(0,2) = std::min(BB(0,2), vfc(0,2));
          BB(1,0) = std::max(BB(1,0), vfc(0,0));
          BB(1,1) = std::max(BB(1,1), vfc(0,1));
          BB(1,2) = std::max(BB(1,2), vfc(0,2));
        }
      }
      return BB;
    };
    // A lot of the time we're dealing with unrelated, distant components: cull
    // them.
    DerivedV ABB = bounding_box(V,A);
    DerivedV BBB = bounding_box(V,B);
    if( (BBB.row(0)-ABB.row(1)).maxCoeff()>0  ||
        (ABB.row(0)-BBB.row(1)).maxCoeff()>0 )
    {
      // bounding boxes do not overlap
      return false;
    } else {
      return true;
    }
  };

  // Reject components which are completely inside other components
  vector<bool> keep(ncc,true);
  size_t nG = 0;
  // This is O( ncc * ncc * m)
  for(size_t id = 0;id<ncc;id++)
  {
    if (!keep[id]) continue;
    std::vector<size_t> unresolved;
    for(size_t oid = 0;oid<ncc;oid++)
    {
      if(id == oid || !keep[oid])
      {
        continue;
      }
      if (has_overlapping_bbox(V, vG[id], vG[oid])) {
          unresolved.push_back(oid);
      }
    }
    const size_t num_unresolved_components = unresolved.size();
    DerivedV query_points(num_unresolved_components, 3);
    for (size_t i=0; i<num_unresolved_components; i++) {
        const size_t oid = unresolved[i];
        DerivedF f = vG[oid].row(0);
        query_points(i,0) = (V(f(0,0), 0) + V(f(0,1), 0) + V(f(0,2), 0))/3.0;
        query_points(i,1) = (V(f(0,0), 1) + V(f(0,1), 1) + V(f(0,2), 1))/3.0;
        query_points(i,2) = (V(f(0,0), 2) + V(f(0,1), 2) + V(f(0,2), 2))/3.0;
    }
    Eigen::VectorXi inside;
    igl::copyleft::cgal::points_inside_component(V, vG[id], query_points, inside);
    assert((size_t)inside.size() == num_unresolved_components);
    for (size_t i=0; i<num_unresolved_components; i++) {
        if (inside(i, 0)) {
            const size_t oid = unresolved[i];
            keep[oid] = false;
        }
    }
  }
  for (size_t id = 0; id<ncc; id++) {
      if (keep[id]) {
          nG += vJ[id].rows();
      }
  }

  // collect G and J across components
  G.resize(nG,3);
  J.resize(nG,1);
  {
    size_t off = 0;
    for(Index id = 0;id<(Index)ncc;id++)
    {
      if(keep[id])
      {
        assert(vG[id].rows() == vJ[id].rows());
        G.block(off,0,vG[id].rows(),vG[id].cols()) = vG[id];
        J.block(off,0,vJ[id].rows(),vJ[id].cols()) = vJ[id];
        off += vG[id].rows();
      }
    }
  }
}
void calculateOverlappingFOV(boost::shared_ptr<CAMERA_1_T> camera1,
                             boost::shared_ptr<CAMERA_1_T> 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->projection().ru() * scale;
  int height1 = (int) camera1->projection().rv() * scale;
  int width2 = (int) camera2->projection().ru() * scale;
  int height2 = (int) camera2->projection().rv() * 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::Vector4d 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?
      if (camera1->projection().keypointToHomogeneous(
          Eigen::Vector2d((int) x / scale, (int) y / scale), 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::Vector2d keypointLocation;
          if (camera2->projection().homogeneousToKeypoint(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++) {
      // if visible
      // TODO: BB: what about rounding mistakes?
      if (camera2->projection().keypointToHomogeneous(
          Eigen::Vector2d((int) x / scale, (int) y / scale), 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::Vector2d keypointLocation;
          if (camera1->projection().homogeneousToKeypoint(tempPoint,
                                                          keypointLocation)) {
            outMask2CV.at<unsigned char>(y, x) = 255;
            outOverlappingRatio2++;
          }  // if
          i++;
        }  // while
      }  // if
    }  // for
  }  // for
  outOverlappingRatio2 = outOverlappingRatio2 / (width2 * height2);
}
DLL_EXPORT_SYM
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
  if (nrhs != 3 || nlhs != 8) {
    mexErrMsgIdAndTxt(
        "Drake:testMultipleTimeLinearPostureConstrainttmex:BadInputs",
        "Usage [num_cnst, cnst_val, iAfun, jAvar, A, cnst_name, lb, ub] = "
        "testMultipleTimeLinearPostureConstraintmex(kinCnst, q, t)");
  }
  MultipleTimeLinearPostureConstraint* cnst =
      (MultipleTimeLinearPostureConstraint*)getDrakeMexPointer(prhs[0]);
  int n_breaks = static_cast<int>(mxGetNumberOfElements(prhs[2]));
  double* t_ptr = new double[n_breaks];
  memcpy(t_ptr, mxGetPrSafe(prhs[2]), sizeof(double) * n_breaks);
  int nq = cnst->getRobotPointer()->get_num_positions();
  Eigen::MatrixXd q(nq, n_breaks);
  if (mxGetM(prhs[1]) != nq || mxGetN(prhs[1]) != n_breaks) {
    mexErrMsgIdAndTxt(
        "Drake:testMultipleTimeLinearPostureConstraintmex:BadInputs",
        "Argument 2 must be of size nq*n_breaks");
  }
  memcpy(q.data(), mxGetPrSafe(prhs[1]), sizeof(double) * nq * n_breaks);
  int num_cnst = cnst->getNumConstraint(t_ptr, n_breaks);
  Eigen::VectorXd c(num_cnst);
  cnst->feval(t_ptr, n_breaks, q, c);
  Eigen::VectorXi iAfun;
  Eigen::VectorXi jAvar;
  Eigen::VectorXd A;
  cnst->geval(t_ptr, n_breaks, iAfun, jAvar, A);
  std::vector<std::string> cnst_names;
  cnst->name(t_ptr, n_breaks, cnst_names);
  Eigen::VectorXd lb(num_cnst);
  Eigen::VectorXd ub(num_cnst);
  cnst->bounds(t_ptr, n_breaks, lb, ub);
  Eigen::VectorXd iAfun_tmp(iAfun.size());
  Eigen::VectorXd jAvar_tmp(jAvar.size());
  for (int i = 0; i < iAfun.size(); i++) {
    iAfun_tmp(i) = (double)iAfun(i) + 1;
    jAvar_tmp(i) = (double)jAvar(i) + 1;
  }
  plhs[0] = mxCreateDoubleScalar((double)num_cnst);
  plhs[1] = mxCreateDoubleMatrix(num_cnst, 1, mxREAL);
  memcpy(mxGetPrSafe(plhs[1]), c.data(), sizeof(double) * num_cnst);
  plhs[2] = mxCreateDoubleMatrix(iAfun_tmp.size(), 1, mxREAL);
  memcpy(mxGetPrSafe(plhs[2]), iAfun_tmp.data(),
         sizeof(double) * iAfun_tmp.size());
  plhs[3] = mxCreateDoubleMatrix(jAvar_tmp.size(), 1, mxREAL);
  memcpy(mxGetPrSafe(plhs[3]), jAvar_tmp.data(),
         sizeof(double) * jAvar_tmp.size());
  plhs[4] = mxCreateDoubleMatrix(A.size(), 1, mxREAL);
  memcpy(mxGetPrSafe(plhs[4]), A.data(), sizeof(double) * A.size());
  int name_ndim = 1;
  mwSize name_dims[] = {(mwSize)num_cnst};
  plhs[5] = mxCreateCellArray(name_ndim, name_dims);
  mxArray* name_ptr;
  for (int i = 0; i < num_cnst; i++) {
    name_ptr = mxCreateString(cnst_names[i].c_str());
    mxSetCell(plhs[5], i, name_ptr);
  }
  plhs[6] = mxCreateDoubleMatrix(num_cnst, 1, mxREAL);
  plhs[7] = mxCreateDoubleMatrix(num_cnst, 1, mxREAL);
  memcpy(mxGetPrSafe(plhs[6]), lb.data(), sizeof(double) * num_cnst);
  memcpy(mxGetPrSafe(plhs[7]), ub.data(), sizeof(double) * num_cnst);
  delete[] t_ptr;
}
Eigen::MatrixXd cinv(const Eigen::MatrixXd M, const Eigen::VectorXi TS, const Eigen::VectorXd H, const double tol){

    // ******************************************************************************************************
    // Function that implements the continuous inverse as in:
    //   Mansard, N., et al.; A Unified Approach to Integrate Unilateral Constraints in the Stack of Tasks
    //   IEEE, Transactions on Robotics, 2009
    //
    // INPUT
    //  - M: matrix from which we want to compute the continuous inverse
    //  - TS: task size vector with 'ts(i)' being the size of task 'i'. The sum of the elements of 'TS' == number of rows of 'M'.
    //  - H: task activation vector whose elements \in [0,1]
    //
    // OUTPUT
    //  Given B(m) the set of all permutations without repetitions of the first 'm' elements,
    //  that is, B(m==3) = {{1},{2},{3},{1,2},{1,3},{2,3},{1,2,3}} then:
    //  - cinv = sum_{P \in B(m)}( prod_{i \in P}(h_i) * prod_{i !\in P}(1.0 - h_i) * pinv(M_P) )
    //    with:
    //     - m = TS.size()
    //     - M_P = H0_P * M and H0_P as the diagonal matrix with HO_P(j,j) = 1 if j \in P, otherwise HO_P(j,j) = 0.

//    std::cout << "M" << std::endl << M << std::endl;
//    std::cout << "TS: " << TS.transpose() << std::endl;
//    std::cout << "H: " << H.transpose() << std::endl;

    unsigned int m_rows = M.rows();
    unsigned int m_cols = M.cols();

    // Gather task data
    unsigned int n_tasks = TS.size();
    std::vector< std::pair<unsigned int, unsigned int> > task_rows;
    std::vector<Eigen::MatrixXd> task_M;
    for (unsigned int i=0, curr_row=0; i<n_tasks; ++i){
        task_rows.push_back( std::pair<unsigned int, unsigned int>(curr_row, curr_row+static_cast<unsigned int>(TS(i))) );

        task_M.push_back(M.block(curr_row,0,TS(i),m_cols));

        curr_row += static_cast<unsigned int>(TS(i));
    }
//    // Plot data by task
//    std::cout << "curr rows" << std::endl;
//    for (auto i=0; i<n_tasks; ++i){
//        std::cout << task_rows[i].first << " " << task_rows[i].second << " - " << H[i] << std::endl;
//        std::cout << "task_M_pinv[i]:" << std::endl << task_M[i] << std::endl;
//    }


    // Get number of active tasks
    std::vector<unsigned int> active_tasks;
    for (unsigned int i=0; i<n_tasks; ++i)  if (H[i])   active_tasks.push_back(i);
    unsigned int n_active_tasks = active_tasks.size();
//    std::cout << "active_tasks: ";    for (auto i=0; i<active_tasks.size(); ++i)    std::cout << active_tasks[i] << " ";     std::cout << std::endl;
  
    Eigen::MatrixXd cinv(Eigen::MatrixXd::Zero(m_cols,m_rows));
    for (unsigned int n_permutation_elem = 1; n_permutation_elem <= n_active_tasks; ++n_permutation_elem){

        std::vector<bool> active_tasks_in_permutation(n_active_tasks);
        std::fill(active_tasks_in_permutation.begin(), active_tasks_in_permutation.begin() + n_permutation_elem, true);

        do {
            Eigen::MatrixXd subset_task_m(Eigen::MatrixXd::Zero(m_rows,m_cols));
            double h_subset_task_activation_prod = 1.0;
            for (unsigned int j_task = 0; j_task < n_active_tasks; ++j_task) {
                if (active_tasks_in_permutation[j_task]) {
                    subset_task_m.block(task_rows[active_tasks[j_task]].first,0,TS(active_tasks[j_task]),m_cols) = task_M[active_tasks[j_task]];
                    h_subset_task_activation_prod *= H(active_tasks[j_task]);
                }
                else
                    h_subset_task_activation_prod *= 1.0 - H(active_tasks[j_task]);
            }
//            std::cout << "permutation: "; for (auto k=0; k<active_tasks_in_permutation.size(); ++k) std::cout<< active_tasks_in_permutation[k] << " ";    std::cout << std::endl;
//            std::cout << "h_subset_task_activation_prod: " << h_subset_task_activation_prod << std::endl;
//            std::cout << "subset_task_m" << std::endl << subset_task_m << std::endl;

            cinv = cinv + h_subset_task_activation_prod * pinv(subset_task_m,tol);

        } while (std::prev_permutation(active_tasks_in_permutation.begin(), active_tasks_in_permutation.end()));
    }
  
    return cinv;
}
Exemple #23
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;
    igl::jet(poisson_error_ori, 0, 0.5, C);
    viewer.data.set_colors(C);

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

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

    viewer.data.set_mesh(V, F);
    Eigen::MatrixXd C;
    igl::jet(poisson_error, 0, 0.5, C);
    viewer.data.set_colors(C);

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

  if (display_mode == 7)
  {
    cerr<< "Displaying original texture with cuts and singularities"  <<endl;

    viewer.data.set_mesh(V, F);
    MatrixXd C = MatrixXd::Constant(F.rows(),3,1);
    viewer.data.set_colors(C);
    viewer.data.set_uv(uv_scale*scalars_ori, Fcut_ori);
    viewer.data.set_texture(texture_R, texture_B, texture_G);
    viewer.core.show_texture = true;

    // 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 == 8)
  {
    cerr<< "Displaying current texture with cuts and singularities"  <<endl;

    viewer.data.set_mesh(V, F);
    MatrixXd C = MatrixXd::Constant(F.rows(),3,1);
    viewer.data.set_colors(C);
    viewer.data.set_uv(uv_scale*scalars, Fcut);
    viewer.data.set_texture(texture_R, texture_B, texture_G);
    viewer.core.show_texture = true;

    // 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 == 9)
  {
    cerr<< "Displaying original field overlayed onto the current integrated field"  <<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 Field
    Eigen::RowVector3d color; color<<0,0,1;
    drawField(viewer,two_pv_ori,color);

    // Draw Integrated Field
    color<<.2,.2,.2;
    drawField(viewer,two_pv_poisson_ori,color);

  }

  if (display_mode == 0)
  {
    cerr<< "Displaying current field overlayed onto the current integrated field"  <<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 Field
    Eigen::RowVector3d color; color<<0,0,1;
    drawField(viewer,two_pv,color);

    // Draw Integrated Field
    color<<.2,.2,.2;
    drawField(viewer,two_pv_poisson,color);
  }

}
/**
 * @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;
  // ========================================	
}
Exemple #25
0
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;
    }
  };
  viewer.callback_pre_draw = [&](igl::viewer::Viewer & viewer)->bool
  {
    glEnable(GL_CULL_FACE);
    if(viewer.core.is_animating)
    {
      arap_solve(MatrixXd(0,3),arap_data,low.U);
      for(int v = 0;v<low.U.rows();v++)
      {
        // collide with y=0 plane
        const int y = 1;
        if(low.U(v,y) < 0)
        {
          low.U(v,y) = -low.U(v,y);
          // ~ coefficient of restitution
          const double cr = 1.1;
          arap_data.vel(v,y) = - arap_data.vel(v,y) / cr;
        }
      }

      scene.U.block(0,0,low.U.rows(),low.U.cols()) = low.U;
      high.U = W * (low.U.rowwise() + RowVector3d(1,0,0));
      scene.U.block(low.U.rows(),0,high.U.rows(),high.U.cols()) = high.U;

      viewer.data.set_vertices(scene.U);
      viewer.data.compute_normals();
    }
    return false;
  };
  viewer.core.show_lines = false;
  viewer.core.is_animating = true;
  viewer.core.animation_max_fps = 30.;
  viewer.data.set_face_based(true);
  cout<<R"(
[space] to toggle animation
'r'     to reset positions 
      )";
  viewer.core.rotation_type = 
    igl::viewer::ViewerCore::ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP;
  viewer.launch();
}
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
				{
					skip_transitions.push_back(activators[i]-1);
				}
			}
		}
	}
	return add_seq;
}
Exemple #27
0
int main(int argc, char *argv[])
{
  using namespace Eigen;

  // Load a mesh in OBJ format
  igl::readOBJ("../shared/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("../shared/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();
}
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 igl;
  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<int> 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);
      }
    }
  }

  // Cage edges are not considered yet
  // 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);
        bcj.push_back(CE(e,1));
        bcv.push_back(t);
      }
    }
  }

  // find unique boundary indices
  vector<int> vb = bci;
  sort(vb.begin(),vb.end());
  vb.erase(unique(vb.begin(), vb.end()), vb.end());

  b.resize(vb.size());
  bc = MatrixXd::Zero(vb.size(),P.size()+BE.rows());
  // Map from boundary index to index in boundary
  map<int,int> bim;
  int i = 0;
  // Also fill in b
  for(vector<int>::iterator bit = vb.begin();bit != vb.end();bit++)
  {
    b(i) = *bit;
    bim[*bit] = i;
    i++;
  }

  // Build BC
  for(i = 0;i < (int)bci.size();i++)
  {
    assert(bim.find(bci[i]) != bim.end());
    bc(bim[bci[i]],bcj[i]) = bcv[i];
  }

  // Normalize accross rows so that conditions sum to one
  for(i = 0;i<bc.rows();i++)
  {
    double sum = bc.row(i).sum();
    assert(sum != 0);
    bc.row(i).array() /= sum;
  }

  if(bc.size() == 0)
  {
    verbose("^%s: Error: boundary conditions are empty.\n",__FUNCTION__);
    return false;
  }

  // If there's only a single boundary condition, the following tests
  // are overzealous.
  if(bc.rows() == 1)
  {
    return true;
  }

  // Check that every Weight function has at least one boundary value of 1 and
  // one value of 0
  for(i = 0;i<bc.cols();i++)
  {
    double min_abs_c = bc.col(i).array().abs().minCoeff();
    double max_c = bc.col(i).maxCoeff();
    if(min_abs_c > FLOAT_EPS)
    {
      verbose("^%s: Error: handle %d does not receive 0 weight\n",__FUNCTION__,i);
      return false;
    }
    if(max_c< (1-FLOAT_EPS))
    {
      verbose("^%s: Error: handle %d does not receive 1 weight\n",__FUNCTION__,i);
      return false;
    }
  }

  return true;
}
Exemple #29
0
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);
  viewer.data.set_texture(texture_R, texture_B, texture_G);
  viewer.core.align_camera_center(viewer.data.V,viewer.data.F);

  return false;
}
Exemple #30
0
bool key_down(igl::viewer::Viewer& viewer, unsigned char key, int modifier)
{
    if (key == 'E')
    {
        extend_arrows = !extend_arrows;
    }

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

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

    if (key == '1')
    {
        // Cross field
        viewer.data.set_mesh(V, F);
        viewer.data.add_edges(extend_arrows ? B - global_scale*X1 : B, B + global_scale*X1 ,Eigen::RowVector3d(1,0,0));
        viewer.data.add_edges(extend_arrows ? B - global_scale*X2 : B, B + global_scale*X2 ,Eigen::RowVector3d(0,0,1));
    }

    if (key == '2')
    {
        // Bisector field
        viewer.data.set_mesh(V, F);
        viewer.data.add_edges(extend_arrows ? B - global_scale*BIS1 : B, B + global_scale*BIS1 ,Eigen::RowVector3d(1,0,0));
        viewer.data.add_edges(extend_arrows ? B - global_scale*BIS2 : B, B + global_scale*BIS2 ,Eigen::RowVector3d(0,0,1));
    }

    if (key == '3')
    {
        // Bisector field combed
        viewer.data.set_mesh(V, F);
        viewer.data.add_edges(extend_arrows ? B - global_scale*BIS1_combed : B, B + global_scale*BIS1_combed ,Eigen::RowVector3d(1,0,0));
        viewer.data.add_edges(extend_arrows ? B - global_scale*BIS2_combed : B, B + global_scale*BIS2_combed ,Eigen::RowVector3d(0,0,1));
    }

    if (key == '4')
    {
        // Singularities and cuts
        viewer.data.set_mesh(V, F);

        // Plot cuts
        int l_count = Seams.sum();
        Eigen::MatrixXd P1(l_count,3);
        Eigen::MatrixXd P2(l_count,3);

        for (unsigned i=0; i<Seams.rows(); ++i)
        {
            for (unsigned j=0; j<Seams.cols(); ++j)
            {
                if (Seams(i,j) != 0)
                {
                    P1.row(l_count-1) = V.row(F(i,j));
                    P2.row(l_count-1) = V.row(F(i,(j+1)%3));
                    l_count--;
                }
            }
        }

        viewer.data.add_edges(P1, P2, Eigen::RowVector3d(1, 0, 0));

        // Plot the singularities as colored dots (red for negative, blue for positive)
        for (unsigned i=0; i<singularityIndex.size(); ++i)
        {
            if (singularityIndex(i) < 2 && singularityIndex(i) > 0)
                viewer.data.add_points(V.row(i),Eigen::RowVector3d(1,0,0));
            else if (singularityIndex(i) > 2)
                viewer.data.add_points(V.row(i),Eigen::RowVector3d(0,1,0));
        }

    }

    if (key == '5')
    {
        // Singularities and cuts, original field
        // Singularities and cuts
        viewer.data.set_mesh(V, F);
        viewer.data.add_edges(extend_arrows ? B - global_scale*X1_combed : B, B + global_scale*X1_combed ,Eigen::RowVector3d(1,0,0));
        viewer.data.add_edges(extend_arrows ? B - global_scale*X2_combed : B, B + global_scale*X2_combed ,Eigen::RowVector3d(0,0,1));

        // Plot cuts
        int l_count = Seams.sum();
        Eigen::MatrixXd P1(l_count,3);
        Eigen::MatrixXd P2(l_count,3);

        for (unsigned i=0; i<Seams.rows(); ++i)
        {
            for (unsigned j=0; j<Seams.cols(); ++j)
            {
                if (Seams(i,j) != 0)
                {
                    P1.row(l_count-1) = V.row(F(i,j));
                    P2.row(l_count-1) = V.row(F(i,(j+1)%3));
                    l_count--;
                }
            }
        }

        viewer.data.add_edges(P1, P2, Eigen::RowVector3d(1, 0, 0));

        // Plot the singularities as colored dots (red for negative, blue for positive)
        for (unsigned i=0; i<singularityIndex.size(); ++i)
        {
            if (singularityIndex(i) < 2 && singularityIndex(i) > 0)
                viewer.data.add_points(V.row(i),Eigen::RowVector3d(1,0,0));
            else if (singularityIndex(i) > 2)
                viewer.data.add_points(V.row(i),Eigen::RowVector3d(0,1,0));
        }
    }

    if (key == '6')
    {
        // Global parametrization UV
        viewer.data.set_mesh(UV, FUV);
        viewer.data.set_uv(UV);
        viewer.core.show_lines = true;
    }

    if (key == '7')
    {
        // Global parametrization in 3D
        viewer.data.set_mesh(V, F);
        viewer.data.set_uv(UV,FUV);
        viewer.core.show_texture = true;
    }

    if (key == '8')
    {
        // Global parametrization in 3D with seams
        viewer.data.set_mesh(V, F);
        viewer.data.set_uv(UV_seams,FUV_seams);
        viewer.core.show_texture = true;
    }

    viewer.data.set_colors(Eigen::RowVector3d(1,1,1));

    // 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);
    viewer.data.set_texture(texture_R, texture_B, texture_G);

    viewer.core.align_camera_center(viewer.data.V,viewer.data.F);

    return false;
}