void StaticPlaneSphereConstraint::computeGeneralizedFrictionGivenTangentSample( const VectorXs& q, const VectorXs& t, const unsigned column, SparseMatrixsc& D ) const
{
assert( t.size() == 3 );
assert( column < unsigned( D.cols() ) );
assert( q.size() % 12 == 0 );
assert( fabs( t.norm() - 1.0 ) <= 1.0e-6 );
assert( fabs( m_plane.n().dot( t ) ) <= 1.0e-6 );
// Effect on center of mass
D.insert( 3 * m_sphere_idx + 0, column ) = t.x();
D.insert( 3 * m_sphere_idx + 1, column ) = t.y();
D.insert( 3 * m_sphere_idx + 2, column ) = t.z();
// Effect on orientation
{
const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };
// Compute the displacement from the center of mass to the point of contact
assert( fabs( m_plane.n().norm() - 1.0 ) <= 1.0e-10 );
assert( m_r >= 0.0 );
const Vector3s r_world{ - m_r * m_plane.n() };
const Vector3s ntilde{ r_world.cross( Eigen::Map<const Vector3s>( t.data() ) ) };
D.insert( 3 * ( nbodies + m_sphere_idx ) + 0, column ) = ntilde.x();
D.insert( 3 * ( nbodies + m_sphere_idx ) + 1, column ) = ntilde.y();
D.insert( 3 * ( nbodies + m_sphere_idx ) + 2, column ) = ntilde.z();
}
}
scalar HertzianPenaltyForce::computePotential( const VectorXs& q, const SparseMatrixsc& M, const VectorXs& r ) const
{
assert( q.size() % 2 == 0 ); assert( q.size() == M.rows() ); assert( q.size() == M.cols() ); assert( r.size() == q.size() / 2 );
scalar U{ 0.0 };
// For each ball
for( unsigned ball0 = 0; ball0 < r.size(); ++ball0 )
{
// For each subsequent ball
for( unsigned ball1 = ball0 + 1; ball1 < r.size(); ++ball1 )
{
// Compute the total radius
const scalar total_radius{ r(ball0) + r(ball1) };
// Compute a vector pointing from ball0 to ball1
const Vector2s n{ q.segment<2>( 2 * ball1 ) - q.segment<2>( 2 * ball0 ) };
// If the squared distance is greater or equal to the sum of the radii squared, no force
if( n.squaredNorm() > total_radius * total_radius )
{
continue;
}
// Compute the penetration depth
const scalar delta{ n.norm() - total_radius };
assert( delta < 0.0 );
// U = 0.5 * k * pen_depth^(5/2)
U += 0.5 * m_k * std::pow( -delta, scalar( 2.5 ) );
}
}
return U;
}
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 BodyBodyConstraint::computeGeneralizedFrictionGivenTangentSample( const VectorXs& q, const VectorXs& t, const unsigned column, SparseMatrixsc& D ) const
{
assert( column < unsigned( D.cols() ) );
assert( q.size() % 12 == 0 );
assert( t.size() == 3 );
assert( fabs( t.norm() - 1.0 ) <= 1.0e-6 );
assert( fabs( m_n.dot( t ) ) <= 1.0e-6 );
assert( m_idx0 < m_idx1 );
const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };
// Effect on center of mass of body i
D.insert( 3 * m_idx0 + 0, column ) = t.x();
D.insert( 3 * m_idx0 + 1, column ) = t.y();
D.insert( 3 * m_idx0 + 2, column ) = t.z();
// Effect on orientation of body i
{
const Vector3s ntilde0{ m_r0.cross( Eigen::Map<const Vector3s>{ t.data() } ) };
D.insert( 3 * ( m_idx0 + nbodies ) + 0, column ) = ntilde0.x();
D.insert( 3 * ( m_idx0 + nbodies ) + 1, column ) = ntilde0.y();
D.insert( 3 * ( m_idx0 + nbodies ) + 2, column ) = ntilde0.z();
}
// Effect on center of mass of body j
D.insert( 3 * m_idx1 + 0, column ) = - t.x();
D.insert( 3 * m_idx1 + 1, column ) = - t.y();
D.insert( 3 * m_idx1 + 2, column ) = - t.z();
// Effect on orientation of body j
{
const Vector3s ntilde1{ m_r1.cross( Eigen::Map<const Vector3s>{ t.data() } ) };
D.insert( 3 * ( m_idx1 + nbodies ) + 0, column ) = - ntilde1.x();
D.insert( 3 * ( m_idx1 + nbodies ) + 1, column ) = - ntilde1.y();
D.insert( 3 * ( m_idx1 + nbodies ) + 2, column ) = - ntilde1.z();
}
}
void TeleportedCircleCircleConstraint::evalgradg( const VectorXs& q, const int col, SparseMatrixsc& G, const FlowableSystem& fsys ) const
{
assert( col >= 0 ); assert( col < G.cols() );
// MUST BE ADDED GOING DOWN THE COLUMN. DO NOT TOUCH ANOTHER COLUMN.
assert( m_idx0 < m_idx1 );
assert( 3 * m_idx0 + 1 < unsigned( G.rows() ) );
G.insert( 3 * m_idx0 + 0, col ) = m_n.x();
G.insert( 3 * m_idx0 + 1, col ) = m_n.y();
assert( 3 * m_idx1 + 1 < unsigned( G.rows() ) );
G.insert( 3 * m_idx1 + 0, col ) = - m_n.x();
G.insert( 3 * m_idx1 + 1, col ) = - m_n.y();
}
void StaticPlaneSphereConstraint::evalgradg( const VectorXs& q, const int col, SparseMatrixsc& G, const FlowableSystem& fsys ) const
{
assert( col >= 0 );
assert( col < G.cols() );
assert( 3 * m_sphere_idx + 2 < unsigned( G.rows() ) );
const Vector3s n{ m_plane.n() };
assert( fabs( n.norm() - 1.0 ) <= 1.0e-6 );
// MUST BE ADDED GOING DOWN THE COLUMN. DO NOT TOUCH ANOTHER COLUMN.
G.insert( 3 * m_sphere_idx + 0, col ) = n.x();
G.insert( 3 * m_sphere_idx + 1, col ) = n.y();
G.insert( 3 * m_sphere_idx + 2, col ) = n.z();
}
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();
}
static void buildLinearFrictionBasis( const VectorXs& q, const VectorXs& v, const int nsamples, const std::vector<std::unique_ptr<Constraint>>& K, SparseMatrixsc& D, VectorXs& drel )
{
assert( D.cols() % nsamples == 0 );
const unsigned ncons{ static_cast<unsigned>( D.cols() ) / nsamples };
std::vector<std::unique_ptr<Constraint>>::const_iterator itr{ K.begin() };
for( unsigned con_idx = 0; con_idx < ncons; ++con_idx )
{
(*itr)->computeGeneralizedFrictionDisk( q, v, con_idx * nsamples, nsamples, D, drel );
++itr;
}
assert( itr == K.end() );
}
void MathUtilities::extractLowerTriangularMatrix( const SparseMatrixsc& A, SparseMatrixsr& 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();
}
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;
}
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() );
}
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 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 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;
}
// TODO: Use utility class here
void MathUtilities::deserialize( SparseMatrixsc& A, std::istream& stm )
{
assert( stm.good() );
VectorXi col_ptr;
VectorXi row_ind;
VectorXs val;
// Read the number of rows in the matrix
int rows{ -1 };
stm.read((char*)&rows,sizeof(int));
assert( rows >= 0 );
// Read the number of columns in the matrix
int cols{ -1 };
stm.read((char*)&cols,sizeof(int));
assert( cols >= 0 );
// Read the column pointer array
col_ptr.resize(cols+1);
stm.read((char*)col_ptr.data(),col_ptr.size()*sizeof(int));
// Read the number of nonzeros in the array
int nnz{ -1 };
stm.read((char*)&nnz,sizeof(int));
assert( nnz >= 0 );
// Read the row-index array
row_ind.resize(nnz);
stm.read((char*)row_ind.data(),row_ind.size()*sizeof(int));
// Read the value array
val.resize(nnz);
stm.read((char*)val.data(),val.size()*sizeof(scalar));
A.resize(rows,cols);
A.reserve(nnz);
for( int col = 0; col < cols; ++col )
{
A.startVec(col);
for( int curel = col_ptr(col); curel < col_ptr(col+1); ++curel )
{
int row = row_ind(curel);
scalar curval = val(curel);
A.insertBack(row,col) = curval;
}
}
A.finalize();
}
// This method and the smooth version share the second half of code. Abstract that out.
void StaticPlaneSphereConstraint::computeGeneralizedFrictionDisk( const VectorXs& q, const VectorXs& v, const int start_column, const int num_samples, SparseMatrixsc& D, VectorXs& drel ) const
{
assert( start_column >= 0 );
assert( start_column < D.cols() );
assert( num_samples > 0 );
assert( start_column + num_samples - 1 < D.cols() );
assert( q.size() % 12 == 0 );
assert( q.size() == 2 * v.size() );
const Vector3s n{ m_plane.n() };
assert( fabs( n.norm() - 1.0 ) <= 1.0e-6 );
std::vector<Vector3s> friction_disk( static_cast<std::vector<Vector3s>::size_type>( num_samples ) );
assert( friction_disk.size() == std::vector<Vector3s>::size_type( num_samples ) );
{
// Compute the relative velocity
Vector3s tangent_suggestion{ computeRelativeVelocity( q, v ) };
if( tangent_suggestion.cross( n ).squaredNorm() < 1.0e-9 )
{
tangent_suggestion = FrictionUtilities::orthogonalVector( n );
}
tangent_suggestion *= -1.0;
// Sample the friction disk
FrictionUtilities::generateOrthogonalVectors( n, friction_disk, tangent_suggestion );
}
assert( unsigned( num_samples ) == friction_disk.size() );
// Compute the displacement from the center of mass to the point of contact
assert( fabs( n.norm() - 1.0 ) <= 1.0e-10 ); assert( m_r >= 0.0 );
const Vector3s r_world{ - m_r * n };
// Cache the velocity of the collision point on the plane
const Vector3s plane_collision_point_vel{ computePlaneCollisionPointVelocity( q ) };
// For each sample of the friction disk
const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };
for( unsigned friction_sample = 0; friction_sample < unsigned( num_samples ); ++friction_sample )
{
const unsigned cur_col{ start_column + friction_sample };
assert( cur_col < unsigned( D.cols() ) );
// Effect on center of mass
D.insert( 3 * m_sphere_idx + 0, cur_col ) = friction_disk[friction_sample].x();
D.insert( 3 * m_sphere_idx + 1, cur_col ) = friction_disk[friction_sample].y();
D.insert( 3 * m_sphere_idx + 2, cur_col ) = friction_disk[friction_sample].z();
// Effect on orientation
{
const Vector3s ntilde{ r_world.cross( friction_disk[friction_sample] ) };
D.insert( 3 * ( nbodies + m_sphere_idx ) + 0, cur_col ) = ntilde.x();
D.insert( 3 * ( nbodies + m_sphere_idx ) + 1, cur_col ) = ntilde.y();
D.insert( 3 * ( nbodies + m_sphere_idx ) + 2, cur_col ) = ntilde.z();
}
// Relative velocity contribution from kinematic scripting
assert( cur_col < drel.size() );
drel( cur_col ) = - friction_disk[friction_sample].dot( plane_collision_point_vel );
}
}
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 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;
}
}
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) );
}
// 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;
}
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 );
}
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;
}
// 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 );
}
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();
}
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 LCPOperatorQL::flow( const std::vector<std::unique_ptr<Constraint>>& cons, const SparseMatrixsc& M, const SparseMatrixsc& Minv, const VectorXs& q0, const VectorXs& v0, const VectorXs& v0F, const SparseMatrixsc& N, const SparseMatrixsc& Q, const VectorXs& nrel, const VectorXs& CoR, VectorXs& alpha )
{
// Q in 1/2 \alpha^T Q \alpha
assert( Q.rows() == Q.cols() );
MatrixXXsc Qdense = Q;
// Linear term in the objective
VectorXs Adense;
ImpactOperatorUtilities::computeLCPQPLinearTerm( N, nrel, CoR, v0, v0F, Adense );
// Solve the QP
assert( Qdense.rows() == Adense.size() ); assert( Adense.size() == alpha.size() );
const int status = solveQP( m_tol, Qdense, Adense, alpha );
// Check for problems
if( 0 != status )
{
std::cerr << "Warning, failed to solve QP in LCPOperatorQL::flow: " << QLUtilities::QLReturnStatusToString(status) << std::endl;
}
// TODO: Sanity check the solution here
}
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() );
}
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 HertzianPenaltyForce::computeForce( const VectorXs& q, const VectorXs& v, const SparseMatrixsc& M, const VectorXs& r, VectorXs& result ) const
{
assert( q.size() % 2 == 0 ); assert( q.size() == v.size() ); assert( q.size() == M.rows() );
assert( q.size() == M.cols() ); assert( r.size() == q.size() / 2 ); assert( q.size() == result.size() );
// For each ball
for( unsigned ball0 = 0; ball0 < r.size(); ++ball0 )
{
// For each subsequent ball
for( unsigned ball1 = ball0 + 1; ball1 < r.size(); ++ball1 )
{
// Compute the total radius
const scalar total_radius{ r(ball0) + r(ball1) };
// Compute a vector pointing from ball0 to ball1
Vector2s n{ q.segment<2>( 2 * ball1 ) - q.segment<2>( 2 * ball0 ) };
// Compute the squared length of the vector
scalar d{ n.squaredNorm() };
// If the squared distance is greater or equal to the sum of the radii squared, no force
if( d > total_radius * total_radius )
{
continue;
}
// Normalize the vector between the balls
d = sqrt( d );
assert( d != 0.0 );
n /= d;
assert( fabs( n.norm() - 1.0 ) <= 1.0e-6 );
// Compute the penetration depth
d -= total_radius;
assert( d < 0.0 );
// F = 5 * k * pen_depth^(3/2) * n
const Vector2s F{ ( 5.0 / 4.0 ) * m_k * std::pow( -d, scalar( 1.5 ) ) * n };
result.segment<2>( 2 * ball1 ) += F;
result.segment<2>( 2 * ball0 ) -= F;
}
}
}
void Ball2DGravityForce::computeForce( const VectorXs& q, const VectorXs& v, const SparseMatrixsc& M, const VectorXs& r, VectorXs& result ) const
{
assert( q.size() % 2 == 0 ); assert( q.size() == v.size() ); assert( q.size() == M.rows() );
assert( q.size() == M.cols() ); assert( q.size() == result.size() );
for( int i = 0; i < q.size(); i += 2 )
{
assert( M.valuePtr()[ i ] == M.valuePtr()[ i + 1 ] ); assert( M.valuePtr()[ i ] > 0.0 );
result.segment<2>( i ) += M.valuePtr()[ i ] * m_g;
}
}
