// TODO: Despecialize from smooth void FrictionOperator::formGeneralizedSmoothFrictionBasis( const unsigned ndofs, const unsigned ncons, const VectorXs& q, const std::vector<std::unique_ptr<Constraint>>& K, const MatrixXXsc& bases, SparseMatrixsc& D ) { assert( ncons == K.size() ); const unsigned nambientdims{ static_cast<unsigned>( bases.rows() ) }; const unsigned nsamples{ nambientdims - 1 }; D.resize( ndofs, nsamples * ncons ); auto itr = K.cbegin(); { VectorXi column_nonzeros( D.cols() ); for( unsigned collision_number = 0; collision_number < ncons; ++collision_number ) { for( unsigned sample_number = 0; sample_number < nsamples; ++sample_number ) { assert( nsamples * collision_number + sample_number < column_nonzeros.size() ); column_nonzeros( nsamples * collision_number + sample_number ) = (*itr)->frictionStencilSize(); } ++itr; } assert( ( column_nonzeros.array() > 0 ).all() ); assert( itr == K.cend() ); D.reserve( column_nonzeros ); } itr = K.cbegin(); for( unsigned collision_number = 0; collision_number < ncons; ++collision_number ) { for( unsigned sample_number = 0; sample_number < nsamples; ++sample_number ) { const unsigned current_column{ nsamples * collision_number + sample_number }; const VectorXs current_sample{ bases.col( nambientdims * collision_number + sample_number + 1 ) }; assert( fabs( current_sample.dot( bases.col( nambientdims * collision_number ) ) ) <= 1.0e-6 ); (*itr)->computeGeneralizedFrictionGivenTangentSample( q, current_sample, current_column, D ); } ++itr; } assert( itr == K.cend() ); D.prune( []( const Eigen::Index& row, const Eigen::Index& col, const scalar& value ) { return value != 0.0; } ); assert( D.innerNonZeroPtr() == nullptr ); }
// TODO: Pull the outerIndexPtr arithmetic into a helper function void MathUtilities::extractColumns( const SparseMatrixsc& A0, const std::vector<unsigned>& cols, SparseMatrixsc& A1 ) { const unsigned ncols_to_extract{ static_cast<unsigned>( cols.size() ) }; assert( ncols_to_extract <= static_cast<unsigned>( A0.cols() ) ); #ifndef NDEBUG for( unsigned i = 0; i < ncols_to_extract; ++i ) { assert( cols[i] < unsigned( A0.cols() ) ); } #endif // Compute the number of nonzeros in each column of the new matrix VectorXi column_nonzeros{ ncols_to_extract }; for( unsigned i = 0; i < ncols_to_extract; ++i ) { column_nonzeros( i ) = A0.outerIndexPtr()[cols[i]+1] - A0.outerIndexPtr()[cols[i]]; } // Resize A1 and reserve space A1.resize( A0.rows(), ncols_to_extract ); A1.reserve( column_nonzeros ); // Copy the data over, column by column for( unsigned cur_col = 0; cur_col < ncols_to_extract; ++cur_col ) { for( SparseMatrixsc::InnerIterator it( A0, cols[ cur_col ] ); it; ++it ) { A1.insert( it.row(), cur_col ) = it.value(); } } A1.makeCompressed(); #ifndef NDEBUG for( int i = 0 ; i < A1.cols(); ++i ) { assert( ( A1.outerIndexPtr()[i+1] - A1.outerIndexPtr()[i] ) == column_nonzeros( i ) ); } #endif }
static void reserveSpaceInBasisMatrix( const int nsamples, const std::vector<std::unique_ptr<Constraint>>& K, SparseMatrixsc& D ) { assert( D.cols() % nsamples == 0 ); const int ncons{ D.cols() / nsamples }; VectorXi column_nonzeros{ D.cols() }; std::vector<std::unique_ptr<Constraint>>::const_iterator itr{ K.begin() }; for( int con_idx = 0; con_idx < ncons; ++con_idx ) { for( int smpl_num = 0; smpl_num < nsamples; ++smpl_num ) { column_nonzeros( con_idx * nsamples + smpl_num ) = (*itr)->frictionStencilSize(); } ++itr; } assert( itr == K.end() ); D.reserve( column_nonzeros ); }