static void initializeImpulses( const ImpulsesToCache cache_mode, const unsigned ambient_dims, const std::vector<std::unique_ptr<Constraint>>& active_set, ConstrainedSystem& csys, VectorXs& alpha, VectorXs& beta )
{
switch( cache_mode )
{
case ImpulsesToCache::NONE:
alpha.setZero();
beta.setZero();
assert( csys.constraintCacheEmpty() );
break;
case ImpulsesToCache::NORMAL:
{
unsigned col_num{ 0 };
for( const std::unique_ptr<Constraint>& constraint : active_set )
{
VectorXs cached_impulse{ 1 };
csys.getCachedConstraintImpulse( *constraint, cached_impulse );
alpha( col_num++ ) = cached_impulse( 0 );
}
assert( col_num == active_set.size() );
beta.setZero();
csys.clearConstraintCache();
break;
}
case ImpulsesToCache::NORMAL_AND_FRICTION:
{
if( ambient_dims == 2 )
{
unsigned col_num{ 0 };
for( const std::unique_ptr<Constraint>& constraint : active_set )
{
VectorXs cached_impulse{ 2 };
csys.getCachedConstraintImpulse( *constraint, cached_impulse );
alpha( col_num ) = cached_impulse( 0 );
beta( col_num++ ) = cached_impulse( 1 );
}
assert( col_num == active_set.size() );
}
else
{
std::cerr << "Decaching in " << ambient_dims << " space not yet supported." << std::endl;
std::exit( EXIT_FAILURE );
}
csys.clearConstraintCache();
break;
}
}
}
void RigidBody2DSim::computeForce( const VectorXs& q, const VectorXs& v, const scalar& t, VectorXs& F )
{
assert( q.size() % 3 == 0 ); assert( v.size() == q.size() ); assert( v.size() == F.size() );
F.setZero();
const std::vector<std::unique_ptr<RigidBody2DForce>>& forces{ m_state.forces() };
for( const std::unique_ptr<RigidBody2DForce>& force : forces )
{
force->computeForce( q, v, m_state.M(), F );
}
}