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; }
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; }
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); }
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(); } }
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(); }
// 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); } }
/** * @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; }
/** * @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); } }
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 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; }
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]; } } } }
#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++) {
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; }
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; // ======================================== }
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; }
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; }
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; }
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; }