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

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

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

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

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

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