C++ SparseMatrixsc类代码示例

void StaticPlaneSphereConstraint::computeGeneralizedFrictionGivenTangentSample( const VectorXs& q, const VectorXs& t, const unsigned column, SparseMatrixsc& D ) const
{
  assert( t.size() == 3 );
  assert( column < unsigned( D.cols() ) );
  assert( q.size() % 12 == 0 );
  assert( fabs( t.norm() - 1.0 ) <= 1.0e-6 );
  assert( fabs( m_plane.n().dot( t ) ) <= 1.0e-6 );

  // Effect on center of mass
  D.insert( 3 * m_sphere_idx + 0, column ) = t.x();
  D.insert( 3 * m_sphere_idx + 1, column ) = t.y();
  D.insert( 3 * m_sphere_idx + 2, column ) = t.z();

  // Effect on orientation
  {
    const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };
    // Compute the displacement from the center of mass to the point of contact
    assert( fabs( m_plane.n().norm() - 1.0 ) <= 1.0e-10 );
    assert( m_r >= 0.0 );
    const Vector3s r_world{ - m_r * m_plane.n() };
    const Vector3s ntilde{ r_world.cross( Eigen::Map<const Vector3s>( t.data() ) ) };
    D.insert( 3 * ( nbodies + m_sphere_idx ) + 0, column ) = ntilde.x();
    D.insert( 3 * ( nbodies + m_sphere_idx ) + 1, column ) = ntilde.y();
    D.insert( 3 * ( nbodies + m_sphere_idx ) + 2, column ) = ntilde.z();
  }
}

scalar HertzianPenaltyForce::computePotential( const VectorXs& q, const SparseMatrixsc& M, const VectorXs& r ) const
{
  assert( q.size() % 2 == 0 ); assert( q.size() == M.rows() ); assert( q.size() == M.cols() ); assert( r.size() == q.size() / 2 );

  scalar U{ 0.0 };
  // For each ball
  for( unsigned ball0 = 0; ball0 < r.size(); ++ball0 )
  {
    // For each subsequent ball
    for( unsigned ball1 = ball0 + 1; ball1 < r.size(); ++ball1 )
    {
      // Compute the total radius
      const scalar total_radius{ r(ball0) + r(ball1) };
      // Compute a vector pointing from ball0 to ball1
      const Vector2s n{ q.segment<2>( 2 * ball1 ) - q.segment<2>( 2 * ball0 ) };
      // If the squared distance is greater or equal to the sum of the radii squared, no force
      if( n.squaredNorm() > total_radius * total_radius )
      {
        continue;
      }
      // Compute the penetration depth
      const scalar delta{ n.norm() - total_radius };
      assert( delta < 0.0 );
      // U = 0.5 * k * pen_depth^(5/2)
      U += 0.5 * m_k * std::pow( -delta, scalar( 2.5 ) );
    }
  }

  return U;
}

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 BodyBodyConstraint::computeGeneralizedFrictionGivenTangentSample( const VectorXs& q, const VectorXs& t, const unsigned column, SparseMatrixsc& D ) const
{
  assert( column < unsigned( D.cols() ) );
  assert( q.size() % 12 == 0 );
  assert( t.size() == 3 );
  assert( fabs( t.norm() - 1.0 ) <= 1.0e-6 );
  assert( fabs( m_n.dot( t ) ) <= 1.0e-6 );
  assert( m_idx0 < m_idx1 );

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

  // Effect on center of mass of body i
  D.insert( 3 * m_idx0 + 0, column ) = t.x();
  D.insert( 3 * m_idx0 + 1, column ) = t.y();
  D.insert( 3 * m_idx0 + 2, column ) = t.z();
  // Effect on orientation of body i
  {
    const Vector3s ntilde0{ m_r0.cross( Eigen::Map<const Vector3s>{ t.data() } ) };
    D.insert( 3 * ( m_idx0 + nbodies ) + 0, column ) = ntilde0.x();
    D.insert( 3 * ( m_idx0 + nbodies ) + 1, column ) = ntilde0.y();
    D.insert( 3 * ( m_idx0 + nbodies ) + 2, column ) = ntilde0.z();
  }

  // Effect on center of mass of body j
  D.insert( 3 * m_idx1 + 0, column ) = - t.x();
  D.insert( 3 * m_idx1 + 1, column ) = - t.y();
  D.insert( 3 * m_idx1 + 2, column ) = - t.z();
  // Effect on orientation of body j
  {
    const Vector3s ntilde1{ m_r1.cross( Eigen::Map<const Vector3s>{ t.data() } ) };
    D.insert( 3 * ( m_idx1 + nbodies ) + 0, column ) = - ntilde1.x();
    D.insert( 3 * ( m_idx1 + nbodies ) + 1, column ) = - ntilde1.y();
    D.insert( 3 * ( m_idx1 + nbodies ) + 2, column ) = - ntilde1.z();
  }
}

void TeleportedCircleCircleConstraint::evalgradg( const VectorXs& q, const int col, SparseMatrixsc& G, const FlowableSystem& fsys ) const
{
  assert( col >= 0 ); assert( col < G.cols() );

  // MUST BE ADDED GOING DOWN THE COLUMN. DO NOT TOUCH ANOTHER COLUMN.
  assert( m_idx0 < m_idx1 );
  assert( 3 * m_idx0 + 1 < unsigned( G.rows() ) );
  G.insert( 3 * m_idx0 + 0, col ) = m_n.x();
  G.insert( 3 * m_idx0 + 1, col ) = m_n.y();
  assert( 3 * m_idx1 + 1 < unsigned( G.rows() ) );
  G.insert( 3 * m_idx1 + 0, col ) = - m_n.x();
  G.insert( 3 * m_idx1 + 1, col ) = - m_n.y();
}

void StaticPlaneSphereConstraint::evalgradg( const VectorXs& q, const int col, SparseMatrixsc& G, const FlowableSystem& fsys ) const
{
  assert( col >= 0 );
  assert( col < G.cols() );
  assert( 3 * m_sphere_idx + 2 < unsigned( G.rows() ) );

  const Vector3s n{ m_plane.n() };
  assert( fabs( n.norm() - 1.0 ) <= 1.0e-6 );

  // MUST BE ADDED GOING DOWN THE COLUMN. DO NOT TOUCH ANOTHER COLUMN.
  G.insert( 3 * m_sphere_idx + 0, col ) = n.x();
  G.insert( 3 * m_sphere_idx + 1, col ) = n.y();
  G.insert( 3 * m_sphere_idx + 2, col ) = n.z();
}

void FrictionOperatorUtilities::formGeneralizedFrictionBasis( const VectorXs& q0, const VectorXs& v0, const std::vector<std::unique_ptr<Constraint>>& K, const int num_samples, SparseMatrixsc& D, VectorXs& drel )
{
  assert( num_samples > 0 );
  assert( D.rows() == v0.size() );
  assert( num_samples * int( K.size() ) == D.cols() );

  // Reserve space for entries
  reserveSpaceInBasisMatrix( num_samples, K, D );

  // Build the matrix
  buildLinearFrictionBasis( q0, v0, num_samples, K, D, drel );

  D.makeCompressed();
}

static void buildLinearFrictionBasis( const VectorXs& q, const VectorXs& v, const int nsamples, const std::vector<std::unique_ptr<Constraint>>& K, SparseMatrixsc& D, VectorXs& drel )
{
  assert( D.cols() % nsamples == 0 );

  const unsigned ncons{ static_cast<unsigned>( D.cols() ) / nsamples };

  std::vector<std::unique_ptr<Constraint>>::const_iterator itr{ K.begin() };
  for( unsigned con_idx = 0; con_idx < ncons; ++con_idx )
  {
    (*itr)->computeGeneralizedFrictionDisk( q, v, con_idx * nsamples, nsamples, D, drel );
    ++itr;
  }
  assert( itr == K.end() );
}

void MathUtilities::extractLowerTriangularMatrix( const SparseMatrixsc& A, SparseMatrixsr& B )
{
  std::vector< Eigen::Triplet<scalar> > triplets;
  for( int col = 0; col < A.outerSize(); ++col )
  {
    for( SparseMatrixsc::InnerIterator it( A, col ); it; ++it )
    {
      if( col > it.row() ) { continue; }
      triplets.push_back( Eigen::Triplet<scalar>( it.row(), col, it.value() ) );
    }
  }
  B.resize( A.rows(), A.cols() );
  B.setFromTriplets( triplets.begin(), triplets.end() );
  B.makeCompressed();
}

bool MathUtilities::isIdentity( const SparseMatrixsc& A, const scalar& tol )
{
  if( !isSquare( A ) )
  {
    return false;
  }
  for( int outer_idx = 0; outer_idx < A.outerSize(); ++outer_idx )
  {
    for( SparseMatrixsc::InnerIterator it( A, outer_idx ); it; ++it )
    {
      if( it.row() == it.col() )
      {
        if( fabs( it.value() - 1.0 ) > tol )
        {
          return false;
        }
      }
      else
      {
        if( fabs( it.value() ) > tol )
        {
          return false;
        }
      }
    }
  }
  return true;
}

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

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 MathUtilities::convertDenseToSparse( const bool filter_zeros, const MatrixXXsc& dense_matrix, SparseMatrixsc& sparse_matrix )
{
  std::vector<Eigen::Triplet<scalar>> triplets;
  for( int row = 0; row < dense_matrix.rows(); ++row )
  {
    for( int col = 0; col < dense_matrix.cols(); ++col )
    {
      if( dense_matrix( row, col ) != 0.0 || !filter_zeros )
      {
        triplets.emplace_back( Eigen::Triplet<scalar>{ row, col, dense_matrix( row, col ) } );
      }
    }
  }
  sparse_matrix.resize( dense_matrix.rows(), dense_matrix.cols() );
  sparse_matrix.setFromTriplets( std::begin( triplets ), std::end( triplets ) );
  sparse_matrix.makeCompressed();
}

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

// TODO: Use utility class here
void MathUtilities::deserialize( SparseMatrixsc& A, std::istream& stm )
{
  assert( stm.good() );

  VectorXi col_ptr;
  VectorXi row_ind;
  VectorXs val;

  // Read the number of rows in the matrix
  int rows{ -1 };
  stm.read((char*)&rows,sizeof(int));
  assert( rows >= 0 );
  // Read the number of columns in the matrix
  int cols{ -1 };
  stm.read((char*)&cols,sizeof(int));
  assert( cols >= 0 );
  // Read the column pointer array
  col_ptr.resize(cols+1);
  stm.read((char*)col_ptr.data(),col_ptr.size()*sizeof(int));
  // Read the number of nonzeros in the array
  int nnz{ -1 };
  stm.read((char*)&nnz,sizeof(int));
  assert( nnz >= 0 );
  // Read the row-index array
  row_ind.resize(nnz);
  stm.read((char*)row_ind.data(),row_ind.size()*sizeof(int));
  // Read the value array
  val.resize(nnz);
  stm.read((char*)val.data(),val.size()*sizeof(scalar));

  A.resize(rows,cols);
  A.reserve(nnz);

  for( int col = 0; col < cols; ++col )
  {
    A.startVec(col);
    for( int curel = col_ptr(col); curel < col_ptr(col+1); ++curel )
    {
      int row = row_ind(curel);
      scalar curval = val(curel);
      A.insertBack(row,col) = curval;
    }
  }
  A.finalize();
}