void LegOdoCommon::getCovariance(LegOdoCommonMode mode_current, bool delta_certain,
Eigen::MatrixXd &cov_legodo, Eigen::VectorXi &z_indices){
// Determine which velocity variance to use
double R_legodo_vxyz_current = R_legodo_vxyz_;
double R_legodo_vang_current = R_legodo_vang_;
if (!delta_certain){
R_legodo_vxyz_current = R_legodo_vxyz_uncertain_;
R_legodo_vang_current = R_legodo_vang_uncertain_;
}
Eigen::VectorXd R_legodo;
if (mode_current == MODE_LIN_AND_ROT_RATE) {
z_indices.resize(6);
R_legodo.resize(6);
}else if (mode_current == MODE_LIN_RATE){
z_indices.resize(3);
R_legodo.resize(3);
}else if (mode_current == MODE_POSITION_AND_LIN_RATE){
z_indices.resize(6);
R_legodo.resize(6);
}
// Initialize covariance matrix based on mode.
if (mode_current == MODE_LIN_AND_ROT_RATE) {
R_legodo(0) = bot_sq(R_legodo_vxyz_current);
R_legodo(1) = bot_sq(R_legodo_vxyz_current);
R_legodo(2) = bot_sq(R_legodo_vxyz_current);
R_legodo(3) = bot_sq(R_legodo_vang_current);
R_legodo(4) = bot_sq(R_legodo_vang_current);
R_legodo(5) = bot_sq(R_legodo_vang_current);
z_indices.head<3>() = eigen_utils::RigidBodyState::velocityInds();
z_indices.tail<3>() = eigen_utils::RigidBodyState::angularVelocityInds();
}else if (mode_current == MODE_LIN_RATE){
R_legodo(0) = bot_sq(R_legodo_vxyz_current);
R_legodo(1) = bot_sq(R_legodo_vxyz_current);
R_legodo(2) = bot_sq(R_legodo_vxyz_current);
z_indices.head<3>() = eigen_utils::RigidBodyState::velocityInds();
}else if (mode_current == MODE_POSITION_AND_LIN_RATE){
R_legodo(0) = bot_sq(R_legodo_xyz_);
R_legodo(1) = bot_sq(R_legodo_xyz_);
R_legodo(2) = bot_sq(R_legodo_xyz_);
R_legodo(3) = bot_sq(R_legodo_vxyz_current);
R_legodo(4) = bot_sq(R_legodo_vxyz_current);
R_legodo(5) = bot_sq(R_legodo_vxyz_current);
z_indices.head<3>() = eigen_utils::RigidBodyState::positionInds();
z_indices.tail<3>() = eigen_utils::RigidBodyState::velocityInds();
}
cov_legodo = R_legodo.asDiagonal();
}
void FunctionApproximator::getParameterVectorMask(const std::set<std::string> selected_values_labels, Eigen::VectorXi& selected_mask) const {
if (checkModelParametersInitialized())
model_parameters_->getParameterVectorMask(selected_values_labels,selected_mask);
else
selected_mask.resize(0);
};
Eigen::VectorXi
readLabelFromFile(const std::string &file, int padding)
{
// check if file exists
boost::filesystem::path path = file;
if ( ! (boost::filesystem::exists ( path ) && boost::filesystem::is_regular_file(path)) )
throw std::runtime_error ("Given file path to read Matrix does not exist!");
std::ifstream in (file.c_str (), std::ifstream::in);
char linebuf[819200];
Eigen::VectorXi Vector;
in.getline (linebuf, 819200);
std::string line (linebuf);
std::vector < std::string > strs_2;
boost::split (strs_2, line, boost::is_any_of (" "));
Vector.resize(strs_2.size()-1);
for (int i = 0; i < strs_2.size()-1; i++)
Vector ( i) = static_cast<int> (atof (strs_2[i].c_str ()));
return Vector;
}
int main(int argc, char *argv[])
{
using namespace std;
using namespace Eigen;
// Load a mesh in OFF format
igl::readOFF("../shared/bumpy.off", V, F);
// Threshold faces with high anisotropy
b.resize(1);
b << 0;
bc.resize(1,3);
bc << 1,1,1;
igl::Viewer viewer;
// Interpolate the field and plot
key_down(viewer, '4', 0);
// Plot the mesh
viewer.data.set_mesh(V, F);
viewer.callback_key_down = &key_down;
// Disable wireframe
viewer.core.show_lines = false;
// Launch the viewer
viewer.launch();
}
void EigsGen::findMatchedIndex(const Eigen::VectorXcd &target,
const Eigen::VectorXcd &collection,
Eigen::VectorXi &result)
{
int nfound = 0;
int maxn = target.size();
if(result.size() < maxn)
result.resize(maxn);
for(int i = 0; i < collection.size(); i++)
{
int j;
for(j = 0; j < maxn; j++)
{
if(abs(collection[i] - target[j]) < 1e-8 * abs(target[j]))
break;
}
if(j < maxn)
{
result[nfound] = i;
nfound++;
if(collection[i].imag() != 0)
{
i++;
result[nfound] = i;
nfound++;
}
}
if(nfound >= maxn) break;
}
if(result.size() > nfound)
result.conservativeResize(nfound);
}
void parse_rhs(
const int nrhs,
const mxArray *prhs[],
Eigen::MatrixXd & V,
Eigen::MatrixXi & Ele,
Eigen::MatrixXd & Q,
Eigen::MatrixXd & bb_mins,
Eigen::MatrixXd & bb_maxs,
Eigen::VectorXi & elements)
{
using namespace std;
using namespace igl;
using namespace igl::matlab;
mexErrMsgTxt(nrhs >= 3, "The number of input arguments must be >=3.");
const int dim = mxGetN(prhs[0]);
mexErrMsgTxt(dim == 3 || dim == 2,
"Mesh vertex list must be #V by 2 or 3 list of vertex positions");
mexErrMsgTxt(dim+1 == mxGetN(prhs[1]),
"Mesh \"face\" simplex size must equal dimension+1");
parse_rhs_double(prhs,V);
parse_rhs_index(prhs+1,Ele);
parse_rhs_double(prhs+2,Q);
mexErrMsgTxt(Q.cols() == dim,"Dimension of Q should match V");
if(nrhs > 3)
{
mexErrMsgTxt(nrhs >= 6, "The number of input arguments must be 3 or >=6.");
parse_rhs_double(prhs+3,bb_mins);
if(bb_mins.size()>0)
{
mexErrMsgTxt(bb_mins.cols() == dim,"Dimension of bb_mins should match V");
mexErrMsgTxt(bb_mins.rows() >= Ele.rows(),"|bb_mins| should be > |Ele|");
}
parse_rhs_double(prhs+4,bb_maxs);
mexErrMsgTxt(bb_maxs.cols() == bb_mins.cols(),
"|bb_maxs| should match |bb_mins|");
mexErrMsgTxt(bb_mins.rows() == bb_maxs.rows(),
"|bb_mins| should match |bb_maxs|");
parse_rhs_index(prhs+5,elements);
mexErrMsgTxt(elements.cols() == 1,"Elements should be column vector");
mexErrMsgTxt(bb_mins.rows() == elements.rows(),
"|bb_mins| should match |elements|");
}else
{
// Defaults
bb_mins.resize(0,dim);
bb_maxs.resize(0,dim);
elements.resize(0,1);
}
}
template<typename T> Eigen::VectorXi vectorToIntEigenVector( T &array )
{
Eigen::VectorXi eigenVector;
size_t N_elements = array.size();
eigenVector.resize( N_elements );
for ( size_t i = 0; i < N_elements; i++ )
eigenVector( i ) = array[i];
return eigenVector;
}
IGL_INLINE void igl::signed_distance_pseudonormal(
const Eigen::MatrixXd & P,
const Eigen::MatrixXd & V,
const Eigen::MatrixXi & F,
const AABB<Eigen::MatrixXd,3> & tree,
const Eigen::MatrixXd & FN,
const Eigen::MatrixXd & VN,
const Eigen::MatrixXd & EN,
const Eigen::VectorXi & EMAP,
Eigen::VectorXd & S,
Eigen::VectorXi & I,
Eigen::MatrixXd & C,
Eigen::MatrixXd & N)
{
using namespace Eigen;
const size_t np = P.rows();
S.resize(np,1);
I.resize(np,1);
N.resize(np,3);
C.resize(np,3);
# pragma omp parallel for if(np>1000)
for(size_t p = 0;p<np;p++)
{
double s,sqrd;
RowVector3d n,c;
int i = -1;
RowVector3d q = P.row(p);
signed_distance_pseudonormal(tree,V,F,FN,VN,EN,EMAP,q,s,sqrd,i,c,n);
S(p) = s*sqrt(sqrd);
I(p) = i;
N.row(p) = n;
C.row(p) = c;
}
// igl::AABB<MatrixXd,3> tree_P;
// MatrixXi J = VectorXi::LinSpaced(P.rows(),0,P.rows()-1);
// tree_P.init(P,J);
// tree.squared_distance(V,F,tree_P,P,J,S,I,C);
//# pragma omp parallel for if(np>1000)
// for(size_t p = 0;p<np;p++)
// {
// RowVector3d c = C.row(p);
// RowVector3d q = P.row(p);
// const int f = I(p);
// double s;
// RowVector3d n;
// pseudonormal_test(V,F,FN,VN,EN,EMAP,q,f,c,s,n);
// N.row(p) = n;
// S(p) = s*sqrt(S(p));
// }
}
void readSamples(const std::string &fname, Eigen::VectorXi &samples)
{
int numSamples;
FILE *fp = fopen(fname.c_str(),"r");
if (fscanf(fp, "%d", &numSamples)!=1)
{
fclose(fp);
return;
}
samples.resize(numSamples,1);
int vali;
for (int i =0; i<numSamples; ++i)
{
if (fscanf(fp, "%d", &vali)!=1 || vali<0)
{
fclose(fp);
samples.resize(0,1);
return;
}
samples[i]=vali;
}
fclose(fp);
}
int main(int argc, char *argv[])
{
using namespace Eigen;
using namespace std;
MatrixXd V;
MatrixXi F;
igl::readOFF(TUTORIAL_SHARED_PATH "/cheburashka.off",V,F);
// Plot the mesh
igl::opengl::glfw::Viewer viewer;
viewer.data().set_mesh(V, F);
viewer.data().show_lines = false;
viewer.callback_key_down = &key_down;
// One fixed point
b.resize(1,1);
// point on belly.
b<<2556;
bc.resize(1,1);
bc<<1;
// Construct Laplacian and mass matrix
SparseMatrix<double> L,M,Minv;
igl::cotmatrix(V,F,L);
igl::massmatrix(V,F,igl::MASSMATRIX_TYPE_VORONOI,M);
//M = (M/M.diagonal().maxCoeff()).eval();
igl::invert_diag(M,Minv);
// Bi-Laplacian
Q = L.transpose() * (Minv * L);
// Zero linear term
B = VectorXd::Zero(V.rows(),1);
// Lower and upper bound
lx = VectorXd::Zero(V.rows(),1);
ux = VectorXd::Ones(V.rows(),1);
// Equality constraint constrain solution to sum to 1
Beq.resize(1,1);
Beq(0) = 0.08;
Aeq = M.diagonal().sparseView().transpose();
// (Empty inequality constraints)
solve(viewer);
cout<<
"Press '.' to increase scale and resolve."<<endl<<
"Press ',' to decrease scale and resolve."<<endl;
viewer.launch();
}
void Dmp::getParameterVectorMask(const std::set<std::string> selected_values_labels, Eigen::VectorXi& selected_mask) const
{
cout << __FILE__ << ":" << __LINE__ << ":Here" << endl;
selected_mask.resize(getParameterVectorAllSize());
selected_mask.fill(0);
int offset = 0;
VectorXi cur_mask;
for (int dd=0; dd<dim_orig(); dd++)
{
function_approximators_[dd]->getParameterVectorMask(selected_values_labels,cur_mask);
selected_mask.segment(offset,cur_mask.size()) = cur_mask;
offset += cur_mask.size();
}
//nnn Check for multi-dim dmps
assert(offset == getParameterVectorAllSize());
}
void calin::iact_data::instrument_layout::map_channels_using_from_coordinates(
Eigen::VectorXi& map,
const std::vector<double>& from_x, const std::vector<double>& from_y,
const calin::ix::iact_data::instrument_layout::CameraLayout& to,
double tolerance)
{
tolerance *= tolerance;
if(from_x.size() != from_y.size())
throw std::runtime_error("X and Y coordinate vectors must be same size.");
map.resize(to.channel_size());
for(int ichan=0; ichan<to.channel_size(); ichan++) {
const auto& chan = to.channel(ichan);
map[ichan] = -1;
for(unsigned jchan=0; jchan<from_x.size(); jchan++) {
double d2 = SQR(chan.x()-from_x[jchan])+SQR(chan.y()-from_y[jchan]);
if(d2 < tolerance) {
map[ichan] = jchan;
break;
}
}
}
}
IGL_INLINE void igl::copyleft::cgal::point_mesh_squared_distance(
const Eigen::MatrixXd & P,
const CGAL::AABB_tree<
CGAL::AABB_traits<Kernel,
CGAL::AABB_triangle_primitive<Kernel,
typename std::vector<CGAL::Triangle_3<Kernel> >::iterator
>
>
> & tree,
const std::vector<CGAL::Triangle_3<Kernel> > & T,
Eigen::VectorXd & sqrD,
Eigen::VectorXi & I,
Eigen::MatrixXd & C)
{
typedef CGAL::Triangle_3<Kernel> Triangle_3;
typedef typename std::vector<Triangle_3>::iterator Iterator;
typedef CGAL::AABB_triangle_primitive<Kernel, Iterator> Primitive;
typedef CGAL::AABB_traits<Kernel, Primitive> AABB_triangle_traits;
typedef CGAL::AABB_tree<AABB_triangle_traits> Tree;
typedef typename Tree::Point_and_primitive_id Point_and_primitive_id;
typedef CGAL::Point_3<Kernel> Point_3;
assert(P.cols() == 3);
const int n = P.rows();
sqrD.resize(n,1);
I.resize(n,1);
C.resize(n,P.cols());
for(int p = 0;p < n;p++)
{
Point_3 query(P(p,0),P(p,1),P(p,2));
// Find closest point and primitive id
Point_and_primitive_id pp = tree.closest_point_and_primitive(query);
Point_3 closest_point = pp.first;
C(p,0) = CGAL::to_double(closest_point[0]);
C(p,1) = CGAL::to_double(closest_point[1]);
C(p,2) = CGAL::to_double(closest_point[2]);
sqrD(p) = CGAL::to_double((closest_point-query).squared_length());
I(p) = pp.second - T.begin();
}
}
void calin::iact_data::instrument_layout::map_channels_using_grid(
Eigen::VectorXi& map,
const calin::ix::iact_data::instrument_layout::CameraLayout& from,
const calin::ix::iact_data::instrument_layout::CameraLayout& to)
{
std::map<unsigned, int> grid_map;
for(int ichan=0; ichan<from.channel_size(); ichan++) {
const auto& chan = from.channel(ichan);
if(chan.pixel_grid_index() >= 0)
grid_map[chan.pixel_grid_index()] = chan.channel_index();
}
map.resize(to.channel_size());
for(int ichan=0; ichan<to.channel_size(); ichan++) {
const auto& chan = to.channel(ichan);
if(chan.pixel_grid_index() < 0)
map[ichan] = -1;
else if(grid_map.find(chan.pixel_grid_index()) == grid_map.end())
map[ichan] = -1;
else
map[ichan] = grid_map[chan.pixel_grid_index()];
}
}
void igl::polyvector_field_one_ring_matchings(const Eigen::PlainObjectBase<DerivedV> &V,
const Eigen::PlainObjectBase<DerivedF> &F,
const std::vector<std::vector<VFType> >& VF,
const Eigen::MatrixXi& E2F,
const Eigen::MatrixXi& F2E,
const Eigen::PlainObjectBase<DerivedTT>& TT,
const Eigen::PlainObjectBase<DerivedM> &match_ab,
const Eigen::PlainObjectBase<DerivedM> &match_ba,
const int vi,
Eigen::MatrixXi &mvi,
Eigen::VectorXi &fi)
{
int half_degree = match_ab.cols();
mvi.resize(VF[vi].size()+1,half_degree);
fi.resize(VF[vi].size()+1,1);
//start from one face
//first, check if the vertex is on a boundary
//then there must be two faces that are on the boundary
//(other cases not supported)
int fstart = -1;
int ind = 0;
for (int i =0; i<VF[vi].size(); ++i)
{
int fi = VF[vi][i];
for (int j=0; j<3; ++j)
if (F(fi,j)==vi && TT(fi,j) == -1)
{
ind ++;
fstart = fi;
// break;
}
}
if (ind >1 )
{
std::cerr<<"igl::polyvector_field_one_ring_matchings -- vertex "<<vi<< " is on an unusual boundary"<<std::endl;
exit(1);
}
if (fstart == -1)
fstart = VF[vi][0];
int current_face = fstart;
int i =0;
fi[i] = current_face;
for (int j=0; j<half_degree; ++j)
mvi(i,j) = j;
int next_face = -1;
while (next_face != fstart && current_face!=-1)
{
// look for the vertex
int j=-1;
for (unsigned z=0; z<3; ++z)
if (F(current_face,(z+1)%3) == vi)
{
j=z;
break;
}
assert(j!=-1);
next_face = TT(current_face, j);
++i;
if (next_face == -1)
mvi.row(i).setConstant(-1);
else
{
// look at the edge between the two faces
const int ¤t_edge = F2E(current_face,j);
for (int k=0; k<half_degree; ++k)
{
// check its orientation to determine whether match_ab or match_ba should be used
if ((E2F(current_edge,0) == current_face) &&
(E2F(current_edge,1) == next_face) )
{
//look at match_ab
mvi(i,k) = match_ab(current_edge,(mvi(i-1,k))%half_degree);
}
else
{
assert((E2F(current_edge,1) == current_face) &&
(E2F(current_edge,0) == next_face));
//look at match_ba
mvi(i,k) = match_ba(current_edge,(mvi(i-1,k))%half_degree);
}
if (mvi(i-1,k)>=half_degree)
mvi(i,k) = (mvi(i,k)+half_degree)%(2*half_degree);
}
}
current_face = next_face;
fi[i] = current_face;
}
}
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;
}
IGL_INLINE void igl::boundary_loop(
const Eigen::MatrixXd& V,
const Eigen::MatrixXi& F,
Eigen::VectorXi& b)
{
std::vector<int> bnd;
bnd.clear();
std::vector<bool> isVisited(V.rows(),false);
// Actually mesh only needs to be manifold near boundary, so this is
// over zealous (see gptoolbox's outline_loop for a more general
// (and probably faster) implementation)
assert(is_edge_manifold(V,F) && "Mesh must be manifold");
Eigen::MatrixXi TT,TTi;
std::vector<std::vector<int> > VF, VFi;
igl::triangle_triangle_adjacency(V,F,TT,TTi);
igl::vertex_triangle_adjacency(V,F,VF,VFi);
// Extract one boundary edge
bool done = false;
for (int i = 0; i < TT.rows() && !done; i++)
{
for (int j = 0; j < TT.cols(); j++)
{
if (TT(i,j) < 0)
{
int idx1, idx2;
idx1 = F(i,j);
idx2 = F(i,(j+1) % F.cols());
bnd.push_back(idx1);
bnd.push_back(idx2);
isVisited[idx1] = true;
isVisited[idx2] = true;
done = true;
break;
}
}
}
// Traverse boundary
while(1)
{
bool changed = false;
int lastV;
lastV = bnd[bnd.size()-1];
for (int i = 0; i < (int)VF[lastV].size(); i++)
{
int curr_neighbor = VF[lastV][i];
if (TT.row(curr_neighbor).minCoeff() < 0.) // Face contains boundary edge
{
int idx_lastV_in_face;
if (F(curr_neighbor,0) == lastV) idx_lastV_in_face = 0;
if (F(curr_neighbor,1) == lastV) idx_lastV_in_face = 1;
if (F(curr_neighbor,2) == lastV) idx_lastV_in_face = 2;
int idx_prev = (idx_lastV_in_face + F.cols()-1) % F.cols();
int idx_next = (idx_lastV_in_face + 1) % F.cols();
bool isPrevVisited = isVisited[F(curr_neighbor,idx_prev)];
bool isNextVisited = isVisited[F(curr_neighbor,idx_next)];
bool gotBndEdge = false;
int next_bnd_vertex;
if (!isNextVisited && TT(curr_neighbor,idx_lastV_in_face) < 0)
{
next_bnd_vertex = idx_next;
gotBndEdge = true;
}
else if (!isPrevVisited && TT(curr_neighbor,(idx_lastV_in_face+2) % F.cols()) < 0)
{
next_bnd_vertex = idx_prev;
gotBndEdge = true;
}
if (gotBndEdge)
{
changed = true;
bnd.push_back(F(curr_neighbor,next_bnd_vertex));
isVisited[F(curr_neighbor,next_bnd_vertex)] = true;
break;
}
}
}
if (!changed)
break;
}
b.resize(bnd.size());
for(unsigned i=0;i<bnd.size();++i)
b(i) = bnd[i];
}
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;
}
void remesh(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F, const Eigen::VectorXi &M,
Eigen::MatrixXd &Vout, Eigen::MatrixXi &Fout, Eigen::VectorXi &Mout,
double e_length) {
// The boundary vertices.
double zmin = 1e10;
double zmax = -1e10;
for (int i = 0; i < V.rows(); i++) {
zmin = std::min(zmin, V(i, 2));
zmax = std::max(zmax, V(i, 2));
}
Eigen::VectorXd minV = V.colwise().minCoeff(),
maxV = V.colwise().maxCoeff();
// Compute average edge length.
if (e_length <= 0) {
e_length = avg_edgeLength(V, F, M);
}
// Convert to OpenMesh.
TriangleMesh tri;
IGLToOpenMesh(V, F, tri);
// Do some more setup.
tri.update_face_normals();
tri.update_vertex_normals();
tri.request_vertex_status();
tri.request_edge_status();
tri.request_face_status();
// Check all edges to see if they're protected or not.
TriangleMesh::EIter e_it;
for (e_it = tri.edges_begin(); e_it != tri.edges_end(); ++e_it) {
// Get both vertices.
TriangleMesh::VHandle v0 = tri.to_vertex_handle(tri.halfedge_handle(*e_it, 0));
TriangleMesh::VHandle v1 = tri.to_vertex_handle(tri.halfedge_handle(*e_it, 1));
// See if both are nonoriginal
if (M(tri.data(v0).getOriginalIndex()) != GLOBAL::nonoriginal_marker &&
M(tri.data(v1).getOriginalIndex()) != GLOBAL::nonoriginal_marker) {
tri.data(*e_it).setProtected(true);
}
// Also set vertices if they're original.
if (M(tri.data(v0).getOriginalIndex()) != GLOBAL::nonoriginal_marker) {
tri.data(v0).setProtected(true);
}
if (M(tri.data(v1).getOriginalIndex()) != GLOBAL::nonoriginal_marker) {
tri.data(v1).setProtected(true);
}
}
/*
// Check for bad vertices.
for (int i = 0; i < tri.n_vertices(); ++i) {
TriangleMesh::VertexHandle vv = TriangleMesh::VertexHandle(i);
const TriangleMesh::Point &pt = tMesh.point(vv);
// Set statitonary if original marker set.
if (M(tri.data(vv).getOriginalIndex()) == GLOBAL::original_marker) {
}
}
*/
// Then, do the remeshing--but only split long edge, collapse short edges,
// and equalize valences.
IsotropicRemeshing ir(e_length);
//int types = ISOTROPIC_REMESHING_TYPES::SPLIT_LONG_EDGES |
//ISOTROPIC_REMESHING_TYPES::COLLAPSE_SHORT_EDGES |
//ISOTROPIC_REMESHING_TYPES::EQUALIZE_VALENCES |
//ISOTROPIC_REMESHING_TYPES::AREA_EQUALIZATION;
ir.setBoundingBox(TriangleMesh::Point(maxV(0), maxV(1), maxV(2)),
TriangleMesh::Point(minV(0), minV(1), minV(2)));
int types = ISOTROPIC_REMESHING_TYPES::EQUALIZE_VALENCES |
ISOTROPIC_REMESHING_TYPES::AREA_EQUALIZATION;
ir.remesh(&tri, 1, types);
// Then, copy it all back.
Eigen::VectorXi origIdx;
OpenMeshToIGL(tri, Vout, Fout, origIdx);
// Set the new markers as well.
Mout.resize(Vout.rows());
for (int i = 0; i < Vout.rows(); ++i) {
// If it doesn't correspond to a previous vertex, it's a "nonoriginal".
if (origIdx(i) == -1) {
Mout(i) = GLOBAL::nonoriginal_marker;
} else {
// Otherwise, it's the same marker as it was before.
Mout(i) = M(origIdx(i));
int oidx = origIdx(i);
double moved = ( V.row(oidx) - Vout.row(i) ).norm();
if ( moved > GLOBAL::EPS && M(oidx) != GLOBAL::nonoriginal_marker) {
printf("point %d(m:%d new:%d) moved by %lf\n", oidx, M(oidx), i, moved);
Mout(i) = 10;
}
}
int oidx = origIdx(i);
if (Vout(i, 2) - zmin < 0 ||
zmax - Vout(i, 2) < 0) {
printf("point %d(%d) moved out of bounds! by %lf vs %lf,%lf (%le,%le)\n",
oidx, M(oidx), Vout(i, 2), zmin, zmax,
Vout(i,2) - zmin, zmax - Vout(i,2));
exit(1);
}
}
}
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;
}
IGL_INLINE void igl::signed_distance(
const Eigen::MatrixXd & P,
const Eigen::MatrixXd & V,
const Eigen::MatrixXi & F,
const SignedDistanceType sign_type,
Eigen::VectorXd & S,
Eigen::VectorXi & I,
Eigen::MatrixXd & C,
Eigen::MatrixXd & N)
{
using namespace Eigen;
using namespace std;
assert(V.cols() == 3 && "V should have 3d positions");
assert(P.cols() == 3 && "P should have 3d positions");
// Only unsigned distance is supported for non-triangles
if(sign_type != SIGNED_DISTANCE_TYPE_UNSIGNED)
{
assert(F.cols() == 3 && "F should have triangles");
}
// Prepare distance computation
AABB<MatrixXd,3> tree;
tree.init(V,F);
Eigen::MatrixXd FN,VN,EN;
Eigen::MatrixXi E;
Eigen::VectorXi EMAP;
WindingNumberAABB<Eigen::Vector3d> hier;
switch(sign_type)
{
default:
assert(false && "Unknown SignedDistanceType");
case SIGNED_DISTANCE_TYPE_UNSIGNED:
// do nothing
break;
case SIGNED_DISTANCE_TYPE_DEFAULT:
case SIGNED_DISTANCE_TYPE_WINDING_NUMBER:
hier.set_mesh(V,F);
hier.grow();
break;
case SIGNED_DISTANCE_TYPE_PSEUDONORMAL:
// "Signed Distance Computation Using the Angle Weighted Pseudonormal"
// [Bærentzen & Aanæs 2005]
per_face_normals(V,F,FN);
per_vertex_normals(V,F,PER_VERTEX_NORMALS_WEIGHTING_TYPE_ANGLE,FN,VN);
per_edge_normals(
V,F,PER_EDGE_NORMALS_WEIGHTING_TYPE_UNIFORM,FN,EN,E,EMAP);
N.resize(P.rows(),3);
break;
}
S.resize(P.rows(),1);
I.resize(P.rows(),1);
C.resize(P.rows(),3);
for(int p = 0;p<P.rows();p++)
{
const RowVector3d q = P.row(p);
double s,sqrd;
RowVector3d c;
int i=-1;
switch(sign_type)
{
default:
assert(false && "Unknown SignedDistanceType");
case SIGNED_DISTANCE_TYPE_UNSIGNED:
s = 1.;
sqrd = tree.squared_distance(V,F,q,i,c);
break;
case SIGNED_DISTANCE_TYPE_DEFAULT:
case SIGNED_DISTANCE_TYPE_WINDING_NUMBER:
signed_distance_winding_number(tree,V,F,hier,q,s,sqrd,i,c);
break;
case SIGNED_DISTANCE_TYPE_PSEUDONORMAL:
{
Eigen::RowVector3d n;
signed_distance_pseudonormal(tree,V,F,FN,VN,EN,EMAP,q,s,sqrd,i,c,n);
N.row(p) = n;
break;
}
}
I(p) = i;
S(p) = s*sqrt(sqrd);
C.row(p) = c;
}
}
// 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;
}
// A tet is organized in the following fashion:
//
// 0
//
// 3 (3 is in the background; 1 and 2 are in the foreground)
// 2 1
//
// So the faces (with counterclockwise normals) are:
// (0 1 3)
// (0 2 1)
// (3 2 0)
// (1 2 3)
//
//
// This method will perform three steps:
// 1. Add all faces, duplicating ones on the interior
// 2. Remove all duplicate verts (might have been caused during #1)
// 3. Remove all duplicate faces
void marching_tets(
const Eigen::MatrixXd& V,
const Eigen::MatrixXi& T,
const Eigen::VectorXd& H,
double offset,
Eigen::MatrixXd& NV,
Eigen::MatrixXi& NF,
Eigen::VectorXi& I)
{
min_how_much = 1;
max_how_much = 0;
using namespace Eigen;
using namespace std;
// Count the faces.
std::map<std::vector<int>, int> face_counts;
for (int i = 0; i < T.rows(); ++i) {
std::vector<std::vector<int> > fs;
fs.push_back({T(i, 0), T(i, 1), T(i, 3)});
fs.push_back({T(i, 0), T(i, 2), T(i, 1)});
fs.push_back({T(i, 3), T(i, 2), T(i, 0)});
fs.push_back({T(i, 1), T(i, 2), T(i, 3)});
for (auto &f : fs) {
std::sort(f.begin(), f.end());
// Add it to the map.
face_counts[f]++;
}
}
vector<Eigen::RowVector3i> faces;
vector<int> faces_markers;
vector<Eigen::RowVector3d> new_verts;
int times[6];
for (int i = 0; i < 6; i++) {
times[i] = 0;
}
// Create data structure.
MarchingTetsDat dd(V, H, faces, faces_markers, face_counts, new_verts, offset);
int numEq = 0;
// Check each tet face, add as needed.
for (int i = 0; i < T.rows(); ++i) {
// See if the tet is entirely inside.
vector<int> inside, outside, inside_t, outside_t, identical;
for (int j = 0; j < T.cols(); ++j) {
//if (H(T(i, j)) > offset + 1e-4) {
if (H(T(i, j)) > offset) {
outside.push_back(j);
} else if (H(T(i, j)) < offset) {
inside.push_back(j);
} else {
numEq++;
identical.push_back(j);
}
if (H(T(i, j)) == GLOBAL::outside_temp) {
outside_t.push_back(j);
} else if (H(T(i, j)) == GLOBAL::inside_temp) {
inside_t.push_back(j);
}
}
// Ignore this tet if it's entirely outside.
if (outside.size() == 4) {
continue;
}
if (outside.size() == 0 && inside.size() == 0) {
// degenerate, ignore.
printf("WARNING: degenerate tet face found!!\n");
} else if (inside.size() == 0 && identical.size() < 3) {
// Nothing to add.
} else if (identical.size() == 3) {
//addOrig(T.row(i), 7, dd.faces, dd.faces_markers);
// Ignore it if there's only one on the outside.
//if (inside.size() == 0) continue;
if (outside.size() == 0) continue;
times[1]++;
// Add just a single face (always)
int i1 = T(i,identical[0]), i2 = T(i,identical[1]), i3 = T(i,identical[2]);
Eigen::RowVector3i f({i1, i2, i3});
dd.faces.push_back(f);
dd.faces_markers.push_back(1);
} else if (outside.size() == 0) {
// (these are colored blue)
times[0]++;
// (A) Takes care of:
// inside: 1, identical: 3 (remove three duplicated faces later)
// inside: 2, identical: 2 (remove all four duplicated faces later)
// inside: 3, identical: 1 (remove all four duplicated faces later)
// inside: 4, identical: 0 (remove all four duplicated faces later)
case0Out(dd, T.row(i), inside, outside, identical, inside_t, outside_t);
} else if (inside.size() == 1) {
// (these are colored green)
times[2]++;
// (B) Takes care of:
// inside: 1 outside: 3
// inside: 1 outside: 2 identical: 1
// inside: 1 outside: 1 identical: 2
//
case1In(dd, T.row(i), inside, outside, identical, inside_t, outside_t);
} else if (inside.size() == 3 && outside.size() == 1) {
// (these are colored orange)
times[3]++;
// (C) takes care of:
// inside: 3 outside: 1
//
case3In1Out(dd, T.row(i), inside, outside, identical, inside_t, outside_t);
} else if (inside.size() == 2 && outside.size() >= 1) {
// (these are colored red)
times[4]++;
// (D) takes care of:
// inside: 2 outside: 1 identical: 1
// inside: 2 outside: 2 identical: 0
//
case2In2Out(dd, T.row(i), inside, outside, identical, inside_t, outside_t);
} else {
times[5]++;
fprintf(stderr, "WARN: marching tets found something weird, with in:%lu out:%lu\n",
inside.size(), outside.size());
}
}
printf("Finished marching tets with usages:\n");
for (int i = 0; i < 6; ++i) {
printf(" %d: %d\n", i, times[i]);
}
printf("how_much is %lf and EPS is %lf\n", min_how_much, GLOBAL::EPS);
printf(" max is %lf\n", max_how_much);
printf("Num equal is %d\n", numEq);
// Copy verts
NV.resize(V.rows() + new_verts.size(), 3);
for (int i = 0; i < V.rows(); ++i) {
NV.row(i) = V.row(i);
}
for (int i = 0; i < new_verts.size(); ++i) {
NV.row(i + V.rows()) = new_verts[i];
}
// Set I
I.resize(NV.rows());
for (int i = 0; i < I.rows(); ++i) {
if (i < V.rows()) {
I(i) = i;
} else {
I(i) = -1;
}
}
Eigen::VectorXi facesMarkers;
facesMarkers.resize(faces.size());
// Copy faces
NF.resize(faces.size(), 3);
for (int i = 0; i < faces.size(); ++i) {
NF.row(i) = faces[i];
facesMarkers(i) = faces_markers[i];
}
Eigen::MatrixXd newV;
Eigen::MatrixXi newF;
Eigen::VectorXi SVJ, SVI, I2;
// Helpers::viewTriMesh(NV, NF, facesMarkers);
//igl::writeOFF("offset_mesh.off", NV, NF)
Helpers::writeMeshWithMarkers("offset_mesh", NV, NF, facesMarkers);
/*
igl::collapse_small_triangles(NV, NF, 1e-8, newF);
printf("Collapsed %d small triangles\n", NF.rows() - newF.rows());
NF = newF;
*/
///*
igl::remove_duplicate_vertices(NV, NF, 1e-20, newV, SVI, SVJ, newF);
I2.resize(newV.rows());
I2.setConstant(-1);
for (int i = 0; i < NV.rows(); ++i) {
if (I2(SVJ(i)) == -1) {
I2(SVJ(i)) = I(i);
} else {
I2(SVJ(i)) = std::min(I2(SVJ(i)), I(i));
}
}
NF = newF;
NV = newV;
I = I2;
// Now see if we have duplicated faces.
//igl::resolve_duplicated_faces(NF, newF, SVJ);
//NF = newF;
//*/
// Other option is to do these two:
// These are bad because sometimes the "small" triangles are not area zero,
// and doing the removeDuplicates will delete these triangles and make the
// mesh non-manifold. Better to wait for remeshing later.
//Helpers::removeDuplicates(NV, NF, I);
//Helpers::collapseSmallTriangles(NV, NF);
igl::remove_unreferenced(NV, NF, newV, newF, SVI, SVJ);
I2.resize(newV.rows());
I2.setConstant(-1);
for (int i = 0; i < I2.rows(); ++i) {
I2(i) = I(SVJ(i));
}
I = I2;
NV = newV;
NF = newF;
// orient everything correctly.
Eigen::VectorXi C;
igl::orientable_patches(NF, C);
igl::bfs_orient(NF, newF, C);
NF = newF;
igl::orient_outward(NV, NF, C, newF, SVJ);
NF = newF;
//igl::writeOFF("offset_mesh_normals.off", NV, NF);
#ifdef DEBUG_MESH
if (!Helpers::isMeshOkay(NV, NF)) {
printf("Error: Mesh is not okay at first...\n");
}
if (!Helpers::isManifold(NV, NF, I)) {
printf("Error: Mesh from marching tets not manifold!\n");
Eigen::VectorXi temp;
temp.resize(I.rows());
temp.setZero();
Helpers::isManifold(NV, NF, temp, true);
Helpers::viewTriMesh(NV, NF, temp);
Helpers::writeMeshWithMarkers("marching_tets_manifold", NV, NF, temp);
cout << "See marching_tets_manifold.off for problems.\n";
exit(1);
}
#endif
}