Beispiel #1
0
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;
    }
  }
}
Beispiel #2
0
 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();
 }
Beispiel #3
0
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();
  }

}
Beispiel #4
0
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;
    }
  }
}
Beispiel #6
0
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];
};
Beispiel #8
0
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++);
    }
  }
}
Beispiel #10
0
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++);
    }
  }
}
Beispiel #11
0
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();
}
Beispiel #12
0
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;
}
Beispiel #13
0
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());
  }
}
Beispiel #14
0
    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++);
    }
  }
}
Beispiel #16
0
        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();
}
Beispiel #18
0
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);
}
Beispiel #19
0
 typename Eigen::MatrixBase<D>::ConstRowXpr
 operator*(const Eigen::MatrixBase<D> & F) const
 {
   assert(F.rows()==6);
   return F.row(ANGULAR + axis);
 }
Beispiel #20
0
 friend typename Eigen::MatrixBase<D>::ConstRowXpr
 operator*( const TransposeConst &, const Eigen::MatrixBase<D> & F )
 {
   assert(F.rows()==6);
   return F.row(axis);
 }
Beispiel #21
0
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++);
    }
  }
}
Beispiel #22
0
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);
}
Beispiel #23
0
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();
}