C++ SparseMatrixsc::rows方法代码示例

void BodyBodyConstraint::evalgradg( const VectorXs& q, const int col, SparseMatrixsc& G, const FlowableSystem& fsys ) const
{
  assert( q.size() % 12 == 0 );
  assert( col >= 0 );
  assert( col < G.cols() );

  const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };

  // MUST BE ADDED GOING DOWN THE COLUMN. DO NOT TOUCH ANOTHER COLUMN.
  {
    assert( 3 * nbodies + 3 * m_idx0 + 2 < unsigned( G.rows() ) );
    G.insert( 3 * m_idx0 + 0, col ) = m_n.x();
    G.insert( 3 * m_idx0 + 1, col ) = m_n.y();
    G.insert( 3 * m_idx0 + 2, col ) = m_n.z();
    const Vector3s ntilde_0{ m_r0.cross( m_n ) };
    G.insert( 3 * ( m_idx0 + nbodies ) + 0, col ) = ntilde_0.x();
    G.insert( 3 * ( m_idx0 + nbodies ) + 1, col ) = ntilde_0.y();
    G.insert( 3 * ( m_idx0 + nbodies ) + 2, col ) = ntilde_0.z();
  }

  {
    assert( 3 * nbodies + 3 * m_idx1 + 2 < unsigned( G.rows() ) );
    G.insert( 3 * m_idx1 + 0, col ) = - m_n.x();
    G.insert( 3 * m_idx1 + 1, col ) = - m_n.y();
    G.insert( 3 * m_idx1 + 2, col ) = - m_n.z();
    const Vector3s ntilde_1{ m_r1.cross( m_n ) };
    G.insert( 3 * ( m_idx1 + nbodies ) + 0, col ) = - ntilde_1.x();
    G.insert( 3 * ( m_idx1 + nbodies ) + 1, col ) = - ntilde_1.y();
    G.insert( 3 * ( m_idx1 + nbodies ) + 2, col ) = - ntilde_1.z();
  }
}

void TeleportedCircleCircleConstraint::evalgradg( const VectorXs& q, const int col, SparseMatrixsc& G, const FlowableSystem& fsys ) const
{
  assert( col >= 0 ); assert( col < G.cols() );

  // MUST BE ADDED GOING DOWN THE COLUMN. DO NOT TOUCH ANOTHER COLUMN.
  assert( m_idx0 < m_idx1 );
  assert( 3 * m_idx0 + 1 < unsigned( G.rows() ) );
  G.insert( 3 * m_idx0 + 0, col ) = m_n.x();
  G.insert( 3 * m_idx0 + 1, col ) = m_n.y();
  assert( 3 * m_idx1 + 1 < unsigned( G.rows() ) );
  G.insert( 3 * m_idx1 + 0, col ) = - m_n.x();
  G.insert( 3 * m_idx1 + 1, col ) = - m_n.y();
}

void MathUtilities::createDiagonalMatrix( const scalar& c, SparseMatrixsc& D )
{
  assert( D.rows() == D.cols() );
  D.reserve( VectorXi::Constant( D.cols(), 1 ) );
  for( int i = 0; i < D.cols(); ++i ) { D.insert(i,i) = c; }
  D.makeCompressed();
}

scalar HertzianPenaltyForce::computePotential( const VectorXs& q, const SparseMatrixsc& M, const VectorXs& r ) const
{
  assert( q.size() % 2 == 0 ); assert( q.size() == M.rows() ); assert( q.size() == M.cols() ); assert( r.size() == q.size() / 2 );

  scalar U{ 0.0 };
  // For each ball
  for( unsigned ball0 = 0; ball0 < r.size(); ++ball0 )
  {
    // For each subsequent ball
    for( unsigned ball1 = ball0 + 1; ball1 < r.size(); ++ball1 )
    {
      // Compute the total radius
      const scalar total_radius{ r(ball0) + r(ball1) };
      // Compute a vector pointing from ball0 to ball1
      const Vector2s n{ q.segment<2>( 2 * ball1 ) - q.segment<2>( 2 * ball0 ) };
      // If the squared distance is greater or equal to the sum of the radii squared, no force
      if( n.squaredNorm() > total_radius * total_radius )
      {
        continue;
      }
      // Compute the penetration depth
      const scalar delta{ n.norm() - total_radius };
      assert( delta < 0.0 );
      // U = 0.5 * k * pen_depth^(5/2)
      U += 0.5 * m_k * std::pow( -delta, scalar( 2.5 ) );
    }
  }

  return U;
}

void Ball2DGravityForce::computeForce( const VectorXs& q, const VectorXs& v, const SparseMatrixsc& M, const VectorXs& r, VectorXs& result ) const
{
  assert( q.size() % 2 == 0 ); assert( q.size() == v.size() ); assert( q.size() == M.rows() );
  assert( q.size() == M.cols() ); assert( q.size() == result.size() );

  for( int i = 0; i < q.size(); i += 2 )
  {
    assert( M.valuePtr()[ i ] == M.valuePtr()[ i + 1 ] ); assert( M.valuePtr()[ i ] > 0.0 );
    result.segment<2>( i ) += M.valuePtr()[ i ] * m_g;
  }
}

void FrictionOperatorUtilities::formGeneralizedFrictionBasis( const VectorXs& q0, const VectorXs& v0, const std::vector<std::unique_ptr<Constraint>>& K, const int num_samples, SparseMatrixsc& D, VectorXs& drel )
{
  assert( num_samples > 0 );
  assert( D.rows() == v0.size() );
  assert( num_samples * int( K.size() ) == D.cols() );

  // Reserve space for entries
  reserveSpaceInBasisMatrix( num_samples, K, D );

  // Build the matrix
  buildLinearFrictionBasis( q0, v0, num_samples, K, D, drel );

  D.makeCompressed();
}

void StaticPlaneSphereConstraint::evalgradg( const VectorXs& q, const int col, SparseMatrixsc& G, const FlowableSystem& fsys ) const
{
  assert( col >= 0 );
  assert( col < G.cols() );
  assert( 3 * m_sphere_idx + 2 < unsigned( G.rows() ) );

  const Vector3s n{ m_plane.n() };
  assert( fabs( n.norm() - 1.0 ) <= 1.0e-6 );

  // MUST BE ADDED GOING DOWN THE COLUMN. DO NOT TOUCH ANOTHER COLUMN.
  G.insert( 3 * m_sphere_idx + 0, col ) = n.x();
  G.insert( 3 * m_sphere_idx + 1, col ) = n.y();
  G.insert( 3 * m_sphere_idx + 2, col ) = n.z();
}

void MathUtilities::extractLowerTriangularMatrix( const SparseMatrixsc& A, SparseMatrixsc& B )
{
  std::vector< Eigen::Triplet<scalar> > triplets;
  for( int col = 0; col < A.outerSize(); ++col )
  {
    for( SparseMatrixsc::InnerIterator it( A, col ); it; ++it )
    {
      if( col > it.row() ) { continue; }
      triplets.push_back( Eigen::Triplet<scalar>( it.row(), col, it.value() ) );
    }
  }
  B.resize( A.rows(), A.cols() );
  B.setFromTriplets( triplets.begin(), triplets.end() );
  B.makeCompressed();
}

void NearEarthGravityForce::computeForce( const VectorXs& q, const VectorXs& v, const SparseMatrixsc& M, VectorXs& result ) const
{
    assert( q.size() % 12 == 0 );
    assert( v.size() == q.size() / 2 );
    assert( M.rows() == M.cols() );
    assert( M.nonZeros() == q.size() );

    const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };

    const Eigen::Map<const VectorXs> masses{ M.valuePtr(), 3 * nbodies };

    for( unsigned i = 0; i < nbodies; ++i )
    {
        assert( masses( 3 * i + 0 ) == masses( 3 * i + 1 ) );
        assert( masses( 3 * i + 1 ) == masses( 3 * i + 2 ) );
        result.segment<3>( 3 * i ) += masses( 3 * i ) * m_g;
    }
}

void MathUtilities::serialize( const SparseMatrixsc& A, std::ostream& stm )
{
  assert( stm.good() );

  VectorXi col_ptr;
  VectorXi row_ind;
  VectorXs val;
  MathUtilities::extractDataCCS( A, col_ptr, row_ind, val );
  assert( col_ptr.size() == A.cols() + 1 ); assert( row_ind.size() == A.nonZeros() ); assert( val.size() == A.nonZeros() );
  // Size of col_ptr == A.cols() + 1
  Utilities::serializeBuiltInType( A.rows(), stm );
  Utilities::serializeBuiltInType( A.cols(), stm );
  stm.write( reinterpret_cast<char*>( col_ptr.data() ), col_ptr.size() * sizeof(int) );
  // Size of row_ind == size of val == A.nonZeros()
  Utilities::serializeBuiltInType( A.nonZeros(), stm );
  stm.write( reinterpret_cast<char*>( row_ind.data() ), row_ind.size() * sizeof(int) );
  stm.write( reinterpret_cast<char*>( val.data() ), val.size() * sizeof(scalar) );
}

scalar NearEarthGravityForce::computePotential( const VectorXs& q, const SparseMatrixsc& M ) const
{
    assert( q.size() % 12 == 0 );
    assert( M.rows() == M.cols() );
    assert( M.nonZeros() == q.size() );

    const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };

    const Eigen::Map<const VectorXs> masses{ M.valuePtr(), 3 * nbodies };

    scalar U = 0.0;
    for( unsigned i = 0; i < nbodies; ++i )
    {
        assert( masses( 3 * i + 0 ) == masses( 3 * i + 1 ) );
        assert( masses( 3 * i + 1 ) == masses( 3 * i + 2 ) );
        U += - masses( 3 * i ) * m_g.dot( q.segment<3>( 3 * i ) );
    }
    return U;
}

void StaticPlaneSphereConstraint::resolveImpact( const scalar& CoR, const SparseMatrixsc& M, const scalar& ndotv, VectorXs& vout, scalar& alpha ) const
{
  assert( CoR >= 0.0 );
  assert( CoR <= 1.0 );
  assert( ndotv < 0.0 );
  assert( vout.size() % 3 == 0 );
  assert( M.rows() == M.cols() );
  assert( M.nonZeros() == 2 * vout.size() );
  assert( 3 * m_sphere_idx + 2 < vout.size() );

  const Eigen::Map<const VectorXs> m{ M.valuePtr(), vout.size() };
  assert( m( 3 * m_sphere_idx ) == m( 3 * m_sphere_idx + 1 ) );
  assert( m( 3 * m_sphere_idx ) == m( 3 * m_sphere_idx + 2 ) );

  const scalar msphere{ m( 3 * m_sphere_idx ) };

  // Compute the impulse
  alpha = - ( 1.0 + CoR ) * ndotv * msphere;
  assert( alpha >= 0.0 );
  vout.segment<3>( 3 * m_sphere_idx ) += - ( 1.0 + CoR ) * ndotv * m_plane.n();
}

// TODO: Pull the outerIndexPtr arithmetic into a helper function
void MathUtilities::extractColumns( const SparseMatrixsc& A0, const std::vector<unsigned>& cols, SparseMatrixsc& A1 )
{
  const unsigned ncols_to_extract{ static_cast<unsigned>( cols.size() ) };

  assert( ncols_to_extract <= static_cast<unsigned>( A0.cols() ) );
  #ifndef NDEBUG
  for( unsigned i = 0; i < ncols_to_extract; ++i )
  {
    assert( cols[i] < unsigned( A0.cols() ) );
  }
  #endif
    
  // Compute the number of nonzeros in each column of the new matrix
  VectorXi column_nonzeros{ ncols_to_extract };
  for( unsigned i = 0; i < ncols_to_extract; ++i )
  {
    column_nonzeros( i ) = A0.outerIndexPtr()[cols[i]+1] - A0.outerIndexPtr()[cols[i]];
  }

  // Resize A1 and reserve space
  A1.resize( A0.rows(), ncols_to_extract );
  A1.reserve( column_nonzeros );
  // Copy the data over, column by column
  for( unsigned cur_col = 0; cur_col < ncols_to_extract; ++cur_col )
  {
    for( SparseMatrixsc::InnerIterator it( A0, cols[ cur_col ] ); it; ++it )
    {
      A1.insert( it.row(), cur_col ) = it.value();
    }
  }
  
  A1.makeCompressed();
  
  #ifndef NDEBUG
  for( int i = 0 ; i < A1.cols(); ++i )
  {
    assert( ( A1.outerIndexPtr()[i+1] - A1.outerIndexPtr()[i] ) == column_nonzeros( i ) );
  }
  #endif
}

void LCPOperatorQL::flow( const std::vector<std::unique_ptr<Constraint>>& cons, const SparseMatrixsc& M, const SparseMatrixsc& Minv, const VectorXs& q0, const VectorXs& v0, const VectorXs& v0F, const SparseMatrixsc& N, const SparseMatrixsc& Q, const VectorXs& nrel, const VectorXs& CoR, VectorXs& alpha )
{
  // Q in 1/2 \alpha^T Q \alpha
  assert( Q.rows() == Q.cols() );
  MatrixXXsc Qdense = Q;

  // Linear term in the objective
  VectorXs Adense;
  ImpactOperatorUtilities::computeLCPQPLinearTerm( N, nrel, CoR, v0, v0F, Adense );

  // Solve the QP
  assert( Qdense.rows() == Adense.size() ); assert( Adense.size() == alpha.size() );
  const int status = solveQP( m_tol, Qdense, Adense, alpha );

  // Check for problems
  if( 0 != status )
  {
    std::cerr << "Warning, failed to solve QP in LCPOperatorQL::flow: " << QLUtilities::QLReturnStatusToString(status) << std::endl;
  }

  // TODO: Sanity check the solution here
}

void HertzianPenaltyForce::computeForce( const VectorXs& q, const VectorXs& v, const SparseMatrixsc& M, const VectorXs& r, VectorXs& result ) const
{
  assert( q.size() % 2 == 0 ); assert( q.size() == v.size() ); assert( q.size() == M.rows() );
  assert( q.size() == M.cols() ); assert( r.size() == q.size() / 2 ); assert( q.size() == result.size() );

  // For each ball
  for( unsigned ball0 = 0; ball0 < r.size(); ++ball0 )
  {
    // For each subsequent ball
    for( unsigned ball1 = ball0 + 1; ball1 < r.size(); ++ball1 )
    {
      // Compute the total radius
      const scalar total_radius{ r(ball0) + r(ball1) };
      // Compute a vector pointing from ball0 to ball1
      Vector2s n{ q.segment<2>( 2 * ball1 ) - q.segment<2>( 2 * ball0 ) };
      // Compute the squared length of the vector
      scalar d{ n.squaredNorm() };
      // If the squared distance is greater or equal to the sum of the radii squared, no force
      if( d > total_radius * total_radius )
      {
        continue;
      }
      // Normalize the vector between the balls
      d = sqrt( d );
      assert( d != 0.0 );
      n /= d;
      assert( fabs( n.norm() - 1.0 ) <= 1.0e-6 );
      // Compute the penetration depth
      d -= total_radius;
      assert( d < 0.0 );
      // F = 5 * k * pen_depth^(3/2) * n
      const Vector2s F{ ( 5.0 / 4.0 ) * m_k * std::pow( -d, scalar( 1.5 ) ) * n };
      result.segment<2>( 2 * ball1 ) += F;
      result.segment<2>( 2 * ball0 ) -= F;
    }
  }
}