IGL_INLINE void igl::per_face_normals( const Eigen::MatrixBase<DerivedV>& V, const Eigen::MatrixBase<DerivedF>& F, const Eigen::MatrixBase<DerivedZ> & Z, Eigen::PlainObjectBase<DerivedN> & N) { N.resize(F.rows(),3); // loop over faces int Frows = F.rows(); #pragma omp parallel for if (Frows>10000) for(int i = 0; i < Frows;i++) { const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v1 = V.row(F(i,1)) - V.row(F(i,0)); const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v2 = V.row(F(i,2)) - V.row(F(i,0)); N.row(i) = v1.cross(v2);//.normalized(); typename DerivedV::Scalar r = N.row(i).norm(); if(r == 0) { N.row(i) = Z; }else { N.row(i) /= r; } } }
void jacobian(const double t, const std::vector<double>& y, Eigen::MatrixBase<Derived1>& dy_dt, Eigen::MatrixBase<Derived2>& Jy, Eigen::MatrixBase<Derived2>& Jtheta) const { using Eigen::Dynamic; using Eigen::Map; using Eigen::Matrix; using Eigen::RowVectorXd; using stan::math::var; using std::vector; vector<double> grad(y.size() + theta_.size()); Map<RowVectorXd> grad_eig(&grad[0], y.size() + theta_.size()); try { start_nested(); vector<var> y_var(y.begin(), y.end()); vector<var> theta_var(theta_.begin(), theta_.end()); vector<var> z_var; z_var.reserve(y.size() + theta_.size()); z_var.insert(z_var.end(), y_var.begin(), y_var.end()); z_var.insert(z_var.end(), theta_var.begin(), theta_var.end()); vector<var> dy_dt_var = f_(t, y_var, theta_var, x_, x_int_, msgs_); for (size_t i = 0; i < dy_dt_var.size(); ++i) { dy_dt(i) = dy_dt_var[i].val(); set_zero_all_adjoints_nested(); dy_dt_var[i].grad(z_var, grad); Jy.row(i) = grad_eig.leftCols(y.size()); Jtheta.row(i) = grad_eig.rightCols(theta_.size()); } } catch (const std::exception& e) { recover_memory_nested(); throw; } stan::math::recover_memory_nested(); }
IGL_INLINE void igl::per_face_normals_stable( const Eigen::MatrixBase<DerivedV>& V, const Eigen::MatrixBase<DerivedF>& F, Eigen::PlainObjectBase<DerivedN> & N) { using namespace Eigen; typedef Matrix<typename DerivedV::Scalar,1,3> RowVectorV3; typedef typename DerivedV::Scalar Scalar; const size_t m = F.rows(); N.resize(F.rows(),3); // Grad all points for(size_t f = 0;f<m;f++) { const RowVectorV3 p0 = V.row(F(f,0)); const RowVectorV3 p1 = V.row(F(f,1)); const RowVectorV3 p2 = V.row(F(f,2)); const RowVectorV3 n0 = (p1 - p0).cross(p2 - p0); const RowVectorV3 n1 = (p2 - p1).cross(p0 - p1); const RowVectorV3 n2 = (p0 - p2).cross(p1 - p2); // careful sum for(int d = 0;d<3;d++) { // This is a little _silly_ in terms of complexity, but its recursive // implementation is clean looking... const std::function<Scalar(Scalar,Scalar,Scalar)> sum3 = [&sum3](Scalar a, Scalar b, Scalar c)->Scalar { if(fabs(c)>fabs(a)) { return sum3(c,b,a); } // c < a if(fabs(c)>fabs(b)) { return sum3(a,c,b); } // c < a, c < b if(fabs(b)>fabs(a)) { return sum3(b,a,c); } return (a+b)+c; }; N(f,d) = sum3(n0(d),n1(d),n2(d)); } // sum better not be sure, or else NaN N.row(f) /= N.row(f).norm(); } }
IGL_INLINE void igl::internal_angles( const Eigen::MatrixBase<DerivedV>& V, const Eigen::MatrixBase<DerivedF>& F, Eigen::PlainObjectBase<DerivedK> & K) { using namespace Eigen; using namespace std; typedef typename DerivedV::Scalar Scalar; if(F.cols() == 3) { // Edge lengths Matrix< Scalar, DerivedF::RowsAtCompileTime, DerivedF::ColsAtCompileTime> L_sq; igl::squared_edge_lengths(V,F,L_sq); assert(F.cols() == 3 && "F should contain triangles"); igl::internal_angles_using_squared_edge_lengths(L_sq,K); }else { assert(V.cols() == 3 && "If F contains non-triangle facets, V must be 3D"); K.resizeLike(F); auto corner = []( const typename DerivedV::ConstRowXpr & x, const typename DerivedV::ConstRowXpr & y, const typename DerivedV::ConstRowXpr & z) { typedef Eigen::Matrix<Scalar,1,3> RowVector3S; RowVector3S v1 = (x-y).normalized(); RowVector3S v2 = (z-y).normalized(); // http://stackoverflow.com/questions/10133957/signed-angle-between-two-vectors-without-a-reference-plane Scalar s = v1.cross(v2).norm(); Scalar c = v1.dot(v2); return atan2(s, c); }; for(unsigned i=0; i<F.rows(); ++i) { for(unsigned j=0; j<F.cols(); ++j) { K(i,j) = corner( V.row(F(i,int(j-1+F.cols())%F.cols())), V.row(F(i,j)), V.row(F(i,(j+1+F.cols())%F.cols())) ); } } } }
IGL_INLINE void igl::project_to_line_segment( const Eigen::MatrixBase<DerivedP> & P, const Eigen::MatrixBase<DerivedS> & S, const Eigen::MatrixBase<DerivedD> & D, Eigen::PlainObjectBase<Derivedt> & t, Eigen::PlainObjectBase<DerivedsqrD> & sqrD) { project_to_line(P,S,D,t,sqrD); const int np = P.rows(); // loop over points and fix those that projected beyond endpoints #pragma omp parallel for if (np>10000) for(int p = 0;p<np;p++) { const DerivedP Pp = P.row(p); if(t(p)<0) { sqrD(p) = (Pp-S).squaredNorm(); t(p) = 0; }else if(t(p)>1) { sqrD(p) = (Pp-D).squaredNorm(); t(p) = 1; } } }
void angularvel2quatdotMatrix(const Eigen::MatrixBase<DerivedQ>& q, Eigen::MatrixBase<DerivedM>& M, typename Gradient<DerivedM, QUAT_SIZE, 1>::type* dM) { // note: not normalizing to match MATLAB implementation EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<DerivedQ>, QUAT_SIZE); EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<DerivedM>, QUAT_SIZE, SPACE_DIMENSION); M.row(0) << -q(1), -q(2), -q(3); M.row(1) << q(0), q(3), -q(2); M.row(2) << -q(3), q(0), q(1); M.row(3) << q(2), -q(1), q(0); M *= 0.5; if (dM) { (*dM) << 0.0, -0.5, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.5, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, -0.5, 0.0, 0.0, 0.0, 0.0, 0.5, 0.5, 0.0, 0.0, 0.0, 0.0, -0.5, 0.0, 0.0, 0.0, 0.0, 0.0, -0.5, 0.0, 0.0, -0.5, 0.0, 0.0, 0.5, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0; } }
IGL_INLINE void igl::average_onto_vertices(const Eigen::MatrixBase<DerivedV> &V, const Eigen::MatrixBase<DerivedF> &F, const Eigen::MatrixBase<DerivedS> &S, Eigen::MatrixBase<DerivedS> &SV) { SV = DerivedS::Zero(V.rows(),S.cols()); Eigen::Matrix<typename DerivedF::Scalar,Eigen::Dynamic,1> COUNT(V.rows()); COUNT.setZero(); for (int i = 0; i <F.rows(); ++i) { for (int j = 0; j<F.cols(); ++j) { SV.row(F(i,j)) += S.row(i); COUNT[F(i,j)] ++; } } for (int i = 0; i <V.rows(); ++i) SV.row(i) /= COUNT[i]; };
void doSomeRankPreservingOperations(Eigen::MatrixBase<Derived>& m) { typedef typename Derived::RealScalar RealScalar; for(int a = 0; a < 3*(m.rows()+m.cols()); a++) { RealScalar d = Eigen::ei_random<RealScalar>(-1,1); int i = Eigen::ei_random<int>(0,m.rows()-1); // i is a random row number int j; do { j = Eigen::ei_random<int>(0,m.rows()-1); } while (i==j); // j is another one (must be different) m.row(i) += d * m.row(j); i = Eigen::ei_random<int>(0,m.cols()-1); // i is a random column number do { j = Eigen::ei_random<int>(0,m.cols()-1); } while (i==j); // j is another one (must be different) m.col(i) += d * m.col(j); } }
void setSubMatrixGradient(Eigen::MatrixBase<DerivedA>& dM, const Eigen::MatrixBase<DerivedB>& dM_submatrix, const std::vector<int>& rows, const std::vector<int>& cols, typename DerivedA::Index M_rows, typename DerivedA::Index q_start, typename DerivedA::Index q_subvector_size) { if (q_subvector_size < 0) { q_subvector_size = dM.cols() - q_start; } int index = 0; for (typename std::vector<int>::const_iterator col = cols.begin(); col != cols.end(); ++col) { for (typename std::vector<int>::const_iterator row = rows.begin(); row != rows.end(); ++row) { dM.block((*row) + (*col) * M_rows, q_start, 1, q_subvector_size) = dM_submatrix.row(index++); } } }
void setSubMatrixGradient(Eigen::MatrixBase<DerivedA>& dM, const Eigen::MatrixBase<DerivedB>& dM_submatrix, const std::vector<int>& rows, const std::vector<int>& cols, int M_rows, int q_start, int q_subvector_size) { if (q_subvector_size < 0) { q_subvector_size = dM.cols() - q_start; } int index = 0; for (int col : cols) { for (int row : rows) { dM.block(row + col * M_rows, q_start, 1, q_subvector_size) = dM_submatrix.row(index++); } } }
IGL_INLINE void igl::normalize_row_sums( const Eigen::MatrixBase<DerivedA>& A, Eigen::MatrixBase<DerivedB> & B) { #ifndef NDEBUG // loop over rows for(int i = 0; i < A.rows();i++) { typename DerivedB::Scalar sum = A.row(i).sum(); assert(sum != 0); } #endif B = (A.array().colwise() / A.rowwise().sum().array()).eval(); }
typename Derived::PlainObject transposeGrad( const Eigen::MatrixBase<Derived>& dX, int rows_X) { typename Derived::PlainObject dX_transpose(dX.rows(), dX.cols()); int numel = dX.rows(); int index = 0; for (int i = 0; i < numel; i++) { dX_transpose.row(i) = dX.row(index); index += rows_X; if (index >= numel) { index = (index % numel) + 1; } } return dX_transpose; }
IGL_INLINE void igl::barycenter( const Eigen::MatrixBase<DerivedV> & V, const Eigen::MatrixBase<DerivedF> & F, Eigen::PlainObjectBase<DerivedBC> & BC) { BC.setZero(F.rows(),V.cols()); // Loop over faces for(int i = 0;i<F.rows();i++) { // loop around face for(int j = 0;j<F.cols();j++) { // Accumulate BC.row(i) += V.row(F(i,j)); } // average BC.row(i) /= double(F.cols()); } }
void filter(const Eigen::MatrixBase<Derived>& data, const std::vector<bool>& to_keep, Derived& out) { const size_t num_data = data.rows(); const size_t dim = data.cols(); assert(num_data == to_keep.size()); size_t num_kept= 0; for (size_t i=0; i<num_data; i++) { if (to_keep[i]) num_kept++; } out.resize(num_kept, dim); size_t count=0; for (size_t i=0; i<num_data; i++) { if (to_keep[i]) { out.row(count) = data.row(i); count++; } } }
void setSubMatrixGradient(Eigen::MatrixBase<DerivedA>& dM, const Eigen::MatrixBase<DerivedB>& dM_submatrix, const std::array<int, NRows>& rows, const std::array<int, NCols>& cols, typename DerivedA::Index M_rows, typename DerivedA::Index q_start, typename DerivedA::Index q_subvector_size) { if (q_subvector_size == Eigen::Dynamic) { q_subvector_size = dM.cols() - q_start; } int index = 0; for (typename std::array<int, NCols>::const_iterator col = cols.begin(); col != cols.end(); ++col) { for (typename std::array<int, NRows>::const_iterator row = rows.begin(); row != rows.end(); ++row) { dM.template block<1, QSubvectorSize> ((*row) + (*col) * M_rows, q_start, 1, q_subvector_size) = dM_submatrix.row(index++); } } }
bool CameraModel::_lift( const Eigen::MatrixBase<Derived1>& mP2D, Eigen::MatrixBase<Derived2>& mP3D ) const { typedef typename Eigen::internal::traits<Derived2>::Scalar ScalarType; const unsigned int nNumPoints = mP2D.cols(); #if 0 typedef Eigen::Matrix<ScalarType,1,Eigen::Dynamic> RowVect; const int nWidth = 1000; const int nHeight = 1000; const int nWidthSize = nWidth/10; const int nHeightSize = nHeight/10; Derived1 mP2DUndist( 3, nWidthSize*nHeightSize ); Derived1 m1 = RowVect::LinSpaced( nWidthSize, 0, nWidth-1 ).replicate( nHeightSize, 1 ); mP2DUndist.row( 0 ) = Eigen::Map<Derived1>( m1.data(), 1, nWidthSize*nHeightSize ); mP2DUndist.row( 1 ) = RowVect::LinSpaced( nHeightSize, 0, nHeight-1 ).replicate( 1, nWidthSize ); mP2DUndist.row( 2 ) = RowVect::Ones( 1, nWidthSize*nHeightSize ); Derived1 mP2DDist( 2, nWidthSize*nHeightSize ); project( mP2DUndist, mP2DDist ); // Initialise with closest values for( int ii=0; ii<mP2D.cols(); ii++ ) { //Derived1::Index nMinIndex; int nMinIndex; Derived1 mDiff = mP2DDist; mDiff.colwise() -= mP2D.col( ii ); mDiff.colwise().norm().minCoeff( &nMinIndex ); mP3D.col( ii ) = mP2DUndist.col( nMinIndex ); } #else // Start from distortionless points if possible if( get( "fx" ) != "" && get( "fy" ) != "" && get( "cx" ) != "" && get( "cy" ) != "" ) { double fx = get<double>( "fx" ); double fy = get<double>( "fy" ); double cx = get<double>( "cx" ); double cy = get<double>( "cy" ); mP3D.row( 0 ) = ( mP2D.row( 0 ) - cx*Derived2::Ones( 1, nNumPoints ) ) / fx; mP3D.row( 1 ) = ( mP2D.row( 1 ) - cy*Derived2::Ones( 1, nNumPoints ) ) / fy; mP3D.row( 2 ) = Derived2::Ones( 1, nNumPoints ); } else { mP3D.row( 0 ) = mP2D.row( 0 ); mP3D.row( 1 ) = mP2D.row( 1 ); mP3D.row( 2 ) = Derived2::Ones( 1, nNumPoints ); } #endif for( size_t nIndex=0; nIndex<nNumPoints; nIndex++ ) { mP3D.col( nIndex ) /= mP3D.col( nIndex ).norm(); } Derived1 mP2DEst( 2, nNumPoints ); Derived2 mdP2DE( 2, 3*nNumPoints ); Derived2 mdP2DI( 2, get_num_parameters()*nNumPoints ); double dMaxErr = 0.001; double dLastMaxErr = 10; const unsigned int nMaxIter = 30; const unsigned int nMaxOuterIter = 5; for( size_t nOuterIter=0; nOuterIter<nMaxOuterIter; nOuterIter++ ) { for( size_t nIter=0; nIter<nMaxIter; nIter++ ) { project( mP3D, mP2DEst, mdP2DE, mdP2DI ); for( size_t nIndex=0; nIndex<nNumPoints; nIndex++ ) { Eigen::Matrix<ScalarType,2,3> mJ = mdP2DE.block( 0, 3*nIndex, 2, 3 ); Eigen::Matrix<ScalarType,3,3> mJtJ = mJ.transpose()*mJ + 0.1*Eigen::Matrix<ScalarType,3,3>::Identity(); Eigen::Matrix<ScalarType,3,1> mJte = mJ.transpose() * ( mP2DEst.col( nIndex ) - mP2D.col( nIndex ) ); mP3D.col( nIndex ) -= mJtJ.ldlt().solve( mJte ); double dErr = ( mP2DEst.col( nIndex ) - mP2D.col( nIndex ) ).norm(); if( nIndex > 0 ) { if( std::isnan( dErr ) ) { mP3D.col( nIndex ) = mP3D.col( nIndex-1 ); } } mP3D.col( nIndex ) /= mP3D.col( nIndex ).norm(); } dLastMaxErr = ( mP2DEst - mP2D ).colwise().norm().maxCoeff(); if( dLastMaxErr < dMaxErr ) { break; } } if( dLastMaxErr >= dMaxErr ) { Derived1 mErrors = ( mP2DEst - mP2D ).colwise().norm(); for( int ii=0; ii<mErrors.cols(); ii++ ) { if( mErrors(0,ii) > dMaxErr ) { // Find closest value double dMinDiff = 1e10; int nBestIndex = -1; for( int jj=0; jj<mErrors.cols(); jj++ ) { if( jj != ii && mErrors(0,jj) < dMaxErr ) { double dDiff = ( mP2D.col( ii ) - mP2D.col( jj ) ).norm(); if( dDiff < dMinDiff ) { dMinDiff = dDiff; nBestIndex = jj; } } } if( nBestIndex == -1 ) { // All is lost, could not find an index // that passes the error test return false; } mP3D.col( ii ) = mP3D.col( nBestIndex ) ; } } } } return dLastMaxErr < dMaxErr; }
IGL_INLINE void igl::point_simplex_squared_distance( const Eigen::MatrixBase<Derivedp> & p, const Eigen::MatrixBase<DerivedV> & V, const Eigen::MatrixBase<DerivedEle> & Ele, const typename DerivedEle::Index primitive, Derivedsqr_d & sqr_d, Eigen::PlainObjectBase<Derivedc> & c) { typedef typename Derivedp::Scalar Scalar; typedef typename Eigen::Matrix<Scalar,1,DIM> Vector; typedef Vector Point; const auto & Dot = [](const Point & a, const Point & b)->Scalar { return a.dot(b); }; // Real-time collision detection, Ericson, Chapter 5 const auto & ClosestPtPointTriangle = [&Dot](Point p, Point a, Point b, Point c)->Point { // Check if P in vertex region outside A Vector ab = b - a; Vector ac = c - a; Vector ap = p - a; Scalar d1 = Dot(ab, ap); Scalar d2 = Dot(ac, ap); if (d1 <= 0.0 && d2 <= 0.0) return a; // barycentric coordinates (1,0,0) // Check if P in vertex region outside B Vector bp = p - b; Scalar d3 = Dot(ab, bp); Scalar d4 = Dot(ac, bp); if (d3 >= 0.0 && d4 <= d3) return b; // barycentric coordinates (0,1,0) // Check if P in edge region of AB, if so return projection of P onto AB Scalar vc = d1*d4 - d3*d2; if( a != b) { if (vc <= 0.0 && d1 >= 0.0 && d3 <= 0.0) { Scalar v = d1 / (d1 - d3); return a + v * ab; // barycentric coordinates (1-v,v,0) } } // Check if P in vertex region outside C Vector cp = p - c; Scalar d5 = Dot(ab, cp); Scalar d6 = Dot(ac, cp); if (d6 >= 0.0 && d5 <= d6) return c; // barycentric coordinates (0,0,1) // Check if P in edge region of AC, if so return projection of P onto AC Scalar vb = d5*d2 - d1*d6; if (vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0) { Scalar w = d2 / (d2 - d6); return a + w * ac; // barycentric coordinates (1-w,0,w) } // Check if P in edge region of BC, if so return projection of P onto BC Scalar va = d3*d6 - d5*d4; if (va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0) { Scalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); return b + w * (c - b); // barycentric coordinates (0,1-w,w) } // P inside face region. Compute Q through its barycentric coordinates (u,v,w) Scalar denom = 1.0 / (va + vb + vc); Scalar v = vb * denom; Scalar w = vc * denom; return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = 1.0-v-w }; assert(p.size() == DIM); assert(V.cols() == DIM); assert(Ele.cols() <= DIM+1); assert(Ele.cols() <= 3 && "Only simplices up to triangles are considered"); c = ClosestPtPointTriangle( p, V.row(Ele(primitive,0)), V.row(Ele(primitive,1%Ele.cols())), V.row(Ele(primitive,2%Ele.cols()))); sqr_d = (p-c).squaredNorm(); }
Eigen::Matrix<typename Derived::Scalar, 1, Derived::ColsAtCompileTime> getSubMatrixGradient(const Eigen::MatrixBase<Derived>& dM, int row, int col, int M_rows) { return dM.row(row + col * M_rows); }
typename Eigen::MatrixBase<D>::ConstRowXpr operator*(const Eigen::MatrixBase<D> & F) const { assert(F.rows()==6); return F.row(ANGULAR + axis); }
friend typename Eigen::MatrixBase<D>::ConstRowXpr operator*( const TransposeConst &, const Eigen::MatrixBase<D> & F ) { assert(F.rows()==6); return F.row(axis); }
void setSubMatrixGradient(Eigen::MatrixBase<DerivedA>& dM, const Eigen::MatrixBase<DerivedB>& dM_submatrix, const std::array<int, NRows>& rows, const std::array<int, NCols>& cols, int M_rows, int q_start, int q_subvector_size) { if (q_subvector_size == Eigen::Dynamic) { q_subvector_size = dM.cols() - q_start; } int index = 0; for (int col : cols) { for (int row : rows) { dM.template block<1, QSubvectorSize> (row + col * M_rows, q_start, 1, q_subvector_size) = dM_submatrix.row(index++); } } }
IGL_INLINE void igl::massmatrix( const Eigen::MatrixBase<DerivedV> & V, const Eigen::MatrixBase<DerivedF> & F, const MassMatrixType type, Eigen::SparseMatrix<Scalar>& M) { using namespace Eigen; using namespace std; const int n = V.rows(); const int m = F.rows(); const int simplex_size = F.cols(); MassMatrixType eff_type = type; // Use voronoi of for triangles by default, otherwise barycentric if(type == MASSMATRIX_TYPE_DEFAULT) { eff_type = (simplex_size == 3?MASSMATRIX_TYPE_VORONOI:MASSMATRIX_TYPE_BARYCENTRIC); } // Not yet supported assert(type!=MASSMATRIX_TYPE_FULL); Matrix<int,Dynamic,1> MI; Matrix<int,Dynamic,1> MJ; Matrix<Scalar,Dynamic,1> MV; if(simplex_size == 3) { // Triangles // edge lengths numbered same as opposite vertices Matrix<Scalar,Dynamic,3> l(m,3); // loop over faces for(int i = 0;i<m;i++) { l(i,0) = (V.row(F(i,1))-V.row(F(i,2))).norm(); l(i,1) = (V.row(F(i,2))-V.row(F(i,0))).norm(); l(i,2) = (V.row(F(i,0))-V.row(F(i,1))).norm(); } Matrix<Scalar,Dynamic,1> dblA; doublearea(l,dblA); switch(eff_type) { case MASSMATRIX_TYPE_BARYCENTRIC: // diagonal entries for each face corner MI.resize(m*3,1); MJ.resize(m*3,1); MV.resize(m*3,1); MI.block(0*m,0,m,1) = F.col(0); MI.block(1*m,0,m,1) = F.col(1); MI.block(2*m,0,m,1) = F.col(2); MJ = MI; repmat(dblA,3,1,MV); MV.array() /= 6.0; break; case MASSMATRIX_TYPE_VORONOI: { // diagonal entries for each face corner // http://www.alecjacobson.com/weblog/?p=874 MI.resize(m*3,1); MJ.resize(m*3,1); MV.resize(m*3,1); MI.block(0*m,0,m,1) = F.col(0); MI.block(1*m,0,m,1) = F.col(1); MI.block(2*m,0,m,1) = F.col(2); MJ = MI; // Holy shit this needs to be cleaned up and optimized Matrix<Scalar,Dynamic,3> cosines(m,3); cosines.col(0) = (l.col(2).array().pow(2)+l.col(1).array().pow(2)-l.col(0).array().pow(2))/(l.col(1).array()*l.col(2).array()*2.0); cosines.col(1) = (l.col(0).array().pow(2)+l.col(2).array().pow(2)-l.col(1).array().pow(2))/(l.col(2).array()*l.col(0).array()*2.0); cosines.col(2) = (l.col(1).array().pow(2)+l.col(0).array().pow(2)-l.col(2).array().pow(2))/(l.col(0).array()*l.col(1).array()*2.0); Matrix<Scalar,Dynamic,3> barycentric = cosines.array() * l.array(); normalize_row_sums(barycentric,barycentric); Matrix<Scalar,Dynamic,3> partial = barycentric; partial.col(0).array() *= dblA.array() * 0.5; partial.col(1).array() *= dblA.array() * 0.5; partial.col(2).array() *= dblA.array() * 0.5; Matrix<Scalar,Dynamic,3> quads(partial.rows(),partial.cols()); quads.col(0) = (partial.col(1)+partial.col(2))*0.5; quads.col(1) = (partial.col(2)+partial.col(0))*0.5; quads.col(2) = (partial.col(0)+partial.col(1))*0.5; quads.col(0) = (cosines.col(0).array()<0).select( 0.25*dblA,quads.col(0)); quads.col(1) = (cosines.col(0).array()<0).select(0.125*dblA,quads.col(1)); quads.col(2) = (cosines.col(0).array()<0).select(0.125*dblA,quads.col(2)); quads.col(0) = (cosines.col(1).array()<0).select(0.125*dblA,quads.col(0)); quads.col(1) = (cosines.col(1).array()<0).select(0.25*dblA,quads.col(1)); quads.col(2) = (cosines.col(1).array()<0).select(0.125*dblA,quads.col(2)); quads.col(0) = (cosines.col(2).array()<0).select(0.125*dblA,quads.col(0)); quads.col(1) = (cosines.col(2).array()<0).select(0.125*dblA,quads.col(1)); quads.col(2) = (cosines.col(2).array()<0).select( 0.25*dblA,quads.col(2)); MV.block(0*m,0,m,1) = quads.col(0); MV.block(1*m,0,m,1) = quads.col(1); MV.block(2*m,0,m,1) = quads.col(2); break; } case MASSMATRIX_TYPE_FULL: assert(false && "Implementation incomplete"); break; default: assert(false && "Unknown Mass matrix eff_type"); } }else if(simplex_size == 4) { assert(V.cols() == 3); assert(eff_type == MASSMATRIX_TYPE_BARYCENTRIC); MI.resize(m*4,1); MJ.resize(m*4,1); MV.resize(m*4,1); MI.block(0*m,0,m,1) = F.col(0); MI.block(1*m,0,m,1) = F.col(1); MI.block(2*m,0,m,1) = F.col(2); MI.block(3*m,0,m,1) = F.col(3); MJ = MI; // loop over tets for(int i = 0;i<m;i++) { // http://en.wikipedia.org/wiki/Tetrahedron#Volume Matrix<Scalar,3,1> v0m3 = V.row(F(i,0)) - V.row(F(i,3)); Matrix<Scalar,3,1> v1m3 = V.row(F(i,1)) - V.row(F(i,3)); Matrix<Scalar,3,1> v2m3 = V.row(F(i,2)) - V.row(F(i,3)); Scalar v = fabs(v0m3.dot(v1m3.cross(v2m3)))/6.0; MV(i+0*m) = v/4.0; MV(i+1*m) = v/4.0; MV(i+2*m) = v/4.0; MV(i+3*m) = v/4.0; } }else { // Unsupported simplex size assert(false && "Unsupported simplex size"); } sparse(MI,MJ,MV,n,n,M); }
IGL_INLINE void igl::per_vertex_normals( const Eigen::MatrixBase<DerivedV>& V, const Eigen::MatrixBase<DerivedF>& F, const igl::PerVertexNormalsWeightingType weighting, const Eigen::MatrixBase<DerivedFN>& FN, Eigen::PlainObjectBase<DerivedN> & N) { using namespace std; // Resize for output N.setZero(V.rows(),3); Eigen::Matrix<typename DerivedN::Scalar,DerivedF::RowsAtCompileTime,3> W(F.rows(),3); switch(weighting) { case PER_VERTEX_NORMALS_WEIGHTING_TYPE_UNIFORM: W.setConstant(1.); break; default: assert(false && "Unknown weighting type"); case PER_VERTEX_NORMALS_WEIGHTING_TYPE_DEFAULT: case PER_VERTEX_NORMALS_WEIGHTING_TYPE_AREA: { Eigen::Matrix<typename DerivedN::Scalar,DerivedF::RowsAtCompileTime,1> A; doublearea(V,F,A); W = A.replicate(1,3); break; } case PER_VERTEX_NORMALS_WEIGHTING_TYPE_ANGLE: internal_angles(V,F,W); break; } // loop over faces for(int i = 0;i<F.rows();i++) { // throw normal at each corner for(int j = 0; j < 3;j++) { N.row(F(i,j)) += W(i,j) * FN.row(i); } } //// loop over faces //std::mutex critical; //std::vector<DerivedN> NN; //parallel_for( // F.rows(), // [&NN,&N](const size_t n){ NN.resize(n,DerivedN::Zero(N.rows(),3));}, // [&F,&W,&FN,&NN,&critical](const int i, const size_t t) // { // // throw normal at each corner // for(int j = 0; j < 3;j++) // { // // Q: Does this need to be critical? // // A: Yes. Different (i,j)'s could produce the same F(i,j) // NN[t].row(F(i,j)) += W(i,j) * FN.row(i); // } // }, // [&N,&NN](const size_t t){ N += NN[t]; }, // 1000l); // take average via normalization N.rowwise().normalize(); }