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;
}
// This method and the smooth version share the second half of code. Abstract that out.
void StaticPlaneSphereConstraint::computeGeneralizedFrictionDisk( const VectorXs& q, const VectorXs& v, const int start_column, const int num_samples, SparseMatrixsc& D, VectorXs& drel ) const
{
  assert( start_column >= 0 );
  assert( start_column < D.cols() );
  assert( num_samples > 0 );
  assert( start_column + num_samples - 1 < D.cols() );
  assert( q.size() % 12 == 0 );
  assert( q.size() == 2 * v.size() );

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

  std::vector<Vector3s> friction_disk( static_cast<std::vector<Vector3s>::size_type>( num_samples ) );
  assert( friction_disk.size() == std::vector<Vector3s>::size_type( num_samples ) );
  {
    // Compute the relative velocity
    Vector3s tangent_suggestion{ computeRelativeVelocity( q, v ) };
    if( tangent_suggestion.cross( n ).squaredNorm() < 1.0e-9 )
    {
      tangent_suggestion = FrictionUtilities::orthogonalVector( n );
    }
    tangent_suggestion *= -1.0;

    // Sample the friction disk
    FrictionUtilities::generateOrthogonalVectors( n, friction_disk, tangent_suggestion );
  }
  assert( unsigned( num_samples ) == friction_disk.size() );

  // Compute the displacement from the center of mass to the point of contact
  assert( fabs( n.norm() - 1.0 ) <= 1.0e-10 ); assert( m_r >= 0.0 );
  const Vector3s r_world{ - m_r * n };

  // Cache the velocity of the collision point on the plane
  const Vector3s plane_collision_point_vel{ computePlaneCollisionPointVelocity( q ) };

  // For each sample of the friction disk
  const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };
  for( unsigned friction_sample = 0; friction_sample < unsigned( num_samples ); ++friction_sample )
  {
    const unsigned cur_col{ start_column + friction_sample };
    assert( cur_col < unsigned( D.cols() ) );

    // Effect on center of mass
    D.insert( 3 * m_sphere_idx + 0, cur_col ) = friction_disk[friction_sample].x();
    D.insert( 3 * m_sphere_idx + 1, cur_col ) = friction_disk[friction_sample].y();
    D.insert( 3 * m_sphere_idx + 2, cur_col ) = friction_disk[friction_sample].z();

    // Effect on orientation
    {
      const Vector3s ntilde{ r_world.cross( friction_disk[friction_sample] ) };
      D.insert( 3 * ( nbodies + m_sphere_idx ) + 0, cur_col ) = ntilde.x();
      D.insert( 3 * ( nbodies + m_sphere_idx ) + 1, cur_col ) = ntilde.y();
      D.insert( 3 * ( nbodies + m_sphere_idx ) + 2, cur_col ) = ntilde.z();
    }

    // Relative velocity contribution from kinematic scripting
    assert( cur_col < drel.size() );
    drel( cur_col ) = - friction_disk[friction_sample].dot( plane_collision_point_vel );
  }
}
예제 #3
0
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 );
  }
}
예제 #4
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 );
}
예제 #5
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
}
예제 #6
0
void BodyBodyConstraint::computeGeneralizedFrictionDisk( const VectorXs& q, const VectorXs& v, const int start_column, const int num_samples, SparseMatrixsc& D, VectorXs& drel ) const
{
  assert( start_column >= 0 );
  assert( start_column < D.cols() );
  assert( num_samples > 0 );
  assert( start_column + num_samples - 1 < D.cols() );
  assert( q.size() % 12 == 0 );
  assert( q.size() == 2 * v.size() );

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

  assert( fabs( m_n.norm() - 1.0 ) <= 1.0e-6 );
  std::vector<Vector3s> friction_disk;
  {
    // Compute the relative velocity
    Vector3s tangent_suggestion{ computeRelativeVelocity( q, v ) };
    if( tangent_suggestion.cross( m_n ).squaredNorm() < 1.0e-9 )
    {
      tangent_suggestion = FrictionUtilities::orthogonalVector( m_n );
    }
    tangent_suggestion *= -1.0;

    // Sample the friction disk
    friction_disk.resize( num_samples );
    FrictionUtilities::generateOrthogonalVectors( m_n, friction_disk, tangent_suggestion );
  }
  assert( unsigned( num_samples ) == friction_disk.size() );

  // For each sample of the friction disk
  assert( m_idx0 < m_idx1 );
  for( int i = 0; i < num_samples; ++i )
  {
    const int cur_col{ start_column + i };
    assert( cur_col >= 0 );
    assert( cur_col < D.cols() );

    // Effect on center of mass of body i
    D.insert( 3 * m_idx0 + 0, cur_col ) = friction_disk[i].x();
    D.insert( 3 * m_idx0 + 1, cur_col ) = friction_disk[i].y();
    D.insert( 3 * m_idx0 + 2, cur_col ) = friction_disk[i].z();
    // Effect on orientation of body i
    {
      const Vector3s ttilde0{ m_r0.cross( friction_disk[i] ) };
      D.insert( 3 * ( m_idx0 + nbodies ) + 0, cur_col ) = ttilde0.x();
      D.insert( 3 * ( m_idx0 + nbodies ) + 1, cur_col ) = ttilde0.y();
      D.insert( 3 * ( m_idx0 + nbodies ) + 2, cur_col ) = ttilde0.z();
    }

    // Effect on center of mass of body j
    D.insert( 3 * m_idx1 + 0, cur_col ) = - friction_disk[i].x();
    D.insert( 3 * m_idx1 + 1, cur_col ) = - friction_disk[i].y();
    D.insert( 3 * m_idx1 + 2, cur_col ) = - friction_disk[i].z();
    // Effect on orientation of body j
    {
      const Vector3s ttilde1{ m_r1.cross( friction_disk[i] ) };
      D.insert( 3 * ( m_idx1 + nbodies ) + 0, cur_col ) = - ttilde1.x();
      D.insert( 3 * ( m_idx1 + nbodies ) + 1, cur_col ) = - ttilde1.y();
      D.insert( 3 * ( m_idx1 + nbodies ) + 2, cur_col ) = - ttilde1.z();
    }

    // Relative velocity contribution from kinematic scripting
    assert( cur_col < drel.size() );
    // Zero for now
    drel( cur_col ) = 0.0;
  }
}
예제 #7
0
scalar BodyBodyConstraint::evalNdotV( const VectorXs& q, const VectorXs& v ) const
{
  return m_n.dot( computeRelativeVelocity( q, v ) );
}
예제 #8
0
void BodyBodyConstraint::computeSmoothGeneralizedFrictionDisk( const VectorXs& q, const VectorXs& v, const int start_column, SparseMatrixsc& D ) const
{
  assert( start_column >= 0 );
  assert( start_column < D.cols() );
  assert( start_column+1 < D.cols() );
  assert( q.size() % 12 == 0 );
  assert( q.size() == 2 * v.size() );

  std::vector<Vector3s> friction_disk{ 2 };

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

  // Create a second orthogonal sample in the tangent plane
  friction_disk[1] = m_n.cross( friction_disk[0] ).normalized(); // Don't need to normalize but it won't hurt
  assert( MathUtilities::isRightHandedOrthoNormal( m_n, friction_disk[0], friction_disk[1], 1.0e-6 ) );

  // For each sample of the friction disk
  assert( m_idx0 < m_idx1 );
  const unsigned nbodies{ static_cast<unsigned>( q.size() / 12 ) };
  for( int i = 0; i < 2; ++i )
  {
    const int cur_col = start_column + i;
    assert( cur_col >= 0 );
    assert( cur_col < D.cols() );

    // Effect on center of mass of body i
    D.insert( 3 * m_idx0 + 0, cur_col ) = friction_disk[i].x();
    D.insert( 3 * m_idx0 + 1, cur_col ) = friction_disk[i].y();
    D.insert( 3 * m_idx0 + 2, cur_col ) = friction_disk[i].z();
    // Effect on orientation of body i
    {
      const Vector3s ntilde0{ m_r0.cross( friction_disk[i] ) };
      D.insert( 3 * ( m_idx0 + nbodies ) + 0, cur_col ) = ntilde0.x();
      D.insert( 3 * ( m_idx0 + nbodies ) + 1, cur_col ) = ntilde0.y();
      D.insert( 3 * ( m_idx0 + nbodies ) + 2, cur_col ) = ntilde0.z();
    }

    // Effect on center of mass of body j
    D.insert( 3 * m_idx1 + 0, cur_col ) = - friction_disk[i].x();
    D.insert( 3 * m_idx1 + 1, cur_col ) = - friction_disk[i].y();
    D.insert( 3 * m_idx1 + 2, cur_col ) = - friction_disk[i].z();
    // Effect on orientation of body j
    {
      const Vector3s ntilde1{ m_r1.cross( friction_disk[i] ) };
      D.insert( 3 * ( m_idx1 + nbodies ) + 0, cur_col ) = - ntilde1.x();
      D.insert( 3 * ( m_idx1 + nbodies ) + 1, cur_col ) = - ntilde1.y();
      D.insert( 3 * ( m_idx1 + nbodies ) + 2, cur_col ) = - ntilde1.z();
    }
  }
}