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