void MathUtilities::createDiagonalMatrix( const scalar& c, SparseMatrixsc& D )
{
assert( D.rows() == D.cols() );
D.reserve( VectorXi::Constant( D.cols(), 1 ) );
for( int i = 0; i < D.cols(); ++i ) { D.insert(i,i) = c; }
D.makeCompressed();
}
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();
}
void MathUtilities::extractLowerTriangularMatrix( const SparseMatrixsc& A, SparseMatrixsc& 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();
}
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 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() );
}
// 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
}
