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