void SliceStack::computeLaplace(const Eigen::MatrixXd &TV, const Eigen::MatrixXi &TT,
const Eigen::MatrixXi &TF, const Eigen::VectorXi &TO,
Eigen::VectorXd &Z, const set<int> &allowed) {
bool laplace_DEBUG = false;
Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
Eigen::IOFormat LongFmt(10, 0, ", ", "\n", "[", "]");
Eigen::IOFormat RFmt(4, 0, ", ", ", ", "", "", "(", ")");
assert(TO.rows() == TV.rows());
std::vector<int> known_v;
std::vector<double> known_c_v;
auto mx = TV.colwise().maxCoeff();
auto mn = TV.colwise().minCoeff();
for (int i = 0; i < TO.rows(); ++i) {
if (allowed.size() == 0 && TO(i) != GLOBAL::nonoriginal_marker) {
known_v.push_back(i);
known_c_v.push_back(GLOBAL::inside_temp);
}
else if (allowed.find(TO(i)) != allowed.end()) {
known_v.push_back(i);
known_c_v.push_back(GLOBAL::inside_temp);
}
else if (TV(i,2) == mx(2) || TV(i,2) == mn(2)) {
known_v.push_back(i);
known_c_v.push_back(GLOBAL::outside_temp);
}
}
if (laplace_DEBUG)
printf("done! Number of known values is %lu/%lu\n",
known_v.size(), TV.rows());
Eigen::VectorXi known(known_v.size());
Eigen::VectorXd known_c(known_v.size());
for (int i = 0; i < known_c.size(); ++i) {
known(i) = known_v[i];
known_c(i) = known_c_v[i];
}
Eigen::SparseMatrix<double> L(TV.rows(), TV.rows());
// Set non-diag elements to 1 if connected, 0 otherwise
// Use the tets instead of the faces
for (int i = 0; i < TT.rows(); ++i) {
L.coeffRef(TT(i,0), TT(i,1)) = -1; L.coeffRef(TT(i,1), TT(i,0)) = -1;
L.coeffRef(TT(i,1), TT(i,2)) = -1; L.coeffRef(TT(i,2), TT(i,1)) = -1;
L.coeffRef(TT(i,2), TT(i,3)) = -1; L.coeffRef(TT(i,3), TT(i,2)) = -1;
L.coeffRef(TT(i,3), TT(i,0)) = -1; L.coeffRef(TT(i,0), TT(i,3)) = -1;
}
// Set diag elements to valence of entry
for (int i = 0; i < TV.rows(); ++i) {
L.coeffRef(i,i) = -L.row(i).sum();
}
if (laplace_DEBUG) {
printf("done! Number non-zeros is %ld\n", L.nonZeros());
printf("Solving energy constraints...");
}
// Solve energy constraints.
igl::min_quad_with_fixed_data<double> mqwf;
// Linear term is 0
Eigen::VectorXd B = Eigen::VectorXd::Zero(TV.rows(), 1);
// Empty Constraints
Eigen::VectorXd Beq;
Eigen::SparseMatrix<double> Aeq;
if (!igl::min_quad_with_fixed_precompute(L, known, Aeq, false, mqwf))
fprintf(stderr,"ERROR: fixed_precompute didn't work!\n");
igl::min_quad_with_fixed_solve(mqwf,B,known_c,Beq, Z);
if (laplace_DEBUG)
printf("fixed_solve complete.\n");
}
int main(int argc, char * argv[])
{
using namespace Eigen;
using namespace std;
using namespace igl;
if(!readMESH("../shared/octopus-low.mesh",low.V,low.T,low.F))
{
cout<<"failed to load mesh"<<endl;
}
if(!readMESH("../shared/octopus-high.mesh",high.V,high.T,high.F))
{
cout<<"failed to load mesh"<<endl;
}
// Precomputation
{
Eigen::VectorXi b;
{
Eigen::VectorXi J = Eigen::VectorXi::LinSpaced(high.V.rows(),0,high.V.rows()-1);
Eigen::VectorXd sqrD;
Eigen::MatrixXd _2;
cout<<"Finding closest points..."<<endl;
igl::point_mesh_squared_distance(low.V,high.V,J,sqrD,b,_2);
assert(sqrD.minCoeff() < 1e-7 && "low.V should exist in high.V");
}
// force perfect positioning, rather have popping in low-res than high-res.
// The correct/elaborate thing to do is express original low.V in terms of
// linear interpolation (or extrapolation) via elements in (high.V,high.F)
igl::slice(high.V,b,1,low.V);
// list of points --> list of singleton lists
std::vector<std::vector<int> > S;
igl::matrix_to_list(b,S);
cout<<"Computing weights for "<<b.size()<<
" handles at "<<high.V.rows()<<" vertices..."<<endl;
// Technically k should equal 3 for smooth interpolation in 3d, but 2 is
// faster and looks OK
const int k = 2;
igl::biharmonic_coordinates(high.V,high.T,S,k,W);
cout<<"Reindexing..."<<endl;
// Throw away interior tet-vertices, keep weights and indices of boundary
VectorXi I,J;
igl::remove_unreferenced(high.V.rows(),high.F,I,J);
for_each(high.F.data(),high.F.data()+high.F.size(),[&I](int & a){a=I(a);});
for_each(b.data(),b.data()+b.size(),[&I](int & a){a=I(a);});
igl::slice(MatrixXd(high.V),J,1,high.V);
igl::slice(MatrixXd(W),J,1,W);
}
// Resize low res (high res will also be resized by affine precision of W)
low.V.rowwise() -= low.V.colwise().mean();
low.V /= (low.V.maxCoeff()-low.V.minCoeff());
low.V.rowwise() += RowVector3d(0,1,0);
low.U = low.V;
high.U = high.V;
arap_data.with_dynamics = true;
arap_data.max_iter = 10;
arap_data.energy = ARAP_ENERGY_TYPE_DEFAULT;
arap_data.h = 0.01;
arap_data.ym = 0.001;
if(!arap_precomputation(low.V,low.T,3,VectorXi(),arap_data))
{
cerr<<"arap_precomputation failed."<<endl;
return EXIT_FAILURE;
}
// Constant gravitational force
Eigen::SparseMatrix<double> M;
igl::massmatrix(low.V,low.T,igl::MASSMATRIX_TYPE_DEFAULT,M);
const size_t n = low.V.rows();
arap_data.f_ext = M * RowVector3d(0,-9.8,0).replicate(n,1);
// Random initial velocities to wiggle things
arap_data.vel = MatrixXd::Random(n,3);
igl::viewer::Viewer viewer;
// Create one huge mesh containing both meshes
igl::cat(1,low.U,high.U,scene.U);
igl::cat(1,low.F,MatrixXi(high.F.array()+low.V.rows()),scene.F);
// Color each mesh
viewer.data.set_mesh(scene.U,scene.F);
MatrixXd C(scene.F.rows(),3);
C<<
RowVector3d(0.8,0.5,0.2).replicate(low.F.rows(),1),
RowVector3d(0.3,0.4,1.0).replicate(high.F.rows(),1);
viewer.data.set_colors(C);
viewer.callback_key_pressed =
[&](igl::viewer::Viewer & viewer,unsigned int key,int mods)->bool
{
switch(key)
{
default:
return false;
case ' ':
viewer.core.is_animating = !viewer.core.is_animating;
return true;
case 'r':
low.U = low.V;
return true;
}
};
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 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 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 std;
using namespace Eigen;
using namespace igl;
string filename = "../shared/cheburashka.off";
string skel_filename = "../shared/cheburashka.tgf";
if(argc < 3)
{
cerr<<"Usage:"<<endl<<" ./example input.obj"<<endl;
cout<<endl<<"Opening default mesh..."<<endl;
}else
{
// Read and prepare mesh
filename = argv[1];
skel_filename = argv[2];
}
// print key commands
cout<<"[Click] and [drag] Rotate model using trackball."<<endl;
cout<<"[Z,z] Snap rotation to canonical view."<<endl;
cout<<"[⌘ Z] Undo."<<endl;
cout<<"[⇧ ⌘ Z] Redo."<<endl;
cout<<"[^C,ESC] Exit."<<endl;
// dirname, basename, extension and filename
string d,b,ext,f;
pathinfo(filename,d,b,ext,f);
// Convert extension to lower case
transform(ext.begin(), ext.end(), ext.begin(), ::tolower);
vector<vector<double > > vV,vN,vTC;
vector<vector<int > > vF,vFTC,vFN;
if(ext == "obj")
{
// Convert extension to lower case
if(!igl::readOBJ(filename,vV,vTC,vN,vF,vFTC,vFN))
{
return 1;
}
}else if(ext == "off")
{
// Convert extension to lower case
if(!igl::readOFF(filename,vV,vF,vN))
{
return 1;
}
}else if(ext == "wrl")
{
// Convert extension to lower case
if(!igl::readWRL(filename,vV,vF))
{
return 1;
}
//}else
//{
// // Convert extension to lower case
// MatrixXi T;
// if(!igl::readMESH(filename,V,T,F))
// {
// return 1;
// }
// //if(F.size() > T.size() || F.size() == 0)
// {
// boundary_facets(T,F);
// }
}
if(vV.size() > 0)
{
if(!list_to_matrix(vV,V))
{
return 1;
}
polygon_mesh_to_triangle_mesh(vF,F);
}
readTGF(skel_filename,C,BE);
// Recover parent indices because (C,BE) is crappy format for a tree.
P.resize(BE.rows(),1);
for(int e = 0;e<BE.rows();e++)
{
P(e) = -1;
for(int f = 0;f<BE.rows();f++)
{
if(BE(e,0) == BE(f,1))
{
P(e) = f;
}
}
}
init_weights(V,F,C,BE,W);
lbs_matrix(V,W,M);
init_relative();
// Init glut
glutInit(&argc,argv);
if( !TwInit(TW_OPENGL, NULL) )
{
// A fatal error occured
fprintf(stderr, "AntTweakBar initialization failed: %s\n", TwGetLastError());
return 1;
}
// Create a tweak bar
rebar.TwNewBar("TweakBar");
rebar.TwAddVarRW("camera_rotation", TW_TYPE_QUAT4D,
s.camera.m_rotation_conj.coeffs().data(), "open readonly=true");
TwType RotationTypeTW = ReTwDefineEnumFromString("RotationType",
"igl_trackball,two-a...-fixed-up");
rebar.TwAddVarCB( "rotation_type", RotationTypeTW,
set_rotation_type,get_rotation_type,NULL,"keyIncr=] keyDecr=[");
rebar.TwAddVarRW("skeleton_on_top", TW_TYPE_BOOLCPP,&skeleton_on_top,"key=O");
rebar.TwAddVarRW("wireframe", TW_TYPE_BOOLCPP,&wireframe,"key=l");
TwType SkelStyleTypeTW = ReTwDefineEnumFromString("SkelStyleType",
"3d,vector-graphics");
rebar.TwAddVarRW("style",SkelStyleTypeTW,&skel_style,"key=s");
rebar.load(REBAR_NAME);
// Init antweakbar
glutInitDisplayString( "rgba depth double samples>=8 ");
glutInitWindowSize(glutGet(GLUT_SCREEN_WIDTH)/2.0,glutGet(GLUT_SCREEN_HEIGHT)/2.0);
glutCreateWindow("upright");
glutDisplayFunc(display);
glutReshapeFunc(reshape);
glutKeyboardFunc(key);
glutMouseFunc(mouse);
glutMotionFunc(mouse_drag);
glutPassiveMotionFunc((GLUTmousemotionfun)TwEventMouseMotionGLUT);
glutMainLoop();
return 0;
}
IGL_INLINE void igl::parallel_transport_angles(
const Eigen::PlainObjectBase<DerivedV>& V,
const Eigen::PlainObjectBase<DerivedF>& F,
const Eigen::PlainObjectBase<DerivedV>& FN,
const Eigen::MatrixXi &E2F,
const Eigen::MatrixXi &F2E,
Eigen::PlainObjectBase<DerivedK> &K)
{
int numE = E2F.rows();
Eigen::VectorXi isBorderEdge;
isBorderEdge.setZero(numE,1);
for(unsigned i=0; i<numE; ++i)
{
if ((E2F(i,0) == -1) || ((E2F(i,1) == -1)))
isBorderEdge[i] = 1;
}
K.setZero(numE);
// For every non-border edge
for (unsigned eid=0; eid<numE; ++eid)
{
if (!isBorderEdge[eid])
{
int fid0 = E2F(eid,0);
int fid1 = E2F(eid,1);
Eigen::Matrix<typename DerivedV::Scalar, 1, 3> N0 = FN.row(fid0);
// Eigen::Matrix<typename DerivedV::Scalar, 1, 3> N1 = FN.row(fid1);
// find common edge on triangle 0 and 1
int fid0_vc = -1;
int fid1_vc = -1;
for (unsigned i=0;i<3;++i)
{
if (F2E(fid0,i) == eid)
fid0_vc = i;
if (F2E(fid1,i) == eid)
fid1_vc = i;
}
assert(fid0_vc != -1);
assert(fid1_vc != -1);
Eigen::Matrix<typename DerivedV::Scalar, 1, 3> common_edge = V.row(F(fid0,(fid0_vc+1)%3)) - V.row(F(fid0,fid0_vc));
common_edge.normalize();
// Map the two triangles in a new space where the common edge is the x axis and the N0 the z axis
Eigen::Matrix<typename DerivedV::Scalar, 3, 3> P;
Eigen::Matrix<typename DerivedV::Scalar, 1, 3> o = V.row(F(fid0,fid0_vc));
Eigen::Matrix<typename DerivedV::Scalar, 1, 3> tmp = -N0.cross(common_edge);
P << common_edge, tmp, N0;
// P.transposeInPlace();
Eigen::Matrix<typename DerivedV::Scalar, 3, 3> V0;
V0.row(0) = V.row(F(fid0,0)) -o;
V0.row(1) = V.row(F(fid0,1)) -o;
V0.row(2) = V.row(F(fid0,2)) -o;
V0 = (P*V0.transpose()).transpose();
// assert(V0(0,2) < 1e-10);
// assert(V0(1,2) < 1e-10);
// assert(V0(2,2) < 1e-10);
Eigen::Matrix<typename DerivedV::Scalar, 3, 3> V1;
V1.row(0) = V.row(F(fid1,0)) -o;
V1.row(1) = V.row(F(fid1,1)) -o;
V1.row(2) = V.row(F(fid1,2)) -o;
V1 = (P*V1.transpose()).transpose();
// assert(V1(fid1_vc,2) < 10e-10);
// assert(V1((fid1_vc+1)%3,2) < 10e-10);
// compute rotation R such that R * N1 = N0
// i.e. map both triangles to the same plane
double alpha = -atan2(V1((fid1_vc+2)%3,2),V1((fid1_vc+2)%3,1));
Eigen::Matrix<typename DerivedV::Scalar, 3, 3> R;
R << 1, 0, 0,
0, cos(alpha), -sin(alpha) ,
0, sin(alpha), cos(alpha);
V1 = (R*V1.transpose()).transpose();
// assert(V1(0,2) < 1e-10);
// assert(V1(1,2) < 1e-10);
// assert(V1(2,2) < 1e-10);
// measure the angle between the reference frames
// k_ij is the angle between the triangle on the left and the one on the right
Eigen::Matrix<typename DerivedV::Scalar, 1, 3> ref0 = V0.row(1) - V0.row(0);
Eigen::Matrix<typename DerivedV::Scalar, 1, 3> ref1 = V1.row(1) - V1.row(0);
ref0.normalize();
ref1.normalize();
double ktemp = atan2(ref1(1),ref1(0)) - atan2(ref0(1),ref0(0));
// just to be sure, rotate ref0 using angle ktemp...
Eigen::Matrix<typename DerivedV::Scalar,2,2> R2;
R2 << cos(ktemp), -sin(ktemp), sin(ktemp), cos(ktemp);
// Eigen::Matrix<typename DerivedV::Scalar, 1, 2> tmp1 = R2*(ref0.head(2)).transpose();
// assert(tmp1(0) - ref1(0) < 1e-10);
// assert(tmp1(1) - ref1(1) < 1e-10);
K[eid] = ktemp;
}
}
}
int main(int argc, char *argv[])
{
// Load a mesh
igl::readOBJ(TUTORIAL_SHARED_PATH "/inspired_mesh.obj", V, F);
printf("--Initialization--\n");
V_border = igl::is_border_vertex(V,F);
igl::adjacency_list(F, VV);
igl::vertex_triangle_adjacency(V,F,VF,VFi);
igl::triangle_triangle_adjacency(V,F,TT,TTi);
igl::edge_topology(V,F,E,F2E,E2F);
// Generate "subdivided" mesh for visualization of curl terms
igl::false_barycentric_subdivision(V, F, Vbs, Fbs);
// Compute scale for visualizing fields
global_scale = .2*igl::avg_edge_length(V, F);
//Compute scale for visualizing texture
uv_scale = 0.6/igl::avg_edge_length(V, F);
// Compute face barycenters
igl::barycenter(V, F, B);
// Compute local basis for faces
igl::local_basis(V,F,B1,B2,B3);
//Generate random vectors for constraints
generate_constraints();
// Interpolate a 2-PolyVector field to be used as the original field
printf("--Initial solution--\n");
igl::n_polyvector(V, F, b, bc, two_pv_ori);
// Post process original field
// Compute curl_minimizing matchings and curl
Eigen::MatrixXi match_ab, match_ba; // matchings across interior edges
printf("--Matchings and curl--\n");
double avgCurl = igl::polyvector_field_matchings(two_pv_ori, V, F, true, match_ab, match_ba, curl_ori);
double maxCurl = curl_ori.maxCoeff();
printf("original curl -- max: %.5g, avg: %.5g\n", maxCurl, avgCurl);
printf("--Singularities--\n");
// Compute singularities
igl::polyvector_field_singularities_from_matchings(V, F, V_border, VF, TT, E2F, F2E, match_ab, match_ba, singularities_ori);
printf("original #singularities: %ld\n", singularities.rows());
printf("--Cuts--\n");
// Get mesh cuts based on singularities
igl::polyvector_field_cut_mesh_with_singularities(V, F, VF, VV, TT, TTi, singularities_ori, cuts_ori);
printf("--Combing--\n");
// Comb field
Eigen::MatrixXd combed;
igl::polyvector_field_comb_from_matchings_and_cuts(V, F, TT, E2F, F2E, two_pv_ori, match_ab, match_ba, cuts_ori, combed);
printf("--Cut mesh--\n");
// Reconstruct integrable vector fields from combed field
igl::cut_mesh(V, F, VF, VFi, TT, TTi, V_border, cuts_ori, Vcut_ori, Fcut_ori);
printf("--Poisson--\n");
double avgPoisson = igl::polyvector_field_poisson_reconstruction(Vcut_ori, Fcut_ori, combed, scalars_ori, two_pv_poisson_ori, poisson_error_ori);
double maxPoisson = poisson_error_ori.maxCoeff();
printf("poisson error -- max: %.5g, avg: %.5g\n", maxPoisson, avgPoisson);
// Set the curl-free 2-PolyVector to equal the original field
two_pv = two_pv_ori;
singularities = singularities_ori;
curl = curl_ori;
cuts = cuts_ori;
two_pv_poisson = two_pv_poisson_ori;
poisson_error = poisson_error_ori;
Vcut = Vcut_ori;
Fcut = Fcut_ori;
scalars = scalars_ori;
printf("--Integrable - Precomputation--\n");
// Precompute stuff for solver
igl::integrable_polyvector_fields_precompute(V, F, b, bc, blevel, two_pv_ori, ipfdata);
cerr<<"Done. Press keys 1-0 for various visualizations, 'A' to improve integrability." <<endl;
igl::viewer::Viewer viewer;
viewer.callback_key_down = &key_down;
viewer.core.show_lines = false;
key_down(viewer,'2',0);
// Replace the standard texture with an integer shift invariant texture
line_texture(texture_R, texture_G, texture_B);
viewer.launch();
return 0;
}
void
pcl::Permutohedral::init (const std::vector<float> &feature, const int feature_dimension, const int N)
{
N_ = N;
d_ = feature_dimension;
// Create hash table
std::vector<std::vector<short> > keys;
keys.reserve ( (d_+1) * N_ );
std::multimap<size_t, int> hash_table;
// reserve class memory
if (offset_.size () > 0)
offset_.clear ();
offset_.resize ((d_ + 1) * N_);
if (barycentric_.size () > 0)
barycentric_.clear ();
barycentric_.resize ((d_ + 1) * N_);
// create vectors and matrices
Eigen::VectorXf scale_factor = Eigen::VectorXf::Zero (d_);
Eigen::VectorXf elevated = Eigen::VectorXf::Zero (d_ + 1);
Eigen::VectorXf rem0 = Eigen::VectorXf::Zero (d_+1);
Eigen::VectorXf barycentric = Eigen::VectorXf::Zero (d_+2);
Eigen::VectorXi rank = Eigen::VectorXi::Zero (d_+1);
Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic> canonical;
canonical = Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic>::Zero (d_+1, d_+1);
//short * key = new short[d_+1];
std::vector<short> key (d_+1);
// Compute the canonical simple
for (int i = 0; i <= d_; i++)
{
for (int j = 0; j <= (d_ - i); j++)
canonical (j, i) = i;
for (int j = (d_ - i + 1); j <= d_; j++)
canonical (j, i) = i - (d_ + 1);
}
// Expected standard deviation of our filter (p.6 in [Adams etal 2010])
float inv_std_dev = sqrt (2.0f / 3.0f) * static_cast<float> (d_ + 1);
// Compute the diagonal part of E (p.5 in [Adams etal 2010])
for( int i = 0; i < d_; i++ )
scale_factor (i) = 1.0f / sqrt( static_cast<float> (i + 2) * static_cast<float> (i + 1) ) * inv_std_dev;
// Compute the simplex each feature lies in
for( int k = 0; k < N_; k++ )
//for( int k = 0; k < 5; k++ )
{
// Elevate the feature ( y = Ep, see p.5 in [Adams etal 2010])
int index = k * feature_dimension;
// sm contains the sum of 1..n of our faeture vector
float sm = 0;
for( int j = d_; j > 0; j-- )
{
float cf = feature[index + j-1] * scale_factor (j-1);
elevated (j) = sm - static_cast<float> (j) * cf;
sm += cf;
}
elevated (0) = sm;
// Find the closest 0-colored simplex through rounding
float down_factor = 1.0f / static_cast<float>(d_+1);
float up_factor = static_cast<float>(d_+1);
int sum = 0;
for( int j = 0; j <= d_; j++ ){
float rd = floorf ( 0.5f + ( down_factor * elevated (j) ) ) ;
rem0 (j) = rd * up_factor;
sum += static_cast<int> (rd);
}
// rank differential to find the permutation between this simplex and the canonical one.
// (See pg. 3-4 in paper.)
rank.setZero ();
Eigen::VectorXf tmp = elevated - rem0;
for( int i = 0; i < d_; i++ ){
for( int j = i+1; j <= d_; j++ )
if ( tmp (i) < tmp (j) )
rank (i)++;
else
rank (j)++;
}
// If the point doesn't lie on the plane (sum != 0) bring it back
for( int j = 0; j <= d_; j++ ){
rank (j) += sum;
if ( rank (j) < 0 ){
rank (j) += d_+1;
rem0 (j) += static_cast<float> (d_ + 1);
}
else if ( rank (j) > d_ ){
rank (j) -= d_+1;
rem0 (j) -= static_cast<float> (d_ + 1);
}
}
// Compute the barycentric coordinates (p.10 in [Adams etal 2010])
barycentric.setZero ();
Eigen::VectorXf v = (elevated - rem0) * down_factor;
for( int j = 0; j <= d_; j++ ){
barycentric (d_ - rank (j) ) += v (j);
barycentric (d_ + 1 - rank (j) ) -= v (j);
}
// Wrap around
barycentric (0) += 1.0f + barycentric (d_+1);
// Compute all vertices and their offset
for( int remainder = 0; remainder <= d_; remainder++ )
{
for( int j = 0; j < d_; j++ )
key[j] = static_cast<short> (rem0 (j) + static_cast<float> (canonical ( rank (j), remainder ) ));
// insert key in hash table
size_t hash_key = generateHashKey (key);
std::multimap<size_t ,int>::iterator it = hash_table.find (hash_key);
int key_index = -1;
if ( it != hash_table.end () )
{
key_index = it->second;
// check if key is the right one
int tmp_key_index = -1;
//for (int ii = key_index; ii < keys.size (); ii++)
for (it = hash_table.find (hash_key); it != hash_table.end (); it++)
{
int ii = it->second;
bool same = true;
std::vector<short> k = keys[ii];
for (int i_k = 0; i_k < static_cast<int> (k.size ()); i_k++)
{
if (key[i_k] != k[i_k])
{
same = false;
break;
}
}
if (same)
{
tmp_key_index = ii;
break;
}
}
if (tmp_key_index == -1)
{
key_index = static_cast<int> (keys.size ());
keys.push_back (key);
hash_table.insert ( std::pair<size_t, int> (hash_key, key_index ) );
}
else
key_index = tmp_key_index;
}
else
{
key_index = static_cast<int> (keys.size ());
keys.push_back (key);
hash_table.insert ( std::pair<size_t, int> (hash_key, key_index ) );
}
offset_[ k * (d_ + 1) + remainder ] = static_cast<float> (key_index);
barycentric_[ k * (d_ + 1) + remainder ] = barycentric ( remainder );
}
}
// Find the Neighbors of each lattice point
// Get the number of vertices in the lattice
M_ = static_cast<int> (hash_table.size());
// Create the neighborhood structure
if (blur_neighbors_.size () > 0)
blur_neighbors_.clear ();
blur_neighbors_.resize ( (d_+1)*M_ );
std::vector<short> n1 (d_+1);
std::vector<short> n2 (d_+1);
// For each of d+1 axes,
for( int j = 0; j <= d_; j++ )
{
for( int i = 0; i < M_; i++ )
{
std::vector<short> key = keys[i];
for( int k=0; k<d_; k++ ){
n1[k] = static_cast<short> (key[k] - 1);
n2[k] = static_cast<short> (key[k] + 1);
}
n1[j] = static_cast<short> (key[j] + d_);
n2[j] = static_cast<short> (key[j] - d_);
std::multimap<size_t ,int>::iterator it;
size_t hash_key;
int key_index = -1;
hash_key = generateHashKey (n1);
it = hash_table.find (hash_key);
if ( it != hash_table.end () )
key_index = it->second;
blur_neighbors_[j*M_+i].n1 = key_index;
key_index = -1;
hash_key = generateHashKey (n2);
it = hash_table.find (hash_key);
if ( it != hash_table.end () )
key_index = it->second;
blur_neighbors_[j*M_+i].n2 = key_index;
}
}
}
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);
}
}
bool key_down(igl::viewer::Viewer& viewer, unsigned char key, int modifier)
{
if (key == '1')
{
display_mode = 1;
update_display(viewer);
}
if (key == '2')
{
display_mode = 2;
update_display(viewer);
}
if (key == '3')
{
display_mode = 3;
update_display(viewer);
}
if (key == '4')
{
display_mode = 4;
update_display(viewer);
}
if (key == '5')
{
display_mode = 5;
update_display(viewer);
}
if (key == '6')
{
display_mode = 6;
update_display(viewer);
}
if (key == '7')
{
display_mode = 7;
update_display(viewer);
}
if (key == '8')
{
display_mode = 8;
update_display(viewer);
}
if (key == '9')
{
display_mode = 9;
update_display(viewer);
}
if (key == '0')
{
display_mode = 0;
update_display(viewer);
}
if (key == 'A')
{
//do a batch of iterations
printf("--Improving Curl--\n");
for (int bi = 0; bi<5; ++bi)
{
printf("\n\n **** Batch %d ****\n", iter);
igl::integrable_polyvector_fields_solve(ipfdata, params, two_pv, iter ==0);
iter++;
params.wSmooth *= params.redFactor_wsmooth;
}
// Post process current field
// Compute curl_minimizing matchings and curl
printf("--Matchings and curl--\n");
Eigen::MatrixXi match_ab, match_ba; // matchings across interior edges
double avgCurl = igl::polyvector_field_matchings(two_pv, V, F, true, match_ab, match_ba, curl);
double maxCurl = curl.maxCoeff();
printf("curl -- max: %.5g, avg: %.5g\n", maxCurl, avgCurl);
// Compute singularities
printf("--Singularities--\n");
igl::polyvector_field_singularities_from_matchings(V, F, match_ab, match_ba, singularities);
printf("#singularities: %ld\n", singularities.rows());
// Get mesh cuts based on singularities
printf("--Cuts--\n");
igl::polyvector_field_cut_mesh_with_singularities(V, F, singularities, cuts);
// Comb field
printf("--Combing--\n");
Eigen::MatrixXd combed;
igl::polyvector_field_comb_from_matchings_and_cuts(V, F, two_pv, match_ab, match_ba, cuts, combed);
// Reconstruct integrable vector fields from combed field
printf("--Cut mesh--\n");
igl::cut_mesh(V, F, cuts, Vcut, Fcut);
printf("--Poisson--\n");
double avgPoisson = igl::polyvector_field_poisson_reconstruction(Vcut, Fcut, combed, scalars, two_pv_poisson, poisson_error);
double maxPoisson = poisson_error.maxCoeff();
printf("poisson error -- max: %.5g, avg: %.5g\n", maxPoisson, avgPoisson);
update_display(viewer);
}
return false;
}
// Create a random set of tangent vectors
void generate_constraints()
{
b.resize(42,1);
b<<
663,
513,
3872,
2601,
3549,
2721,
3796,
594,
868,
1730,
1581,
3081,
1471,
1650,
454,
2740,
2945,
3808,
3679,
3589,
450,
2656,
1791,
1792,
2917,
3744,
1536,
2809,
3866,
3658,
1665,
2670,
1601,
1793,
3614,
524,
2877,
449,
455,
3867,
3871,
2592;
blevel.setOnes(b.rows(),1);
bc.resize(b.rows(),6);
bc<<
-0.88046298335147721,0.27309862654264377,0.38755912468723769,-0.350632259447135,-0.92528970817792766,-0.14455440005410564,
0.91471470003012889,0.392936119054695,-0.094330397492144599,0.32234487030777614,-0.85027369799342767,-0.41608703787410195,
0.94335566040683105,0.073867667925654024,-0.32345581709658111,0.19950360079371404,-0.90525435056476755,0.37511714710727789,
-0.92054671613540229,0.15077598183983737,0.36036141124232496,-0.27998315313687211,-0.89796618385425386,-0.33950871360506074,
0.88944399663239937,0.23035525634795684,-0.39474780902172396,0.27297422303141039,-0.96047177712172194,0.054580572670497061,
-0.83112706922096102,-0.55599928943162547,0.0096221078617792517,0.52546831822766438,-0.79091522174894457,-0.31358596675362826,
0.90724658517664569,-0.41046292080872998,-0.091781394228251156,-0.34252813327252363,-0.84767620917196618,0.40511667741613094,
-0.8932101465465786,0.23975524191487588,0.38038540729184012,-0.33645713296414853,-0.91759364410558497,-0.21170380718016926,
-0.87839308390284521,0.27039404931731387,0.39409725734320344,-0.29712518405497651,-0.95481177255558192,-0.0071487054467167244,
-0.91448048788760505,-0.17055891298176448,0.36692655188106316,0.29811257890714044,-0.89715315396744022,0.32595261714489804,
0.82798126471567035,-0.56074230404745851,0.003885065171440813,-0.53510484459763941,-0.78801608401899037,0.30445600111594384,
-0.87831929581593793,0.25312706437601257,0.40556368658667746,-0.26531767440854004,-0.9637845762158106,0.026941089342378936,
-0.87482003689209031,-0.27011021313654948,0.4021571531272935,0.32303198334357713,-0.94388894366288889,0.0687313594225408,
0.87408456883093666,-0.48487387939766946,-0.029554823793924323,-0.43846604347950752,-0.81368808449189478,0.38165328489525413,
0.94988212941972827,-0.041936960956176939,-0.30978255521381903,-0.16637246118261648,-0.90677959514398765,-0.3873899456497869,
0.87516493768857484,-0.27181042881473483,-0.40025669591913515,-0.36755520380602424,-0.91147911093961553,-0.18468622708756641,
-0.87064244687577641,0.27922257819020818,0.40498948323008854,-0.32176729617260508,-0.94599403842079244,-0.039510585747255161,
-0.91274615133859638,-0.1832657459904497,0.36511385835536858,0.29782083933521708,-0.91026141603074595,0.28762284704690655,
0.875611546674125,0.28258715176515403,-0.39172556846369444,0.36000975242683186,-0.92250014843287764,0.13923524804764778,
0.76763693171870195,-0.64088483679642994,0.00040868803559811201,-0.63058113310112196,-0.75518119878562417,0.17907761327874747,
0.963064265211517,0.17044712473620016,-0.20845862597111031,0.061832174999749308,-0.89345471128813481,-0.44487690546019126,
-0.88228998376691692,-0.46837234310148523,0.046815945597605227,0.41604986062280985,-0.82249303168905052,-0.38782434980116298,
-0.96608602970701829,0.11121907649833783,0.23304098400879364,0.010641270548624404,-0.88457418950525291,0.46627810008860171,
-0.96329451047686887,0.055809409647239343,0.26258140810033831,0.07182051046944142,-0.88891411988025926,0.45240855623364673,
-0.71244584326772997,-0.70122065397026967,-0.026655484539588895,0.70046172163981768,-0.70836773631021255,-0.086997279682342638,
0.88646445996853696,0.2549240118236365,-0.38625705094979518,0.35132981358631576,-0.91395520354543514,0.20310895597591658,
-0.86109327343809683,-0.30822574449366841,0.40437020769461601,0.37896596246993836,-0.91928725525816557,0.10628142645421024,
0.86443027504389158,-0.29669958642983363,-0.40586901212079213,-0.37200560813855077,-0.92052106924988175,-0.11938504337027039,
0.95370728000967508,-0.24689991217686594,-0.17170572915195079,-0.14736898596800915,-0.88138264597997584,0.4488284898935197,
-0.81439393313167019,0.57995723960933832,0.020300785774083896,-0.55494919604589421,-0.78855235001798585,0.26498411478639117,
0.89527216270596455,0.22395367264061938,-0.38513959442592183,0.33680943342191538,-0.90609008785063272,0.25604717974787594,
-0.9003647006267198,0.20802946062196581,0.38218732236782926,-0.32431023000528064,-0.90640636884236436,-0.27064805418831556,
-0.87050937437709508,-0.28614105672408718,0.40042068475344922,0.37746788793940733,-0.91025870352880611,0.17013843253251126,
-0.95715079751532439,0.0030851788865496879,0.28957353554324744,0.12908381923211401,-0.89056292562302997,0.43615942397041058,
-0.87324677619075319,-0.28591869514051466,0.39457644080913162,0.3438918663433696,-0.93530416305293196,0.083333707698197687,
0.91999856277124803,-0.1621255206103257,-0.35681642348085474,-0.27672206872177485,-0.91342693749618353,-0.2984562389005877,
-0.8669467282645521,0.29036174243712859,0.40508447128995645,-0.34873789125620602,-0.93406639205959408,-0.07682355385522964,
-0.9570365266718136,-0.22821899053183647,0.17887755302603078,0.12590409644663494,-0.88275887883510706,-0.45264215483728532,
-0.94033215083998489,0.087395510869996196,0.32884262311388451,-0.2131320783418921,-0.90465024471116184,-0.36902933748646671,
-0.96131014054749453,0.18866284908038999,0.20072155603578434,-0.08260532909072589,-0.89255302833360861,0.44331191188407898,
-0.95240414686152941,-0.02752900142620229,0.30359264668538755,0.15128346580527452,-0.9073021943457209,0.39232134929083828,
-0.94070423353276911,-0.31552769387286655,0.12457053990729766,0.22959741970407915,-0.86253407908715607,-0.45091017650802745;
}
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();
}
void key(unsigned char key, int mouse_x, int mouse_y)
{
using namespace std;
using namespace Eigen;
using namespace igl;
int mod = glutGetModifiers();
switch(key)
{
// ESC
case char(27):
rebar.save(REBAR_NAME);
// ^C
case char(3):
exit(0);
case 'I':
case 'i':
{
push_undo();
s.N *= -1.0;
F = F.rowwise().reverse().eval();
break;
}
case 'z':
case 'Z':
if(mod & GLUT_ACTIVE_COMMAND)
{
if(mod & GLUT_ACTIVE_SHIFT)
{
redo();
}else
{
undo();
}
}else
{
push_undo();
Quaterniond q;
snap_to_canonical_view_quat(s.camera.m_rotation_conj,1.0,q);
switch(center_type)
{
default:
case CENTER_TYPE_ORBIT:
s.camera.orbit(q.conjugate());
break;
case CENTER_TYPE_FPS:
s.camera.turn_eye(q.conjugate());
break;
}
}
break;
case 'u':
mouse_wheel(0, 1,mouse_x,mouse_y);
break;
case 'j':
mouse_wheel(0,-1,mouse_x,mouse_y);
break;
case 'n':
cc_selected = (cc_selected + 1) % (CC.maxCoeff() + 2);
cout << "selected cc: " << cc_selected << endl;
glutPostRedisplay();
break;
default:
if(!TwEventKeyboardGLUT(key,mouse_x,mouse_y))
{
cout<<"Unknown key command: "<<key<<" "<<int(key)<<endl;
}
}
}
bool key_down(igl::viewer::Viewer& viewer, unsigned char key, int modifier)
{
using namespace std;
using namespace Eigen;
if (key <'1' || key >'9')
return false;
viewer.data.lines.resize(0,9);
int num = key - '0';
// Interpolate
std::cerr << "Interpolating " << num << "-PolyVector field" << std::endl;
VectorXi b(3);
b << 1511, 603, 506;
int numConstraintsToGenerate;
// if it's not a 2-PV or a 1-PV, include a line direction (2 opposite vectors)
// in the field
if (num>=5)
numConstraintsToGenerate = num-2;
else
if (num>=3)
numConstraintsToGenerate = num-1;
else
numConstraintsToGenerate = num;
MatrixXd bc(b.size(),numConstraintsToGenerate*3);
for (unsigned i=0; i<b.size(); ++i)
{
VectorXd t = random_constraints(B1.row(b(i)),B2.row(b(i)),numConstraintsToGenerate);
bc.row(i) = t;
}
VectorXi rootsIndex(num);
for (int i =0; i<numConstraintsToGenerate; ++i)
rootsIndex[i] = i+1;
if (num>=5)
rootsIndex[num-2] = -2;
if (num>=3)
rootsIndex[num-1] = -1;
// Interpolated PolyVector field
Eigen::MatrixXd pvf;
igl::n_polyvector_general(V, F, b, bc, rootsIndex, pvf);
ofstream ofs;
ofs.open("pvf.txt", ofstream::out);
ofs<<pvf;
ofs.close();
igl::writeOFF("pvf.off",V,F);
// Highlight in red the constrained faces
MatrixXd C = MatrixXd::Constant(F.rows(),3,1);
for (unsigned i=0; i<b.size();++i)
C.row(b(i)) << 1, 0, 0;
viewer.data.set_colors(C);
for (int n=0; n<num; ++n)
{
// const MatrixXd &VF = pvf.block(0,n*3,F.rows(),3);
MatrixXd VF = MatrixXd::Zero(F.rows(),3);
for (unsigned i=0; i<b.size(); ++i)
VF.row(b[i]) = pvf.block(b[i],n*3,1,3);
for (int i=0; i<samples.rows(); ++i)
VF.row(samples[i]) = pvf.block(samples[i],n*3,1,3);
VectorXd c = VF.rowwise().norm();
MatrixXd C2;
igl::jet(c,1,1+rand_factor,C2);
// viewer.data.add_edges(B - global_scale*VF, B + global_scale*VF , C2);
viewer.data.add_edges(B, B + global_scale*VF , C2);
}
return false;
}
/**
* @function smoothPath
*/
void PathPlanner::smoothPath( int _robotId,
const Eigen::VectorXi &_links,
std::list<Eigen::VectorXd> &_path ) {
// =========== YOUR CODE HERE ==================
// HINT: Use whatever technique you like better, first try to shorten a path and then you can try to make it smoother
std::cout << "Starting path shortening with simple search..." << _links.size() << std::endl;
std::cout << "start path length: " << _path.size() << std::endl;
std::list<Eigen::VectorXd>::iterator start_point=_path.begin(),end_point=_path.end();
end_point--;
// loop while start has not reached the end of the path
while (start_point != _path.end())
{
if (start_point == end_point)
{
//std::cout << "End iteration\n";
++start_point;
end_point = _path.end();
end_point--;
}
else
{
//Eigen::VectorXd start_node=*start_point, end_node=*end_point;
std::list<Eigen::VectorXd> segment = createPath(_robotId,_links,*start_point, *end_point);
double curDist = countDist(start_point, end_point) * stepSize;
double shortcutDist = segment.size() * stepSize;
if (segment.size()>0 && shortcutDist < curDist)
{
std::cout << "Shortcut length: " << shortcutDist << std::endl;
std::cout << "Current distance: " << curDist << std::endl;
std::cout << "Found shortcut!" << std::endl;
// reconstruct path
// first segment
std::list<Eigen::VectorXd> new_path(_path.begin(), start_point);
// middle segment
new_path.insert(new_path.end(), segment.begin(), segment.end());
// last segment
new_path.insert(new_path.end(), end_point, _path.end());
std::cout << "New path length: " << new_path.size() << std::endl;
// replace optimized
_path = new_path;
start_point = _path.begin();
end_point = _path.end();
end_point--;
}
else
{
--end_point;
}
}
}
std::cout << "Finished Optimizing! Final path length: " << _path.size() << std::endl;
return;
return;
// ========================================
}
IGL_INLINE bool igl::decimate(
const Eigen::MatrixXd & OV,
const Eigen::MatrixXi & OF,
const std::function<void(
const int,
const Eigen::MatrixXd &,
const Eigen::MatrixXi &,
const Eigen::MatrixXi &,
const Eigen::VectorXi &,
const Eigen::MatrixXi &,
const Eigen::MatrixXi &,
double &,
Eigen::RowVectorXd &)> & cost_and_placement,
const std::function<bool(
const Eigen::MatrixXd &,
const Eigen::MatrixXi &,
const Eigen::MatrixXi &,
const Eigen::VectorXi &,
const Eigen::MatrixXi &,
const Eigen::MatrixXi &,
const std::set<std::pair<double,int> > &,
const std::vector<std::set<std::pair<double,int> >::iterator > &,
const Eigen::MatrixXd &,
const int,
const int,
const int,
const int,
const int)> & stopping_condition,
const std::function<bool(
const Eigen::MatrixXd & ,/*V*/
const Eigen::MatrixXi & ,/*F*/
const Eigen::MatrixXi & ,/*E*/
const Eigen::VectorXi & ,/*EMAP*/
const Eigen::MatrixXi & ,/*EF*/
const Eigen::MatrixXi & ,/*EI*/
const std::set<std::pair<double,int> > & ,/*Q*/
const std::vector<std::set<std::pair<double,int> >::iterator > &,/*Qit*/
const Eigen::MatrixXd & ,/*C*/
const int /*e*/
)> & pre_collapse,
const std::function<void(
const Eigen::MatrixXd & , /*V*/
const Eigen::MatrixXi & , /*F*/
const Eigen::MatrixXi & , /*E*/
const Eigen::VectorXi & ,/*EMAP*/
const Eigen::MatrixXi & , /*EF*/
const Eigen::MatrixXi & , /*EI*/
const std::set<std::pair<double,int> > & , /*Q*/
const std::vector<std::set<std::pair<double,int> >::iterator > &, /*Qit*/
const Eigen::MatrixXd & , /*C*/
const int , /*e*/
const int , /*e1*/
const int , /*e2*/
const int , /*f1*/
const int , /*f2*/
const bool /*collapsed*/
)> & post_collapse,
const Eigen::MatrixXi & OE,
const Eigen::VectorXi & OEMAP,
const Eigen::MatrixXi & OEF,
const Eigen::MatrixXi & OEI,
Eigen::MatrixXd & U,
Eigen::MatrixXi & G,
Eigen::VectorXi & J,
Eigen::VectorXi & I
)
{
// Decimate 1
using namespace Eigen;
using namespace std;
// Working copies
Eigen::MatrixXd V = OV;
Eigen::MatrixXi F = OF;
Eigen::MatrixXi E = OE;
Eigen::VectorXi EMAP = OEMAP;
Eigen::MatrixXi EF = OEF;
Eigen::MatrixXi EI = OEI;
typedef std::set<std::pair<double,int> > PriorityQueue;
PriorityQueue Q;
std::vector<PriorityQueue::iterator > Qit;
Qit.resize(E.rows());
// If an edge were collapsed, we'd collapse it to these points:
MatrixXd C(E.rows(),V.cols());
for(int e = 0;e<E.rows();e++)
{
double cost = e;
RowVectorXd p(1,3);
cost_and_placement(e,V,F,E,EMAP,EF,EI,cost,p);
C.row(e) = p;
Qit[e] = Q.insert(std::pair<double,int>(cost,e)).first;
}
int prev_e = -1;
bool clean_finish = false;
while(true)
{
if(Q.empty())
{
break;
}
if(Q.begin()->first == std::numeric_limits<double>::infinity())
{
// min cost edge is infinite cost
break;
}
int e,e1,e2,f1,f2;
if(collapse_edge(
cost_and_placement, pre_collapse, post_collapse,
V,F,E,EMAP,EF,EI,Q,Qit,C,e,e1,e2,f1,f2))
{
if(stopping_condition(V,F,E,EMAP,EF,EI,Q,Qit,C,e,e1,e2,f1,f2))
{
clean_finish = true;
break;
}
}else
{
if(prev_e == e)
{
assert(false && "Edge collapse no progress... bad stopping condition?");
break;
}
// Edge was not collapsed... must have been invalid. collapse_edge should
// have updated its cost to inf... continue
}
prev_e = e;
}
// remove all IGL_COLLAPSE_EDGE_NULL faces
MatrixXi F2(F.rows(),3);
J.resize(F.rows());
int m = 0;
for(int f = 0;f<F.rows();f++)
{
if(
F(f,0) != IGL_COLLAPSE_EDGE_NULL ||
F(f,1) != IGL_COLLAPSE_EDGE_NULL ||
F(f,2) != IGL_COLLAPSE_EDGE_NULL)
{
F2.row(m) = F.row(f);
J(m) = f;
m++;
}
}
F2.conservativeResize(m,F2.cols());
J.conservativeResize(m);
VectorXi _1;
remove_unreferenced(V,F2,U,G,_1,I);
return clean_finish;
}
int main(int argc, char *argv[])
{
using namespace std;
// Load a mesh in OFF format
igl::readOBJ(TUTORIAL_SHARED_PATH "/camel_b.obj", V, F);
Eigen::MatrixXd bnd_uv, uv_init;
Eigen::VectorXd M;
igl::doublearea(V, F, M);
std::vector<std::vector<int>> all_bnds;
igl::boundary_loop(F, all_bnds);
// Heuristic primary boundary choice: longest
auto primary_bnd = std::max_element(all_bnds.begin(), all_bnds.end(), [](const std::vector<int> &a, const std::vector<int> &b) { return a.size()<b.size(); });
Eigen::VectorXi bnd = Eigen::Map<Eigen::VectorXi>(primary_bnd->data(), primary_bnd->size());
igl::map_vertices_to_circle(V, bnd, bnd_uv);
bnd_uv *= sqrt(M.sum() / (2 * igl::PI));
if (all_bnds.size() == 1)
{
if (bnd.rows() == V.rows()) // case: all vertex on boundary
{
uv_init.resize(V.rows(), 2);
for (int i = 0; i < bnd.rows(); i++)
uv_init.row(bnd(i)) = bnd_uv.row(i);
}
else
{
igl::harmonic(V, F, bnd, bnd_uv, 1, uv_init);
if (igl::flipped_triangles(uv_init, F).size() != 0)
igl::harmonic(F, bnd, bnd_uv, 1, uv_init); // fallback uniform laplacian
}
}
else
{
// if there is a hole, fill it and erase additional vertices.
all_bnds.erase(primary_bnd);
Eigen::MatrixXi F_filled;
igl::topological_hole_fill(F, bnd, all_bnds, F_filled);
igl::harmonic(F_filled, bnd, bnd_uv ,1, uv_init);
uv_init = uv_init.topRows(V.rows());
}
Eigen::VectorXi b; Eigen::MatrixXd bc;
igl::scaf_precompute(V, F, uv_init, scaf_data, igl::MappingEnergyType::SYMMETRIC_DIRICHLET, b, bc, 0);
// Plot the mesh
igl::opengl::glfw::Viewer viewer;
viewer.data().set_mesh(V, F);
const auto& V_uv = uv_scale * scaf_data.w_uv.topRows(V.rows());
viewer.data().set_uv(V_uv);
viewer.callback_key_down = &key_down;
// Enable wireframe
viewer.data().show_lines = true;
// Draw checkerboard texture
viewer.data().show_texture = true;
std::cerr << "Press space for running an iteration." << std::endl;
std::cerr << "Press 1 for Mesh 2 for UV" << std::endl;
// Launch the viewer
viewer.launch();
}
IGL_INLINE bool igl::boundary_conditions(
const Eigen::MatrixXd & V ,
const Eigen::MatrixXi & /*Ele*/,
const Eigen::MatrixXd & C ,
const Eigen::VectorXi & P ,
const Eigen::MatrixXi & BE ,
const Eigen::MatrixXi & CE ,
Eigen::VectorXi & b ,
Eigen::MatrixXd & bc )
{
using namespace Eigen;
using namespace std;
if(P.size()+BE.rows() == 0)
{
verbose("^%s: Error: no handles found\n",__FUNCTION__);
return false;
}
vector<int> bci;
vector<int> bcj;
vector<double> bcv;
// loop over points
for(int p = 0;p<P.size();p++)
{
VectorXd pos = C.row(P(p));
// loop over domain vertices
for(int i = 0;i<V.rows();i++)
{
// Find samples just on pos
//Vec3 vi(V(i,0),V(i,1),V(i,2));
// EIGEN GOTCHA:
// double sqrd = (V.row(i)-pos).array().pow(2).sum();
// Must first store in temporary
VectorXd vi = V.row(i);
double sqrd = (vi-pos).squaredNorm();
if(sqrd <= FLOAT_EPS)
{
//cout<<"sum((["<<
// V(i,0)<<" "<<
// V(i,1)<<" "<<
// V(i,2)<<"] - ["<<
// pos(0)<<" "<<
// pos(1)<<" "<<
// pos(2)<<"]).^2) = "<<sqrd<<endl;
bci.push_back(i);
bcj.push_back(p);
bcv.push_back(1.0);
}
}
}
// loop over bone edges
for(int e = 0;e<BE.rows();e++)
{
// loop over domain vertices
for(int i = 0;i<V.rows();i++)
{
// Find samples from tip up to tail
VectorXd tip = C.row(BE(e,0));
VectorXd tail = C.row(BE(e,1));
// Compute parameter along bone and squared distance
double t,sqrd;
project_to_line(
V(i,0),V(i,1),V(i,2),
tip(0),tip(1),tip(2),
tail(0),tail(1),tail(2),
t,sqrd);
if(t>=-FLOAT_EPS && t<=(1.0f+FLOAT_EPS) && sqrd<=FLOAT_EPS)
{
bci.push_back(i);
bcj.push_back(P.size()+e);
bcv.push_back(1.0);
}
}
}
// loop over cage edges
for(int e = 0;e<CE.rows();e++)
{
// loop over domain vertices
for(int i = 0;i<V.rows();i++)
{
// Find samples from tip up to tail
VectorXd tip = C.row(P(CE(e,0)));
VectorXd tail = C.row(P(CE(e,1)));
// Compute parameter along bone and squared distance
double t,sqrd;
project_to_line(
V(i,0),V(i,1),V(i,2),
tip(0),tip(1),tip(2),
tail(0),tail(1),tail(2),
t,sqrd);
if(t>=-FLOAT_EPS && t<=(1.0f+FLOAT_EPS) && sqrd<=FLOAT_EPS)
{
bci.push_back(i);
bcj.push_back(CE(e,0));
bcv.push_back(1.0-t);
bci.push_back(i);
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 && "Some boundary vertex getting all zero BCs");
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 pre_draw(igl::viewer::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;
}
bool key_down(igl::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;
}
void calculateOverlappingFOV(const cameras::CameraGeometryBase & camera1,
const cameras::CameraGeometryBase & camera2,
const sm::kinematics::Transformation& T_cam1_cam2,
cameras::ImageMask& outMask1,
cameras::ImageMask& outMask2,
double& outOverlappingRatio1,
double& outOverlappingRatio2,
const Eigen::VectorXi& sampleDistances,
double scale) {
// get dimensions
int width1 = (int) camera1.width() * scale;
int height1 = (int) camera1.height() * scale;
int width2 = (int) camera2.width() * scale;
int height2 = (int) camera2.height() * scale;
// clear incoming masks
cv::Mat outMask1CV = outMask1.getMask();
cv::Mat outMask2CV = outMask2.getMask();
outMask1CV = cv::Mat::zeros(height1, width1, CV_8UC1);
outMask2CV = cv::Mat::zeros(height2, width2, CV_8UC1);
outMask1.setScale(scale);
outMask2.setScale(scale);
Eigen::VectorXd point;
// build outMask1
outOverlappingRatio1 = 0;
for (int x = 0; x < width1; x++) {
for (int y = 0; y < height1; y++) {
// if visible
// TODO: BB: what about rounding mistakes?
Eigen::VectorXd kp(2);
kp << (double) x / scale, (double) y / scale;
if (camera1.vsKeypointToHomogeneous(kp, point)) {
double norm = point.norm();
int i = 0;
// check points on the beam until one is found or no other points to check
while (i < sampleDistances.size()
&& outMask1CV.at<unsigned char>(y, x) == 0) {
// Send point to distance sampleDistances(i) on the beam
Eigen::Vector4d tempPoint = point;
tempPoint(3) = norm / (norm + sampleDistances(i));
// Project point to other camera frame
tempPoint = T_cam1_cam2.inverse() * tempPoint;
Eigen::VectorXd keypointLocation;
if (camera2.vsHomogeneousToKeypoint(tempPoint, keypointLocation)) {
outMask1CV.at<unsigned char>(y, x) = 255;
outOverlappingRatio1++;
} // if
i++;
} // while
} // if
} // for
} // for
outOverlappingRatio1 = outOverlappingRatio1 / (width1 * height1);
// build outMask2
outOverlappingRatio2 = 0;
for (int x = 0; x < width2; x++) {
for (int y = 0; y < height2; y++) {
Eigen::VectorXd kp(2);
kp << (double) x / scale, (double) y / scale;
// if visible
// TODO: BB: what about rounding mistakes?
if (camera2.vsKeypointToHomogeneous(kp, point)) {
double norm = point.norm();
int i = 0;
// check points on the beam until one is found or no other points to check
while (i < sampleDistances.size()
&& outMask2CV.at<unsigned char>(y, x) == 0) {
// Send point to distance sampleDistances(i) on the beam
Eigen::Vector4d tempPoint = point;
tempPoint(3) = norm / (norm + sampleDistances(i));
// Project point to other camera frame
tempPoint = T_cam1_cam2 * tempPoint;
Eigen::VectorXd keypointLocation;
if (camera1.vsHomogeneousToKeypoint(tempPoint, keypointLocation)) {
outMask2CV.at<unsigned char>(y, x) = 255;
outOverlappingRatio2++;
} // if
i++;
} // while
} // if
} // for
} // for
outOverlappingRatio2 = outOverlappingRatio2 / (width2 * height2);
}
int main(int argc, char *argv[])
{
using namespace Eigen;
// Load a mesh in OBJ format
igl::readOBJ(TUTORIAL_SHARED_PATH "/bumpy-cube.obj", V, F);
// Compute face barycenters
igl::barycenter(V, F, B);
// Compute scale for visualizing fields
global_scale = .2*igl::avg_edge_length(V, F);
// Load constraints
MatrixXd temp;
igl::readDMAT(TUTORIAL_SHARED_PATH "/bumpy-cube.dmat",temp);
b = temp.block(0,0,temp.rows(),1).cast<int>();
bc1 = temp.block(0,1,temp.rows(),3);
bc2 = temp.block(0,4,temp.rows(),3);
// Interpolate the frame field
igl::comiso::frame_field(V, F, b, bc1, bc2, FF1, FF2);
// Deform the mesh to transform the frame field in a cross field
igl::frame_field_deformer(
V,F,FF1,FF2,V_deformed,FF1_deformed,FF2_deformed);
// Compute face barycenters deformed mesh
igl::barycenter(V_deformed, F, B_deformed);
// Find the closest crossfield to the deformed frame field
igl::frame_to_cross_field(V_deformed,F,FF1_deformed,FF2_deformed,X1_deformed);
// Find a smooth crossfield that interpolates the deformed constraints
MatrixXd bc_x(b.size(),3);
for (unsigned i=0; i<b.size();++i)
bc_x.row(i) = X1_deformed.row(b(i));
VectorXd S;
igl::comiso::nrosy(
V,
F,
b,
bc_x,
VectorXi(),
VectorXd(),
MatrixXd(),
4,
0.5,
X1_deformed,
S);
// The other representative of the cross field is simply rotated by 90 degrees
MatrixXd B1,B2,B3;
igl::local_basis(V_deformed,F,B1,B2,B3);
X2_deformed =
igl::rotate_vectors(X1_deformed, VectorXd::Constant(1,M_PI/2), B1, B2);
// Global seamless parametrization
igl::comiso::miq(V_deformed,
F,
X1_deformed,
X2_deformed,
V_uv,
F_uv,
60.0,
5.0,
false,
2);
igl::viewer::Viewer viewer;
// Plot the original mesh with a texture parametrization
key_down(viewer,'6',0);
// Launch the viewer
viewer.callback_key_down = &key_down;
viewer.launch();
}
bool init_arap()
{
using namespace igl;
using namespace Eigen;
using namespace std;
VectorXi b(num_in_selection(S));
assert(S.rows() == V.rows());
C.resize(S.rows(),3);
MatrixXd bc = MatrixXd::Zero(b.size(),S.maxCoeff()+1);
MatrixXi * Ele;
if(T.rows()>0)
{
Ele = &T;
}else
{
Ele = &F;
}
// get b from S
{
int bi = 0;
for(int v = 0;v<S.rows(); v++)
{
if(S(v) >= 0)
{
b(bi) = v;
bc(bi,S(v)) = 1;
bi++;
switch(S(v))
{
case 0:
C.row(v) = RowVector3d(0.039,0.31,1);
break;
case 1:
C.row(v) = RowVector3d(1,0.41,0.70);
break;
default:
C.row(v) = RowVector3d(0.4,0.8,0.3);
break;
}
}else
{
C.row(v) = RowVector3d(
GOLD_DIFFUSE[0],
GOLD_DIFFUSE[1],
GOLD_DIFFUSE[2]);
}
}
}
// Store current mesh
U = V;
VectorXi _S;
VectorXd _D;
MatrixXd W;
if(!harmonic(V,*Ele,b,bc,1,W))
{
return false;
}
arap_data.with_dynamics = true;
arap_data.h = 0.05;
//arap_data.max_iter = 100;
//partition(W,100,arap_data.G,_S,_D);
return arap_precomputation(V,*Ele,V.cols(),b,arap_data);
}
IGL_INLINE void igl::slice(
const Eigen::SparseMatrix<TX>& X,
const Eigen::Matrix<int,Eigen::Dynamic,1> & R,
const Eigen::Matrix<int,Eigen::Dynamic,1> & C,
Eigen::SparseMatrix<TY>& Y)
{
#if 1
int xm = X.rows();
int xn = X.cols();
int ym = R.size();
int yn = C.size();
// special case when R or C is empty
if(ym == 0 || yn == 0)
{
Y.resize(ym,yn);
return;
}
assert(R.minCoeff() >= 0);
assert(R.maxCoeff() < xm);
assert(C.minCoeff() >= 0);
assert(C.maxCoeff() < xn);
// Build reindexing maps for columns and rows, -1 means not in map
std::vector<std::vector<int> > RI;
RI.resize(xm);
for(int i = 0;i<ym;i++)
{
RI[R(i)].push_back(i);
}
std::vector<std::vector<int> > CI;
CI.resize(xn);
// initialize to -1
for(int i = 0;i<yn;i++)
{
CI[C(i)].push_back(i);
}
// Resize output
Eigen::DynamicSparseMatrix<TY, Eigen::RowMajor> dyn_Y(ym,yn);
// Take a guess at the number of nonzeros (this assumes uniform distribution
// not banded or heavily diagonal)
dyn_Y.reserve((X.nonZeros()/(X.rows()*X.cols())) * (ym*yn));
// Iterate over outside
for(int k=0; k<X.outerSize(); ++k)
{
// Iterate over inside
for(typename Eigen::SparseMatrix<TX>::InnerIterator it (X,k); it; ++it)
{
std::vector<int>::iterator rit, cit;
for(rit = RI[it.row()].begin();rit != RI[it.row()].end(); rit++)
{
for(cit = CI[it.col()].begin();cit != CI[it.col()].end(); cit++)
{
dyn_Y.coeffRef(*rit,*cit) = it.value();
}
}
}
}
Y = Eigen::SparseMatrix<TY>(dyn_Y);
#else
// Alec: This is _not_ valid for arbitrary R,C since they don't necessary
// representation a strict permutation of the rows and columns: rows or
// columns could be removed or replicated. The removal of rows seems to be
// handled here (although it's not clear if there is a performance gain when
// the #removals >> #remains). If this is sufficiently faster than the
// correct code above, one could test whether all entries in R and C are
// unique and apply the permutation version if appropriate.
//
int xm = X.rows();
int xn = X.cols();
int ym = R.size();
int yn = C.size();
// special case when R or C is empty
if(ym == 0 || yn == 0)
{
Y.resize(ym,yn);
return;
}
assert(R.minCoeff() >= 0);
assert(R.maxCoeff() < xm);
assert(C.minCoeff() >= 0);
assert(C.maxCoeff() < xn);
// initialize row and col permutation vectors
Eigen::VectorXi rowIndexVec = Eigen::VectorXi::LinSpaced(xm,0,xm-1);
Eigen::VectorXi rowPermVec = Eigen::VectorXi::LinSpaced(xm,0,xm-1);
for(int i=0;i<ym;i++)
{
int pos = rowIndexVec.coeffRef(R(i));
if(pos != i)
{
int& val = rowPermVec.coeffRef(i);
std::swap(rowIndexVec.coeffRef(val),rowIndexVec.coeffRef(R(i)));
std::swap(rowPermVec.coeffRef(i),rowPermVec.coeffRef(pos));
}
}
Eigen::PermutationMatrix<Eigen::Dynamic,Eigen::Dynamic,int> rowPerm(rowIndexVec);
Eigen::VectorXi colIndexVec = Eigen::VectorXi::LinSpaced(xn,0,xn-1);
Eigen::VectorXi colPermVec = Eigen::VectorXi::LinSpaced(xn,0,xn-1);
for(int i=0;i<yn;i++)
{
int pos = colIndexVec.coeffRef(C(i));
if(pos != i)
{
int& val = colPermVec.coeffRef(i);
std::swap(colIndexVec.coeffRef(val),colIndexVec.coeffRef(C(i)));
std::swap(colPermVec.coeffRef(i),colPermVec.coeffRef(pos));
}
}
Eigen::PermutationMatrix<Eigen::Dynamic,Eigen::Dynamic,int> colPerm(colPermVec);
Eigen::SparseMatrix<T> M = (rowPerm * X);
Y = (M * colPerm).block(0,0,ym,yn);
#endif
}
IGL_INLINE void igl::copyleft::cgal::propagate_winding_numbers(
const Eigen::PlainObjectBase<DerivedV>& V,
const Eigen::PlainObjectBase<DerivedF>& F,
const Eigen::PlainObjectBase<DerivedL>& labels,
Eigen::PlainObjectBase<DerivedW>& W) {
#ifdef PROPAGATE_WINDING_NUMBER_TIMING
const auto & tictoc = []()
{
static double t_start = igl::get_seconds();
double diff = igl::get_seconds()-t_start;
t_start += diff;
return diff;
};
tictoc();
#endif
const size_t num_faces = F.rows();
//typedef typename DerivedF::Scalar Index;
Eigen::MatrixXi E, uE;
Eigen::VectorXi EMAP;
std::vector<std::vector<size_t> > uE2E;
igl::unique_edge_map(F, E, uE, EMAP, uE2E);
if (!propagate_winding_numbers_helper::is_orientable(F, uE, uE2E)) {
std::cerr << "Input mesh is not orientable!" << std::endl;
}
Eigen::VectorXi P;
const size_t num_patches = igl::extract_manifold_patches(F, EMAP, uE2E, P);
#ifdef PROPAGATE_WINDING_NUMBER_TIMING
std::cout << "extract manifold patches: " << tictoc() << std::endl;
#endif
DerivedW per_patch_cells;
const size_t num_cells =
igl::copyleft::cgal::extract_cells(
V, F, P, E, uE, uE2E, EMAP, per_patch_cells);
#ifdef PROPAGATE_WINDING_NUMBER_TIMING
std::cout << "extract cells: " << tictoc() << std::endl;;
#endif
typedef std::tuple<size_t, bool, size_t> CellConnection;
std::vector<std::set<CellConnection> > cell_adjacency(num_cells);
for (size_t i=0; i<num_patches; i++) {
const int positive_cell = per_patch_cells(i,0);
const int negative_cell = per_patch_cells(i,1);
cell_adjacency[positive_cell].emplace(negative_cell, false, i);
cell_adjacency[negative_cell].emplace(positive_cell, true, i);
}
#ifdef PROPAGATE_WINDING_NUMBER_TIMING
std::cout << "cell connection: " << tictoc() << std::endl;
#endif
auto save_cell = [&](const std::string& filename, size_t cell_id) {
std::vector<size_t> faces;
for (size_t i=0; i<num_patches; i++) {
if ((per_patch_cells.row(i).array() == cell_id).any()) {
for (size_t j=0; j<num_faces; j++) {
if ((size_t)P[j] == i) {
faces.push_back(j);
}
}
}
}
Eigen::MatrixXi cell_faces(faces.size(), 3);
for (size_t i=0; i<faces.size(); i++) {
cell_faces.row(i) = F.row(faces[i]);
}
Eigen::MatrixXd vertices(V.rows(), 3);
for (size_t i=0; i<(size_t)V.rows(); i++) {
assign_scalar(V(i,0), vertices(i,0));
assign_scalar(V(i,1), vertices(i,1));
assign_scalar(V(i,2), vertices(i,2));
}
writePLY(filename, vertices, cell_faces);
};
#ifndef NDEBUG
{
// Check for odd cycle.
Eigen::VectorXi cell_labels(num_cells);
cell_labels.setZero();
Eigen::VectorXi parents(num_cells);
parents.setConstant(-1);
auto trace_parents = [&](size_t idx) {
std::list<size_t> path;
path.push_back(idx);
while ((size_t)parents[path.back()] != path.back()) {
path.push_back(parents[path.back()]);
}
return path;
};
for (size_t i=0; i<num_cells; i++) {
if (cell_labels[i] == 0) {
cell_labels[i] = 1;
std::queue<size_t> Q;
Q.push(i);
parents[i] = i;
while (!Q.empty()) {
size_t curr_idx = Q.front();
Q.pop();
int curr_label = cell_labels[curr_idx];
for (const auto& neighbor : cell_adjacency[curr_idx]) {
if (cell_labels[std::get<0>(neighbor)] == 0) {
cell_labels[std::get<0>(neighbor)] = curr_label * -1;
Q.push(std::get<0>(neighbor));
parents[std::get<0>(neighbor)] = curr_idx;
} else {
if (cell_labels[std::get<0>(neighbor)] !=
curr_label * -1) {
std::cerr << "Odd cell cycle detected!" << std::endl;
auto path = trace_parents(curr_idx);
path.reverse();
auto path2 = trace_parents(std::get<0>(neighbor));
path.insert(path.end(),
path2.begin(), path2.end());
for (auto cell_id : path) {
std::cout << cell_id << " ";
std::stringstream filename;
filename << "cell_" << cell_id << ".ply";
save_cell(filename.str(), cell_id);
}
std::cout << std::endl;
}
// Do not fail when odd cycle is detected because the resulting
// integer winding number field, although inconsistent, may still
// be used if the problem region is local and embedded within a
// valid volume.
//assert(cell_labels[std::get<0>(neighbor)] == curr_label * -1);
}
}
}
}
}
#ifdef PROPAGATE_WINDING_NUMBER_TIMING
std::cout << "check for odd cycle: " << tictoc() << std::endl;
#endif
}
#endif
size_t outer_facet;
bool flipped;
Eigen::VectorXi I;
I.setLinSpaced(num_faces, 0, num_faces-1);
igl::copyleft::cgal::outer_facet(V, F, I, outer_facet, flipped);
#ifdef PROPAGATE_WINDING_NUMBER_TIMING
std::cout << "outer facet: " << tictoc() << std::endl;
#endif
const size_t outer_patch = P[outer_facet];
const size_t infinity_cell = per_patch_cells(outer_patch, flipped?1:0);
Eigen::VectorXi patch_labels(num_patches);
const int INVALID = std::numeric_limits<int>::max();
patch_labels.setConstant(INVALID);
for (size_t i=0; i<num_faces; i++) {
if (patch_labels[P[i]] == INVALID) {
patch_labels[P[i]] = labels[i];
} else {
assert(patch_labels[P[i]] == labels[i]);
}
}
assert((patch_labels.array() != INVALID).all());
const size_t num_labels = patch_labels.maxCoeff()+1;
Eigen::MatrixXi per_cell_W(num_cells, num_labels);
per_cell_W.setConstant(INVALID);
per_cell_W.row(infinity_cell).setZero();
std::queue<size_t> Q;
Q.push(infinity_cell);
while (!Q.empty()) {
size_t curr_cell = Q.front();
Q.pop();
for (const auto& neighbor : cell_adjacency[curr_cell]) {
size_t neighbor_cell, patch_idx;
bool direction;
std::tie(neighbor_cell, direction, patch_idx) = neighbor;
if ((per_cell_W.row(neighbor_cell).array() == INVALID).any()) {
per_cell_W.row(neighbor_cell) = per_cell_W.row(curr_cell);
for (size_t i=0; i<num_labels; i++) {
int inc = (patch_labels[patch_idx] == (int)i) ?
(direction ? -1:1) :0;
per_cell_W(neighbor_cell, i) =
per_cell_W(curr_cell, i) + inc;
}
Q.push(neighbor_cell);
} else {
#ifndef NDEBUG
// Checking for winding number consistency.
// This check would inevitably fail for meshes that contain open
// boundary or non-orientable. However, the inconsistent winding number
// field would still be useful in some cases such as when problem region
// is local and embedded within the volume. This, unfortunately, is the
// best we can do because the problem of computing integer winding
// number is ill-defined for open and non-orientable surfaces.
for (size_t i=0; i<num_labels; i++) {
if ((int)i == patch_labels[patch_idx]) {
int inc = direction ? -1:1;
//assert(per_cell_W(neighbor_cell, i) ==
// per_cell_W(curr_cell, i) + inc);
} else {
//assert(per_cell_W(neighbor_cell, i) ==
// per_cell_W(curr_cell, i));
}
}
#endif
}
}
}
#ifdef PROPAGATE_WINDING_NUMBER_TIMING
std::cout << "propagate winding number: " << tictoc() << std::endl;
#endif
W.resize(num_faces, num_labels*2);
for (size_t i=0; i<num_faces; i++) {
const size_t patch = P[i];
const size_t positive_cell = per_patch_cells(patch, 0);
const size_t negative_cell = per_patch_cells(patch, 1);
for (size_t j=0; j<num_labels; j++) {
W(i,j*2 ) = per_cell_W(positive_cell, j);
W(i,j*2+1) = per_cell_W(negative_cell, j);
}
}
#ifdef PROPAGATE_WINDING_NUMBER_TIMING
std::cout << "save result: " << tictoc() << std::endl;
#endif
}
IGL_INLINE void igl::GeneralPolyVectorFieldFinder<DerivedV, DerivedF>::
minQuadWithKnownMini(const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &Q,
const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &f,
const Eigen::VectorXi isConstrained,
const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &xknown,
Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &x)
{
int N = Q.rows();
int nc = xknown.rows();
Eigen::VectorXi known; known.setZero(nc,1);
Eigen::VectorXi unknown; unknown.setZero(N-nc,1);
int indk = 0, indu = 0;
for (int i = 0; i<N; ++i)
if (isConstrained[i])
{
known[indk] = i;
indk++;
}
else
{
unknown[indu] = i;
indu++;
}
Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar>> Quu, Quk;
igl::slice(Q,unknown, unknown, Quu);
igl::slice(Q,unknown, known, Quk);
std::vector<typename Eigen::Triplet<std::complex<typename DerivedV::Scalar> > > tripletList;
Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > fu(N-nc,1);
igl::slice(f,unknown, Eigen::VectorXi::Zero(1,1), fu);
Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > rhs = (Quk*xknown).sparseView()+.5*fu;
Eigen::SparseLU< Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar>>> solver;
solver.compute(-Quu);
if(solver.info()!=Eigen::Success)
{
std::cerr<<"Decomposition failed!"<<std::endl;
return;
}
Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar>> b = solver.solve(rhs);
if(solver.info()!=Eigen::Success)
{
std::cerr<<"Solving failed!"<<std::endl;
return;
}
indk = 0, indu = 0;
x.setZero(N,1);
for (int i = 0; i<N; ++i)
if (isConstrained[i])
x[i] = xknown[indk++];
else
x[i] = b.coeff(indu++,0);
}
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;
}
