Ejemplo n.º 1
0
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;
    }
  }
}
Ejemplo n.º 2
0
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 );
  }
}