Exemplo n.º 1
0
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 ) );
    }
  }
}
Exemplo n.º 2
0
// Adds the gradient of the penalty potential (-1 * force) for a particle-
// half-plane pair to the total.
// Read the positions of the particle from the input variable x.
// Inputs:
//   x:    The positions of the particles in the scene.
//   vidx: The index of the particle.
//   pidx: The index of the half-plane, i.e. the position and normal vectors
//         for the half-plane can be retrieved by calling
//         m_scene.getHalfplane(pidx).
// Outputs:
//   gradE: The total gradient of the penalty force. *ADD* the particle-
//          half-plane gradient to this total gradient.
void PenaltyForce::addParticleHalfplaneGradEToTotal(const VectorXs &x, int vidx, int pidx, VectorXs &gradE)
{
//    VectorXs x1 = x.segment<2>(2*vidx);
//    VectorXs nh = m_scene.getHalfplane(pidx).second;
//    
//    // your implementation here
//    
    VectorXs x1 = x.segment<2>(2*vidx);
    VectorXs nh = m_scene.getHalfplane(pidx).second;
    VectorXs n = (m_scene.getHalfplane(pidx).first - x1).dot(nh)/(nh.dot(nh))*nh;
    VectorXs nhat = n;
    nhat.normalize();
    
    double r = m_scene.getRadius(vidx);
    
    if(n.norm() < r + m_thickness)
    {
        gradE.segment<2>(2*vidx) -= m_k*(n.norm()-r - m_thickness)*nhat.dot(nh)/(nh.dot(nh))*nh;
    }
}
Exemplo n.º 3
0
// 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 );
}
Exemplo n.º 4
0
static VectorXs parallelTransport2D( const VectorXs& n0, const VectorXs& n1, const VectorXs& t0 )
{
  assert( fabs( n0.norm() - 1.0 ) <= 1.0e-6 ); assert( fabs( n1.norm() - 1.0 ) <= 1.0e-6 );

  // x is cos of angle, y is sin of angle
  const Vector2s r{ n0.dot( n1 ), MathUtilities::cross( n0, n1 ) };
  assert( fabs( r.norm() - 1.0 ) <= 1.0e-6 );

  // Rotate t0
  VectorXs t1{ 2 };
  t1( 0 ) = r.x() * t0.x() - r.y() * t0.y();
  t1( 1 ) = r.y() * t0.x() + r.x() * t0.y();
  assert( fabs( t0.norm() - t1.norm() ) <= 1.0e-6 );
  // TODO: Assert n0,t0 angle is same as n1,t1 angle

  return t1;
}
Exemplo n.º 5
0
// 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 );
}
Exemplo n.º 6
0
// 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
}