C++ SparseMatrixsc::nonZeros方法代码示例

void MathUtilities::extractTripletData( const SparseMatrixsc& matrix, VectorXi& rows, VectorXi& cols, VectorXs& vals )
{
  rows.resize( matrix.nonZeros() );
  cols.resize( matrix.nonZeros() );
  vals.resize( matrix.nonZeros() );
  int flat_index{ 0 };
  for( int outer_index = 0; outer_index < matrix.outerSize(); ++outer_index )
  {
    for( Eigen::SparseMatrix<double>::InnerIterator it( matrix, outer_index ); it; ++it )
    {
      rows( flat_index ) = it.row();
      cols( flat_index ) = it.col();
      vals( flat_index++ ) = it.value();
    }
  }
  assert( flat_index == matrix.nonZeros() );
}

void MathUtilities::serialize( const SparseMatrixsc& A, std::ostream& stm )
{
  assert( stm.good() );

  VectorXi col_ptr;
  VectorXi row_ind;
  VectorXs val;
  MathUtilities::extractDataCCS( A, col_ptr, row_ind, val );
  assert( col_ptr.size() == A.cols() + 1 ); assert( row_ind.size() == A.nonZeros() ); assert( val.size() == A.nonZeros() );
  // Size of col_ptr == A.cols() + 1
  Utilities::serializeBuiltInType( A.rows(), stm );
  Utilities::serializeBuiltInType( A.cols(), stm );
  stm.write( reinterpret_cast<char*>( col_ptr.data() ), col_ptr.size() * sizeof(int) );
  // Size of row_ind == size of val == A.nonZeros()
  Utilities::serializeBuiltInType( A.nonZeros(), stm );
  stm.write( reinterpret_cast<char*>( row_ind.data() ), row_ind.size() * sizeof(int) );
  stm.write( reinterpret_cast<char*>( val.data() ), val.size() * sizeof(scalar) );
}

void Ball2DState::setMass( const VectorXs& m )
{
  m_M = createM( m );
  m_Minv = createMinv( m );
  #ifndef NDEBUG
  const SparseMatrixsc should_be_id{ m_M * m_Minv };
  const Eigen::Map<const ArrayXs> should_be_id_data{ should_be_id.valuePtr(), should_be_id.nonZeros() };
  assert( ( ( should_be_id_data - 1.0 ).abs() <= 1.0e-6 ).all() );
  #endif
}

void FrictionOperatorUtilities::formLinearFrictionDiskConstraint( const int num_samples, SparseMatrixsc& E )
{
  {
    const VectorXi column_nonzeros{ VectorXi::Constant( E.cols(), num_samples ) };
    E.reserve( column_nonzeros );
  }
  // For each column
  for( int col = 0; col < E.cols(); ++col )
  {
    for( int samplenum = 0; samplenum < num_samples; ++samplenum )
    {
      // Note the negative for QL
      E.insert( num_samples * col + samplenum, col ) = 1.0;
    }
  }
  E.makeCompressed();
  assert( E.nonZeros() == E.cols() * num_samples );
  assert( E.sum() == E.nonZeros() );
}

void MathUtilities::extractDataCCS( const SparseMatrixsc& A, VectorXi& col_ptr, VectorXi& row_ind, VectorXs& val )
{
  col_ptr.resize( A.cols() + 1 );
  row_ind.resize( A.nonZeros() );
  val.resize( A.nonZeros() );

  col_ptr(0) = 0;
  for( int col = 0; col < A.outerSize(); ++col )
  {
    col_ptr(col+1) = col_ptr(col);
    for( SparseMatrixsc::InnerIterator it(A,col); it; ++it )
    {
      const int row{ it.row() };

      val(col_ptr(col+1)) = it.value();
      row_ind(col_ptr(col+1)) = row;
      ++col_ptr(col+1);
    }
  }

  assert( col_ptr( col_ptr.size() - 1 ) == row_ind.size() );
}

void MathUtilities::printSparseMathematicaMatrix( const SparseMatrixsc& A, const scalar& eps )
{
  std::cout << "{";
  int entry_num = 0;
  for( int k = 0; k < A.outerSize(); ++k )
  {
    for( typename SparseMatrixsc::InnerIterator it(A,k); it; ++it )
    {
      std::cout << "{" << (it.row()+1) << "," << (it.col()+1) << "}->";
      if( fabs(it.value()) < eps ) { std::cout << 0.0; }
      else { std::cout << it.value(); }
      entry_num++;
      if( entry_num != A.nonZeros() ) { std::cout << ","; }
    }
  }
  std::cout << "}" << std::endl;
}

int MathUtilities::values( const SparseMatrixsc& A, scalar* vals )
{
  assert( vals != nullptr );
  
  int curel{ 0 };
  for( int col = 0; col < A.outerSize(); ++col )
  {
    for( SparseMatrixsc::InnerIterator it( A, col ); it; ++it )
    {
      vals[curel] = it.value();
      ++curel;
    }
  }
  
  assert( curel == A.nonZeros() );
  return curel;
}

void NearEarthGravityForce::computeForce( const VectorXs& q, const VectorXs& v, const SparseMatrixsc& M, VectorXs& result ) const
{
    assert( q.size() % 12 == 0 );
    assert( v.size() == q.size() / 2 );
    assert( M.rows() == M.cols() );
    assert( M.nonZeros() == q.size() );

    const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };

    const Eigen::Map<const VectorXs> masses{ M.valuePtr(), 3 * nbodies };

    for( unsigned i = 0; i < nbodies; ++i )
    {
        assert( masses( 3 * i + 0 ) == masses( 3 * i + 1 ) );
        assert( masses( 3 * i + 1 ) == masses( 3 * i + 2 ) );
        result.segment<3>( 3 * i ) += masses( 3 * i ) * m_g;
    }
}

// Determine which elements are non-zero
int MathUtilities::sparsityPattern( const SparseMatrixsc& A, int* rows, int* cols )
{
  assert( rows != nullptr );
  assert( cols != nullptr );
  
  int curel{ 0 };
  for( int col = 0; col < A.outerSize(); ++col )
  {
    for( SparseMatrixsc::InnerIterator it( A, col ); it; ++it )
    {
      rows[curel] = it.row();
      cols[curel] = col;
      ++curel;
    }
  }

  assert( curel == A.nonZeros() );
  return curel;
}

scalar NearEarthGravityForce::computePotential( const VectorXs& q, const SparseMatrixsc& M ) const
{
    assert( q.size() % 12 == 0 );
    assert( M.rows() == M.cols() );
    assert( M.nonZeros() == q.size() );

    const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };

    const Eigen::Map<const VectorXs> masses{ M.valuePtr(), 3 * nbodies };

    scalar U = 0.0;
    for( unsigned i = 0; i < nbodies; ++i )
    {
        assert( masses( 3 * i + 0 ) == masses( 3 * i + 1 ) );
        assert( masses( 3 * i + 1 ) == masses( 3 * i + 2 ) );
        U += - masses( 3 * i ) * m_g.dot( q.segment<3>( 3 * i ) );
    }
    return U;
}

void StaticPlaneSphereConstraint::resolveImpact( const scalar& CoR, const SparseMatrixsc& M, const scalar& ndotv, VectorXs& vout, scalar& alpha ) const
{
  assert( CoR >= 0.0 );
  assert( CoR <= 1.0 );
  assert( ndotv < 0.0 );
  assert( vout.size() % 3 == 0 );
  assert( M.rows() == M.cols() );
  assert( M.nonZeros() == 2 * vout.size() );
  assert( 3 * m_sphere_idx + 2 < vout.size() );

  const Eigen::Map<const VectorXs> m{ M.valuePtr(), vout.size() };
  assert( m( 3 * m_sphere_idx ) == m( 3 * m_sphere_idx + 1 ) );
  assert( m( 3 * m_sphere_idx ) == m( 3 * m_sphere_idx + 2 ) );

  const scalar msphere{ m( 3 * m_sphere_idx ) };

  // Compute the impulse
  alpha = - ( 1.0 + CoR ) * ndotv * msphere;
  assert( alpha >= 0.0 );
  vout.segment<3>( 3 * m_sphere_idx ) += - ( 1.0 + CoR ) * ndotv * m_plane.n();
}