C++ (Cpp) FlowableSystem::numBodies示例

示例#1

文件： SymplecticEulerMap.cpp 项目： alecjacobson/scisim

void SymplecticEulerMap::flow( const VectorXs& q0, const VectorXs& v0, FlowableSystem& fsys, const unsigned iteration, const scalar& dt, VectorXs& q1, VectorXs& v1 )
{
  assert( iteration > 0 );
  const scalar start_time{ ( iteration - 1 ) * dt };

  // Use q1 as temporary storage for the force
  fsys.computeForce( q0, v0, start_time, q1 );

  // Zero the force for fixed bodies
  const unsigned nbodies{ fsys.numBodies() };
  for( unsigned bdy_idx = 0; bdy_idx < nbodies; ++bdy_idx )
  {
    if( fsys.isKinematicallyScripted( bdy_idx ) )
    {
      q1.segment<3>( 3 * bdy_idx ).setZero();
    }
  }

  // Velocity update
  v1 = v0 + dt * fsys.Minv() * q1;

  // Position update
  q1 = q0 + dt * v1;
}