Пример #1
0
void RigidBody2DSim::flow( PythonScripting& call_back, const unsigned iteration, const Rational<std::intmax_t>& dt, UnconstrainedMap& umap )
{
  call_back.setState( m_state );
  call_back.startOfStepCallback( iteration, dt );
  call_back.forgetState();

  VectorXs q1{ m_state.q().size() };
  VectorXs v1{ m_state.v().size() };

  updatePeriodicBoundaryConditionsStartOfStep( iteration, scalar(dt) );

  umap.flow( m_state.q(), m_state.v(), *this, iteration, scalar(dt), q1, v1 );

  q1.swap( m_state.q() );
  v1.swap( m_state.v() );

  enforcePeriodicBoundaryConditions( m_state.q(), m_state.v() );

  call_back.setState( m_state );
  call_back.endOfStepCallback( iteration, dt );
  call_back.forgetState();
}
Пример #2
0
void GeometricImpactFrictionMap::flow( ScriptingCallback& call_back, FlowableSystem& fsys, ConstrainedSystem& csys, UnconstrainedMap& umap, FrictionSolver& friction_solver, const unsigned iteration, const scalar& dt, const scalar& CoR_default, const scalar& mu_default, const VectorXs& q0, const VectorXs& v0, VectorXs& q1, VectorXs& v1 )
{
  // TODO: Sanity check input sizes

  // TODO: Do something more elegant when the number of bodies changes due to insertions and deletions, like registering the maps with the scripting callbacks so they can update the cache.
  // Initialize the friction impulse cache
  if( m_f.size() != v0.size() )
  {
    m_f = VectorXs::Zero( v0.size() );
  }

  // Compute an unconstrained predictor step
  umap.flow( q0, v0, fsys, iteration, dt, q1, v1 );

  // Using the configuration at the predictor step, compute the set of active constraints
  std::vector<std::unique_ptr<Constraint>> active_set;
  csys.computeActiveSet( q0, q1, v0, active_set );

  // If there are no active constraints, there is no need to perform collision response
  if( active_set.empty() )
  {
    csys.clearConstraintCache();
    if( m_write_constraint_forces )
    {
      exportConstraintForcesToBinary( q0, active_set, MatrixXXsc{ fsys.ambientSpaceDimensions(), 0 }, VectorXs::Zero(0), VectorXs::Zero(0), dt );
    }
    m_write_constraint_forces = false;
    m_constraint_force_stream = nullptr;
    return;
  }

  const unsigned ncollisions{ static_cast<unsigned>( active_set.size() ) };

  // Pre-compute the full contact basis
  MatrixXXsc contact_bases;
  csys.computeContactBases( q0, v0, active_set, contact_bases );
  assert( contact_bases.rows() == fsys.ambientSpaceDimensions() );
  assert( contact_bases.cols() == fsys.ambientSpaceDimensions() * ncollisions );

  // Set the coefficients of friction to the default
  VectorXs mu{ VectorXs::Constant( ncollisions, mu_default ) };
  // If scripting is enabled, use the scripted version
  call_back.frictionCoefficientCallback( active_set, mu );
  assert( ( mu.array() >= 0.0 ).all() );

  // Coefficients of restitution
  VectorXs CoR{ VectorXs::Constant( ncollisions, CoR_default ) };
  // If scripting is enabled, use the scripted version
  call_back.restitutionCoefficientCallback( active_set, CoR );
  assert( ( CoR.array() >= 0.0 ).all() ); assert( ( CoR.array() <= 1.0 ).all() );

  // Normal impulse magnitudes
  VectorXs alpha{ ncollisions };
  // Friction impulses magnitudes
  VectorXs beta{ friction_solver.numFrictionImpulsesPerNormal( fsys.ambientSpaceDimensions() ) * ncollisions };

  initializeImpulses( m_impulses_to_cache, fsys.ambientSpaceDimensions(), active_set, csys, alpha, beta );

  // Compute the initial momentum and angular momentum
  #ifndef NDEBUG
  const bool momentum_should_be_conserved{ constraintSetShouldConserveMomentum( active_set ) };
  VectorXs p0;
  if( momentum_should_be_conserved ) { fsys.computeMomentum( v0, p0 ); }
  const bool angular_momentum_should_be_conserved{ constraintSetShouldConserveAngularMomentum( active_set ) };
  VectorXs L0;
  if( angular_momentum_should_be_conserved ) { fsys.computeAngularMomentum( v0, L0 ); }
  #endif

  // Perform the coupled impact/friction solve
  VectorXs v2{ v0.size() };
  {
    scalar error;
    bool solve_succeeded;
    friction_solver.solve( iteration, dt, fsys, fsys.M(), fsys.Minv(), CoR, mu, q0, v0, active_set, contact_bases, m_max_iters, m_abs_tol, m_f, alpha, beta, v2, solve_succeeded, error );
    assert( error >= 0.0 );
    if( !solve_succeeded )
    {
      std::cerr << "Warning, coupled impact/friction solve exceeded max iterations " << m_max_iters;
      std::cerr << " with absolute error " << error << " stepping to time " << iteration * dt << std::endl;
    }
  }

  // Cache the impulses, verify the expected uncached impulses
  // Note: temporarily disabled test as it requires impulse caching support for all collision types
  //#ifndef NDEBUG
  //{
  //  cacheImpulses( ImpulsesToCache::NONE, fsys.ambientSpaceDimensions(), active_set, csys, alpha, beta );
  //  VectorXs alpha_test{ alpha.size() };
  //  VectorXs beta_test{ beta.size() };
  //  initializeImpulses( ImpulsesToCache::NONE, fsys.ambientSpaceDimensions(), active_set, csys, alpha_test, beta_test );
  //  assert( ( alpha_test.array() == 0.0 ).all() );
  //  assert( ( beta_test.array() == 0.0 ).all() );
  //}
  //{
  //  cacheImpulses( ImpulsesToCache::NORMAL, fsys.ambientSpaceDimensions(), active_set, csys, alpha, beta );
  //  VectorXs alpha_test{ alpha.size() };
  //  VectorXs beta_test{ beta.size() };
  //  initializeImpulses( ImpulsesToCache::NORMAL, fsys.ambientSpaceDimensions(), active_set, csys, alpha_test, beta_test );
  //  assert( ( alpha_test.array() == alpha.array() ).all() );
  //  assert( ( beta_test.array() == 0.0 ).all() );
  //}
  //{
  //  cacheImpulses( ImpulsesToCache::NORMAL_AND_FRICTION, fsys.ambientSpaceDimensions(), active_set, csys, alpha, beta );
  //  VectorXs alpha_test{ alpha.size() };
  //  VectorXs beta_test{ beta.size() };
  //  initializeImpulses( ImpulsesToCache::NORMAL_AND_FRICTION, fsys.ambientSpaceDimensions(), active_set, csys, alpha_test, beta_test );
  //  assert( ( alpha_test.array() == alpha.array() ).all() );
  //  assert( ( beta_test.array() == beta.array() ).all() );
  //}
  //#endif

  // For the special case mu == 0, friction should be zero
  #ifndef NDEBUG
  {
    const unsigned friction_impulses_per_collision{ friction_solver.numFrictionImpulsesPerNormal( fsys.ambientSpaceDimensions() ) };
    for( unsigned col_idx = 0; col_idx < ncollisions; ++col_idx )
    {
      if( mu(col_idx) == 0.0 )
      {
        assert( ( beta.segment( friction_impulses_per_collision, col_idx * friction_impulses_per_collision ).array() == 0.0 ).all() );
      }
    }
  }
  #endif

  // Verify that momentum and angular momentum are conserved
  #ifndef NDEBUG
  if( momentum_should_be_conserved )
  {
    VectorXs p1;
    if( momentum_should_be_conserved ) { fsys.computeMomentum( v2, p1 ); }
    assert( ( p0 - p1 ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
  }
  if( angular_momentum_should_be_conserved )
  {
    VectorXs L1;
    if( angular_momentum_should_be_conserved ) { fsys.computeAngularMomentum( v2, L1 ); }
    assert( ( L0 - L1 ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
  }
  #endif

  // TODO: Derive bounds for gdotN and gdotD != 0.0
  // Verify that the total energy decreased
  //if( ( gdotN.array() == 0.0 ).all() && ( gdotD.array() == 0.0 ).all() )
  //{
  //  const scalar T_init = v0.dot( fsys.M() * v0 );
  //  const scalar T_final = v2.dot( fsys.M() * v2 );
  //  if( T_final > T_init + 1.0e-6 )
  //  {
  //    std::cerr << "WARNING, energy increase detected in GeometricImpactFrictionMap: " << T_final - T_init << "     error: " << error << std::endl;
  //  }
  //}

  // Sanity check: no impulses should apply to kinematic geometry
  //assert( ImpactFrictionMap::noImpulsesToKinematicGeometry( fsys, N, alpha, D, beta, v0 ) );

  // Cache the constraints for warm starting
  cacheImpulses( m_impulses_to_cache, fsys.ambientSpaceDimensions(), active_set, csys, alpha, beta );

  // Export constraint forces, if requested
  if( m_write_constraint_forces )
  {
    exportConstraintForcesToBinary( q0, active_set, contact_bases, alpha, beta, dt );
  }
  m_write_constraint_forces = false;
  m_constraint_force_stream = nullptr;

  // Using the initial configuration and the new velocity, compute the final state
  umap.flow( q0, v2, fsys, iteration, dt, q1, v1 );
}