C++ MatrixXXsc::cols方法代码示例

void Constraint::computeBasis( const VectorXs& q, const VectorXs& v, MatrixXXsc& basis ) const
{
  computeContactBasis( q, v, basis );
  assert( basis.rows() == basis.cols() );
  assert( ( basis * basis.transpose() - MatrixXXsc::Identity( basis.rows(), basis.cols() ) ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
  assert( fabs( basis.determinant() - 1.0 ) <= 1.0e-6 );
}

void Constraint::evalKinematicRelVelGivenBases( const VectorXs& q, const VectorXs& v, const std::vector<std::unique_ptr<Constraint>>& constraints, const MatrixXXsc& bases, VectorXs& nrel, VectorXs& drel )
{
  assert( bases.cols() == nrel.size() + drel.size() );
  assert( nrel.size() % constraints.size() == 0 );
  assert( drel.size() % constraints.size() == 0 );

  // Number of constraints in the system
  const unsigned ncons{ static_cast<unsigned>( constraints.size() ) };

  // Number of tangent friction samples per constraint in the system
  const unsigned friction_vectors_per_con{ static_cast<unsigned>( drel.size() / ncons ) };

  for( unsigned con_num = 0; con_num < ncons; ++con_num )
  {
    // Grab the kinematic relative velocity
    const VectorXs kinematic_rel_vel{ constraints[con_num]->computeKinematicRelativeVelocity( q, v ) };
    assert( kinematic_rel_vel.size() == bases.rows() );

    // Compute the column of the normal in the bases matrix
    const unsigned n_idx{ ( friction_vectors_per_con + 1 ) * con_num };
    assert( n_idx < bases.cols() ); assert( fabs( bases.col( n_idx ).norm() - 1.0 ) <= 1.0e-6 );
    // Project the relative velocity onto the normal
    nrel( con_num ) = - kinematic_rel_vel.dot( bases.col( n_idx ) );
    // For each tangent friction sample
    for( unsigned friction_sample = 0; friction_sample < friction_vectors_per_con; ++friction_sample )
    {
      // Compute the column of the current friction sample in the bases matrix
      const unsigned f_idx{ ( friction_vectors_per_con + 1 ) * con_num + friction_sample + 1 };
      assert( f_idx < bases.cols() );
      assert( fabs( bases.col( f_idx ).norm() - 1.0 ) <= 1.0e-6 );
      assert( fabs( bases.col( n_idx ).dot( bases.col( f_idx ) ) ) <= 1.0e-6 );
      drel( friction_vectors_per_con * con_num + friction_sample ) = - kinematic_rel_vel.dot( bases.col( f_idx ) );
    }
  }
}

void TeleportedCircleCircleConstraint::evalH( const VectorXs& q, const MatrixXXsc& basis, MatrixXXsc& H0, MatrixXXsc& H1 ) const
{
  assert( H0.rows() == 2 ); assert( H0.cols() == 3 );
  assert( H1.rows() == 2 ); assert( H1.cols() == 3 );
  assert( ( basis * basis.transpose() - MatrixXXsc::Identity( 2, 2 ) ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
  assert( fabs( basis.determinant() - 1.0 ) <= 1.0e-6 );

  // Grab the contact normal
  const Vector2s n{ basis.col( 0 ) };
  // Grab the tangent basis
  const Vector2s t{ basis.col( 1 ) };

  // Format for H:
  //   n^T  r x n
  //   t^T  r x t

  H0.block<1,2>(0,0) = n;
  assert( fabs( MathUtilities::cross( m_r0, n ) ) <= 1.0e-6 );
  H0(0,2) = 0.0;

  H0.block<1,2>(1,0) = t;
  H0(1,2) = MathUtilities::cross( m_r0, t );

  H1.block<1,2>(0,0) = n;
  assert( fabs( MathUtilities::cross( m_r1, n ) ) <= 1.0e-6 );
  H1(0,2) = 0.0;

  H1.block<1,2>(1,0) = t;
  H1(1,2) = MathUtilities::cross( m_r1, t );
}

void StaticPlaneSphereConstraint::evalH( const VectorXs& q, const MatrixXXsc& basis, MatrixXXsc& H0, MatrixXXsc& H1 ) const
{
  assert( H0.rows() == 3 );
  assert( H0.cols() == 6 );
  assert( H1.rows() == 3 );
  assert( H1.cols() == 6 );

  // Grab the contact normal
  const Vector3s n{ basis.col( 0 ) };
  // Grab the tangent basis
  const Vector3s s{ basis.col( 1 ) };
  const Vector3s t{ basis.col( 2 ) };
  assert( MathUtilities::isRightHandedOrthoNormal( n, s, t, 1.0e-6 ) );

  // Compute the displacement from the center of mass to the point of contact
  assert( m_r >= 0.0 );
  const Vector3s r_world{ - m_r * n };

  H0.block<1,3>(0,0) = n;
  H0.block<1,3>(0,3).setZero();

  H0.block<1,3>(1,0) = s;
  H0.block<1,3>(1,3) = r_world.cross( s );

  H0.block<1,3>(2,0) = t;
  H0.block<1,3>(2,3) = r_world.cross( t );
}

void MathUtilities::convertDenseToSparse( const bool filter_zeros, const MatrixXXsc& dense_matrix, SparseMatrixsc& sparse_matrix )
{
  std::vector<Eigen::Triplet<scalar>> triplets;
  for( int row = 0; row < dense_matrix.rows(); ++row )
  {
    for( int col = 0; col < dense_matrix.cols(); ++col )
    {
      if( dense_matrix( row, col ) != 0.0 || !filter_zeros )
      {
        triplets.emplace_back( Eigen::Triplet<scalar>{ row, col, dense_matrix( row, col ) } );
      }
    }
  }
  sparse_matrix.resize( dense_matrix.rows(), dense_matrix.cols() );
  sparse_matrix.setFromTriplets( std::begin( triplets ), std::end( triplets ) );
  sparse_matrix.makeCompressed();
}

void Constraint::computeForcingTerm( const VectorXs& q, const VectorXs& v, const MatrixXXsc& basis, const scalar& CoR, const scalar& nrel, const VectorXs& drel, VectorXs& constant_term ) const
{
  assert( basis.rows() == basis.cols() );
  assert( ( basis * basis.transpose() - MatrixXXsc::Identity( basis.rows(), basis.cols() ) ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
  assert( fabs( basis.determinant() - 1.0 ) <= 1.0e-6 );
  assert( CoR >= 0.0 ); assert( CoR <= 1.0 );

  constant_term = VectorXs::Zero( basis.rows() );
  assert( fabs( evalNdotV( q, v ) - basis.col(0).dot( computeRelativeVelocity( q, v ) ) ) <= 1.0e-6 );
  //constant_term += basis.col(0) * ( CoR * ( evalNdotV( q, v ) - nrel ) + ( 1.0 + CoR ) * nrel );
  constant_term += basis.col(0) * ( CoR * evalNdotV( q, v ) + nrel );
  assert( drel.size() == basis.cols() - 1 );
  for( unsigned friction_sample = 0; friction_sample < drel.size(); ++friction_sample )
  {
    constant_term += basis.col( friction_sample + 1 ) * drel( friction_sample );
  }
}

// TODO: Don't do the if here, have children do what they need to do
scalar Constraint::computeLambda( const VectorXs& q, const VectorXs& v ) const
{
  MatrixXXsc basis;
  computeBasis( q, v, basis );
  assert( basis.rows() == basis.cols() );
  assert( basis.cols() == 2 || basis.cols() == 3 );
  const VectorXs rel_vel = computeRelativeVelocity( q, v );
  assert( rel_vel.size() == basis.rows() );
  if( basis.cols() == 3 )
  {
    const Vector2s tangent_vel( rel_vel.dot( basis.col( 1 ) ), rel_vel.dot( basis.col( 2 ) ) );
    return tangent_vel.norm();
  }
  else if( basis.cols() == 2 )
  {
    return fabs( rel_vel.dot( basis.col( 1 ) ) );
  }
  std::cerr << "Unhandled case in Constraint::computeLambda" << std::endl;
  std::exit( EXIT_FAILURE );
}

void BodyBodyConstraint::evalH( const VectorXs& q, const MatrixXXsc& basis, MatrixXXsc& H0, MatrixXXsc& H1 ) const
{
  assert( H0.rows() == 3 );
  assert( H0.cols() == 6 );
  assert( H1.rows() == 3 );
  assert( H1.cols() == 6 );
  assert( ( m_n - basis.col( 0 ) ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );

  // Grab the contact normal
  const Vector3s n{ basis.col( 0 ) };
  // Grab the tangent basis
  const Vector3s s{ basis.col( 1 ) };
  const Vector3s t{ basis.col( 2 ) };
  assert( MathUtilities::isRightHandedOrthoNormal( n, s, t, 1.0e-6 ) );

  // Format for H:
  //   n^T  \tilde{n}^T
  //   s^T  \tilde{s}^T
  //   t^T  \tilde{t}^T

  H0.block<1,3>(0,0) = n;
  H0.block<1,3>(0,3) = m_r0.cross( n );

  H0.block<1,3>(1,0) = s;
  H0.block<1,3>(1,3) = m_r0.cross( s );

  H0.block<1,3>(2,0) = t;
  H0.block<1,3>(2,3) = m_r0.cross( t );

  H1.block<1,3>(0,0) = n;
  H1.block<1,3>(0,3) = m_r1.cross( n );

  H1.block<1,3>(1,0) = s;
  H1.block<1,3>(1,3) = m_r1.cross( s );

  H1.block<1,3>(2,0) = t;
  H1.block<1,3>(2,3) = m_r1.cross( t );
}

void RigidBody2DSim::computeContactBases( const VectorXs& q, const VectorXs& v, const std::vector<std::unique_ptr<Constraint>>& active_set, MatrixXXsc& contact_bases ) const
{
  const unsigned ncols{ static_cast<unsigned>( active_set.size() ) };
  contact_bases.resize( 2, 2 * ncols );
  for( unsigned col_num = 0; col_num < ncols; ++col_num )
  {
    MatrixXXsc basis;
    active_set[col_num]->computeBasis( q, v, basis );
    assert( basis.rows() == basis.cols() ); assert( basis.rows() == 2 );
    assert( ( basis * basis.transpose() - MatrixXXsc::Identity( 2, 2 ) ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
    assert( fabs( basis.determinant() - 1.0 ) <= 1.0e-6 );
    contact_bases.block<2,2>( 0, 2 * col_num ) = basis;
  }
}

// TODO: Unspecialize from 3D
void Constraint::computeNormalAndRelVelAlignedTangent( const VectorXs& q, const VectorXs& v, VectorXs& n, VectorXs& t, VectorXs& tangent_rel_vel ) const
{
  MatrixXXsc basis;
  computeBasis( q, v, basis );
  assert( basis.rows() == basis.cols() ); assert( basis.rows() == 3 );
  n = basis.col( 0 );
  t = basis.col( 1 );

  // Compute the relative velocity
  tangent_rel_vel = computeRelativeVelocity( q, v );
  // Project out normal component of relative velocity
  tangent_rel_vel = tangent_rel_vel - tangent_rel_vel.dot( n ) * n;

  #ifndef NDEBUG
  // Relative velocity and tangent should be parallel
  assert( Eigen::Map<Vector3s>( tangent_rel_vel.data() ).cross( Eigen::Map<Vector3s>( t.data() ) ).lpNorm<Eigen::Infinity>() <= 1.0e-6 );
  // If tangent relative velocity is non-negligble, tangent should oppose
  if( tangent_rel_vel.norm() > 1.0e-9 )
  {
    assert( fabs( tangent_rel_vel.normalized().dot( t ) + 1.0 ) <= 1.0e-6 );
  }
  #endif
}

static int solveQP( const scalar& tol, MatrixXXsc& C, VectorXs& dvec, VectorXs& alpha )
{
  static_assert( std::is_same<scalar,double>::value, "QL only supports doubles." );

  assert( dvec.size() == alpha.size() );

  // All constraints are bound constraints.
  int m = 0;
  int me = 0;
  int mmax = 0;

  // C should be symmetric
  assert( ( C - C.transpose() ).lpNorm<Eigen::Infinity>() < 1.0e-14 );
  // Number of degrees of freedom.
  assert( C.rows() == C.cols() );
  int n{ int( C.rows() ) };
  int nmax = n;
  int mnn = m + n + n;
  assert( dvec.size() == nmax );

  // Impose non-negativity constraints on all variables
  Eigen::VectorXd xl = Eigen::VectorXd::Zero( nmax );
  Eigen::VectorXd xu = Eigen::VectorXd::Constant( nmax, std::numeric_limits<double>::infinity() );

  // u will contain the constraint multipliers
  Eigen::VectorXd u( mnn );

  // Status of the solve
  int ifail = -1;
  // Use the built-in cholesky decomposition
  int mode = 1;
  // Some fortran output stuff
  int iout = 0;
  // 1 => print output, 0 => silent
  int iprint = 1;

  // Working space
  assert( m == 0 && me == 0 && mmax == 0 );
  int lwar = 3 * ( nmax * nmax ) / 2 + 10 * nmax + 2;
  Eigen::VectorXd war( lwar );
  // Additional working space
  int liwar = n;
  Eigen::VectorXi iwar( liwar );

  {
    scalar tol_local = tol;
    ql_( &m,
        &me,
        &mmax,
        &n,
        &nmax,
        &mnn,
        C.data(),
        dvec.data(),
        nullptr,
        nullptr,
        xl.data(),
        xu.data(),
        alpha.data(),
        u.data(),
        &tol_local,
        &mode,
        &iout,
        &ifail,
        &iprint,
        war.data(),
        &lwar,
        iwar.data(),
        &liwar );
  }

  return ifail;
}

void ImpactFrictionMap::exportConstraintForcesToBinaryFile( const VectorXs& q, const std::vector<std::unique_ptr<Constraint>>& constraints, const MatrixXXsc& contact_bases, const VectorXs& alpha, const VectorXs& beta, const scalar& dt, HDF5File& output_file )
{
  const unsigned ncons = constraints.size();
  assert( ncons == alpha.size() );
  assert( std::vector<std::unique_ptr<Constraint>>::size_type( ncons ) == constraints.size() );
  assert( alpha.size() == ncons );

  const unsigned ambient_space_dims{ static_cast<unsigned>( contact_bases.rows() ) };
  assert( ambient_space_dims == 2 || ambient_space_dims == 3 );
  assert( beta.size() == ncons * ( ambient_space_dims - 1 ) );

  // Write out the number of collisions
  output_file.writeScalar( "", "collision_count", ncons );

  // NB: Prior to version 1.8.7, HDF5 did not support zero sized dimensions.
  //     Some versions of Ubuntu still have old versions of HDF5, so workaround.
  // TODO: Remove once our servers are updated.
  if( ncons == 0 )
  {
    return;
  }

  // Write out the indices of all bodies involved
  {
    // Place all indices into a single matrix for output
    Matrix2Xic collision_indices{ 2, ncons };
    for( unsigned con = 0; con < ncons; ++con )
    {
      std::pair<int,int> indices;
      assert( constraints[con] != nullptr );
      getCollisionIndices( *constraints[con], indices );
      collision_indices.col( con ) << indices.first, indices.second;
    }
    // Output the indices
    output_file.writeMatrix( "", "collision_indices", collision_indices );
  }

  // Write out the world space contact points
  {
    // Place all contact points into a single matrix for output
    MatrixXXsc collision_points{ ambient_space_dims, ncons };
    for( unsigned con = 0; con < ncons; ++con )
    {
      VectorXs contact_point;
      assert( constraints[con] != nullptr );
      constraints[con]->getWorldSpaceContactPoint( q, contact_point );
      assert( contact_point.size() == ambient_space_dims );
      collision_points.col( con ) = contact_point;
    }
    // Output the points
    output_file.writeMatrix( "", "collision_points", collision_points );
  }

  // Write out the world space contact normals
  {
    // Place all contact normals into a single matrix for output
    MatrixXXsc collision_normals{ ambient_space_dims, ncons };
    for( unsigned con = 0; con < ncons; ++con )
    {
      collision_normals.col( con ) = contact_bases.col( ambient_space_dims * con );
    }
    #ifndef NDEBUG
    for( unsigned con = 0; con < ncons; ++con )
    {
      assert( fabs( collision_normals.col( con ).norm() - 1.0 ) <= 1.0e-6 );
    }
    #endif
    // Output the normals
    output_file.writeMatrix( "", "collision_normals", collision_normals );
  }

  // Write out the collision forces
  {
    // Place all contact forces into a single matrix for output
    MatrixXXsc collision_forces{ ambient_space_dims, ncons };
    for( unsigned con = 0; con < ncons; ++con )
    {
      // Contribution from normal
      collision_forces.col( con ) = alpha( con ) * contact_bases.col( ambient_space_dims * con );
      // Contribution from friction
      for( unsigned friction_sample = 0; friction_sample < ambient_space_dims - 1; ++friction_sample )
      {
        assert( ( ambient_space_dims - 1 ) * con + friction_sample < beta.size() );
        const scalar impulse{ beta( ( ambient_space_dims - 1 ) * con + friction_sample ) };

        const unsigned column_number{ ambient_space_dims * con + friction_sample + 1 };
        assert( column_number < contact_bases.cols() );
        assert( fabs( contact_bases.col( ambient_space_dims * con ).dot( contact_bases.col( column_number ) ) ) <= 1.0e-6 );

        collision_forces.col( con ) += impulse * contact_bases.col( column_number );
      }
    }
    // Output the forces
    output_file.writeMatrix( "", "collision_forces", collision_forces );
  }
}

MatrixXXsc Constraint::computeFrictionBasis( const VectorXs& q, const VectorXs& v ) const
{
  MatrixXXsc basis;
  computeBasis( q, v, basis );
  return basis.block( 0, 1, basis.rows(), basis.cols() - 1 );
}