RigidBody::RigidBody( const Vector2s& v, const scalar& omega, const VectorXs& vertices, const VectorXs& masses, const scalar& radius )
: m_M(-1.0)
//, m_masses(masses)
, m_I(-1.0)
, m_vertices(vertices)
, m_r(radius)
, m_X(-1.0,-1.0)
, m_theta(0.0)
, m_V(v)
, m_omega(omega)
// theta == 0 => identity matrix
, m_R(Matrix2s::Identity())
, m_F(0.0,0.0)
, m_tau(0.0)
{
m_M = computeTotalMass(masses);
m_X = computeCenterOfMass(vertices,masses);
m_I = computeMomentOfInertia(vertices,masses);
assert( (masses.array()>0.0).all() );
assert( m_M > 0.0 );
assert( m_I > 0.0 );
assert( m_vertices.size() >= 2 );
assert( m_r >= 0.0 );
// Translate the rigid body so the body space center of mass is the origin
for( int i = 0; i < m_vertices.size()/2; ++i ) m_vertices.segment<2>(2*i) -= m_X;
}
void ScriptingCallback::frictionCoefficientCallback( const std::vector<std::unique_ptr<Constraint>>& active_set, VectorXs& mu )
{
if( name() == "" )
{
return;
}
frictionCoefficient( active_set, mu );
assert( ( mu.array() >= 0.0 ).all() );
}
void ScriptingCallback::restitutionCoefficientCallback( const std::vector<std::unique_ptr<Constraint>>& active_set, VectorXs& cor )
{
if( name() == "" )
{
return;
}
restitutionCoefficient( active_set, cor );
assert( ( cor.array() >= 0.0 ).all() );
assert( ( cor.array() <= 1.0 ).all() );
}
void GRROperator::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 )
{
// Not intended for use with staggered projections
assert( ( v0.array() == v0F.array() ).all() );
// Only works with one CoR
assert( ( CoR.array() == CoR(0) ).all() );
if( ( CoR.array() != CoR(0) ).any() )
{
std::cerr << "Error, GRR only supports a single coefficient of restitution. Exiting." << std::endl;
std::exit( EXIT_FAILURE );
}
// If the CoR is 0 or 1, don't waste time calling the other map
if( CoR(0) == 0.0 )
{
m_inelastic_operator->flow( cons, M, Minv, q0, v0, v0F, N, Q, nrel, CoR, alpha );
return;
}
else if( CoR(0) == 1.0 )
{
m_elastic_operator->flow( cons, M, Minv, q0, v0, v0F, N, Q, nrel, CoR, alpha );
return;
}
// Create temporary storage
VectorXs alpha_out{ alpha.size() };
alpha_out.setZero();
assert( m_inelastic_operator != nullptr );
m_inelastic_operator->flow( cons, M, Minv, q0, v0, v0F, N, Q, nrel, VectorXs::Zero( CoR.size() ), alpha_out );
//v1 = ( 1.0 - CoR ) * v_out;
alpha = ( 1.0 - CoR(0) ) * alpha_out;
alpha_out.setZero();
assert( m_elastic_operator != nullptr );
m_elastic_operator->flow( cons, M, Minv, q0, v0, v0F, N, Q, nrel, VectorXs::Ones( CoR.size() ), alpha_out );
//v1 += CoR * v_out;
alpha += CoR(0) * alpha_out;
}
void FrictionOperatorUtilities::projectOnFrictionDisc( const VectorXs& disc_bounds, VectorXs& beta )
{
assert( beta.size() % 2 == 0 ); assert( beta.size() / 2 == disc_bounds.size() ); assert( ( disc_bounds.array() >= 0.0 ).all() );
for( int con_num = 0; con_num < disc_bounds.size(); ++con_num )
{
const scalar dsc_nrm_sqrd{ beta.segment<2>( 2 * con_num ).squaredNorm() };
// If the squared norm of the friction impulse is greater than the \mu \alpha squared
if( dsc_nrm_sqrd > disc_bounds( con_num ) * disc_bounds( con_num ) )
{
// Normalize beta and rescale so its magnitude is \mu \alpha
beta.segment<2>( 2 * con_num ) *= disc_bounds( con_num ) / sqrt( dsc_nrm_sqrd );
assert( fabs( beta.segment<2>( 2 * con_num ).norm() - disc_bounds( con_num ) ) <= 1.0e-6 );
}
}
}
