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;
}
