C++ MatrixXXsc类代码示例

void StaticPlaneSphereConstraint::computeContactBasis( const VectorXs& q, const VectorXs& v, MatrixXXsc& basis ) const
{
  const Vector3s n{ m_plane.n() };
  assert( fabs( n.norm() - 1.0 ) <= 1.0e-6 );

  // Compute the relative velocity to use as a direction for the tangent sample
  Vector3s s{ computeRelativeVelocity( q, v ) };
  // If the relative velocity is zero, any vector will do
  if( n.cross( s ).squaredNorm() < 1.0e-9 )
  {
    s = FrictionUtilities::orthogonalVector( n );
  }
  // Otherwise project out the component along the normal and normalize the relative velocity
  else
  {
    s = ( s - s.dot( n ) * n ).normalized();
  }
  // Invert the tangent vector in order to oppose
  s *= -1.0;

  // Create a second orthogonal sample in the tangent plane
  const Vector3s t{ n.cross( s ).normalized() }; // Don't need to normalize but it won't hurt

  assert( MathUtilities::isRightHandedOrthoNormal( n, s, t, 1.0e-6 ) );
  basis.resize( 3, 3 );
  basis.col( 0 ) = n;
  basis.col( 1 ) = s;
  basis.col( 2 ) = t;
}

void TeleportedCircleCircleConstraint::computeContactBasis( const VectorXs& q, const VectorXs& v, MatrixXXsc& basis ) const
{
  assert( fabs( m_n.norm() - 1.0 ) <= 1.0e-6 );
  const Vector2s t{ -m_n.y(), m_n.x() };
  assert( fabs( t.norm() - 1.0 ) <= 1.0e-6 ); assert( fabs( m_n.dot( t ) ) <= 1.0e-6 );
  basis.resize( 2, 2 );
  basis.col( 0 ) = m_n;
  basis.col( 1 ) = t;
}

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 RigidBody2DSim::computeImpactBases( const VectorXs& q, const std::vector<std::unique_ptr<Constraint>>& active_set, MatrixXXsc& impact_bases ) const
{
  const unsigned ncols{ static_cast<unsigned>( active_set.size() ) };
  impact_bases.resize( 2, ncols );
  for( unsigned col_num = 0; col_num < ncols; ++col_num )
  {
    VectorXs current_normal;
    active_set[col_num]->getWorldSpaceContactNormal( q, current_normal );
    assert( fabs( current_normal.norm() - 1.0 ) <= 1.0e-6 );
    impact_bases.col( col_num ) = current_normal;
  }
}

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 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();
}

// TODO: Despecialize from smooth
void FrictionOperator::formGeneralizedSmoothFrictionBasis( const unsigned ndofs, const unsigned ncons, const VectorXs& q, const std::vector<std::unique_ptr<Constraint>>& K, const MatrixXXsc& bases, SparseMatrixsc& D )
{
  assert( ncons == K.size() );

  const unsigned nambientdims{ static_cast<unsigned>( bases.rows() ) };
  const unsigned nsamples{ nambientdims - 1 };

  D.resize( ndofs, nsamples * ncons );

  auto itr = K.cbegin();
  {
    VectorXi column_nonzeros( D.cols() );
    for( unsigned collision_number = 0; collision_number < ncons; ++collision_number )
    {
      for( unsigned sample_number = 0; sample_number < nsamples; ++sample_number )
      {
        assert( nsamples * collision_number + sample_number < column_nonzeros.size() );
        column_nonzeros( nsamples * collision_number + sample_number ) = (*itr)->frictionStencilSize();
      }
      ++itr;
    }
    assert( ( column_nonzeros.array() > 0 ).all() );
    assert( itr == K.cend() );
    D.reserve( column_nonzeros );
  }

  itr = K.cbegin();
  for( unsigned collision_number = 0; collision_number < ncons; ++collision_number )
  {
    for( unsigned sample_number = 0; sample_number < nsamples; ++sample_number )
    {
      const unsigned current_column{ nsamples * collision_number + sample_number };
      const VectorXs current_sample{ bases.col( nambientdims * collision_number + sample_number + 1 ) };
      assert( fabs( current_sample.dot( bases.col( nambientdims * collision_number ) ) ) <= 1.0e-6 );
      (*itr)->computeGeneralizedFrictionGivenTangentSample( q, current_sample, current_column, D );
    }
    ++itr;
  }
  assert( itr == K.cend() );

  D.prune( []( const Eigen::Index& row, const Eigen::Index& col, const scalar& value ) { return value != 0.0; } );
  assert( D.innerNonZeroPtr() == nullptr );
}

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 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 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
}

// 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
}

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 );
}

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 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 );
  }
}

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 );
}