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;
}
int MathUtilities::nzLowerTriangular( const SparseMatrixsc& A )
{
int num{ 0 };
for( int col = 0; col < A.outerSize(); ++col )
{
for( SparseMatrixsc::InnerIterator it( A, col ); it; ++it )
{
// Skip entries above the diagonal
if( col > it.row() ) { continue; }
++num;
}
}
return num;
}
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::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::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::valuesLowerTriangular( 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 )
{
if( col > it.row() ) continue;
vals[curel] = it.value();
++curel;
}
}
return curel;
}
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;
}
bool MathUtilities::writeToMatlabTripletText( const SparseMatrixsc& matrix, const std::string& file_name )
{
std::ofstream output_file( file_name );
if( !output_file.is_open() )
{
return false;
}
for( int outer_idx = 0; outer_idx < matrix.outerSize(); ++outer_idx )
{
for( SparseMatrixsc::InnerIterator it( matrix, outer_idx ); it; ++it )
{
// Matlab is 1 indexed
output_file << it.row() + 1 << "\t" << it.col() + 1 << "\t" << it.value() << std::endl;
}
}
return true;
}
int MathUtilities::sparsityPatternLowerTriangular( 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 )
{
if( col > it.row() ) continue;
rows[curel] = it.row();
cols[curel] = col;
++curel;
}
}
return curel;
}
// 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;
}
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() );
}
