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